From e0539be966588c145f571be2897d07bdad58ad76 Mon Sep 17 00:00:00 2001 From: astomodynamics Date: Thu, 14 Nov 2024 16:22:55 +0900 Subject: [PATCH] Add bicycle model test --- include/cddp/model/Bicycle.hpp | 115 ++++++++++++++++++++++++++ test/CDDPProblemTests_Bicycle.cpp | 129 ++++++++++++++++++++++++++++++ test/CMakeLists.txt | 8 ++ 3 files changed, 252 insertions(+) create mode 100644 include/cddp/model/Bicycle.hpp create mode 100644 test/CDDPProblemTests_Bicycle.cpp diff --git a/include/cddp/model/Bicycle.hpp b/include/cddp/model/Bicycle.hpp new file mode 100644 index 0000000..6b96e96 --- /dev/null +++ b/include/cddp/model/Bicycle.hpp @@ -0,0 +1,115 @@ +#ifndef CDDP_BICYCLE_HPP +#define CDDP_BICYCLE_HPP + +#include "Eigen/Dense" +#include +#include "cddp_core/DynamicalSystem.hpp" + +namespace cddp { + +class Bicycle : public cddp::DynamicalSystem { +public: + int state_size_; // State dimension (x, y, theta, v) + int control_size_; // Control dimension (a, delta) + double timestep_; // Time step + double wheelbase_; // Distance between front and rear axles + int integration_type_; + + Bicycle(int state_dim, int control_dim, double timestep, double wheelbase, int integration_type) : + DynamicalSystem(state_dim, control_dim, timestep, integration_type) { + state_size_ = state_dim; // Should be 4: [x, y, theta, v] + control_size_ = control_dim; // Should be 2: [acceleration, steering_angle] + timestep_ = timestep; + wheelbase_ = wheelbase; + integration_type_ = integration_type; + } + + Eigen::VectorXd dynamics(const Eigen::VectorXd &state, const Eigen::VectorXd &control) override { + // State: [x, y, theta, v] + // Control: [acceleration, steering_angle] + Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(state_size_); + + double v = state(3); // velocity + double theta = state(2); // heading angle + double delta = control(1); // steering angle + double a = control(0); // acceleration + + // Kinematic bicycle model equations + state_dot(0) = v * cos(theta); // dx/dt + state_dot(1) = v * sin(theta); // dy/dt + state_dot(2) = (v / wheelbase_) * tan(delta); // dtheta/dt + state_dot(3) = a; // dv/dt + + return state_dot; + } + + std::tuple getDynamicsJacobian( + const Eigen::VectorXd &state, const Eigen::VectorXd &control) override { + // Initialize Jacobian matrices + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(state_size_, state_size_); // df/dx + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(state_size_, control_size_); // df/du + + double v = state(3); // velocity + double theta = state(2); // heading angle + double delta = control(1); // steering angle + + // State Jacobian (A matrix) + // df1/dx = d(dx/dt)/dx + A(0, 2) = -v * sin(theta); // df1/dtheta + A(0, 3) = cos(theta); // df1/dv + + // df2/dx = d(dy/dt)/dx + A(1, 2) = v * cos(theta); // df2/dtheta + A(1, 3) = sin(theta); // df2/dv + + // df3/dx = d(dtheta/dt)/dx + A(2, 3) = tan(delta) / wheelbase_; // df3/dv + + // Control Jacobian (B matrix) + // df/du + B(3, 0) = 1.0; // df4/da (acceleration effect on velocity) + B(2, 1) = v / (wheelbase_ * pow(cos(delta), 2)); // df3/ddelta + + return std::make_tuple(A, B); + } + + std::tuple getDynamicsHessian( + const Eigen::VectorXd &state, const Eigen::VectorXd &control) override { + // Initialize Hessian matrices + Eigen::MatrixXd hxx = Eigen::MatrixXd::Zero(state_size_ * state_size_, state_size_); + Eigen::MatrixXd hxu = Eigen::MatrixXd::Zero(state_size_ * control_size_, state_size_); + Eigen::MatrixXd huu = Eigen::MatrixXd::Zero(state_size_ * control_size_, control_size_); + + double v = state(3); // velocity + double theta = state(2); // heading angle + double delta = control(1); // steering angle + + // Fill in non-zero Hessian terms + // Second derivatives with respect to states + int idx; + + // d²(dx/dt)/dtheta² + idx = 2 * state_size_ + 0; // (theta, x) component + hxx(idx, 2) = -v * cos(theta); + + // d²(dy/dt)/dtheta² + idx = 2 * state_size_ + 1; // (theta, y) component + hxx(idx, 2) = -v * sin(theta); + + // Mixed derivatives (state-control) + // d²(dtheta/dt)/dv/ddelta + idx = 3 * control_size_ + 1; // (v, delta) component + hxu(idx, 2) = 1.0 / (wheelbase_ * pow(cos(delta), 2)); + + // Second derivatives with respect to controls + // d²(dtheta/dt)/ddelta² + idx = 1 * control_size_ + 2; // (delta, theta) component + huu(idx, 1) = 2.0 * v * sin(delta) / (wheelbase_ * pow(cos(delta), 3)); + + return std::make_tuple(hxx, hxu, huu); + } +}; + +} // namespace cddp + +#endif // CDDP_BICYCLE_HPP \ No newline at end of file diff --git a/test/CDDPProblemTests_Bicycle.cpp b/test/CDDPProblemTests_Bicycle.cpp new file mode 100644 index 0000000..41bcc54 --- /dev/null +++ b/test/CDDPProblemTests_Bicycle.cpp @@ -0,0 +1,129 @@ +#include +#include "Eigen/Dense" +// #include "cddp/cddp_core/CDDPProblem.hpp" +#include "CDDP.hpp" +#include "model/DubinsCar.hpp" +#include "matplotlibcpp.hpp" + + +#include +#include "Eigen/Dense" +// #include "cddp/cddp_core/CDDPProblem.hpp" +#include "CDDP.hpp" +#include "model/Bicycle.hpp" +#include "matplotlibcpp.hpp" + +using namespace cddp; +namespace plt = matplotlibcpp; +// Simple Test Function +bool testBasicCDDP() { + const int STATE_DIM = 4; + const int CONTROL_DIM = 2; + const double TIMESTEP = 0.1; + const int HORIZON = 100; + const double WHEEL_BASE = 1.5; + const int INTEGRATION_TYPE = 0; // 0 for Euler, 1 for Heun, 2 for RK3, 3 for RK4 + + // Problem Setup + Eigen::VectorXd initial_state(STATE_DIM); + initial_state << 0.0, 0.0, M_PI/4.0, 0.0; // Initial state + + // Set goal state + Eigen::VectorXd goal_state(STATE_DIM); + goal_state << 5.0, 5.0, M_PI/2.0, 0.0; + + Bicycle system(STATE_DIM, CONTROL_DIM, TIMESTEP, WHEEL_BASE, INTEGRATION_TYPE); + CDDPProblem cddp_solver(initial_state, goal_state, HORIZON, TIMESTEP); + + // Set dynamical system + cddp_solver.setDynamicalSystem(std::make_unique(system)); + + + // Simple Cost Matrices + Eigen::MatrixXd Q(STATE_DIM, STATE_DIM); + Q << 0e-2, 0, 0, 0.0, + 0, 0e-2, 0, 0.0, + 0, 0, 0e-3, 0.0, + 0, 0, 0, 0.0; + + Eigen::MatrixXd R(CONTROL_DIM, CONTROL_DIM); + R << 1e+0, 0, + 0, 1e+0; + + Eigen::MatrixXd Qf(STATE_DIM, STATE_DIM); + Qf << 50, 0, 0, 0.0, + 0, 50, 0, 0.0, + 0, 0, 10, 0.0, + 0, 0, 0, 10.0; + + QuadraticCost objective(Q, R, Qf, goal_state, TIMESTEP); + cddp_solver.setObjective(std::make_unique(objective)); + + // Add constraints + Eigen::VectorXd lower_bound(CONTROL_DIM); + lower_bound << -10.0, -M_PI/3; + + Eigen::VectorXd upper_bound(CONTROL_DIM); + upper_bound << 10.0, M_PI/3; + + ControlBoxConstraint control_constraint(lower_bound, upper_bound); + cddp_solver.addConstraint(std::make_unique(control_constraint)); + + CDDPOptions opts; + // // Set options if needed + opts.max_iterations = 20; + // opts.cost_tolerance = 1e-6; + // opts.grad_tolerance = 1e-8; + // opts.print_iterations = false; + cddp_solver.setOptions(opts); + + + // Set initial trajectory if needed + std::vector X = std::vector(HORIZON + 1, Eigen::VectorXd::Zero(STATE_DIM)); + std::vector U = std::vector(HORIZON, Eigen::VectorXd::Zero(CONTROL_DIM)); + cddp_solver.setInitialTrajectory(X, U); + + // Solve! + std::vector U_sol = cddp_solver.solve(); + + std::vector X_sol = cddp_solver.getTrajectory(); + + // Print solution + std::cout << "Solution: "<< std::endl; + // print last state + std::cout << "Final State: " << X_sol.back().transpose() << std::endl; + // print initial control + std::cout << "Initial Control: " << U_sol[0].transpose() << std::endl; + + // Plotting + std::vector x, y, theta, v; + for (int i = 0; i < X_sol.size(); i++) { + x.push_back(X_sol[i](0)); + y.push_back(X_sol[i](1)); + theta.push_back(X_sol[i](2)); + v.push_back(X_sol[i](3)); + } + + plt::figure(); + plt::plot(x, y); + plt::xlabel("X"); + plt::ylabel("Y"); + plt::title("Bicycle model car Trajectory"); + plt::show(); + + // for (int i = 0; i < U_sol.size(); i++) { + // std::cout << "Control " << i << ": " << U_sol[i].transpose() << std::endl; + // } + + return true; +} + +int main() { + if (testBasicCDDP()) { + std::cout << "Basic CDDP Test Passed!" << std::endl; + } else { + std::cout << "Basic CDDP Test Failed!" << std::endl; + return 1; // Indicate failure + } + return 0; +} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 6c09fe9..09247f3 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -21,5 +21,13 @@ target_link_libraries(cddp_problem_tests PUBLIC Eigen3::Eigen Python3::Python Python3::Module Python3::NumPy PRIVATE osqp-cpp) +# ---- CDDPTests_Bicycle ---- +add_executable(cddp_bicycle_tests + CDDPProblemTests_Bicycle.cpp + ../src/cddp_core/CDDPProblem.cpp) +target_link_libraries(cddp_bicycle_tests PUBLIC Eigen3::Eigen Python3::Python +Python3::Module +Python3::NumPy PRIVATE osqp-cpp) +