Unicycle model and Dubins Car model (#74)

* Modify Dubins Car to Unicycle

* Add dubins car model and example

Author: astomodynamics

CMakeLists.txt

@@ -208,10 +208,11 @@ endif()
 
 set(dynamics_model_srcs
   src/dynamics_model/pendulum.cpp
-  src/dynamics_model/dubins_car.cpp
+  src/dynamics_model/unicycle.cpp
   src/dynamics_model/bicycle.cpp
   src/dynamics_model/cartpole.cpp
   src/dynamics_model/car.cpp
+  src/dynamics_model/dubins_car.cpp
   src/dynamics_model/quadrotor.cpp
   src/dynamics_model/manipulator.cpp
   src/dynamics_model/spacecraft_linear.cpp

README.md

@@ -37,15 +37,15 @@ $$
 $$
 
 ## Examples
-### Dubins Car
+### Unicycle
 
 Simple car-like robot with velocity and steering control:
 
 ```bash
-./examples/cddp_dubins_car // after building
+./examples/cddp_unicycle // after building
 ```
 
-<img src="results/tests/dubins_car.gif" width="300" alt="Dubins Car CDDP">
+<img src="results/tests/unicycle.gif" width="300" alt="Unicycle Model CDDP">
 
 ### Bicycle Model
 

examples/CMakeLists.txt

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

examples/cddp_dubins_car.cpp

@@ -13,192 +13,207 @@
  See the License for the specific language governing permissions and
  limitations under the License.
 */
+
 #include <iostream>
 #include <vector>
 #include <filesystem>
 
-#include "cddp.hpp"
+#include "cddp.hpp"  // Adjust include as needed for your CDDP framework and DubinsCar definition
 
 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";
+    const int state_dim = 3;        // [x, y, theta]
+    const int control_dim = 1;      // [omega]
+    const int horizon = 100;        // planning horizon
+    const double timestep = 0.03;   // integration step
+    const std::string integration_type = "euler";
 
-    // Create a dubins car instance 
-    std::unique_ptr<cddp::DynamicalSystem> system = std::make_unique<cddp::DubinsCar>(timestep, integration_type); // Create unique_ptr
+    // Create a DubinsCar instance (constant speed + single steering input)
+    double forward_speed = 1.0;  // For example, 1.0 m/s
+    std::unique_ptr<cddp::DynamicalSystem> system =
+        std::make_unique<cddp::DubinsCar>(forward_speed, timestep, integration_type);
 
     // Create objective function
+    // State cost matrix Q (typically zero or small if you only penalize final state heavily)
     Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim);
+
+    // Control cost matrix R
+    // For single control dimension, this is a 1x1 matrix:
     Eigen::MatrixXd R = 0.5 * Eigen::MatrixXd::Identity(control_dim, control_dim);
+
+    // Final state cost matrix Qf
+    // For example, a heavier penalty on final position/orientation
     Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim);
-    Qf << 50.0, 0.0, 0.0,
-          0.0, 50.0, 0.0,
-          0.0, 0.0, 10.0;
-    Qf = 0.5 * Qf;
+    Qf(0,0) = 50.0;  // x
+    Qf(1,1) = 50.0;  // y
+    Qf(2,2) = 10.0;  // theta
+    Qf = 0.5 * Qf;   // scaling
+
+    // Goal state
     Eigen::VectorXd goal_state(state_dim);
-    goal_state << 2.0, 2.0, M_PI/2.0;
+    goal_state << 2.0, 2.0, M_PI / 2.0;
 
-    // Create an empty vector of Eigen::VectorXd
+    // Create an empty reference-state sequence (if needed for time-varying references)
     std::vector<Eigen::VectorXd> empty_reference_states; 
-    auto objective = std::make_unique<cddp::QuadraticObjective>(Q, R, Qf, goal_state, empty_reference_states, timestep);
 
-    // Initial and target states
+    // Build the quadratic objective
+    auto objective = std::make_unique<cddp::QuadraticObjective>(
+        Q, R, Qf, goal_state, empty_reference_states, timestep);
+
+    // Initial state
     Eigen::VectorXd initial_state(state_dim);
-    initial_state << 0.0, 0.0, M_PI/4.0; 
+    initial_state << 0.0, 0.0, M_PI / 4.0;
 
     // Create CDDP solver
     cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep);
     cddp_solver.setDynamicalSystem(std::move(system));
     cddp_solver.setObjective(std::move(objective));
 
-    // Define constraints
+    // Define box constraints on control (here, single dimension: -pi to pi)
     Eigen::VectorXd control_lower_bound(control_dim);
-    control_lower_bound << -1.0, -M_PI;
     Eigen::VectorXd control_upper_bound(control_dim);
-    control_upper_bound << 1.0, M_PI;
-    
+    control_lower_bound << -M_PI;  // min turn rate
+    control_upper_bound <<  M_PI;  // max turn rate
+
     // Add the constraint to the solver
-    cddp_solver.addConstraint(std::string("ControlBoxConstraint"), std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
+    cddp_solver.addConstraint(
+        "ControlBoxConstraint",
+        std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
+
+    // (Optional) retrieve the constraint object
     auto constraint = cddp_solver.getConstraint<cddp::ControlBoxConstraint>("ControlBoxConstraint");
 
-    // Set options
+    // Set CDDP options
     cddp::CDDPOptions options;
-    options.max_iterations = 10;
+    options.max_iterations = 10;   // for demonstration
     options.barrier_coeff = 1e-2;
     options.barrier_factor = 0.1;
     cddp_solver.setOptions(options);
 
-    // Set initial trajectory
+    // Set an initial guess for the trajectory
+    // States (X) is horizon+1 in length, each dimension: 3
+    // Controls (U) is horizon in length, each dimension: 1
     std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));
-    std::vector<Eigen::VectorXd> U(horizon, Eigen::VectorXd::Zero(control_dim));
+    std::vector<Eigen::VectorXd> U(horizon,      Eigen::VectorXd::Zero(control_dim));
     cddp_solver.setInitialTrajectory(X, U);
 
     // Solve the problem
-    // cddp::CDDPSolution solution = cddp_solver.solve("CLCDDP");
+    // Possible algorithms: "CLCDDP", "LogCDDP", etc. 
     cddp::CDDPSolution solution = cddp_solver.solve("LogCDDP");
 
-    // Extract solution
-    auto X_sol = solution.state_sequence; // size: horizon + 1
+    // Extract the solution sequences
+    auto X_sol = solution.state_sequence;   // size: horizon + 1
     auto U_sol = solution.control_sequence; // size: horizon
-    auto t_sol = solution.time_sequence; // size: horizon + 1
+    auto t_sol = solution.time_sequence;    // size: horizon + 1
 
-    // Create directory for saving plot (if it doesn't exist)
+    // Create directory for saving plots (if it doesn't exist)
     const std::string plotDirectory = "../results/tests";
     if (!fs::exists(plotDirectory)) {
-        fs::create_directory(plotDirectory);
+        fs::create_directories(plotDirectory);
     }
 
-    // Plot the solution (x-y plane)
+    // Gather data for plotting
     std::vector<double> x_arr, y_arr, theta_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));
     }
 
-    // Plot the solution (control inputs)
-    std::vector<double> v_arr, omega_arr;
+    // For single-dim control: just store the steering rate
+    std::vector<double> omega_arr;
     for (const auto& u : U_sol) {
-        v_arr.push_back(u(0));
-        omega_arr.push_back(u(1));
+        omega_arr.push_back(u(0));
     }
 
-    // Plot the solution by subplots
+    // Plot state trajectory & control
     plt::subplot(2, 1, 1);
     plt::plot(x_arr, y_arr);
-    plt::title("State Trajectory");
+    plt::title("DubinsCar State Trajectory");
     plt::xlabel("x");
     plt::ylabel("y");
 
     plt::subplot(2, 1, 2);
-    plt::plot(v_arr);
     plt::plot(omega_arr);
-    plt::title("Control Inputs");
-    plt::save(plotDirectory + "/dubincs_car_cddp_test.png");
+    plt::title("Steering Rate Control (omega)");
+    plt::save(plotDirectory + "/dubins_car_cddp_test.png");
 
-    // Create figure and axes
+    // Optional: Generate an animation 
+    //           (requires multiple frames, so you may want to store and 
+    //            convert them to a GIF afterward).
     plt::figure_size(800, 600);
-    plt::title("Dubins Car Trajectory");
+    plt::title("DubinsCar Trajectory");
     plt::xlabel("x");
     plt::ylabel("y");
-    plt::xlim(-1, 3); // Adjust limits as needed
-    plt::ylim(-1, 3); // Adjust limits as needed
+    plt::xlim(-1, 3);
+    plt::ylim(-1, 3);
 
     // Car dimensions
     double car_length = 0.2;
-    double car_width = 0.1;
-
-    // Animation function
-    for (int i = 0; i < X_sol.size(); ++i) {
+    double car_width  = 0.1;
 
+    // Animation loop
+    for (int i = 0; i < static_cast<int>(X_sol.size()); ++i) {
         if (i % 5 == 0) {
-            // Clear previous plot
-			plt::clf();
+            plt::clf();
 
-            // Plot car as a box with heading
-            double x = x_arr[i];
-            double y = y_arr[i];
+            // Current pose
+            double x     = x_arr[i];
+            double y     = y_arr[i];
             double theta = theta_arr[i];
-            
-            // Calculate car corner points
-            std::vector<double> car_x(5);
-            std::vector<double> car_y(5);
 
-            car_x[0] = x + car_length/2 * cos(theta) - car_width/2 * sin(theta);
-            car_y[0] = y + car_length/2 * sin(theta) + car_width/2 * cos(theta);
+            // Compute corners of the car rectangle
+            std::vector<double> car_x(5), car_y(5);
 
-            car_x[1] = x + car_length/2 * cos(theta) + car_width/2 * sin(theta);
-            car_y[1] = y + car_length/2 * sin(theta) - car_width/2 * cos(theta);
+            // Front-left corner
+            car_x[0] = x +  car_length/2.0 * cos(theta) - car_width/2.0 * sin(theta);
+            car_y[0] = y +  car_length/2.0 * sin(theta) + car_width/2.0 * cos(theta);
 
-            car_x[2] = x - car_length/2 * cos(theta) + car_width/2 * sin(theta);
-            car_y[2] = y - car_length/2 * sin(theta) - car_width/2 * cos(theta);
+            // Front-right corner
+            car_x[1] = x +  car_length/2.0 * cos(theta) + car_width/2.0 * sin(theta);
+            car_y[1] = y +  car_length/2.0 * sin(theta) - car_width/2.0 * cos(theta);
 
-            car_x[3] = x - car_length/2 * cos(theta) - car_width/2 * sin(theta);
-            car_y[3] = y - car_length/2 * sin(theta) + car_width/2 * cos(theta);
+            // Rear-right corner
+            car_x[2] = x -  car_length/2.0 * cos(theta) + car_width/2.0 * sin(theta);
+            car_y[2] = y -  car_length/2.0 * sin(theta) - car_width/2.0 * cos(theta);
 
-            car_x[4] = car_x[0]; // Close the shape
+            // Rear-left corner
+            car_x[3] = x -  car_length/2.0 * cos(theta) - car_width/2.0 * sin(theta);
+            car_y[3] = y -  car_length/2.0 * sin(theta) + car_width/2.0 * cos(theta);
+
+            // Close the shape
+            car_x[4] = car_x[0];
             car_y[4] = car_y[0];
 
             // Plot the car
             plt::plot(car_x, car_y, "k-");        
 
-            // Plot trajectory up to current point
-            plt::plot(std::vector<double>(x_arr.begin(), x_arr.begin() + i + 1), 
-                    std::vector<double>(y_arr.begin(), y_arr.begin() + i + 1), "b-");
-
-            // Add plot title
-            plt::title("Dubins Car Trajectory");
+            // Plot the trajectory so far
+            plt::plot(std::vector<double>(x_arr.begin(), x_arr.begin() + i + 1),
+                      std::vector<double>(y_arr.begin(), y_arr.begin() + i + 1),
+                      "b-");
 
-            // Set labels
+            // Title and axes
+            plt::title("DubinsCar Trajectory");
             plt::xlabel("x");
             plt::ylabel("y");
+            plt::xlim(-1, 3);
+            plt::ylim(-1, 3);
 
-            // Adjust limits as needed
-            plt::xlim(-1, 3); 
-            plt::ylim(-1, 3); 
-
-            // Enable legend
-            plt::legend();
-
-            // Save current frame as an image
+            // Save frame
             std::string filename = plotDirectory + "/dubins_car_frame_" + std::to_string(i) + ".png";
             plt::save(filename);
-            
-            // Display plot continuously
-            plt::pause(0.01); // Pause for a short time
 
+            // Brief pause for "animation" effect
+            plt::pause(0.01);
         }
-    };
-}
+    }
 
-// Create gif from images using ImageMagick
-// Installation:
-// $ sudo apt-get install imagemagick
+    // Optionally, you can convert frames to a GIF via ImageMagick:
+    //   convert -delay 100 ../results/tests/dubins_car_frame_*.png ../results/tests/dubins_car.gif
 
-// convert -delay 100 ../results/tests/dubins_car_frame_*.png ../results/tests/dubins_car.gif 
+    return 0;
+}