Add ALDDP solver support with Python bindings, tests, and packaging/build hardening

CMakeLists.txt

@@ -136,6 +136,7 @@ set(cddp_core_srcs
   src/cddp_core/logddp_solver.cpp
   src/cddp_core/ipddp_solver.cpp
   src/cddp_core/msipddp_solver.cpp
+  src/cddp_core/alddp_solver.cpp
 )
 
 set(dynamics_model_srcs

include/cddp-cpp/cddp.hpp

@@ -32,6 +32,7 @@
 #include "cddp_core/logddp_solver.hpp"
 #include "cddp_core/ipddp_solver.hpp"
 #include "cddp_core/msipddp_solver.hpp"
+#include "cddp_core/alddp_solver.hpp"
 #include "cddp_core/helper.hpp"
 #include "cddp_core/boxqp.hpp"
 #include "cddp_core/qp_solver.hpp"

include/cddp-cpp/cddp_core/alddp_solver.hpp

@@ -0,0 +1,141 @@
+/*
+ Copyright 2025 Tomo Sasaki
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+      https://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+
+#ifndef CDDP_ALDDP_SOLVER_HPP
+#define CDDP_ALDDP_SOLVER_HPP
+
+#include "cddp_core/boxqp.hpp"
+#include "cddp_core/cddp_solver_base.hpp"
+#include "cddp_core/constraint.hpp"
+#include <Eigen/Dense>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace cddp {
+
+/**
+ * @brief Augmented Lagrangian DDP (ALDDP) solver implementation.
+ *
+ * Implements an ALTRO-like solver that uses augmented Lagrangian penalization
+ * to handle general nonlinear state and control constraints. Features:
+ * - AL outer loop with multiplier/penalty updates
+ * - BoxQP for control bounds (optional, like CLDDP)
+ * - Infeasible state trajectory initialization via slack controls
+ * - Square-root backward pass for numerical robustness
+ */
+class ALDDPSolver : public CDDPSolverBase {
+public:
+  ALDDPSolver();
+
+  void initialize(CDDP &context) override;
+  std::string getSolverName() const override;
+
+protected:
+  // CDDPSolverBase hooks
+  void preIterationSetup(CDDP &context) override;
+  bool backwardPass(CDDP &context) override;
+  bool checkEarlyConvergence(CDDP &context, int iter,
+                             std::string &reason) override;
+  ForwardPassResult forwardPass(CDDP &context, double alpha) override;
+  void applyForwardPassResult(CDDP &context,
+                              const ForwardPassResult &result) override;
+  bool checkConvergence(CDDP &context, double dJ, double dL, int iter,
+                        std::string &reason) override;
+  void postIterationUpdate(CDDP &context, bool forward_pass_success) override;
+  bool handleBackwardPassRegularizationLimit(
+      CDDP &context, std::string &termination_reason) override;
+  void recordIterationHistory(const CDDP &context) override;
+  void populateSolverSpecificSolution(CDDPSolution &solution,
+                                      const CDDP &context) override;
+  void printIteration(int iter, const CDDP &context) const override;
+  void printSolutionSummary(const CDDPSolution &solution) const override;
+
+private:
+  // --- AL variables ---
+  // Per-constraint, per-timestep Lagrange multipliers (dim = getDualDim())
+  std::map<std::string, std::vector<Eigen::VectorXd>> lambda_;
+  // Per-constraint, per-timestep penalty weights
+  std::map<std::string, std::vector<Eigen::VectorXd>> penalty_;
+  // Terminal constraint multipliers and penalties
+  std::map<std::string, Eigen::VectorXd> terminal_lambda_;
+  std::map<std::string, Eigen::VectorXd> terminal_penalty_;
+
+  // Constraint evaluation cache (only written by evaluateConstraints, read by backward/forward pass)
+  std::map<std::string, std::vector<Eigen::VectorXd>> G_;
+  std::map<std::string, Eigen::VectorXd> G_terminal_;
+  std::map<std::string, std::vector<Eigen::MatrixXd>> G_x_;
+  std::map<std::string, std::vector<Eigen::MatrixXd>> G_u_;
+
+  // BoxQP for control bounds
+  BoxQPSolver boxqp_solver_;
+
+  // AL outer loop state
+  int al_outer_iter_ = 0;
+  double inner_tolerance_ = 1e-2;
+  double max_constraint_violation_ = 0.0;
+  double prev_max_constraint_violation_ = 0.0;
+  double prev_outer_cost_ = 0.0;
+  bool inner_converged_ = false;
+  int inner_iter_count_ = 0;
+
+  // Infeasible initialization
+  bool infeasible_start_ = false;
+  double current_slack_penalty_ = 1000.0;
+  std::vector<Eigen::VectorXd> S_;   // Slack controls s_k (state_dim each)
+  std::vector<Eigen::VectorXd> k_s_; // Slack feedforward gains
+  std::vector<Eigen::MatrixXd> K_s_; // Slack feedback gains
+
+  // Square-root backward pass workspace
+  std::vector<Eigen::MatrixXd> S_chol_; // Upper-triangular Cholesky factors √P_k
+  std::vector<Eigen::VectorXd> p_;      // Cost-to-go gradient vectors
+
+  // --- Private helpers ---
+  void initializeMultipliersAndPenalties(CDDP &context);
+  void initializeSlackControls(CDDP &context);
+  void evaluateConstraints(CDDP &context);
+  void updateMultipliers(CDDP &context);
+  void updatePenalties(CDDP &context);
+  double computeMaxConstraintViolation() const;
+  double computeMaxSlackNorm() const;
+
+  // Backward pass variants
+  bool backwardPassStandard(CDDP &context);
+  bool backwardPassSqrt(CDDP &context);
+
+  // AL cost derivative augmentation helpers
+  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;
+
+  // Compute AL merit for a given trajectory (thread-safe, no member writes)
+  double computeALMerit(
+      const CDDP &context,
+      const std::vector<Eigen::VectorXd> &X,
+      const std::vector<Eigen::VectorXd> &U,
+      const std::vector<Eigen::VectorXd> *S_trial = nullptr) const;
+};
+
+} // namespace cddp
+
+#endif // CDDP_ALDDP_SOLVER_HPP

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/cddp_core.hpp

@@ -45,7 +45,8 @@ enum class SolverType {
   CLDDP,   ///< Control-Limited Differential Dynamic Programming
   LogDDP,  ///< Log-Barrier Differential Dynamic Programming
   IPDDP,   ///< Interior Point Differential Dynamic Programming
-  MSIPDDP  ///< Multi-Shooting Interior Point Differential Dynamic Programming
+  MSIPDDP, ///< Multi-Shooting Interior Point Differential Dynamic Programming
+  ALDDP    ///< Augmented Lagrangian Differential Dynamic Programming
 };
 
 /**

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

include/cddp-cpp/cddp_core/options.hpp

@@ -164,6 +164,42 @@ namespace cddp
         double terminal_constraint_tolerance = 1e-6; ///< Tolerance for terminal constraint satisfaction.
     };
 
+    /**
+     * @brief Options for the ALDDP (Augmented Lagrangian DDP) solver.
+     *
+     * ALDDP uses an augmented Lagrangian method to handle general nonlinear
+     * state and control constraints. The solver alternates between an inner
+     * iLQR loop (which minimizes the augmented Lagrangian) and an outer loop
+     * that updates Lagrange multipliers and penalty weights.
+     */
+    struct ALDDPAlgorithmOptions
+    {
+        // Multiplier/penalty initialization
+        double lambda_init = 0.0;       ///< Initial Lagrange multiplier value.
+        double penalty_init = 1.0;      ///< Initial penalty weight.
+        double penalty_max = 1e8;       ///< Maximum penalty weight.
+        double penalty_update_factor = 10.0; ///< Factor to increase penalties.
+
+        // Inner loop control
+        int max_inner_iterations = 50;        ///< Max iLQR iterations per AL outer iteration.
+        double inner_tolerance_init = 1e-2;   ///< Initial inner iLQR convergence tolerance.
+        double inner_tolerance_factor = 0.1;  ///< Factor to tighten inner tolerance.
+        double inner_tolerance_min = 1e-6;    ///< Minimum inner tolerance.
+
+        // Outer loop control
+        int max_outer_iterations = 20;        ///< Max AL outer iterations.
+        double constraint_tolerance = 1e-6;   ///< Constraint satisfaction tolerance.
+
+        // Control constraint handling
+        bool use_boxqp_for_controls = true;   ///< Use BoxQP for control bounds (like CLDDP).
+
+        // Infeasible initialization (ALTRO Eqs. 30-32)
+        double slack_penalty = 1000.0;        ///< R_s weight on slack penalty term.
+
+        // Square-root backward pass (ALTRO Eqs. 19-29)
+        bool use_sqrt_backward_pass = false;  ///< QR-based factorization for numerical robustness.
+    };
+
     /**
      * @brief Main options structure for the CDDP solver.
      *
@@ -210,6 +246,8 @@ namespace cddp
         IPDDPAlgorithmOptions ipddp; ///< Comprehensive options for the IPDDP solver.
         MSIPDDPAlgorithmOptions
             msipddp;                 ///< Comprehensive options for the MSIPDDP solver.
+        ALDDPAlgorithmOptions
+            alddp;                   ///< Comprehensive options for the ALDDP solver.
 
         // Constructor with defaults (relies on member initializers)
         CDDPOptions() = default;

python/pycddp/__init__.py

@@ -24,6 +24,7 @@
     LogBarrierOptions,
     IPDDPOptions,
     MSIPDDPOptions,
+    ALDDPOptions,
 
     # Core solver
     CDDP,

python/src/bind_options.cpp

@@ -18,7 +18,8 @@ void bind_options(py::module_& m) {
         .value("CLDDP", cddp::SolverType::CLDDP)
         .value("LogDDP", cddp::SolverType::LogDDP)
         .value("IPDDP", cddp::SolverType::IPDDP)
-        .value("MSIPDDP", cddp::SolverType::MSIPDDP);
+        .value("MSIPDDP", cddp::SolverType::MSIPDDP)
+        .value("ALDDP", cddp::SolverType::ALDDP);
 
     // BoxQPOptions
     py::class_<cddp::BoxQPOptions>(m, "BoxQPOptions")
@@ -103,6 +104,23 @@ void bind_options(py::module_& m) {
         .def_readwrite("use_controlled_rollout", &cddp::MSIPDDPAlgorithmOptions::use_controlled_rollout)
         .def_readwrite("costate_var_init_scale", &cddp::MSIPDDPAlgorithmOptions::costate_var_init_scale);
 
+    // ALDDPAlgorithmOptions exposed as a flat Python binding.
+    py::class_<cddp::ALDDPAlgorithmOptions>(m, "ALDDPOptions")
+        .def(py::init<>())
+        .def_readwrite("lambda_init", &cddp::ALDDPAlgorithmOptions::lambda_init)
+        .def_readwrite("penalty_init", &cddp::ALDDPAlgorithmOptions::penalty_init)
+        .def_readwrite("penalty_max", &cddp::ALDDPAlgorithmOptions::penalty_max)
+        .def_readwrite("penalty_update_factor", &cddp::ALDDPAlgorithmOptions::penalty_update_factor)
+        .def_readwrite("max_inner_iterations", &cddp::ALDDPAlgorithmOptions::max_inner_iterations)
+        .def_readwrite("inner_tolerance_init", &cddp::ALDDPAlgorithmOptions::inner_tolerance_init)
+        .def_readwrite("inner_tolerance_factor", &cddp::ALDDPAlgorithmOptions::inner_tolerance_factor)
+        .def_readwrite("inner_tolerance_min", &cddp::ALDDPAlgorithmOptions::inner_tolerance_min)
+        .def_readwrite("max_outer_iterations", &cddp::ALDDPAlgorithmOptions::max_outer_iterations)
+        .def_readwrite("constraint_tolerance", &cddp::ALDDPAlgorithmOptions::constraint_tolerance)
+        .def_readwrite("use_boxqp_for_controls", &cddp::ALDDPAlgorithmOptions::use_boxqp_for_controls)
+        .def_readwrite("slack_penalty", &cddp::ALDDPAlgorithmOptions::slack_penalty)
+        .def_readwrite("use_sqrt_backward_pass", &cddp::ALDDPAlgorithmOptions::use_sqrt_backward_pass);
+
     // CDDPOptions
     py::class_<cddp::CDDPOptions>(m, "CDDPOptions")
         .def(py::init<>())
@@ -126,5 +144,6 @@ void bind_options(py::module_& m) {
         .def_readwrite("filter", &cddp::CDDPOptions::filter)
         .def_readwrite("log_barrier", &cddp::CDDPOptions::log_barrier)
         .def_readwrite("ipddp", &cddp::CDDPOptions::ipddp)
-        .def_readwrite("msipddp", &cddp::CDDPOptions::msipddp);
+        .def_readwrite("msipddp", &cddp::CDDPOptions::msipddp)
+        .def_readwrite("alddp", &cddp::CDDPOptions::alddp);
 }

python/src/bind_solver.cpp

@@ -96,6 +96,7 @@ bool isKnownSolverName(const std::string &solver_name) {
     return solver_name == "CLDDP" || solver_name == "CLCDDP" ||
            solver_name == "LogDDP" || solver_name == "LOGDDP" ||
            solver_name == "IPDDP" || solver_name == "MSIPDDP" ||
+           solver_name == "ALDDP" ||
            cddp::CDDP::isSolverRegistered(solver_name);
 }
 

python/tests/test_constraints.py

@@ -112,6 +112,8 @@ def test_custom_python_constraint_with_solver():
     assert counters["evaluate"] > 0
     assert counters["state_jacobian"] > 0
     assert counters["control_jacobian"] > 0
+
+
 def test_constraint_base_is_rejected_cleanly():
     dt = 0.1
     opts = pycddp.CDDPOptions()