Merge pull request #7 from astomodynamics/ros_integration_test

Ros integration test

Author: astomodynamics

include/cddp/cddp_core/CDDPProblem.hpp

@@ -47,7 +47,7 @@ struct CDDPOptions {
 
 class CDDPProblem {
 public:
-    CDDPProblem(DynamicalSystem* system, const Eigen::VectorXd& initialState, int horizon, double dt);
+    CDDPProblem(const Eigen::VectorXd& initial_state, const Eigen::VectorXd& goal_state, int horizon, double dt);
 
     // Problem Setup
     void setInitialState(const Eigen::VectorXd& x0);
@@ -59,6 +59,7 @@ class CDDPProblem {
     void setOptions(const CDDPOptions& opts);
     void setInitialTrajectory(const std::vector<Eigen::VectorXd>& X, const std::vector<Eigen::VectorXd>& U);
     void initializeCost();
+    void setDynamicalSystem(std::unique_ptr<DynamicalSystem> dynamics);
     void setObjective(std::unique_ptr<Objective> objective);
     void addConstraint(std::unique_ptr<Constraint> constraint);
     Eigen::VectorXd getInitialState() { return initial_state_; }
@@ -98,7 +99,7 @@ class CDDPProblem {
 
 private:
     // Problem Data
-    DynamicalSystem* dynamics_;
+    std::unique_ptr<DynamicalSystem> dynamics_;
     Eigen::VectorXd initial_state_;
     Eigen::VectorXd goal_state_;
     int horizon_;

include/cddp/model/DubinsCar.hpp

@@ -3,7 +3,7 @@
 
 #include "Eigen/Dense"
 #include <vector>
-#include "../cddp_core/DynamicalSystem.hpp" 
+#include "cddp_core/DynamicalSystem.hpp" 
 
 namespace cddp {
 

src/cddp_core/CDDPProblem.cpp

@@ -1,4 +1,5 @@
 #include <iostream>
+#include <chrono>
 #include <vector>
 #include "Eigen/Dense"
 #include "Eigen/Sparse"
@@ -19,32 +20,15 @@ TODO
 namespace cddp {
 
 // Constructor 
-CDDPProblem::CDDPProblem(cddp::DynamicalSystem* system, const Eigen::VectorXd& initialState, int horizon, double timestep) :
-    dynamics_(system), 
-    initial_state_(initialState), 
-    goal_state_(initialState), // Pre-allocate goal to initialState 
+CDDPProblem::CDDPProblem(const Eigen::VectorXd& initial_state, const Eigen::VectorXd& goal_state, int horizon, double timestep) :
+    initial_state_(initial_state), 
+    goal_state_(goal_state), // Pre-allocate goal to initialState 
     horizon_(horizon),
     dt_(timestep)
 {
     // Initialize Trajectory
-    X_.resize(horizon + 1, initial_state_);
-    U_.resize(horizon, Eigen::VectorXd::Zero(system->control_size_));
-
-    // Initialize Intermediate Cost
-    J_ = 0.0;
-
-    k_.resize(horizon, Eigen::VectorXd::Zero(system->control_size_));
-    K_.resize(horizon, Eigen::MatrixXd::Zero(system->control_size_, system->state_size_));
-
-    // Initialize Intermediate value function
-    V_.resize(horizon + 1, 0.0);
-    V_X_.resize(horizon + 1, Eigen::VectorXd::Zero(system->state_size_));
-    V_XX_.resize(horizon + 1, Eigen::MatrixXd::Zero(system->state_size_, system->state_size_));
-
-    // Initialize Q-function Matrices
-    Q_UU_.resize(horizon, Eigen::MatrixXd::Zero(system->control_size_, system->control_size_));
-    Q_UX_.resize(horizon, Eigen::MatrixXd::Zero(system->control_size_, system->state_size_));
-    Q_U_.resize(horizon, Eigen::VectorXd::Zero(system->control_size_));
+    // X_.resize(horizon + 1, initial_state_);
+    // U_.resize(horizon, Eigen::VectorXd::Zero(system->control_size_));
 }
 
 // Setup Methods
@@ -83,6 +67,32 @@ void CDDPProblem::setOptions(const CDDPOptions& opts) {
     options_ = opts;
 }
 
+void CDDPProblem::setDynamicalSystem(std::unique_ptr<DynamicalSystem> dynamics) {
+    dynamics_ = std::move(dynamics);
+
+    // Initialize Trajectory
+    X_.resize(horizon_ + 1, initial_state_);
+    U_.resize(horizon_, Eigen::VectorXd::Zero(dynamics_->control_size_));
+
+    // Initialize Intermediate Cost
+    J_ = 0.0;
+
+    // Initialize Control Gains
+    k_.resize(horizon_, Eigen::VectorXd::Zero(dynamics_->control_size_));
+    K_.resize(horizon_, Eigen::MatrixXd::Zero(dynamics_->control_size_, dynamics_->state_size_));
+
+    // Initialize Intermediate value function
+    V_.resize(horizon_ + 1, 0.0);
+    V_X_.resize(horizon_ + 1, Eigen::VectorXd::Zero(dynamics_->state_size_));
+    V_XX_.resize(horizon_ + 1, Eigen::MatrixXd::Zero(dynamics_->state_size_, dynamics_->state_size_));
+    
+    // Initialize Q-function Matrices
+    Q_UU_.resize(horizon_, Eigen::MatrixXd::Zero(dynamics_->control_size_, dynamics_->control_size_));
+    Q_UX_.resize(horizon_, Eigen::MatrixXd::Zero(dynamics_->control_size_, dynamics_->state_size_));
+    Q_U_.resize(horizon_, Eigen::VectorXd::Zero(dynamics_->control_size_));
+
+}
+
 void CDDPProblem::setObjective(std::unique_ptr<Objective> objective) {
     objective_ = std::move(objective);
 }
@@ -160,10 +170,13 @@ std::vector<Eigen::VectorXd> CDDPProblem::solve() {
     double J = std::numeric_limits<double>::infinity();
     double gradientNorm = std::numeric_limits<double>::infinity();
 
+    // Start the timer
+    auto start_time = std::chrono::high_resolution_clock::now();
+
     // 2. Main CDDP Iterative Loop
     for (int iter = 0; iter < options_.max_iterations; ++iter) {
 std::cout << "Iteration: " << iter << std::endl;
-std::cout << "Cost " << J_ << std::endl;
+std::cout << "Cost " << J_ << std::endl; 
         double J_old = J;
         // 3. Backward 
         bool backward_pass_done = solveBackwardPass();
@@ -200,6 +213,15 @@ std::cout << "Cost " << J_ << std::endl;
 
     }
 
+    // Stop the timer
+    auto end_time = std::chrono::high_resolution_clock::now();
+
+    // Calculate the elapsed time
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
+
+    // Print the elapsed time
+std::cout << "Solver time: " << duration.count() << " ms" << std::endl;
+
     // 6. Return Optimal Control Sequence
     // place holder 
     return U_;

test/CDDPProblemTests.cpp

@@ -10,22 +10,25 @@ using namespace cddp;
 bool testBasicCDDP() {
     int state_dim = 3; 
     int control_dim = 2; 
-    double dt = 0.05;
+    double dt = 0.03;
     int horizon = 100;
     int integration_type = 0; // 0 for Euler, 1 for Heun, 2 for RK3, 3 for RK4
 
     // Problem Setup
-    Eigen::VectorXd initialState(state_dim);
-    initialState << 0.0, 0.0, 0.0; // Initial state
+    Eigen::VectorXd initial_state(state_dim);
+    initial_state << 0.0, 0.0, M_PI/4.0; // Initial state
 
-    DubinsCar system(state_dim, control_dim, dt, integration_type); // Your DoubleIntegrator instance
-    CDDPProblem cddp_solver(&system, initialState, horizon, dt);
-    
-    // Set goal state if needed
+    // Set goal state
     Eigen::VectorXd goal_state(state_dim);
     goal_state << 2.0, 2.0, M_PI/2.0;
-    cddp_solver.setGoalState(goal_state);
 
+    DubinsCar system(state_dim, control_dim, dt, integration_type); // Your DoubleIntegrator instance
+    CDDPProblem cddp_solver(initial_state, goal_state, horizon, dt);
+
+    // Set dynamical system
+    cddp_solver.setDynamicalSystem(std::make_unique<DubinsCar>(system));
+    
+   
     // Simple Cost Matrices 
     Eigen::MatrixXd Q(state_dim, state_dim);
     Q << 0e-2, 0, 0, 
@@ -43,10 +46,10 @@ bool testBasicCDDP() {
 
     // Add constraints 
     Eigen::VectorXd lower_bound(control_dim);
-    lower_bound << -0.1, -M_PI;
+    lower_bound << -1.0, -M_PI;
 
     Eigen::VectorXd upper_bound(control_dim);
-    upper_bound << 0.1, M_PI;
+    upper_bound << 1.0, M_PI;
 
     ControlBoxConstraint control_constraint(lower_bound, upper_bound);
     cddp_solver.addConstraint(std::make_unique<ControlBoxConstraint>(control_constraint));
@@ -64,7 +67,6 @@ bool testBasicCDDP() {
     // Set initial trajectory if needed
     std::vector<Eigen::VectorXd> X = std::vector<Eigen::VectorXd>(horizon + 1, Eigen::VectorXd::Zero(state_dim));
     std::vector<Eigen::VectorXd> U = std::vector<Eigen::VectorXd>(horizon, Eigen::VectorXd::Zero(control_dim));
-    // X.front() = initialState;
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve!