Refactor solver core and switch to typed CDDP solutions

CMakeLists.txt

@@ -139,6 +139,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/cddp_solver_base.cpp
   src/cddp_core/clddp_solver.cpp
   src/cddp_core/logddp_solver.cpp
   src/cddp_core/ipddp_solver.cpp

examples/cddp_acrobot.cpp

@@ -22,6 +22,7 @@
 #include <thread>
 #include <numeric>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "dynamics_model/acrobot.hpp"
 #include "matplot/matplot.h"
 
@@ -105,24 +106,18 @@ int main() {
 
     // Solve
     cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP);
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
 
     // Create plot directory
     const std::string plotDirectory = "../results/acrobot";
-    if (!fs::exists(plotDirectory)) {
-        fs::create_directories(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Extract solution data for animation
-    std::vector<double> time_arr, theta1_arr, theta2_arr;
-
-    for (size_t i = 0; i < X_sol.size(); ++i) {
-        time_arr.push_back(t_sol[i]);
-        theta1_arr.push_back(X_sol[i](0));
-        theta2_arr.push_back(X_sol[i](1));
-    }
+    const auto& time_arr = t_sol;
+    auto theta1_arr = cddp::example::extractComponent(X_sol, 0);
+    auto theta2_arr = cddp::example::extractComponent(X_sol, 1);
 
     // --- Animation ---
     auto fig = figure();

examples/cddp_bicycle.cpp

@@ -5,6 +5,7 @@
 #include <thread>
 #include <algorithm>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "matplot/matplot.h"
 
 namespace fs = std::filesystem;
@@ -66,37 +67,26 @@ int main()
     cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP);
 
     // Extract solution
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
 
     // Create directory for saving plots
     const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory))
-    {
-        fs::create_directory(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Create a directory for frame images.
     (void) std::system("mkdir -p frames");
 
     // Extract trajectory data
-    std::vector<double> x_arr, y_arr, theta_arr, v_arr;
-    for (const auto &x : X_sol)
-    {
-        x_arr.push_back(x(0));
-        y_arr.push_back(x(1));
-        theta_arr.push_back(x(2));
-        v_arr.push_back(x(3));
-    }
+    auto x_arr = cddp::example::extractComponent(X_sol, 0);
+    auto y_arr = cddp::example::extractComponent(X_sol, 1);
+    auto theta_arr = cddp::example::extractComponent(X_sol, 2);
+    auto v_arr = cddp::example::extractComponent(X_sol, 3);
 
     // Extract control inputs
-    std::vector<double> acc_arr, steering_arr;
-    for (const auto &u : U_sol)
-    {
-        acc_arr.push_back(u(0));
-        steering_arr.push_back(u(1));
-    }
+    auto acc_arr = cddp::example::extractComponent(U_sol, 0);
+    auto steering_arr = cddp::example::extractComponent(U_sol, 1);
 
     // -----------------------------
     // Plot states and controls

examples/cddp_car.cpp

@@ -19,6 +19,7 @@
 #include <random>
 #include <cmath>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "matplot/matplot.h"
 
 using namespace matplot;
@@ -251,17 +252,13 @@ int main()
     // ========================================
 
     // Extract solution trajectories
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
 
     // 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));
-    }
+    auto x_hist = cddp::example::extractComponent(X_sol, 0);
+    auto y_hist = cddp::example::extractComponent(X_sol, 1);
 
     // Car dimensions.
     double car_length = 2.1;
@@ -275,10 +272,7 @@ int main()
 
     // Create directory for saving plots
     const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory))
-    {
-        fs::create_directory(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Create a directory for frame images.
     (void) std::system("mkdir -p frames");

examples/cddp_car_ipddp.cpp

@@ -21,6 +21,7 @@
 #include <map>
 #include <string>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "matplot/matplot.h"
 
 using namespace matplot;
@@ -204,16 +205,13 @@ int main() {
     cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP);
 
     // Extract solution trajectories
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
 
     // Prepare trajectory data for plotting
-    std::vector<double> x_hist, y_hist;
-    for (const auto& state : X_sol) {
-        x_hist.push_back(state(0));
-        y_hist.push_back(state(1));
-    }
+    auto x_hist = cddp::example::extractComponent(X_sol, 0);
+    auto y_hist = cddp::example::extractComponent(X_sol, 1);
     // Car dimensions.
     double car_length = 2.1;
     double car_width = 0.9;
@@ -226,10 +224,7 @@ int main() {
 
     // Create directory for saving plots
     const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory))
-    {
-        fs::create_directory(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Create a directory for frame images.
     (void) std::system("mkdir -p frames");

examples/cddp_cartpole.cpp

@@ -20,6 +20,7 @@
 #include <string>
 #include <memory>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "matplot/matplot.h"
 #include <random>
 
@@ -99,60 +100,30 @@ int main() {
     //     std::make_unique<cddp::ControlConstraint>( control_lower_bound, control_upper_bound));
 
     // Initial trajectory.
-    std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));
-    std::vector<Eigen::VectorXd> U(horizon, Eigen::VectorXd::Zero(control_dim));
-    // Generate initial trajectory by constant initial state
-    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);
-    // }
+    auto [X, U] = cddp::example::makeInitialTrajectory(initial_state, horizon, control_dim);
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve.
     cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP);
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
 
     // Create plot directory.
     const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory)) {
-        fs::create_directory(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Create a directory for frame images.
     (void) std::system("mkdir -p frames");
 
     // Extract solution data.
-    std::vector<double> x_arr, x_dot_arr, theta_arr, theta_dot_arr, force_arr, time_arr, time_arr2;
-    for (size_t i = 0; i < X_sol.size(); ++i) {
-        time_arr.push_back(t_sol[i]);
-        x_arr.push_back(X_sol[i](0));
-        theta_arr.push_back(X_sol[i](1));
-        x_dot_arr.push_back(X_sol[i](2));
-        theta_dot_arr.push_back(X_sol[i](3));
-    }
-    for (size_t i = 0; i < U_sol.size(); ++i) {
-        force_arr.push_back(U_sol[i](0));
-        time_arr2.push_back(t_sol[i]);
-    }
+    auto x_arr = cddp::example::extractComponent(X_sol, 0);
+    auto theta_arr = cddp::example::extractComponent(X_sol, 1);
+    auto x_dot_arr = cddp::example::extractComponent(X_sol, 2);
+    auto theta_dot_arr = cddp::example::extractComponent(X_sol, 3);
+    auto force_arr = cddp::example::extractComponent(U_sol, 0);
+    const auto& time_arr = t_sol;
+    std::vector<double> time_arr2(t_sol.begin(), t_sol.begin() + U_sol.size());
 
     // --- Plot static results (2x2 plots for state trajectories) ---
     auto fig1 = figure();

examples/cddp_forklift_ipddp.cpp

@@ -21,6 +21,7 @@
 #include <map>
 #include <string>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "dynamics_model/forklift.hpp"
 #include "matplot/matplot.h"
 #include <random>
@@ -246,16 +247,13 @@ int main() {
     cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::MSIPDDP);
 
     // Extract solution trajectories
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
 
     // Prepare trajectory data for plotting
-    std::vector<double> x_hist, y_hist;
-    for (const auto& state : X_sol) {
-        x_hist.push_back(state(0));
-        y_hist.push_back(state(1));
-    }
+    auto x_hist = cddp::example::extractComponent(X_sol, 0);
+    auto y_hist = cddp::example::extractComponent(X_sol, 1);
 
     // Forklift dimensions 
     double forklift_length = 2.5;  // Overall length
@@ -269,10 +267,7 @@ int main() {
 
     // Create directory for saving plots
     const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory))
-    {
-        fs::create_directories(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Animation loop: update plot for each time step and save frame
     for (size_t i = 0; i < X_sol.size(); ++i)

examples/cddp_hcw.cpp

@@ -26,6 +26,7 @@
 #include <Eigen/Dense>
 
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 #include "matplot/matplot.h"
 
 using namespace matplot;
@@ -146,22 +147,18 @@ int main() {
     cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::CLDDP);
 
     // Extract solution and print result
-    auto cost_sequence = std::any_cast<std::vector<double>>(solution.at("cost_trajectory"));
-    double J_final = cost_sequence.back();
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
-
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
+
     std::cout << "\n[Result] CDDP solved." << std::endl;
-    std::cout << "[Result] Final cost: " << J_final << std::endl;
+    std::cout << "[Result] Final cost: " << solution.final_objective << std::endl;
     std::cout << "[Result] Final state: "
               << X_sol.back().transpose() << std::endl;
 
     // Create plot directory
     const std::string plotDirectory = "../results/tests";
-    if (!std::filesystem::exists(plotDirectory)) {
-        std::filesystem::create_directory(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Extract state data arrays
     std::vector<double> x_arr, y_arr, z_arr, vx_arr, vy_arr, vz_arr, time_arr;

examples/cddp_lti_system.cpp

@@ -3,6 +3,7 @@
 #include <filesystem>
 #include <random>
 #include "cddp.hpp"
+#include "cddp_example_utils.hpp"
 
 #include "matplot/matplot.h"
 using namespace matplot;
@@ -147,29 +148,25 @@ int main() {
     // Alternatively: cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::LogDDP);
 
     // Extract solution trajectories
-    auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
-    auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
-    auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
-    auto cost_sequence = std::any_cast<std::vector<double>>(solution.at("cost_trajectory"));
-    auto iterations = std::any_cast<int>(solution.at("iterations"));
-    auto converged = std::any_cast<bool>(solution.at("converged"));
-    auto solve_time = std::any_cast<long long>(solution.at("solve_time"));
+    const auto& X_sol = solution.state_trajectory;
+    const auto& U_sol = solution.control_trajectory;
+    const auto& t_sol = solution.time_points;
+    auto iterations = solution.iterations_completed;
+    bool converged = (solution.status_message == "OptimalSolutionFound" || solution.status_message == "AcceptableSolutionFound");
 
     // Create directory for plots if it doesn't exist
     const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory)) {
-        fs::create_directories(plotDirectory);
-    }
+    cddp::example::ensurePlotDir(plotDirectory);
 
     // Plot state trajectories using the matplot API
     plotStateTrajectories(X_sol, plotDirectory);
 
     // Print optimization statistics
     std::cout << "\nOptimization Results:" << std::endl;
     std::cout << "Iterations: " << iterations << std::endl;
-    std::cout << "Final cost: " << cost_sequence.back() << std::endl;
+    std::cout << "Final cost: " << solution.final_objective << std::endl;
     std::cout << "Converged: " << (converged ? "Yes" : "No") << std::endl;
-    std::cout << "Solve time: " << solve_time << " microseconds" << std::endl;
+    std::cout << "Solve time: " << solution.solve_time_ms << " ms" << std::endl;
 
     return 0;
 }