Refactor: Enhance IPDDP Solver Options and Forward Pass Logic

examples/cddp_car_ipddp.cpp

@@ -189,7 +189,6 @@ int main() {
     options.barrier_coeff = 1e-0;
     options.dual_scale = 1e-1;
     options.slack_scale = 1e-2;
-    options.defect_violation_penalty_initial = 1e-0;
     options.ms_segment_length = horizon / 100;
     options.ms_rollout_type = "nonlinear";
     cddp_solver.setOptions(options);

examples/cddp_cartpole.cpp

@@ -21,6 +21,7 @@
 #include <memory>
 #include "cddp.hpp"
 #include "matplot/matplot.h"
+#include <random>
 
 using namespace matplot;
 namespace fs = std::filesystem;
@@ -74,19 +75,32 @@ int main() {
     Eigen::VectorXd control_upper_bound(control_dim);
     control_upper_bound << 5.0;   // Maximum positive force.
 
+    // FIXME: For MSIPDDP 
     cddp_solver.addConstraint("ControlConstraint", 
         std::make_unique<cddp::ControlConstraint>( control_upper_bound));
 
+    // FIXME: For CLDDP
+    // cddp_solver.addConstraint("ControlBoxConstraint", 
+    //     std::make_unique<cddp::ControlBoxConstraint>( control_lower_bound, control_upper_bound));
+
     // Solver options.
     cddp::CDDPOptions options;
     options.max_iterations = 500;
+    options.cost_tolerance = 1e-7;
+    options.grad_tolerance = 1e-6;
     options.regularization_type = "control";
-    options.regularization_control = 1e-3;
+    options.regularization_control = 1e-5;
     options.is_ilqr = true;
+    options.use_parallel = true;
+    options.num_threads = 12;
     options.debug = false;
-    options.defect_violation_penalty_initial = 1e-0;
-    options.ms_segment_length = horizon / 20;
+    options.barrier_coeff = 1e-1;
+    options.ms_segment_length = horizon;
     options.ms_rollout_type = "nonlinear";
+    options.ms_defect_tolerance_for_single_shooting = 1e-5;
+    options.barrier_update_factor = 0.2;
+    options.barrier_update_power = 1.2;
+    options.minimum_reduction_ratio = 1e-4;
     cddp_solver.setOptions(options);
 
     // Initial trajectory.
@@ -96,10 +110,28 @@ int main() {
     for (int i = 0; i < horizon + 1; ++i) {
         X[i] = initial_state;
     }
+    // // Generate initial trajectory by random
+    // for (int i = 0; i < horizon + 1; ++i) {
+    //     std::random_device rd;
+    //     std::mt19937 gen(rd());
+    //     std::uniform_real_distribution<double> dist(-0.025, 0.025);
+    //     X[i] = initial_state;
+    //     X[i](0) += dist(gen);
+    //     X[i](1) += dist(gen);
+    //     X[i](2) += dist(gen);
+    //     X[i](3) += dist(gen);
+    // }
+    // for (int i = 0; i < horizon; ++i) {
+    //     std::random_device rd;
+    //     std::mt19937 gen(rd());
+    //     std::uniform_real_distribution<double> dist(-0.005, 0.005);
+    //     U[i] = Eigen::VectorXd::Zero(control_dim);
+    //     U[i](0) += dist(gen);
+    // }
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve.
-    cddp::CDDPSolution solution = cddp_solver.solve("MSIPDDP");
+    cddp::CDDPSolution solution = cddp_solver.solve("IPDDP");
     auto X_sol = solution.state_sequence;
     auto U_sol = solution.control_sequence;
     auto t_sol = solution.time_sequence;

examples/cddp_quadrotor_circle.cpp

@@ -202,6 +202,12 @@ int main()
     options.regularization_control = 1e-4;
     options.regularization_state = 0.0;
     options.barrier_coeff = 1e-3;
+    options.ms_segment_length = horizon / 10;
+    options.ms_rollout_type = "nonlinear";
+    options.ms_defect_tolerance_for_single_shooting = 1e-5;
+    options.barrier_update_factor = 0.2;
+    options.barrier_update_power = 1.2;
+    options.minimum_reduction_ratio = 1e-4;
 
     // Create the CDDP solver
     cddp::CDDP cddp_solver(

examples/cddp_quadrotor_figure_eight_horizontal.cpp

@@ -201,7 +201,6 @@ int main()
     options.regularization_state = 0.0;
     options.barrier_coeff = 1e-1;
     options.is_ilqr = true;
-    options.defect_violation_penalty_initial = 1e+2;
     options.ms_segment_length = horizon / 10;
     options.ms_rollout_type = "nonlinear";
 

examples/cddp_spacecraft_rpo.cpp

@@ -174,7 +174,6 @@ int main()
     options.barrier_coeff = 1e-0; // Starting barrier coefficient
     options.is_ilqr = true;
     options.debug = false;
-    options.defect_violation_penalty_initial = 1e-0;
     options.ms_segment_length = horizon;
     options.ms_rollout_type = "nonlinear";
 

examples/cddp_spacecraft_rpo_fuel.cpp

@@ -206,7 +206,6 @@ int main()
     options.barrier_coeff = 1e-1; // Starting barrier coefficient
     options.is_ilqr = true;
     options.debug = true;
-    options.defect_violation_penalty_initial = 1e-0;
     options.ms_segment_length = horizon / 10;
     options.ms_rollout_type = "nonlinear";
 

examples/cddp_unicycle_safe_ipddp.cpp

@@ -66,14 +66,14 @@ int main() {
 
     // CDDP options
     cddp::CDDPOptions options;
-    options.max_iterations = 10;
+    options.max_iterations = 100;
     options.verbose = true;
     options.debug = false;
     options.use_parallel = false;
     options.num_threads = 1;
-    options.cost_tolerance = 1e-5;
-    options.grad_tolerance = 1e-4;
-    options.regularization_type = "both";
+    options.cost_tolerance = 1e-4;
+    options.grad_tolerance = 1e-3;
+    options.regularization_type = "control";
     options.regularization_control = 1e-2;
     options.regularization_state = 1e-3;
     options.barrier_coeff = 1e-1;

include/cddp-cpp/cddp_core/cddp_core.hpp

@@ -69,6 +69,13 @@ struct CDDPOptions {
     double slack_scale = 1e-2;                      // Initial scale for slack variables
     double lambda_scale = 1e-6;                     // Initial scale for lambda variables
 
+    // ipddp line-search options
+    double filter_merit_acceptance = 1e-6;         // Small value for merit filter acceptance
+    double filter_violation_acceptance = 1e-6;     // Small value for violation filter acceptance
+    double filter_maximum_violation = 1e-2;         // Maximum violation for filter acceptance
+    double filter_minimum_violation = 1e-6;         // Minimum violation for filter acceptance
+    double armijo_constant = 1e-4;                   // Armijo constant c1 for filter acceptance
+
     // Regularization options
     std::string regularization_type = "control";    // different regularization types: ["none", "control", "state", "both"]
 
@@ -103,13 +110,13 @@ struct CDDPOptions {
     double boxqp_armijo = 0.1;                      // Armijo parameter for boxqp
     bool boxqp_verbose = false;                     // Print debug info for boxqp
 
-    // msipddp options
-    double defect_violation_penalty_initial = 1.0; // Initial defect penalty
-    double ms_defect_penalty_rho = 0.5;             // Rho parameter for defect penalty update
-    double ms_defect_penalty_kappa_d = 1e-4;        // Kappa_d threshold for defect penalty update
-    double ms_defect_penalty_mu0 = 10.0;            // Mu_0 safety margin for defect penalty update
+    // msipddp optionsupdate
     int ms_segment_length = 5;             // Number of initial steps to use nonlinear dynamics in hybrid rollout (0=fully linear, horizon=fully nonlinear)
     std::string ms_rollout_type = "hybrid"; // Rollout type: ["linear", "nonlinear", "hybrid"]
+    double ms_defect_tolerance_for_single_shooting = 1e-3; // Defect norm tolerance to switch to single shooting at segment boundaries
+    double barrier_update_factor = 0.2; // Factor for barrier update: optimality_gap <= barrier_update_factor * mu; [0.0, 1.0]
+    double barrier_update_power = 1.2; // Power for barrier update: mu_new = mu * barrier_update_power; [1.0, 2.0]
+    double minimum_fraction_to_boundary = 0.99; // Minimum fraction to boundary for barrier update: tau = std::max(0.99, 1.0 - mu_);
 };
 
 struct CDDPSolution {
@@ -326,7 +333,6 @@ class CDDP {
     bool solveIPDDPBackwardPass();
     void resetIPDDPFilter();
     void initialIPDDPRollout();
-    void resetIPDDPRegularization();
 
     // MSIPDDP methods
     void initializeMSIPDDP();
@@ -335,7 +341,6 @@ class CDDP {
     bool solveMSIPDDPBackwardPass();
     void resetMSIPDDPFilter();
     void initialMSIPDDPRollout();
-    void resetMSIPDDPRegularization();
 
     // Feasible IPDDP methods
     void initializeFeasibleIPDDP();
@@ -413,15 +418,8 @@ class CDDP {
 
     // Log-barrier
     double mu_; // Barrier coefficient
-    std::vector<FilterPoint> filter_; // [logcost, error measure
-    int ipddp_regularization_counter_ = 0; // Regularization counter for IPDDP
     double constraint_violation_; // Current constraint violation measure
     double gamma_; // Small value for filter acceptance
-
-    // Regularization parameters
-    double defect_violation_penalty_ = 1.0;
-    double equality_violation_penalty_ = 1.0;
-    double inequality_violation_penalty_ = 1.0;
 
     // Feedforward and feedback gains
     std::vector<Eigen::VectorXd> k_u_;
@@ -451,6 +449,7 @@ class CDDP {
     BoxQPSolver boxqp_solver_;
 
     double optimality_gap_ = 1e+10;
+    double kkt_error_ = 1e+10; // maximum KKT residual except Qu term
 };
 } // namespace cddp
 #endif // CDDP_CDDP_CORE_HPP

src/cddp_core/cddp_core.cpp

@@ -420,10 +420,12 @@ void CDDP::printOptions(const CDDPOptions &options)
     std::cout << "  BoxQP Verbose: " << (options.boxqp_verbose ? "Yes" : "No") << "\n";
 
     std::cout << "\nMSIPDDP:\n";
-    std::cout << "  Defect Penalty Initial: " << options.defect_violation_penalty_initial << "\n";
-    std::cout << "  Defect Penalty Rho: " << options.ms_defect_penalty_rho << "\n";
-    std::cout << "  Defect Penalty Kappa_d: " << options.ms_defect_penalty_kappa_d << "\n";
-    std::cout << "  Defect Penalty Mu0: " << options.ms_defect_penalty_mu0 << "\n";
+    std::cout << "  MS Segment Length: " << options.ms_segment_length << "\n";
+    std::cout << "  MS Rollout Type: " << options.ms_rollout_type << "\n";
+    std::cout << "  MS Defect Tolerance: " << options.ms_defect_tolerance_for_single_shooting << "\n";
+    std::cout << "  Barrier Update Factor: " << options.barrier_update_factor << "\n";
+    std::cout << "  Barrier Update Power: " << options.barrier_update_power << "\n";
+
 
     std::cout << "========================================\n\n";
 }