Add bicycle model test

astomodynamics · astomodynamics · commit e0539be96658 · 2024-11-14T16:22:55.000+09:00
diff --git a/include/cddp/model/Bicycle.hpp b/include/cddp/model/Bicycle.hpp
@@ -0,0 +1,115 @@
+#ifndef CDDP_BICYCLE_HPP
+#define CDDP_BICYCLE_HPP
+
+#include "Eigen/Dense"
+#include <vector>
+#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<Eigen::MatrixXd, Eigen::MatrixXd> 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<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> 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
diff --git a/test/CDDPProblemTests_Bicycle.cpp b/test/CDDPProblemTests_Bicycle.cpp
@@ -0,0 +1,129 @@
+#include <iostream> 
+#include "Eigen/Dense"
+// #include "cddp/cddp_core/CDDPProblem.hpp"
+#include "CDDP.hpp"
+#include "model/DubinsCar.hpp"
+#include "matplotlibcpp.hpp"
+
+
+#include <iostream> 
+#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<Bicycle>(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<QuadraticCost>(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<ControlBoxConstraint>(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<Eigen::VectorXd> X = std::vector<Eigen::VectorXd>(HORIZON + 1, Eigen::VectorXd::Zero(STATE_DIM));
+    std::vector<Eigen::VectorXd> U = std::vector<Eigen::VectorXd>(HORIZON, Eigen::VectorXd::Zero(CONTROL_DIM));
+    cddp_solver.setInitialTrajectory(X, U);
+
+    // Solve!
+    std::vector<Eigen::VectorXd> U_sol = cddp_solver.solve();
+
+    std::vector<Eigen::VectorXd> 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<double> 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
@@ -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)
+
 
 
