Skip to content

Commit 4629df6

Browse files
astomodynamicsastomodynamics
committed
Merge remote-tracking branch 'origin/master' into dev4
2 parents 479ce68 + 8972ef8 commit 4629df6

File tree

9 files changed

+845
-0
lines changed

9 files changed

+845
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -326,6 +326,7 @@ set(dynamics_model_srcs
326326
src/dynamics_model/unicycle.cpp
327327
src/dynamics_model/bicycle.cpp
328328
src/dynamics_model/cartpole.cpp
329+
src/dynamics_model/acrobot.cpp
329330
src/dynamics_model/car.cpp
330331
src/dynamics_model/forklift.cpp
331332
src/dynamics_model/dubins_car.cpp

‎examples/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,9 @@ target_link_libraries(cddp_manipulator cddp)
4141
add_executable(cddp_pendulum cddp_pendulum.cpp)
4242
target_link_libraries(cddp_pendulum cddp)
4343

44+
add_executable(cddp_acrobot cddp_acrobot.cpp)
45+
target_link_libraries(cddp_acrobot cddp)
46+
4447
add_executable(cddp_quadrotor_circle cddp_quadrotor_circle.cpp)
4548
target_link_libraries(cddp_quadrotor_circle cddp)
4649

‎examples/cddp_acrobot.cpp‎

Lines changed: 216 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,216 @@
1+
/*
2+
Copyright 2025 Tomo Sasaki
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
https://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
*/
16+
#include <iostream>
17+
#include <vector>
18+
#include <filesystem>
19+
#include <random>
20+
#include <string>
21+
#include <chrono>
22+
#include <thread>
23+
#include <numeric>
24+
#include "cddp.hpp"
25+
#include "dynamics_model/acrobot.hpp"
26+
#include "matplot/matplot.h"
27+
28+
using namespace matplot;
29+
namespace fs = std::filesystem;
30+
31+
int main() {
32+
int state_dim = 4;
33+
int control_dim = 1;
34+
int horizon = 100;
35+
double timestep = 0.05;
36+
double Tf = timestep * horizon; // Total time
37+
38+
double l1 = 1.0;
39+
double l2 = 1.0;
40+
double m1 = 1.0;
41+
double m2 = 1.0;
42+
double J1 = 1.0;
43+
double J2 = 1.0;
44+
std::string integration_type = "rk4";
45+
46+
std::unique_ptr<cddp::DynamicalSystem> system = std::make_unique<cddp::Acrobot>(
47+
timestep, l1, l2, m1, m2, J1, J2, integration_type);
48+
49+
// Cost matrices
50+
Eigen::MatrixXd Q = 10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim);
51+
Eigen::MatrixXd R = 1e-2 * Eigen::MatrixXd::Identity(control_dim, control_dim);
52+
Eigen::MatrixXd Qf = 100.0 * Q;
53+
54+
// Goal state
55+
Eigen::VectorXd goal_state(state_dim);
56+
goal_state << M_PI/2.0, 0.0, 0.0, 0.0;
57+
58+
std::vector<Eigen::VectorXd> empty_reference_states;
59+
60+
// Initial state
61+
Eigen::VectorXd initial_state(state_dim);
62+
initial_state << -M_PI/2.0, 0.0, 0.0, 0.0;
63+
64+
// Create objective
65+
auto objective = std::make_unique<cddp::QuadraticObjective>(
66+
Q, R, Qf, goal_state, empty_reference_states, timestep);
67+
68+
// Control constraints
69+
Eigen::VectorXd control_lower_bound(control_dim);
70+
control_lower_bound << -15.0;
71+
Eigen::VectorXd control_upper_bound(control_dim);
72+
control_upper_bound << 15.0;
73+
74+
// Solver options
75+
cddp::CDDPOptions options;
76+
options.max_iterations = 200;
77+
options.tolerance = 1e-3;
78+
options.regularization.initial_value = 1e-4;
79+
options.use_ilqr = true;
80+
options.enable_parallel = true;
81+
options.num_threads = 10;
82+
options.debug = false;
83+
options.ipddp.barrier.mu_initial = 1e-1;
84+
85+
// Create CDDP solver
86+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
87+
std::move(system), std::move(objective), options);
88+
89+
// Add control constraint
90+
cddp_solver.addPathConstraint("ControlConstraint",
91+
std::make_unique<cddp::ControlConstraint>(control_upper_bound));
92+
93+
// Initial trajectory
94+
std::vector<Eigen::VectorXd> X_init(horizon + 1, Eigen::VectorXd::Zero(state_dim));
95+
std::vector<Eigen::VectorXd> U_init(horizon, Eigen::VectorXd::Zero(control_dim));
96+
97+
// Rollout initial trajectory
98+
X_init[0] = initial_state;
99+
auto acrobot = std::make_unique<cddp::Acrobot>(timestep, l1, l2, m1, m2, J1, J2, integration_type);
100+
for (int t = 0; t < horizon; ++t) {
101+
X_init[t + 1] = acrobot->getDiscreteDynamics(X_init[t], U_init[t], t * timestep);
102+
}
103+
104+
cddp_solver.setInitialTrajectory(X_init, U_init);
105+
106+
// Solve
107+
cddp::CDDPSolution solution = cddp_solver.solve(cddp::SolverType::IPDDP);
108+
auto X_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("state_trajectory"));
109+
auto U_sol = std::any_cast<std::vector<Eigen::VectorXd>>(solution.at("control_trajectory"));
110+
auto t_sol = std::any_cast<std::vector<double>>(solution.at("time_points"));
111+
112+
// Create plot directory
113+
const std::string plotDirectory = "../results/acrobot";
114+
if (!fs::exists(plotDirectory)) {
115+
fs::create_directories(plotDirectory);
116+
}
117+
118+
// Extract solution data for animation
119+
std::vector<double> time_arr, theta1_arr, theta2_arr;
120+
121+
for (size_t i = 0; i < X_sol.size(); ++i) {
122+
time_arr.push_back(t_sol[i]);
123+
theta1_arr.push_back(X_sol[i](0));
124+
theta2_arr.push_back(X_sol[i](1));
125+
}
126+
127+
// --- Animation ---
128+
auto fig = figure();
129+
auto ax = fig->current_axes();
130+
fig->size(800, 800);
131+
132+
// Generate animation frames
133+
for (size_t i = 0; i < X_sol.size(); i += 5) {
134+
cla(ax);
135+
hold(ax, true);
136+
137+
// Current state
138+
double theta1 = theta1_arr[i];
139+
double theta2 = theta2_arr[i];
140+
141+
// Link 1 endpoint
142+
double x1 = l1 * std::sin(theta1);
143+
double y1 = -l1 * std::cos(theta1);
144+
145+
// Link 2 endpoint (relative to link 1)
146+
double x2 = x1 + l2 * std::sin(theta1 + theta2);
147+
double y2 = y1 - l2 * std::cos(theta1 + theta2);
148+
149+
// Plot link 1
150+
std::vector<double> link1_x = {0.0, x1};
151+
std::vector<double> link1_y = {0.0, y1};
152+
auto link1 = plot(link1_x, link1_y);
153+
link1->line_style("b-");
154+
link1->line_width(5);
155+
156+
// Plot link 2
157+
std::vector<double> link2_x = {x1, x2};
158+
std::vector<double> link2_y = {y1, y2};
159+
auto link2 = plot(link2_x, link2_y);
160+
link2->line_style("r-");
161+
link2->line_width(5);
162+
163+
// Plot joints as circles
164+
std::vector<double> joint0_x = {0.0};
165+
std::vector<double> joint0_y = {0.0};
166+
auto j0 = scatter(joint0_x, joint0_y);
167+
j0->marker_size(10);
168+
j0->marker_color("black");
169+
j0->marker_style("o");
170+
171+
std::vector<double> joint1_x = {x1};
172+
std::vector<double> joint1_y = {y1};
173+
auto j1 = scatter(joint1_x, joint1_y);
174+
j1->marker_size(8);
175+
j1->marker_color("gray");
176+
j1->marker_style("o");
177+
178+
std::vector<double> joint2_x = {x2};
179+
std::vector<double> joint2_y = {y2};
180+
auto j2 = scatter(joint2_x, joint2_y);
181+
j2->marker_size(6);
182+
j2->marker_color("red");
183+
j2->marker_style("o");
184+
185+
// Set axis properties
186+
xlim({-2.5, 2.5});
187+
ylim({-2.5, 2.5});
188+
xlabel("x [m]");
189+
ylabel("y [m]");
190+
title("Acrobot Animation - Time: " + std::to_string(time_arr[i]) + " s");
191+
grid(true);
192+
193+
// Save frame
194+
std::string filename = plotDirectory + "/frame_" + std::to_string(i/5) + ".png";
195+
fig->save(filename);
196+
}
197+
198+
// Combine all saved frames into a GIF using ImageMagick's convert tool
199+
std::string command = "convert -delay 30 " + plotDirectory + "/frame_*.png " + plotDirectory + "/acrobot.gif";
200+
std::system(command.c_str());
201+
202+
// Clean up frame files
203+
std::string cleanup_command = "rm " + plotDirectory + "/frame_*.png";
204+
std::system(cleanup_command.c_str());
205+
206+
std::cout << "Animation saved as " << plotDirectory << "/acrobot.gif" << std::endl;
207+
208+
// Print final state
209+
std::cout << "\nFinal state:" << std::endl;
210+
std::cout << "θ₁ = " << X_sol.back()(0) << " rad" << std::endl;
211+
std::cout << "θ₂ = " << X_sol.back()(1) << " rad" << std::endl;
212+
std::cout << "θ̇₁ = " << X_sol.back()(2) << " rad/s" << std::endl;
213+
std::cout << "θ̇₂ = " << X_sol.back()(3) << " rad/s" << std::endl;
214+
215+
return 0;
216+
}

‎include/cddp-cpp/dynamics_model/acrobot.hpp‎

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
/*
2+
Copyright 2024 Tomo Sasaki
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
https://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
*/
16+
17+
#ifndef CDDP_ACROBOT_HPP
18+
#define CDDP_ACROBOT_HPP
19+
20+
#include "cddp_core/dynamical_system.hpp"
21+
22+
namespace cddp {
23+
24+
/**
25+
* @brief Acrobot model implementation
26+
*
27+
* A double-pendulum with actuation only at the elbow joint.
28+
* State vector: [theta1, theta2, theta1_dot, theta2_dot]
29+
* Control vector: [torque] (applied to second joint only)
30+
*
31+
* This implementation follows the Julia RobotZoo.jl Acrobot model.
32+
*/
33+
class Acrobot : public DynamicalSystem {
34+
public:
35+
/**
36+
* @brief Constructor for the acrobot model
37+
* @param timestep Time step for discretization
38+
* @param l1 Length of first link
39+
* @param l2 Length of second link
40+
* @param m1 Mass of first link
41+
* @param m2 Mass of second link
42+
* @param J1 Inertia of first link
43+
* @param J2 Inertia of second link
44+
* @param integration_type Integration method
45+
*/
46+
Acrobot(double timestep,
47+
double l1 = 1.0, double l2 = 1.0,
48+
double m1 = 1.0, double m2 = 1.0,
49+
double J1 = 1.0, double J2 = 1.0,
50+
std::string integration_type = "euler");
51+
52+
/**
53+
* @brief Computes the continuous-time dynamics of the acrobot
54+
* @param state Current state vector
55+
* @param control Current control input
56+
* @param time Current time (unused in this model)
57+
* @return State derivative vector
58+
*/
59+
Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state,
60+
const Eigen::VectorXd& control, double time) const override;
61+
62+
/**
63+
* @brief Computes the discrete-time dynamics using the specified integration method
64+
* @param state Current state vector
65+
* @param control Current control input
66+
* @param time Current time
67+
* @return Next state vector
68+
*/
69+
Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd& state,
70+
const Eigen::VectorXd& control, double time) const override {
71+
return DynamicalSystem::getDiscreteDynamics(state, control, time);
72+
}
73+
74+
/**
75+
* @brief Computes the Jacobian of the dynamics with respect to the state
76+
* Uses base class autodiff implementation
77+
*/
78+
Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state,
79+
const Eigen::VectorXd& control, double time) const override {
80+
return DynamicalSystem::getStateJacobian(state, control, time);
81+
}
82+
83+
/**
84+
* @brief Computes the Jacobian of the dynamics with respect to the control input
85+
* Uses base class autodiff implementation
86+
*/
87+
Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state,
88+
const Eigen::VectorXd& control, double time) const override {
89+
return DynamicalSystem::getControlJacobian(state, control, time);
90+
}
91+
92+
/**
93+
* @brief Computes the Hessian of the dynamics with respect to the state
94+
* Uses base class autodiff implementation
95+
*/
96+
std::vector<Eigen::MatrixXd> getStateHessian(const Eigen::VectorXd& state,
97+
const Eigen::VectorXd& control, double time) const override {
98+
return DynamicalSystem::getStateHessian(state, control, time);
99+
}
100+
101+
/**
102+
* @brief Computes the Hessian of the dynamics with respect to the control
103+
* Uses base class autodiff implementation
104+
*/
105+
std::vector<Eigen::MatrixXd> getControlHessian(const Eigen::VectorXd& state,
106+
const Eigen::VectorXd& control, double time) const override {
107+
return DynamicalSystem::getControlHessian(state, control, time);
108+
}
109+
110+
// Getters
111+
int getStateDim() const { return STATE_DIM; }
112+
int getControlDim() const { return CONTROL_DIM; }
113+
114+
// Acrobot parameters
115+
double getL1() const { return l1_; }
116+
double getL2() const { return l2_; }
117+
double getM1() const { return m1_; }
118+
double getM2() const { return m2_; }
119+
double getJ1() const { return J1_; }
120+
double getJ2() const { return J2_; }
121+
double getGravity() const { return gravity_; }
122+
double getFriction() const { return friction_; }
123+
124+
/**
125+
* @brief Autodiff implementation for automatic differentiation
126+
*/
127+
VectorXdual2nd getContinuousDynamicsAutodiff(
128+
const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const override;
129+
130+
private:
131+
// Acrobot parameters
132+
double l1_; // length of link 1 [m]
133+
double l2_; // length of link 2 [m]
134+
double m1_; // mass of link 1 [kg]
135+
double m2_; // mass of link 2 [kg]
136+
double J1_; // inertia of link 1 [kg*m^2]
137+
double J2_; // inertia of link 2 [kg*m^2]
138+
double gravity_ = 9.81; // gravity [m/s^2]
139+
double friction_ = 1.0; // friction coefficient
140+
141+
// State indices
142+
static constexpr int STATE_THETA1 = 0; // angle of first link
143+
static constexpr int STATE_THETA2 = 1; // angle of second link
144+
static constexpr int STATE_THETA1_DOT = 2; // angular velocity of first link
145+
static constexpr int STATE_THETA2_DOT = 3; // angular velocity of second link
146+
static constexpr int STATE_DIM = 4; // state dimension
147+
148+
// Control indices
149+
static constexpr int CONTROL_TORQUE = 0; // torque applied to second joint
150+
static constexpr int CONTROL_DIM = 1; // control dimension
151+
};
152+
153+
} // namespace cddp
154+
155+
#endif // CDDP_ACROBOT_HPP

‎results/acrobot/acrobot.gif‎

248 KB
Loading

0 commit comments

Comments
 (0)