Implement active-set DDP for unicycle with obstacle avoidance (#83)

* Update Ball constraint definition

* active-set DDP state constraint done

* Create Unicycle active-set DDP

Author: astomodynamics

CMakeLists.txt

@@ -191,6 +191,14 @@ if (CDDP_CPP_TORCH)
   set(TORCH_INSTALL_PREFIX ${Torch_DIR}/../../../ CACHE PATH "LibTorch installation directory")
 endif()
 
+# OSQP-CPP for ASDDP
+FetchContent_Declare(
+  osqp-cpp
+  GIT_REPOSITORY https://github.com/astomodynamics/osqp-cpp.git
+  GIT_TAG master
+)
+FetchContent_MakeAvailable(osqp-cpp)
+FetchContent_GetProperties(osqp-cpp)
 
 # Include directories
 include_directories(
@@ -246,6 +254,7 @@ target_link_libraries(${PROJECT_NAME}
   Python3::Python
   Python3::Module
   Python3::NumPy
+  osqp-cpp
 ) 
 
 if (CDDP_CPP_TORCH)

README.md

@@ -47,6 +47,16 @@ Simple car-like robot with velocity and steering control:
 
 <img src="results/tests/unicycle.gif" width="300" alt="Unicycle Model CDDP">
 
+### Unicycle with Obstacle Avoidance
+
+Simple car-like robot with velocity and steering control:
+
+```bash
+./examples/cddp_unicycle_safe // after building
+```
+
+<img src="results/tests/unicycle_trajectory_comparison.png" width="300" alt="Unicycle Model CDDP with Obstacle Avoidance">
+
 ### Bicycle Model
 
 Bicycle model with velocity and steering control:

examples/CMakeLists.txt

@@ -28,6 +28,9 @@ target_link_libraries(cddp_dubins_car cddp)
 add_executable(cddp_unicycle cddp_unicycle.cpp)
 target_link_libraries(cddp_unicycle cddp)
 
+add_executable(cddp_unicycle_safe cddp_unicycle_safe.cpp)
+target_link_libraries(cddp_unicycle_safe cddp)
+
 add_executable(cddp_manipulator cddp_manipulator.cpp)
 target_link_libraries(cddp_manipulator cddp)
 

examples/cddp_car.cpp

@@ -183,8 +183,9 @@ int main() {
     options.verbose = true;
     options.cost_tolerance = 1e-7;
     options.grad_tolerance = 1e-4;
-    options.regularization_type = "control";
-    // options.regularization_control = 1.0;
+    options.regularization_type = "state";
+    options.regularization_state = 1e-4;
+    options.regularization_control = 1e-6;
     options.debug = false;
     options.use_parallel = true;
     options.num_threads = 10;
@@ -220,7 +221,7 @@ int main() {
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve the problem
-    cddp::CDDPSolution solution = cddp_solver.solve();
+    // cddp::CDDPSolution solution = cddp_solver.solve("CLCDDP");
         // ========================================
         //    CDDP Solution
         // ========================================
@@ -229,7 +230,7 @@ int main() {
         // Solve Time: 5.507e+05 micro sec
         // Final Cost: 1.90517
         // ========================================
-    // cddp::CDDPSolution solution = cddp_solver.solveLogCDDP();  
+    // cddp::CDDPSolution solution = cddp_solver.solve("LogCDDP");  
         // ========================================
         //    CDDP Solution
         // ========================================
@@ -238,6 +239,7 @@ int main() {
         // Solve Time: 5.441e+05 micro sec
         // Final Cost: 1.90517
         // ========================================
+    cddp::CDDPSolution solution = cddp_solver.solve("ASCDDP");
 
     // Extract solution trajectories
     auto X_sol = solution.state_sequence;
@@ -279,7 +281,7 @@ int main() {
     }
 
     // Create the final GIF
-    animation.createGif("car_parking.gif");
+    animation.createGif("car_parking_asddp.gif");
 
     return 0;
 }

examples/cddp_unicycle_safe.cpp

@@ -0,0 +1,170 @@
+/*
+ Copyright 2024 Tomo Sasaki
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+      https://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+*/
+
+#include <iostream>
+#include <vector>
+#include <cmath>
+#include <filesystem>
+#include <memory>
+#include <cstdlib>
+
+#include "cddp.hpp"          
+
+namespace plt = matplotlibcpp;
+namespace fs = std::filesystem;
+
+int main() {
+    // Problem parameters
+    int state_dim = 3;
+    int control_dim = 2;
+    int horizon = 100;
+    double timestep = 0.03;
+    std::string integration_type = "euler";
+
+    // Create a unicycle dynamical system instance
+    std::unique_ptr<cddp::DynamicalSystem> dyn_system = std::make_unique<cddp::Unicycle>(timestep, integration_type);
+
+    // Create objective function
+    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim);
+    Eigen::MatrixXd R = 0.05 * Eigen::MatrixXd::Identity(control_dim, control_dim);
+    Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim);
+    Qf << 100.0, 0.0, 0.0,
+          0.0, 100.0, 0.0,
+          0.0, 0.0, 100.0;
+    Eigen::VectorXd goal_state(state_dim);
+    goal_state << 2.0, 2.0, M_PI/2.0;
+
+    // Create an empty vector for reference states
+    std::vector<Eigen::VectorXd> empty_reference_states;
+    auto objective = std::make_unique<cddp::QuadraticObjective>(Q, R, Qf, goal_state, empty_reference_states, timestep);
+
+    // Define initial state
+    Eigen::VectorXd initial_state(state_dim);
+    initial_state << 0.0, 0.0, M_PI/4.0;
+
+    // Set up common CDDP options
+    cddp::CDDPOptions options;
+    options.max_iterations = 20;
+    options.verbose = false;
+    options.debug = false;
+    options.cost_tolerance = 1e-5;
+    options.grad_tolerance = 1e-4;
+    options.regularization_type = "none";
+
+    // Define control box constraint bounds
+    Eigen::VectorXd control_lower_bound(control_dim);
+    control_lower_bound << -2.0, -M_PI;
+    Eigen::VectorXd control_upper_bound(control_dim);
+    control_upper_bound << 2.0, M_PI;
+
+    // --------------------------
+    // Solve the CDDP problem without the ball constraint
+    cddp::CDDP solver_baseline(
+        initial_state, 
+        goal_state, 
+        horizon, 
+        timestep, 
+        std::make_unique<cddp::Unicycle>(timestep, integration_type),
+        std::make_unique<cddp::QuadraticObjective>(Q, R, Qf, goal_state, empty_reference_states, timestep),
+        options
+    );
+    solver_baseline.setDynamicalSystem(std::make_unique<cddp::Unicycle>(timestep, integration_type));
+    solver_baseline.setObjective(std::make_unique<cddp::QuadraticObjective>(Q, R, Qf, goal_state, empty_reference_states, timestep));
+    solver_baseline.addConstraint("ControlBoxConstraint", std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
+
+    // Set an initial trajectory (all states equal to the initial state)
+    std::vector<Eigen::VectorXd> X_baseline(horizon + 1, Eigen::VectorXd::Zero(state_dim));
+    std::vector<Eigen::VectorXd> U_baseline(horizon, Eigen::VectorXd::Zero(control_dim));
+    for (int i = 0; i < horizon + 1; ++i) {
+        X_baseline[i] = initial_state;
+    }
+    solver_baseline.setInitialTrajectory(X_baseline, U_baseline);
+
+    cddp::CDDPSolution solution_baseline = solver_baseline.solve("ASCDDP");
+    auto X_baseline_sol = solution_baseline.state_sequence;
+
+    // --------------------------
+    // 2. Solve with BallConstraint
+    cddp::CDDP solver_ball(
+        initial_state, 
+        goal_state, 
+        horizon, 
+        timestep, 
+        std::make_unique<cddp::Unicycle>(timestep, integration_type),
+        std::make_unique<cddp::QuadraticObjective>(Q, R, Qf, goal_state, empty_reference_states, timestep),
+        options
+    );
+    solver_ball.setDynamicalSystem(std::make_unique<cddp::Unicycle>(timestep, integration_type));
+    solver_ball.setObjective(std::make_unique<cddp::QuadraticObjective>(Q, R, Qf, goal_state, empty_reference_states, timestep));
+    solver_ball.addConstraint("ControlBoxConstraint", std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
+    double radius = 0.4;
+    Eigen::Vector2d center(1.0, 1.0);
+    solver_ball.addConstraint("BallConstraint", std::make_unique<cddp::BallConstraint>(radius, center));
+
+    // Set an initial trajectory for the ball-constrained solver
+    std::vector<Eigen::VectorXd> X_ball(horizon + 1, Eigen::VectorXd::Zero(state_dim));
+    std::vector<Eigen::VectorXd> U_ball(horizon, Eigen::VectorXd::Zero(control_dim));
+    for (int i = 0; i < horizon + 1; ++i) {
+        X_ball[i] = initial_state;
+    }
+    solver_ball.setInitialTrajectory(X_ball, U_ball);
+
+    cddp::CDDPSolution solution_ball = solver_ball.solve("ASCDDP");
+    auto X_ball_sol = solution_ball.state_sequence;
+
+    // --------------------------
+    // Plot the trajectories
+    std::vector<double> x_baseline, y_baseline;
+    std::vector<double> x_ball, y_ball;
+    for (const auto &state : X_baseline_sol) {
+        x_baseline.push_back(state(0));
+        y_baseline.push_back(state(1));
+    }
+    for (const auto &state : X_ball_sol) {
+        x_ball.push_back(state(0));
+        y_ball.push_back(state(1));
+    }
+
+    // Plot trajectories on the same figure
+    plt::figure();
+    plt::plot(x_baseline, y_baseline, {{"color", "b"}, {"linestyle", "-"}, {"label", "Without Ball Constraint"}});
+    plt::plot(x_ball, y_ball, {{"color", "r"}, {"linestyle", "-"}, {"label", "With Ball Constraint"}});
+
+    
+    
+    // Also plot the ball for reference
+    std::vector<double> t_ball, x_ball_circle, y_ball_circle;
+    for (double t = 0.0; t < 2 * M_PI; t += 0.01) {
+        t_ball.push_back(t);
+        x_ball_circle.push_back(center(0) + radius * cos(t));
+        y_ball_circle.push_back(center(1) + radius * sin(t));
+    }plt::plot(x_ball_circle, y_ball_circle, {{"color", "g"}, {"linestyle", "--"}, {"label", "Ball Constraint"}});
+
+    plt::xlabel("x");
+    plt::ylabel("y");
+    plt::title("Trajectory Comparison: With vs. Without BallConstraint");
+    plt::legend();
+    
+    // Save the comparison plot
+    std::string plotDirectory = "../results/tests";
+    if (!fs::exists(plotDirectory)) {
+        fs::create_directory(plotDirectory);
+    }
+    plt::save(plotDirectory + "/trajectory_comparison.png");
+    std::cout << "Trajectory comparison saved to " << plotDirectory + "/trajectory_comparison.png" << std::endl;
+
+    return 0;
+}

include/cddp-cpp/cddp_core/cddp_core.hpp

@@ -69,13 +69,13 @@ struct CDDPOptions {
 
     double regularization_state = 1e-6;             // Regularization for state
     double regularization_state_step = 1.0;         // Regularization step for state
-    double regularization_state_max = 1e4;          // Maximum regularization
+    double regularization_state_max = 1e6;          // Maximum regularization
     double regularization_state_min = 1e-8;         // Minimum regularization
     double regularization_state_factor = 1e1;       // Factor for state regularization
 
     double regularization_control = 1e-6;           // Regularization for control
     double regularization_control_step = 1.0;       // Regularization step for control
-    double regularization_control_max = 1e4;        // Maximum regularization
+    double regularization_control_max = 1e5;        // Maximum regularization
     double regularization_control_min = 1e-8;       // Minimum regularization
     double regularization_control_factor = 1e1;     // Factor for control regularization
 

include/cddp-cpp/cddp_core/constraint.hpp

@@ -333,6 +333,7 @@ class BallConstraint : public Constraint {
         center_(center), 
         scale_factor_(scale_factor)
     {
+        dim_ = center.size();
     }
 
     int getDualDim() const override {
@@ -343,35 +344,18 @@ class BallConstraint : public Constraint {
     Eigen::VectorXd evaluate(const Eigen::VectorXd& state, 
                              const Eigen::VectorXd& control) const override 
     {
-        const Eigen::VectorXd& diff = state - center_;
-        return Eigen::VectorXd::Constant(1, scale_factor_ * diff.squaredNorm());
+        const Eigen::VectorXd& diff = state.head(dim_) - center_;
+        return -Eigen::VectorXd::Constant(1, scale_factor_ * diff.squaredNorm());
     }
 
     Eigen::VectorXd getLowerBound() const override {
-        return Eigen::VectorXd::Constant(1, radius_ * radius_);
+        return -Eigen::VectorXd::Constant(1, std::numeric_limits<double>::infinity()); 
     }
 
     Eigen::VectorXd getUpperBound() const override {
-        return Eigen::VectorXd::Constant(1, std::numeric_limits<double>::infinity());
+        return -Eigen::VectorXd::Constant(1, radius_ * radius_) * scale_factor_;
     }
 
-
-    Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, 
-                                     const Eigen::VectorXd& control) const override 
-    {
-        const Eigen::VectorXd& diff = state - center_;
-        Eigen::MatrixXd jac(1, state.size());
-        jac.row(0) = (2.0 * scale_factor_) * diff.transpose();
-        return jac;
-    }
-
-    Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, 
-                                       const Eigen::VectorXd& control) const override 
-    {
-        return Eigen::MatrixXd::Zero(1, control.size());
-    }
-
-
     double computeViolation(const Eigen::VectorXd& state, 
                             const Eigen::VectorXd& control) const override 
     {
@@ -385,12 +369,33 @@ class BallConstraint : public Constraint {
         double lb = getLowerBound()(0);
         return std::max(0.0, val - lb);
     }
+    
+    Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, 
+                                     const Eigen::VectorXd& control) const override 
+    {
+        const Eigen::VectorXd& diff = state.head(dim_) - center_;
+        Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(1, state.size());
+        
+        for (int i = 0; i < dim_; ++i) {
+            jac(0, i) = -2.0 * scale_factor_ * diff(i);
+        }
+
+        return jac;
+    }
+
+    Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, 
+                                       const Eigen::VectorXd& control) const override 
+    {
+        return Eigen::MatrixXd::Zero(1, control.size());
+    }
 
     Eigen::VectorXd getCenter() const { return center_; }
+    double getRadius() const { return radius_; }
 
 private:
     double radius_;
     Eigen::VectorXd center_;
+    int dim_;
     double scale_factor_;
 };