Enhance CDDP with barrier and constraint improvements (#63)

Introduce barrier support in CDDP, enhance constraint handling, and improve solver options. Add new tests for various constraints and update existing examples for better clarity and functionality.

CMakeLists.txt

@@ -192,10 +192,12 @@ set(cddp_core_srcs
   src/cddp_core/dynamical_system.cpp
   src/cddp_core/objective.cpp
   src/cddp_core/constraint.cpp
+  src/cddp_core/barrier.cpp
   src/cddp_core/helper.cpp
   src/cddp_core/boxqp.cpp
   src/cddp_core/qp_solver.cpp
   src/cddp_core/cddp_core.cpp
+  src/cddp_core/clddp_core.cpp
   src/cddp_core/asddp_core.cpp
   src/cddp_core/logddp_core.cpp
   src/cddp_core/neural_dynamical_system.cpp

examples/CMakeLists.txt

@@ -13,7 +13,6 @@
 #  limitations under the License.
 
 # CMakeLists.txt for the CDDP examples
-
 add_executable(cddp_bicycle cddp_bicycle.cpp)
 target_link_libraries(cddp_bicycle cddp)
 
@@ -38,7 +37,13 @@ target_link_libraries(cddp_pendulum cddp)
 add_executable(cddp_quadrotor cddp_quadrotor.cpp)
 target_link_libraries(cddp_quadrotor cddp)
 
-# Neural dynamics example
+# Ipopt examples
+if (CDDP_CPP_CASADI)
+    add_executable(ipopt_car ipopt_car.cpp)
+    target_link_libraries(ipopt_car cddp)
+endif()
+
+# Neural dynamics examples
 add_executable(prepare_pendulum neural_dynamics/prepare_pendulum.cpp)
 target_link_libraries(prepare_pendulum cddp)
 

examples/cddp_car.cpp

@@ -135,15 +135,15 @@ int main() {
     Animation::AnimationConfig config;
     config.width = 800;
     config.height = 800;
-    config.frame_skip = 5;  // Save every 5th frame
-    config.frame_delay = 10;  // 10/100 = 0.1 seconds between frames
+    config.frame_skip = 5;  
+    config.frame_delay = 10;  
     Animation animation(config);
 
    // Problem parameters
     int state_dim = 4;     // [x y theta v]
     int control_dim = 2;   // [wheel_angle acceleration]
-    int horizon = 500;     // Same as MATLAB example
-    double timestep = 0.03;  // h = 0.03 from MATLAB
+    int horizon = 500;  
+    double timestep = 0.03; 
     std::string integration_type = "euler";
 
     // Random number generator setup
@@ -152,13 +152,13 @@ int main() {
     std::normal_distribution<double> d(0.0, 0.1);
 
     // Create car instance
-    double wheelbase = 2.0;  // d = 2.0 from MATLAB example
+    double wheelbase = 2.0; 
     std::unique_ptr<cddp::DynamicalSystem> system = 
         std::make_unique<cddp::Car>(timestep, wheelbase, integration_type);
 
     // Initial and goal states
     Eigen::VectorXd initial_state(state_dim);
-    initial_state << 1.0, 1.0, 1.5*M_PI, 0.0;  // [1; 1; pi*3/2; 0] from MATLAB
+    initial_state << 1.0, 1.0, 1.5*M_PI, 0.0;
 
     Eigen::VectorXd goal_state(state_dim);
     goal_state << 0.0, 0.0, 0.0, 0.0;  // NOT USED IN THIS EXAMPLE
@@ -189,7 +189,7 @@ int main() {
     options.grad_tolerance = 1e-4;
     options.regularization_type = "control";
     // options.regularization_control = 1.0;
-    options.debug = true;
+    options.debug = false;
     options.use_parallel = true;
     options.num_threads = 10;
     cddp_solver.setOptions(options);
@@ -225,8 +225,23 @@ int main() {
 
     // Solve the problem
     cddp::CDDPSolution solution = cddp_solver.solve();
-    // cddp::CDDPSolution solution = cddp_solver.solveCLDDP();
-    // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP();
+        // ========================================
+        //    CDDP Solution
+        // ========================================
+        // Converged: Yes
+        // Iterations: 157
+        // Solve Time: 5.507e+05 micro sec
+        // Final Cost: 1.90517
+        // ========================================
+    // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP();  
+        // ========================================
+        //    CDDP Solution
+        // ========================================
+        // Converged: Yes
+        // Iterations: 153
+        // Solve Time: 5.441e+05 micro sec
+        // Final Cost: 1.90517
+        // ========================================
 
     // Extract solution trajectories
     auto X_sol = solution.state_sequence;

examples/cddp_dubins_car.cpp

@@ -70,6 +70,8 @@ int main() {
     // Set options
     cddp::CDDPOptions options;
     options.max_iterations = 10;
+    options.barrier_coeff = 1e-2;
+    options.barrier_factor = 0.1;
     cddp_solver.setOptions(options);
 
     // Set initial trajectory
@@ -78,7 +80,8 @@ int main() {
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve the problem
-    cddp::CDDPSolution solution = cddp_solver.solve();
+    // cddp::CDDPSolution solution = cddp_solver.solve("CLCDDP");
+    cddp::CDDPSolution solution = cddp_solver.solve("LogCDDP");
 
     // Extract solution
     auto X_sol = solution.state_sequence; // size: horizon + 1