Skip to content

Commit

Permalink
Unicycle model and Dubins Car model (#74)
Browse files Browse the repository at this point in the history 
* Modify Dubins Car to Unicycle

* Add dubins car model and example
  • Loading branch information
@astomodynamics
astomodynamics authored Feb 3, 2025
1 parent 03ad6c7 commit a48fa9a
Show file tree
Hide file tree
Showing 17 changed files with 763 additions and 237 deletions.
3 changes: 2 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 3 additions & 3 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
3 changes: 3 additions & 0 deletions examples/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
199 changes: 107 additions & 92 deletions examples/cddp_dubins_car.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Loading

0 comments on commit a48fa9a

Please sign in to comment.