Fix warm-start handling in constrained solvers

Reuse solver state across repeated constrained solves so warm starts
can keep their gains, and refresh ALDDP slack controls when a new
trajectory seed is supplied.

Simplify LogDDP barrier evaluation for single-shooting mode and add
regression tests for reseeded trajectories and invalid rollout options.

Author: astomodynamics

include/cddp-cpp/cddp_core/barrier.hpp

@@ -59,8 +59,8 @@ class RelaxedLogBarrier {
    * @return Barrier function value.
    */
   double evaluate(const Constraint &constraint, const Eigen::VectorXd &state,
-                  const Eigen::VectorXd &control) const {
-    Eigen::VectorXd g_val = constraint.evaluate(state, control);
+                  const Eigen::VectorXd &control, int index = 0) const {
+    Eigen::VectorXd g_val = constraint.evaluate(state, control, index);
     Eigen::VectorXd L = constraint.getLowerBound();
     Eigen::VectorXd U = constraint.getUpperBound();
     int constraint_dim = g_val.size();
@@ -99,13 +99,13 @@ class RelaxedLogBarrier {
    */
   std::tuple<Eigen::VectorXd, Eigen::VectorXd>
   getGradients(const Constraint &constraint, const Eigen::VectorXd &state,
-               const Eigen::VectorXd &control) const {
+               const Eigen::VectorXd &control, int index = 0) const {
 
-    Eigen::VectorXd g_val = constraint.evaluate(state, control);
+    Eigen::VectorXd g_val = constraint.evaluate(state, control, index);
     Eigen::VectorXd L = constraint.getLowerBound();
     Eigen::VectorXd U = constraint.getUpperBound();
-    Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control);
-    Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control);
+    Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control, index);
+    Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control, index);
 
     int state_dim = state.size();
     int control_dim = control.size();
@@ -151,13 +151,13 @@ class RelaxedLogBarrier {
    */
   std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd>
   getHessians(const Constraint &constraint, const Eigen::VectorXd &state,
-              const Eigen::VectorXd &control) const {
+              const Eigen::VectorXd &control, int index = 0) const {
 
-    Eigen::VectorXd g_val = constraint.evaluate(state, control);
+    Eigen::VectorXd g_val = constraint.evaluate(state, control, index);
     Eigen::VectorXd L = constraint.getLowerBound();
     Eigen::VectorXd U = constraint.getUpperBound();
-    Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control);
-    Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control);
+    Eigen::MatrixXd Gx = constraint.getStateJacobian(state, control, index);
+    Eigen::MatrixXd Gu = constraint.getControlJacobian(state, control, index);
 
     int state_dim = state.size();
     int control_dim = control.size();
@@ -171,7 +171,7 @@ class RelaxedLogBarrier {
         Gux_constraint_vec;
     bool constraint_provides_hessians = true;
     try {
-      auto hess_tuple = constraint.getHessians(state, control);
+      auto hess_tuple = constraint.getHessians(state, control, index);
       Gxx_constraint_vec = std::get<0>(hess_tuple);
       Guu_constraint_vec = std::get<1>(hess_tuple);
       Gux_constraint_vec = std::get<2>(hess_tuple);
@@ -536,4 +536,4 @@ class DiscreteBarrierState {
 
 } // namespace cddp
 
-#endif // CDDP_BARRIER_HPP
+#endif // CDDP_BARRIER_HPP

include/cddp-cpp/cddp_core/logddp_solver.hpp

@@ -56,12 +56,6 @@ class LogDDPSolver : public CDDPSolverBase {
   void printIteration(int iter, const CDDP &context) const override;
 
 private:
-  // Dynamics storage (forward-simulated trajectory)
-  std::vector<Eigen::VectorXd> F_;
-
-  // Constraint values g(x,u) - g_ub
-  std::map<std::string, std::vector<Eigen::VectorXd>> G_;
-
   // Log-barrier method
   std::unique_ptr<RelaxedLogBarrier> relaxed_log_barrier_;
   double mu_;
@@ -70,9 +64,6 @@ class LogDDPSolver : public CDDPSolverBase {
   // Filter-based line search
   double constraint_violation_;
 
-  // Multi-shooting parameters
-  int ms_segment_length_;
-
   /**
    * @brief Evaluate trajectory by computing cost, dynamics, and merit function.
    */
@@ -82,6 +73,22 @@ class LogDDPSolver : public CDDPSolverBase {
    * @brief Reset/initialize the filter for line search.
    */
   void resetFilter(CDDP &context);
+
+  void augmentRunningCostDerivatives(
+      const CDDP &context, int t, const Eigen::VectorXd &x,
+      const Eigen::VectorXd &u, Eigen::VectorXd &l_x, Eigen::VectorXd &l_u,
+      Eigen::MatrixXd &l_xx, Eigen::MatrixXd &l_uu,
+      Eigen::MatrixXd &l_ux) const;
+
+  void augmentTerminalCostDerivatives(const CDDP &context,
+                                      const Eigen::VectorXd &x_N,
+                                      Eigen::VectorXd &V_x,
+                                      Eigen::MatrixXd &V_xx) const;
+
+  double computeBarrierMerit(const CDDP &context,
+                             const std::vector<Eigen::VectorXd> &X,
+                             const std::vector<Eigen::VectorXd> &U,
+                             double *max_constraint_violation = nullptr) const;
 };
 
 } // namespace cddp

src/cddp_core/alddp_solver.cpp

@@ -63,6 +63,11 @@ void ALDDPSolver::initialize(CDDP &context) {
                   << std::endl;
       }
       boxqp_solver_.setOptions(options.box_qp);
+      // The solver state (gains, multipliers, penalties) is reused, but the
+      // current seeded trajectory may have changed since the last solve.
+      // Refresh slack controls so warm starts remain consistent with the
+      // current X/U pair, including dynamically infeasible reseeds.
+      initializeSlackControls(context);
       if (!context.X_.empty() && !context.U_.empty()) {
         computeCost(context);
       }

src/cddp_core/cddp_core.cpp

@@ -269,6 +269,26 @@ std::string solverTypeToString(SolverType solver_type) {
     return "CLDDP"; // Default fallback
   }
 }
+
+std::string canonicalizeSolverType(const std::string &solver_type) {
+  if (solver_type == "CLCDDP" || solver_type == "CLDDP") {
+    return "CLDDP";
+  }
+  if (solver_type == "LogDDP" || solver_type == "LOGDDP") {
+    return "LogDDP";
+  }
+  if (solver_type == "IPDDP") {
+    return "IPDDP";
+  }
+  if (solver_type == "MSIPDDP") {
+    return "MSIPDDP";
+  }
+  if (solver_type == "ALDDP") {
+    return "ALDDP";
+  }
+
+  return solver_type;
+}
 } // namespace
 
 CDDPSolution CDDP::solve(SolverType solver_type) {
@@ -302,25 +322,30 @@ CDDP::createSolver(const std::string &solver_type) {
 
 CDDPSolution CDDP::solve(const std::string &solver_type) {
   // This is where strategy selection and invocation will happen.
+  const std::string canonical_solver_type = canonicalizeSolverType(solver_type);
 
   initializeProblemIfNecessary(); // Ensure X_, U_ are sized etc.
 
-  // Strategy selection and instantiation
-  solver_ = createSolver(solver_type);
+  // Preserve solver state across repeated solves with the same algorithm so
+  // warm-start options can reuse stored gains.
+  if (!solver_ || solver_->getSolverName() != canonical_solver_type) {
+    solver_ = createSolver(canonical_solver_type);
+  }
 
   if (!solver_) {
     // Solver not found - return error solution
     CDDPSolution solution;
-    solution.solver_name = solver_type;
+    solution.solver_name = canonical_solver_type;
     solution.status_message =
-        "UnknownSolver - No solver registered for '" + solver_type + "'";
+        "UnknownSolver - No solver registered for '" + canonical_solver_type +
+        "'";
     solution.iterations_completed = 0;
     solution.solve_time_ms = 0.0;
     solution.final_objective = 0.0;
     solution.final_step_length = 1.0;
 
     if (options_.verbose) {
-      std::cout << "Solver type '" << solver_type
+      std::cout << "Solver type '" << canonical_solver_type
                 << "' not found. Available solvers: ";
       auto available = getRegisteredSolvers();
       for (const auto &name : available) {