CDDP solver cleanup (#60)

* Cleanup CDDP solvers * Organize associated files
astomodynamics · Dec 24, 2024 · b2bb9d5 · b2bb9d5
1 parent b67fccd
commit b2bb9d5
Show file tree

Hide file tree

Showing 14 changed files with 1,038 additions and 1,041 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -196,7 +196,7 @@ set(cddp_core_srcs
   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/torch_dynamical_system.cpp
 )

diff --git a/examples/cddp_car.cpp b/examples/cddp_car.cpp
@@ -189,6 +189,9 @@ int main() {
     options.grad_tolerance = 1e-4;
     options.regularization_type = "control";
     // options.regularization_control = 1.0;
+    options.debug = true;
+    options.use_parallel = true;
+    options.num_threads = 10;
     cddp_solver.setOptions(options);
 
     // Initialize with random controls
@@ -221,51 +224,51 @@ int main() {
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve the problem
-    // cddp::CDDPSolution solution = cddp_solver.solve();
-    cddp::CDDPSolution solution = cddp_solver.solveCLDDP();
+    cddp::CDDPSolution solution = cddp_solver.solve();
+    // cddp::CDDPSolution solution = cddp_solver.solveCLDDP();
     // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP();
 
-    // // Extract solution trajectories
-    // auto X_sol = solution.state_sequence;
-    // auto U_sol = solution.control_sequence;
-    // auto t_sol = solution.time_sequence;
-
-    // // Prepare trajectory data
-    // std::vector<double> x_hist, y_hist;
-    // for(const auto& x : X_sol) {
-    //     x_hist.push_back(x(0));
-    //     y_hist.push_back(x(1));
-    // }
-
-    // // Car dimensions
-    // double car_length = 2.1;
-    // double car_width = 0.9;
-
-    // // Create animation
-    // Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(control_dim);
-    // for(size_t i = 0; i < X_sol.size(); i++) {
-    //     animation.newFrame();
+    // Extract solution trajectories
+    auto X_sol = solution.state_sequence;
+    auto U_sol = solution.control_sequence;
+    auto t_sol = solution.time_sequence;
+
+    // Prepare trajectory data
+    std::vector<double> x_hist, y_hist;
+    for(const auto& x : X_sol) {
+        x_hist.push_back(x(0));
+        y_hist.push_back(x(1));
+    }
+
+    // Car dimensions
+    double car_length = 2.1;
+    double car_width = 0.9;
+
+    // Create animation
+    Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(control_dim);
+    for(size_t i = 0; i < X_sol.size(); i++) {
+        animation.newFrame();
 
-    //     // Plot full trajectory
-    //     plt::plot(x_hist, y_hist, "b-");
+        // Plot full trajectory
+        plt::plot(x_hist, y_hist, "b-");
 
-    //     // Plot goal configuration
-    //     plotCarBox(goal_state, empty_control, car_length, car_width, "r");
+        // Plot goal configuration
+        plotCarBox(goal_state, empty_control, car_length, car_width, "r");
 
-    //     // Plot current car position
-    //     plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k");
+        // Plot current car position
+        plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k");
 
-    //     // Plot settings
-    //     plt::grid(true);
-    //     // plt::axis("equal");
-    //     plt::xlim(-4, 4);
-    //     plt::ylim(-4, 4);
+        // Plot settings
+        plt::grid(true);
+        // plt::axis("equal");
+        plt::xlim(-4, 4);
+        plt::ylim(-4, 4);
 
-    //     animation.saveFrame(i);
-    // }
+        animation.saveFrame(i);
+    }
 
-    // // Create the final GIF
-    // animation.createGif("car_parking.gif");
+    // Create the final GIF
+    animation.createGif("car_parking.gif");
 
     return 0;
 }

diff --git a/examples/cddp_lti_system.cpp b/examples/cddp_lti_system.cpp
@@ -157,8 +157,7 @@ int main() {
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve using different CDDP variants
-    // cddp::CDDPSolution solution = cddp_solver.solve();
-    cddp::CDDPSolution solution = cddp_solver.solveCLDDP();
+    cddp::CDDPSolution solution = cddp_solver.solve();
     // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP();
 
     // Extract solution trajectories

diff --git a/include/cddp-cpp/cddp_core/boxqp.hpp b/include/cddp-cpp/cddp_core/boxqp.hpp
@@ -27,13 +27,13 @@ namespace cddp
  */
 struct BoxQPOptions
 {
-     int maxIter = 100;           ///< Maximum number of iterations
-     double minGrad = 1e-8;       ///< Minimum norm of non-fixed gradient
-     double minRelImprove = 1e-8; ///< Minimum relative improvement
-     double stepDec = 0.6;        ///< Factor for decreasing stepsize
-     double minStep = 1e-22;      ///< Minimal stepsize for linesearch
-     double armijo = 0.1;         ///< Armijo parameter
-     bool verbose = false;        ///< Print debug info
+     int max_iterations = 100;      ///< Maximum number of iterations
+     double min_grad = 1e-8;        ///< Minimum norm of non-fixed gradient
+     double min_rel_improve = 1e-8; ///< Minimum relative improvement
+     double step_dec = 0.6;         ///< Factor for decreasing stepsize
+     double min_step = 1e-22;       ///< Minimal stepsize for linesearch
+     double armijo = 0.1;           ///< Armijo parameter
+     bool verbose = false;          ///< Print debug info
 };
 
 /**
@@ -46,9 +46,8 @@ enum class BoxQPStatus
      MAX_ITER_EXCEEDED = 1, ///< Maximum iterations exceeded
      MAX_LS_EXCEEDED = 2,   ///< Maximum line search iterations exceeded
      NO_BOUNDS = 3,         ///< No bounds, returning Newton point
-     SMALL_IMPROVEMENT = 4, ///< Improvement smaller than tolerance
-     SMALL_GRADIENT = 5,    ///< Gradient norm smaller than tolerance
-     ALL_CLAMPED = 6        ///< All dimensions are clamped
+     SUCCESS = 4,           ///< Converged successfully
+     ALL_CLAMPED = 5        ///< All dimensions are clamped
 };
 
 /**