From 015565fc132575dbcb7a3744bd9e0b41108251fd Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 31 Mar 2025 18:21:47 -0400 Subject: [PATCH 01/18] Add Hessian support for dynamical systems - Introduced `getStateHessian`, `getControlHessian`, and `getCrossHessian` methods across various dynamical models, returning vectors of matrices for Hessian calculations. - Added a new example `hessian_example.cpp` demonstrating Hessian computations for pendulum and Dubins car models. - Updated CMakeLists to include the new example and corresponding test file. - Implemented unit tests for Hessian calculations in both pendulum and Dubins car models. --- examples/CMakeLists.txt | 3 + examples/hessian_example.cpp | 182 ++++++++++++ .../cddp-cpp/cddp_core/dynamical_system.hpp | 34 ++- .../cddp_core/neural_dynamical_system.hpp | 18 +- include/cddp-cpp/dynamics_model/bicycle.hpp | 8 +- include/cddp-cpp/dynamics_model/car.hpp | 8 +- include/cddp-cpp/dynamics_model/cartpole.hpp | 12 +- .../dynamics_model/dreyfus_rocket.hpp | 12 +- .../cddp-cpp/dynamics_model/dubins_car.hpp | 12 +- .../cddp-cpp/dynamics_model/lti_system.hpp | 6 +- .../cddp-cpp/dynamics_model/manipulator.hpp | 8 +- include/cddp-cpp/dynamics_model/pendulum.hpp | 8 +- include/cddp-cpp/dynamics_model/quadrotor.hpp | 8 +- .../dynamics_model/spacecraft_landing2d.hpp | 4 +- .../dynamics_model/spacecraft_linear.hpp | 8 +- .../dynamics_model/spacecraft_nonlinear.hpp | 12 +- .../dynamics_model/spacecraft_roe.hpp | 12 +- .../dynamics_model/spacecraft_twobody.hpp | 12 +- include/cddp-cpp/dynamics_model/unicycle.hpp | 8 +- src/cddp_core/neural_dynamical_system.cpp | 48 +++- src/dynamics_model/bicycle.cpp | 79 +++--- src/dynamics_model/car.cpp | 16 +- src/dynamics_model/cartpole.cpp | 16 +- src/dynamics_model/dreyfus_rocket.cpp | 21 +- src/dynamics_model/dubins_car.cpp | 47 +++- src/dynamics_model/lti_system.cpp | 16 +- src/dynamics_model/manipulator.cpp | 16 +- src/dynamics_model/pendulum.cpp | 36 ++- src/dynamics_model/quadrotor.cpp | 20 +- src/dynamics_model/spacecraft_landing2d.cpp | 16 +- src/dynamics_model/spacecraft_linear.cpp | 16 +- src/dynamics_model/spacecraft_nonlinear.cpp | 16 +- src/dynamics_model/spacecraft_roe.cpp | 16 +- src/dynamics_model/spacecraft_twobody.cpp | 16 +- src/dynamics_model/unicycle.cpp | 31 +- tests/CMakeLists.txt | 4 + tests/test_hessian.cpp | 265 ++++++++++++++++++ 37 files changed, 868 insertions(+), 202 deletions(-) create mode 100644 examples/hessian_example.cpp create mode 100644 tests/test_hessian.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 13fb070..8b8d0a8 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -37,6 +37,9 @@ target_link_libraries(cddp_manipulator cddp) add_executable(cddp_pendulum cddp_pendulum.cpp) target_link_libraries(cddp_pendulum cddp) +add_executable(hessian_example hessian_example.cpp) +target_link_libraries(hessian_example cddp) + add_executable(cddp_quadrotor_circle cddp_quadrotor_circle.cpp) target_link_libraries(cddp_quadrotor_circle cddp) diff --git a/examples/hessian_example.cpp b/examples/hessian_example.cpp new file mode 100644 index 0000000..9f3be7b --- /dev/null +++ b/examples/hessian_example.cpp @@ -0,0 +1,182 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#include +#include +#include +#include + +#include "dynamics_model/pendulum.hpp" +#include "dynamics_model/dubins_car.hpp" + +using namespace cddp; + +// Helper function to print a matrix with a label +void printMatrix(const std::string& label, const Eigen::MatrixXd& matrix) { + std::cout << label << " (" << matrix.rows() << "x" << matrix.cols() << ")" << std::endl; + std::cout << std::fixed << std::setprecision(6) << matrix << std::endl << std::endl; +} + +// Helper function to print the Hessian tensor +void printHessianTensor(const std::string& label, const std::vector& hessian) { + std::cout << label << " (Tensor with " << hessian.size() << " matrices)" << std::endl; + + for (size_t i = 0; i < hessian.size(); ++i) { + std::cout << "Matrix for state dimension " << i << " (" + << hessian[i].rows() << "x" << hessian[i].cols() << "):" << std::endl; + std::cout << std::fixed << std::setprecision(6) << hessian[i] << std::endl; + } + std::cout << std::endl; +} + +// Function to demonstrate pendulum Hessian +void demonstratePendulumHessian() { + std::cout << "\n========== PENDULUM MODEL EXAMPLE ==========" << std::endl; + + // Create a pendulum model + double timestep = 0.01; + double length = 1.0; // Length of the pendulum [m] + double mass = 1.0; // Mass [kg] + double damping = 0.1; // Damping coefficient + std::string integration = "rk4"; + + Pendulum pendulum(timestep, length, mass, damping, integration); + + // Define a state and control + Eigen::VectorXd state = Eigen::VectorXd::Zero(2); + state << M_PI / 4.0, 0.0; // 45-degree angle, zero velocity + + Eigen::VectorXd control = Eigen::VectorXd::Zero(1); + control << 1.0; // Apply a torque of 1 Nm + + // Print system information + std::cout << "Pendulum parameters:" << std::endl; + std::cout << "Length: " << pendulum.getLength() << " m" << std::endl; + std::cout << "Mass: " << pendulum.getMass() << " kg" << std::endl; + std::cout << "Damping: " << pendulum.getDamping() << std::endl; + std::cout << "Gravity: " << pendulum.getGravity() << " m/s²" << std::endl; + std::cout << "Timestep: " << pendulum.getTimestep() << " s" << std::endl; + std::cout << "Integration: " << pendulum.getIntegrationType() << std::endl << std::endl; + + // Print state and control + std::cout << "State: [theta, theta_dot] = [" << state.transpose() << "]" << std::endl; + std::cout << "Control: [torque] = [" << control.transpose() << "]" << std::endl << std::endl; + + // Compute dynamics + Eigen::VectorXd xdot = pendulum.getContinuousDynamics(state, control); + std::cout << "Continuous Dynamics (xdot): [" << xdot.transpose() << "]" << std::endl; + + Eigen::VectorXd next_state = pendulum.getDiscreteDynamics(state, control); + std::cout << "Discrete Dynamics (next state): [" << next_state.transpose() << "]" << std::endl << std::endl; + + // Compute Jacobians + Eigen::MatrixXd A = pendulum.getStateJacobian(state, control); + Eigen::MatrixXd B = pendulum.getControlJacobian(state, control); + + printMatrix("State Jacobian (A)", A); + printMatrix("Control Jacobian (B)", B); + + // Compute Hessians + std::vector state_hessian = pendulum.getStateHessian(state, control); + std::vector control_hessian = pendulum.getControlHessian(state, control); + std::vector cross_hessian = pendulum.getCrossHessian(state, control); + + printHessianTensor("State Hessian (d²f/dx²)", state_hessian); + printHessianTensor("Control Hessian (d²f/du²)", control_hessian); + printHessianTensor("Cross Hessian (d²f/dudx)", cross_hessian); + + // Demonstrate how to access specific elements of the Hessian + // For example, accessing d²theta_dot/dtheta² (second derivative of theta_dot with respect to theta) + int state_idx = 1; // theta_dot is state index 1 + int wrt_state_idx1 = 0; // first derivative with respect to theta (index 0) + int wrt_state_idx2 = 0; // second derivative with respect to theta (index 0) + + double d2f_dx2 = state_hessian[state_idx](wrt_state_idx1, wrt_state_idx2); + std::cout << "d²theta_dot/dtheta² = " << d2f_dx2 << std::endl; +} + +// Function to demonstrate Dubins car Hessian +void demonstrateDubinsCarHessian() { + std::cout << "\n========== DUBINS CAR MODEL EXAMPLE ==========" << std::endl; + + // Create a Dubins car model + double speed = 1.0; // Constant forward speed [m/s] + double timestep = 0.01; // Time step [s] + std::string integration = "rk4"; + + DubinsCar dubins_car(speed, timestep, integration); + + // Define a state and control + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); + state << 0.0, 0.0, M_PI / 4.0; // (x, y, theta) = (0, 0, 45°) + + Eigen::VectorXd control = Eigen::VectorXd::Zero(1); + control << 0.5; // Turn rate of 0.5 rad/s + + // Print system information + std::cout << "Dubins Car parameters:" << std::endl; + std::cout << "Speed: " << speed << " m/s" << std::endl; + std::cout << "Timestep: " << dubins_car.getTimestep() << " s" << std::endl; + std::cout << "Integration: " << dubins_car.getIntegrationType() << std::endl << std::endl; + + // Print state and control + std::cout << "State: [x, y, theta] = [" << state.transpose() << "]" << std::endl; + std::cout << "Control: [omega] = [" << control.transpose() << "]" << std::endl << std::endl; + + // Compute dynamics + Eigen::VectorXd xdot = dubins_car.getContinuousDynamics(state, control); + std::cout << "Continuous Dynamics (xdot): [" << xdot.transpose() << "]" << std::endl; + + Eigen::VectorXd next_state = dubins_car.getDiscreteDynamics(state, control); + std::cout << "Discrete Dynamics (next state): [" << next_state.transpose() << "]" << std::endl << std::endl; + + // Compute Jacobians + Eigen::MatrixXd A = dubins_car.getStateJacobian(state, control); + Eigen::MatrixXd B = dubins_car.getControlJacobian(state, control); + + printMatrix("State Jacobian (A)", A); + printMatrix("Control Jacobian (B)", B); + + // Compute Hessians + std::vector state_hessian = dubins_car.getStateHessian(state, control); + std::vector control_hessian = dubins_car.getControlHessian(state, control); + std::vector cross_hessian = dubins_car.getCrossHessian(state, control); + + printHessianTensor("State Hessian (d²f/dx²)", state_hessian); + printHessianTensor("Control Hessian (d²f/du²)", control_hessian); + printHessianTensor("Cross Hessian (d²f/dudx)", cross_hessian); + + // Demonstrate how to access specific elements of the Hessian + // For example, accessing d²x/dtheta² (second derivative of x with respect to theta) + int state_idx = 0; // x is state index 0 + int wrt_state_idx1 = 2; // first derivative with respect to theta (index 2) + int wrt_state_idx2 = 2; // second derivative with respect to theta (index 2) + + double d2f_dx2 = state_hessian[state_idx](wrt_state_idx1, wrt_state_idx2); + std::cout << "d²x/dtheta² = " << d2f_dx2 << std::endl; +} + +int main() { + std::cout << "Hessian Examples for Dynamical Systems" << std::endl; + std::cout << "=====================================" << std::endl; + + // Demonstrate pendulum Hessian + demonstratePendulumHessian(); + + // Demonstrate Dubins car Hessian + demonstrateDubinsCarHessian(); + return 0; +} \ No newline at end of file diff --git a/include/cddp-cpp/cddp_core/dynamical_system.hpp b/include/cddp-cpp/cddp_core/dynamical_system.hpp index 8c65965..03fd76d 100644 --- a/include/cddp-cpp/cddp_core/dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/dynamical_system.hpp @@ -17,6 +17,7 @@ #define CDDP_DYNAMICAL_SYSTEM_HPP #include +#include #include "cddp_core/helper.hpp" namespace cddp { @@ -51,29 +52,38 @@ class DynamicalSystem { return {getStateJacobian(state, control), getControlJacobian(state, control)}; } - // TODO: Add methods for Hessian calculations // Hessian of dynamics w.r.t state: d^2f/dx^2 - // Note: This is a tensor, but we represent it as a matrix for now. - // Each row corresponds to the Hessian for one state dimension - virtual Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + // This is a tensor (state_dim x state_dim x state_dim), represented as a vector of matrices + // Each matrix is state_dim x state_dim, and corresponds to the Hessian of one state dimension + virtual std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0; // Hessian of dynamics w.r.t control: d^2f/du^2 - // Similar representation as state Hessian - virtual Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + // This is a tensor (state_dim x control_dim x control_dim), represented as a vector of matrices + // Each matrix is control_dim x control_dim, and corresponds to the Hessian of one state dimension + virtual std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0; // Hessian of dynamics w.r.t state and control: d^2f/dudx - // Similar representation - virtual Eigen::MatrixXd getCrossHessian(const Eigen::VectorXd& state, + // This is a tensor (state_dim x control_dim x state_dim), represented as a vector of matrices + // Each matrix is control_dim x state_dim, and corresponds to the Hessian of one state dimension + virtual std::vector getCrossHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(state.size() * control.size(), state.size()); + std::vector cross_hessian(state_dim_); + for (int i = 0; i < state_dim_; ++i) { + cross_hessian[i] = Eigen::MatrixXd::Zero(control_dim_, state_dim_); + } + return cross_hessian; } // Hessian of dynamics w.r.t state and control: d^2f/dx^2, d^2f/du^2, d^2f/dudx - virtual std::tuple getHessians(const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const { - return {getStateHessian(state, control), getControlHessian(state, control), getCrossHessian(state, control)}; + virtual std::tuple, + std::vector, + std::vector> getHessians(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { + return {getStateHessian(state, control), + getControlHessian(state, control), + getCrossHessian(state, control)}; } // Accessor methods diff --git a/include/cddp-cpp/cddp_core/neural_dynamical_system.hpp b/include/cddp-cpp/cddp_core/neural_dynamical_system.hpp index 0ef1d30..6f03854 100644 --- a/include/cddp-cpp/cddp_core/neural_dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/neural_dynamical_system.hpp @@ -97,33 +97,33 @@ class NeuralDynamicalSystem : public DynamicalSystem { const Eigen::VectorXd& control) const override; /** - * @brief Hessian of the dynamics w.r.t. state (flattened or block representation). + * @brief Hessian of the dynamics w.r.t. state * * @param state Current state (Eigen vector) * @param control Current control (Eigen vector) - * @return Eigen::MatrixXd Hessian d^2f/dx^2 + * @return std::vector Vector of Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** - * @brief Hessian of the dynamics w.r.t. control (flattened or block representation). + * @brief Hessian of the dynamics w.r.t. control * * @param state Current state (Eigen vector) * @param control Current control (Eigen vector) - * @return Eigen::MatrixXd Hessian d^2f/du^2 + * @return std::vector Vector of Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** - * @brief Hessian of the dynamics w.r.t. state and control (flattened or block representation). + * @brief Hessian of the dynamics w.r.t. state and control * * @param state Current state (Eigen vector) * @param control Current control (Eigen vector) - * @return Eigen::MatrixXd Hessian d^2f/dudx + * @return std::vector Vector of Hessian matrices, one per state dimension */ - Eigen::MatrixXd getCrossHessian(const Eigen::VectorXd& state, + std::vector getCrossHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/bicycle.hpp b/include/cddp-cpp/dynamics_model/bicycle.hpp index 6db1e21..8068b15 100644 --- a/include/cddp-cpp/dynamics_model/bicycle.hpp +++ b/include/cddp-cpp/dynamics_model/bicycle.hpp @@ -75,18 +75,18 @@ class Bicycle : public DynamicalSystem { * Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/car.hpp b/include/cddp-cpp/dynamics_model/car.hpp index 7f4c3dc..caf2ed2 100644 --- a/include/cddp-cpp/dynamics_model/car.hpp +++ b/include/cddp-cpp/dynamics_model/car.hpp @@ -97,18 +97,18 @@ class Car : public DynamicalSystem { * @brief Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix (d²f/dx²) + * @return Vector of state Hessian matrices, one per state dimension (d²f/dx²) */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * @brief Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix (d²f/du²) + * @return Vector of control Hessian matrices, one per state dimension (d²f/du²) */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/cartpole.hpp b/include/cddp-cpp/dynamics_model/cartpole.hpp index de61313..ebfbf29 100644 --- a/include/cddp-cpp/dynamics_model/cartpole.hpp +++ b/include/cddp-cpp/dynamics_model/cartpole.hpp @@ -93,21 +93,21 @@ class CartPole : public DynamicalSystem { const Eigen::VectorXd& control) const override; /** - * @brief Computes state Hessian matrix (∂²f/∂x²) + * @brief Computes state Hessian tensor (∂²f/∂x²) * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** - * @brief Computes control Hessian matrix (∂²f/∂u²) + * @brief Computes control Hessian tensor (∂²f/∂u²) * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; // Getters diff --git a/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp b/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp index d18bc3d..d0a0e93 100644 --- a/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp +++ b/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp @@ -79,21 +79,21 @@ class DreyfusRocket : public DynamicalSystem { const Eigen::VectorXd& control) const override; /** - * @brief Computes state Hessian matrix (∂²f/∂x²) + * @brief Computes state Hessian tensor (∂²f/∂x²) * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** - * @brief Computes control Hessian matrix (∂²f/∂u²) + * @brief Computes control Hessian tensor (∂²f/∂u²) * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; // Getters diff --git a/include/cddp-cpp/dynamics_model/dubins_car.hpp b/include/cddp-cpp/dynamics_model/dubins_car.hpp index 05c9143..f8758d1 100644 --- a/include/cddp-cpp/dynamics_model/dubins_car.hpp +++ b/include/cddp-cpp/dynamics_model/dubins_car.hpp @@ -84,17 +84,21 @@ class DubinsCar : public DynamicalSystem { /** * @brief Hessian of the dynamics wrt. the state * - * For many simpler models, often zero or handled by separate higher-order libraries. + * @param state Current state + * @param control Current control + * @return Vector of state Hessian matrices (one per state dimension) */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * @brief Hessian of the dynamics wrt. the control * - * Typically zero or not used for simpler models. + * @param state Current state + * @param control Current control + * @return Vector of control Hessian matrices (one per state dimension) */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/lti_system.hpp b/include/cddp-cpp/dynamics_model/lti_system.hpp index cee18de..92491d0 100644 --- a/include/cddp-cpp/dynamics_model/lti_system.hpp +++ b/include/cddp-cpp/dynamics_model/lti_system.hpp @@ -88,14 +88,16 @@ class LTISystem : public DynamicalSystem { /** * @brief Computes state Hessian (zero for linear system) + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * @brief Computes control Hessian (zero for linear system) + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; // Getters diff --git a/include/cddp-cpp/dynamics_model/manipulator.hpp b/include/cddp-cpp/dynamics_model/manipulator.hpp index 136d21c..312d46f 100644 --- a/include/cddp-cpp/dynamics_model/manipulator.hpp +++ b/include/cddp-cpp/dynamics_model/manipulator.hpp @@ -81,18 +81,18 @@ class Manipulator : public DynamicalSystem { * Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** diff --git a/include/cddp-cpp/dynamics_model/pendulum.hpp b/include/cddp-cpp/dynamics_model/pendulum.hpp index dcf84c9..cf6efb8 100644 --- a/include/cddp-cpp/dynamics_model/pendulum.hpp +++ b/include/cddp-cpp/dynamics_model/pendulum.hpp @@ -85,18 +85,18 @@ class Pendulum : public DynamicalSystem { * @brief Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices (one matrix per state dimension) */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * @brief Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices (one matrix per state dimension) */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; // Getters diff --git a/include/cddp-cpp/dynamics_model/quadrotor.hpp b/include/cddp-cpp/dynamics_model/quadrotor.hpp index 4884efa..ad7bb95 100644 --- a/include/cddp-cpp/dynamics_model/quadrotor.hpp +++ b/include/cddp-cpp/dynamics_model/quadrotor.hpp @@ -78,18 +78,18 @@ class Quadrotor : public DynamicalSystem { * Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp b/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp index 2f62b8d..381c868 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp @@ -63,10 +63,10 @@ class SpacecraftLanding2D : public DynamicalSystem { Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; // Accessors diff --git a/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp b/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp index 4984dea..b386372 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp @@ -79,18 +79,18 @@ class HCW : public DynamicalSystem { * Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/spacecraft_nonlinear.hpp b/include/cddp-cpp/dynamics_model/spacecraft_nonlinear.hpp index b32f099..cce01a4 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_nonlinear.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_nonlinear.hpp @@ -114,22 +114,22 @@ class SpacecraftNonlinear : public DynamicalSystem { const Eigen::VectorXd& control) const override; /** - * @brief Compute state Hessian matrix + * @brief Compute state Hessian tensor * @param state Current state vector * @param control Current control vector - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian( + std::vector getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** - * @brief Compute control Hessian matrix + * @brief Compute control Hessian tensor * @param state Current state vector * @param control Current control vector - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian( + std::vector getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; diff --git a/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp b/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp index 5d54bea..194ddfe 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_roe.hpp @@ -105,22 +105,22 @@ namespace cddp const Eigen::VectorXd &control) const override; /** - * @brief Compute state Hessian matrix + * @brief Compute state Hessian tensor * @param state Current state vector * @param control Current control vector - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian( + std::vector getStateHessian( const Eigen::VectorXd &state, const Eigen::VectorXd &control) const override; /** - * @brief Compute control Hessian matrix + * @brief Compute control Hessian tensor * @param state Current state vector * @param control Current control vector - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian( + std::vector getControlHessian( const Eigen::VectorXd &state, const Eigen::VectorXd &control) const override; diff --git a/include/cddp-cpp/dynamics_model/spacecraft_twobody.hpp b/include/cddp-cpp/dynamics_model/spacecraft_twobody.hpp index b039868..317f6fe 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_twobody.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_twobody.hpp @@ -70,23 +70,23 @@ class SpacecraftTwobody : public DynamicalSystem { const Eigen::VectorXd &control) const override; /** - * @brief Computes the state Hessian matrix (∂²f/∂x²) + * @brief Computes the state Hessian tensor (∂²f/∂x²) * * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd &state, + std::vector getStateHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control) const override; /** - * @brief Computes the control Hessian matrix (∂²f/∂u²) + * @brief Computes the control Hessian tensor (∂²f/∂u²) * * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd &state, + std::vector getControlHessian(const Eigen::VectorXd &state, const Eigen::VectorXd &control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/unicycle.hpp b/include/cddp-cpp/dynamics_model/unicycle.hpp index 380a276..d01c483 100644 --- a/include/cddp-cpp/dynamics_model/unicycle.hpp +++ b/include/cddp-cpp/dynamics_model/unicycle.hpp @@ -83,18 +83,18 @@ class Unicycle : public DynamicalSystem { * @brief Computes the Hessian of the dynamics with respect to the state * @param state Current state vector * @param control Current control input - * @return State Hessian matrix + * @return Vector of state Hessian matrices, one per state dimension */ - Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, + std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; /** * @brief Computes the Hessian of the dynamics with respect to the control * @param state Current state vector * @param control Current control input - * @return Control Hessian matrix + * @return Vector of control Hessian matrices, one per state dimension */ - Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, + std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; private: diff --git a/src/cddp_core/neural_dynamical_system.cpp b/src/cddp_core/neural_dynamical_system.cpp index 1a5a47b..dae2581 100644 --- a/src/cddp_core/neural_dynamical_system.cpp +++ b/src/cddp_core/neural_dynamical_system.cpp @@ -140,23 +140,51 @@ Eigen::MatrixXd NeuralDynamicalSystem::getControlJacobian(const Eigen::VectorXd& // ---------------------------------------------------------------------------- // Hessians (placeholders) // ---------------------------------------------------------------------------- -Eigen::MatrixXd NeuralDynamicalSystem::getStateHessian(const Eigen::VectorXd& /*state*/, - const Eigen::VectorXd& /*control*/) const +std::vector NeuralDynamicalSystem::getStateHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { - // Typically a 3D object. Return zero or implement if needed. - return Eigen::MatrixXd::Zero(state_dim_ * state_dim_, state_dim_); + // Initialize vector of matrices (one matrix per state dimension) + std::vector hessian(state_dim_); + for (int i = 0; i < state_dim_; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(state_dim_, state_dim_); + } + + // For neural network models, computing Hessians usually requires + // second-order automatic differentiation or finite differencing. + // This is a placeholder implementation that returns zeros. + + return hessian; } -Eigen::MatrixXd NeuralDynamicalSystem::getControlHessian(const Eigen::VectorXd& /*state*/, - const Eigen::VectorXd& /*control*/) const +std::vector NeuralDynamicalSystem::getControlHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(control_dim_ * control_dim_, state_dim_); + // Initialize vector of matrices (one matrix per state dimension) + std::vector hessian(state_dim_); + for (int i = 0; i < state_dim_; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(control_dim_, control_dim_); + } + + // Placeholder implementation + + return hessian; } -Eigen::MatrixXd NeuralDynamicalSystem::getCrossHessian(const Eigen::VectorXd& /*state*/, - const Eigen::VectorXd& /*control*/) const +std::vector NeuralDynamicalSystem::getCrossHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(state_dim_ * control_dim_, state_dim_); + // Initialize vector of matrices (one matrix per state dimension) + std::vector hessian(state_dim_); + for (int i = 0; i < state_dim_; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(control_dim_, state_dim_); + } + + // Placeholder implementation + + return hessian; } // ---------------------------------------------------------------------------- diff --git a/src/dynamics_model/bicycle.cpp b/src/dynamics_model/bicycle.cpp index 98f581a..c16e36a 100644 --- a/src/dynamics_model/bicycle.cpp +++ b/src/dynamics_model/bicycle.cpp @@ -96,55 +96,56 @@ Eigen::MatrixXd Bicycle::getControlJacobian( return B; } - -Eigen::MatrixXd Bicycle::getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) -const { - // TODO: Compute and return the Hessian tensor d^2f/dx^2 (represented as a matrix) - return Eigen::MatrixXd::Zero(4*4, 2); -} - -Eigen::MatrixXd Bicycle::getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) -const { - // TODO: Compute and return the Hessian tensor d^2f/du^2 (represented as a matrix) - return Eigen::MatrixXd::Zero(4*2, 2); -} - -// Eigen::MatrixXd Bicycle::getStateHessian( -// const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { +std::vector Bicycle::getStateHessian( + const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + + // Extract state variables + const double theta = state(STATE_THETA); // heading angle + const double v = state(STATE_V); // velocity -// Eigen::MatrixXd H = Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + // Second derivatives with respect to states + // d²(dx/dt)/dtheta² + hessians[STATE_X](STATE_THETA, STATE_THETA) = -v * std::cos(theta); -// // Extract state variables -// const double theta = state(STATE_THETA); // heading angle -// const double v = state(STATE_V); // velocity + // d²(dx/dt)/(dtheta*dv) + hessians[STATE_X](STATE_THETA, STATE_V) = -std::sin(theta); + hessians[STATE_X](STATE_V, STATE_THETA) = -std::sin(theta); -// // Second derivatives with respect to states -// // d²(dx/dt)/dtheta² -// H(STATE_THETA * STATE_DIM + STATE_X, STATE_THETA) = -v * std::cos(theta); + // d²(dy/dt)/dtheta² + hessians[STATE_Y](STATE_THETA, STATE_THETA) = -v * std::sin(theta); -// // d²(dy/dt)/dtheta² -// H(STATE_THETA * STATE_DIM + STATE_Y, STATE_THETA) = -v * std::sin(theta); + // d²(dy/dt)/(dtheta*dv) + hessians[STATE_Y](STATE_THETA, STATE_V) = std::cos(theta); + hessians[STATE_Y](STATE_V, STATE_THETA) = std::cos(theta); -// return H; -// } + return hessians; +} -// Eigen::MatrixXd Bicycle::getControlHessian( -// const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { +std::vector Bicycle::getControlHessian( + const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { -// Eigen::MatrixXd H = Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } -// // Extract state variables -// const double v = state(STATE_V); // velocity + // Extract state variables + const double v = state(STATE_V); // velocity -// // Extract control variables -// const double delta = control(CONTROL_DELTA); // steering angle + // Extract control variables + const double delta = control(CONTROL_DELTA); // steering angle -// // Second derivatives with respect to controls -// // d²(dtheta/dt)/ddelta² -// H(CONTROL_DELTA * STATE_DIM + STATE_THETA, CONTROL_DELTA) = -// 2.0 * v * std::sin(delta) / (wheelbase_ * std::pow(std::cos(delta), 3)); + // Second derivatives with respect to controls + // d²(dtheta/dt)/ddelta² + hessians[STATE_THETA](CONTROL_DELTA, CONTROL_DELTA) = + 2.0 * v * std::sin(delta) / (wheelbase_ * std::pow(std::cos(delta), 3)); -// return H; -// } + return hessians; +} } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/car.cpp b/src/dynamics_model/car.cpp index 8b008a3..8733e9d 100644 --- a/src/dynamics_model/car.cpp +++ b/src/dynamics_model/car.cpp @@ -90,14 +90,22 @@ Eigen::MatrixXd Car::getControlJacobian( return J; } -Eigen::MatrixXd Car::getStateHessian( +std::vector Car::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd Car::getControlHessian( +std::vector Car::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/cartpole.cpp b/src/dynamics_model/cartpole.cpp index a377a9e..8970b86 100644 --- a/src/dynamics_model/cartpole.cpp +++ b/src/dynamics_model/cartpole.cpp @@ -82,14 +82,22 @@ Eigen::MatrixXd CartPole::getControlJacobian( } -Eigen::MatrixXd CartPole::getStateHessian( +std::vector CartPole::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd CartPole::getControlHessian( +std::vector CartPole::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/dreyfus_rocket.cpp b/src/dynamics_model/dreyfus_rocket.cpp index 74862ef..12086cd 100644 --- a/src/dynamics_model/dreyfus_rocket.cpp +++ b/src/dynamics_model/dreyfus_rocket.cpp @@ -61,15 +61,28 @@ Eigen::MatrixXd DreyfusRocket::getControlJacobian( return B; } -Eigen::MatrixXd DreyfusRocket::getStateHessian( +std::vector DreyfusRocket::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd DreyfusRocket::getControlHessian( +std::vector DreyfusRocket::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + + // The only non-zero element is the second derivative of x_dot with respect to theta + const double theta = control(CONTROL_THETA); + hessians[STATE_X_DOT](CONTROL_THETA, CONTROL_THETA) = -thrust_acceleration_ * std::cos(theta); + + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/dubins_car.cpp b/src/dynamics_model/dubins_car.cpp index 485d754..f3bdd56 100644 --- a/src/dynamics_model/dubins_car.cpp +++ b/src/dynamics_model/dubins_car.cpp @@ -77,18 +77,49 @@ Eigen::MatrixXd DubinsCar::getControlJacobian( return B; } -Eigen::MatrixXd DubinsCar::getStateHessian( - const Eigen::VectorXd& /*state*/, - const Eigen::VectorXd& /*control*/) const +std::vector DubinsCar::getStateHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, CONTROL_DIM); + (void)control; // Not used for state Hessian + + // Initialize vector of matrices (one matrix per state dimension) + std::vector hessian(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + + // Extract state components + const double theta = state(STATE_THETA); + + // Second derivatives of x with respect to theta: d^2x/dtheta^2 = -speed * cos(theta) + hessian[STATE_X](STATE_THETA, STATE_THETA) = -speed_ * std::cos(theta); + + // Second derivatives of y with respect to theta: d^2y/dtheta^2 = -speed * sin(theta) + hessian[STATE_Y](STATE_THETA, STATE_THETA) = -speed_ * std::sin(theta); + + // No second derivatives for theta state + + return hessian; } -Eigen::MatrixXd DubinsCar::getControlHessian( - const Eigen::VectorXd& /*state*/, - const Eigen::VectorXd& /*control*/) const +std::vector DubinsCar::getControlHessian( + const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + (void)state; + (void)control; + + // Initialize vector of matrices (one matrix per state dimension) + std::vector hessian(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + + // For the Dubins car, all second derivatives with respect to control are zero + // No need to set any values as the matrices are already initialized to zero + + return hessian; } } // namespace cddp diff --git a/src/dynamics_model/lti_system.cpp b/src/dynamics_model/lti_system.cpp index 991fd38..7acbd65 100644 --- a/src/dynamics_model/lti_system.cpp +++ b/src/dynamics_model/lti_system.cpp @@ -87,18 +87,26 @@ Eigen::MatrixXd LTISystem::getControlJacobian( return B_/timestep_; } -Eigen::MatrixXd LTISystem::getStateHessian( +std::vector LTISystem::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { // State Hessian is zero for linear system - return Eigen::MatrixXd::Zero(state_dim_ * state_dim_, state_dim_); + std::vector hessians(state_dim_); + for (int i = 0; i < state_dim_; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(state_dim_, state_dim_); + } + return hessians; } -Eigen::MatrixXd LTISystem::getControlHessian( +std::vector LTISystem::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { // Control Hessian is zero for linear system - return Eigen::MatrixXd::Zero(state_dim_ * control_dim_, control_dim_); + std::vector hessians(state_dim_); + for (int i = 0; i < state_dim_; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(control_dim_, control_dim_); + } + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/manipulator.cpp b/src/dynamics_model/manipulator.cpp index b45aacc..7b817a9 100644 --- a/src/dynamics_model/manipulator.cpp +++ b/src/dynamics_model/manipulator.cpp @@ -66,18 +66,26 @@ Eigen::MatrixXd Manipulator::getControlJacobian( return finite_difference_jacobian(f, control); } -Eigen::MatrixXd Manipulator::getStateHessian( +std::vector Manipulator::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { // For the simplified model, return zero Hessian - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd Manipulator::getControlHessian( +std::vector Manipulator::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { // For the simplified model, return zero Hessian - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } Eigen::Matrix4d Manipulator::rotX(double alpha) const { diff --git a/src/dynamics_model/pendulum.cpp b/src/dynamics_model/pendulum.cpp index 3ec8d61..851c2b2 100644 --- a/src/dynamics_model/pendulum.cpp +++ b/src/dynamics_model/pendulum.cpp @@ -37,11 +37,11 @@ Eigen::VectorXd Pendulum::getContinuousDynamics( const double torque = control(CONTROL_TORQUE); // Precompute constants - const double intertia = mass_ * length_ * length_; + const double inertia = mass_ * length_ * length_; // Pendulum dynamics equations state_dot(STATE_THETA) = theta_dot; - state_dot(STATE_THETA_DOT) = (torque - damping_ * theta_dot + mass_ * gravity_ * length_ * std::sin(theta)) / intertia; + state_dot(STATE_THETA_DOT) = (torque - damping_ * theta_dot + mass_ * gravity_ * length_ * std::sin(theta)) / inertia; return state_dot; } @@ -78,17 +78,39 @@ Eigen::MatrixXd Pendulum::getControlJacobian( return B; } -Eigen::MatrixXd Pendulum::getStateHessian( +std::vector Pendulum::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - Eigen::MatrixXd H = Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + // Initialize a vector of matrices (one matrix per state dimension) + std::vector hessian(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } - return H; + // Extract state variables + const double theta = state(STATE_THETA); + + // For the pendulum, only the second derivative of theta_dot with respect to theta is non-zero + // d^2(dtheta_dot/dt)/dtheta^2 = -g/l * sin(theta) + const double inertia = mass_ * length_ * length_; + hessian[STATE_THETA_DOT](STATE_THETA, STATE_THETA) = -(gravity_ / length_) * std::sin(theta); + + return hessian; } -Eigen::MatrixXd Pendulum::getControlHessian( +std::vector Pendulum::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + + // Initialize a vector of matrices (one matrix per state dimension) + std::vector hessian(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessian[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + + // For the pendulum, all second derivatives with respect to control are zero + // No need to set any values as the matrices are already initialized to zero + + return hessian; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/quadrotor.cpp b/src/dynamics_model/quadrotor.cpp index bb329e5..180ffbe 100644 --- a/src/dynamics_model/quadrotor.cpp +++ b/src/dynamics_model/quadrotor.cpp @@ -140,16 +140,24 @@ Eigen::MatrixXd Quadrotor::getControlJacobian( } -// TODO: Implement this -Eigen::MatrixXd Quadrotor::getStateHessian( +// TODO: Implement a more accurate version if needed +std::vector Quadrotor::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -// TODO: Implement this -Eigen::MatrixXd Quadrotor::getControlHessian( +// TODO: Implement a more accurate version if needed +std::vector Quadrotor::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } } // namespace cddp diff --git a/src/dynamics_model/spacecraft_landing2d.cpp b/src/dynamics_model/spacecraft_landing2d.cpp index 03a13b9..e23a705 100644 --- a/src/dynamics_model/spacecraft_landing2d.cpp +++ b/src/dynamics_model/spacecraft_landing2d.cpp @@ -94,14 +94,22 @@ Eigen::MatrixXd SpacecraftLanding2D::getControlJacobian( return finite_difference_jacobian(f, control); } -Eigen::MatrixXd SpacecraftLanding2D::getStateHessian( +std::vector SpacecraftLanding2D::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd SpacecraftLanding2D::getControlHessian( +std::vector SpacecraftLanding2D::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/spacecraft_linear.cpp b/src/dynamics_model/spacecraft_linear.cpp index 42cf92f..e2c0f0f 100644 --- a/src/dynamics_model/spacecraft_linear.cpp +++ b/src/dynamics_model/spacecraft_linear.cpp @@ -103,16 +103,24 @@ Eigen::MatrixXd HCW::getControlJacobian( return B; } -Eigen::MatrixXd HCW::getStateHessian( +std::vector HCW::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { // HCW equations are linear, so state Hessian is zero - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd HCW::getControlHessian( +std::vector HCW::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { // HCW equations are linear in control, so control Hessian is zero - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/spacecraft_nonlinear.cpp b/src/dynamics_model/spacecraft_nonlinear.cpp index ebe0459..33bcb63 100644 --- a/src/dynamics_model/spacecraft_nonlinear.cpp +++ b/src/dynamics_model/spacecraft_nonlinear.cpp @@ -102,18 +102,26 @@ Eigen::MatrixXd SpacecraftNonlinear::getControlJacobian( return finite_difference_jacobian(f, control); } -Eigen::MatrixXd SpacecraftNonlinear::getStateHessian( +std::vector SpacecraftNonlinear::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd SpacecraftNonlinear::getControlHessian( +std::vector SpacecraftNonlinear::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } diff --git a/src/dynamics_model/spacecraft_roe.cpp b/src/dynamics_model/spacecraft_roe.cpp index 4c03fd9..30ffcfb 100644 --- a/src/dynamics_model/spacecraft_roe.cpp +++ b/src/dynamics_model/spacecraft_roe.cpp @@ -125,21 +125,29 @@ namespace cddp } //----------------------------------------------------------------------------- - Eigen::MatrixXd SpacecraftROE::getStateHessian( + std::vector SpacecraftROE::getStateHessian( const Eigen::VectorXd & /*state*/, const Eigen::VectorXd & /*control*/) const { // For this linear(ish) model, second derivatives wrt state are zero. - return Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } //----------------------------------------------------------------------------- - Eigen::MatrixXd SpacecraftROE::getControlHessian( + std::vector SpacecraftROE::getControlHessian( const Eigen::VectorXd & /*state*/, const Eigen::VectorXd & /*control*/) const { // Similarly, second derivatives wrt control are zero for a linear system. - return Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } //----------------------------------------------------------------------------- diff --git a/src/dynamics_model/spacecraft_twobody.cpp b/src/dynamics_model/spacecraft_twobody.cpp index 9af2753..a056975 100644 --- a/src/dynamics_model/spacecraft_twobody.cpp +++ b/src/dynamics_model/spacecraft_twobody.cpp @@ -60,14 +60,22 @@ Eigen::MatrixXd SpacecraftTwobody::getControlJacobian( return finite_difference_jacobian(f, control); } -Eigen::MatrixXd SpacecraftTwobody::getStateHessian( +std::vector SpacecraftTwobody::getStateHessian( const Eigen::VectorXd &state, const Eigen::VectorXd &control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + return hessians; } -Eigen::MatrixXd SpacecraftTwobody::getControlHessian( +std::vector SpacecraftTwobody::getControlHessian( const Eigen::VectorXd &state, const Eigen::VectorXd &control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/unicycle.cpp b/src/dynamics_model/unicycle.cpp index 74b407d..db300d1 100644 --- a/src/dynamics_model/unicycle.cpp +++ b/src/dynamics_model/unicycle.cpp @@ -85,16 +85,37 @@ Eigen::MatrixXd Unicycle::getControlJacobian( return B; } -Eigen::MatrixXd Unicycle::getStateHessian( +std::vector Unicycle::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, 2); + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + } + + // Non-zero elements + // Hessian of x w.r.t. theta twice + const double v = control(CONTROL_V); + const double theta = state(STATE_THETA); + hessians[STATE_X](STATE_THETA, STATE_THETA) = -v * std::cos(theta); + + // Hessian of y w.r.t. theta twice + hessians[STATE_Y](STATE_THETA, STATE_THETA) = -v * std::sin(theta); + + return hessians; } -Eigen::MatrixXd Unicycle::getControlHessian( +std::vector Unicycle::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - // TODO: Compute and return the Hessian tensor d^2f/du^2 (represented as a matrix) - return Eigen::MatrixXd::Zero(STATE_DIM * 2, 2); + + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { + hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + } + + // No non-zero terms in control Hessian for this model + + return hessians; } } // namespace cddp \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 6f8542b..79a7f13 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -66,6 +66,10 @@ add_executable(test_spacecraft_nonlinear dynamics_model/test_spacecraft_nonlinea target_link_libraries(test_spacecraft_nonlinear gtest gmock gtest_main cddp) gtest_discover_tests(test_spacecraft_nonlinear) +add_executable(test_hessian test_hessian.cpp) +target_link_libraries(test_hessian gtest gmock gtest_main cddp) +gtest_discover_tests(test_hessian) + # Core tests add_executable(test_boxqp cddp_core/test_boxqp.cpp) target_link_libraries(test_boxqp gtest gmock gtest_main cddp) diff --git a/tests/test_hessian.cpp b/tests/test_hessian.cpp new file mode 100644 index 0000000..bad59e6 --- /dev/null +++ b/tests/test_hessian.cpp @@ -0,0 +1,265 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#include +#include +#include + +#include "dynamics_model/pendulum.hpp" +#include "dynamics_model/dubins_car.hpp" + +namespace cddp { +namespace testing { + +// Fixture for pendulum tests +class PendulumHessianTest : public ::testing::Test { +protected: + void SetUp() override { + // Create a pendulum model + timestep = 0.01; + length = 1.0; // Length of the pendulum [m] + mass = 1.0; // Mass [kg] + damping = 0.1; // Damping coefficient + integration = "rk4"; + + pendulum = std::make_unique(timestep, length, mass, damping, integration); + + // Define a state and control + state = Eigen::VectorXd::Zero(2); + state << M_PI / 4.0, 0.0; // 45-degree angle, zero velocity + + control = Eigen::VectorXd::Zero(1); + control << 1.0; // Apply a torque of 1 Nm + } + + std::unique_ptr pendulum; + Eigen::VectorXd state; + Eigen::VectorXd control; + double timestep; + double length; + double mass; + double damping; + std::string integration; +}; + +// Fixture for Dubins car tests +class DubinsCarHessianTest : public ::testing::Test { +protected: + void SetUp() override { + // Create a Dubins car model + speed = 1.0; // Constant forward speed [m/s] + timestep = 0.01; // Time step [s] + integration = "rk4"; + + dubins_car = std::make_unique(speed, timestep, integration); + + // Define a state and control + state = Eigen::VectorXd::Zero(3); + state << 0.0, 0.0, M_PI / 4.0; // (x, y, theta) = (0, 0, 45°) + + control = Eigen::VectorXd::Zero(1); + control << 0.5; // Turn rate of 0.5 rad/s + } + + std::unique_ptr dubins_car; + Eigen::VectorXd state; + Eigen::VectorXd control; + double speed; + double timestep; + std::string integration; +}; + +// Test pendulum model parameters +TEST_F(PendulumHessianTest, ParametersAreCorrect) { + EXPECT_DOUBLE_EQ(pendulum->getLength(), length); + EXPECT_DOUBLE_EQ(pendulum->getMass(), mass); + EXPECT_DOUBLE_EQ(pendulum->getDamping(), damping); + EXPECT_DOUBLE_EQ(pendulum->getTimestep(), timestep); + EXPECT_EQ(pendulum->getIntegrationType(), integration); +} + +// Test pendulum dynamics calculation +TEST_F(PendulumHessianTest, DynamicsCalculation) { + Eigen::VectorXd xdot = pendulum->getContinuousDynamics(state, control); + + // Expected values for theta_dot and theta_ddot + double expected_theta_dot = state(1); // state(1) is the current angular velocity + double expected_theta_ddot = (control(0) - pendulum->getDamping() * state(1) + + pendulum->getMass() * pendulum->getGravity() * + pendulum->getLength() * std::sin(state(0))) / + (pendulum->getMass() * pendulum->getLength() * pendulum->getLength()); + + EXPECT_NEAR(xdot(0), expected_theta_dot, 1e-10); + EXPECT_NEAR(xdot(1), expected_theta_ddot, 1e-10); +} + +// Test pendulum state Jacobian +TEST_F(PendulumHessianTest, StateJacobian) { + Eigen::MatrixXd A = pendulum->getStateJacobian(state, control); + + // Expected values for the state Jacobian + double expected_A_11 = 0.0; // d(theta_dot)/d(theta) + double expected_A_12 = 1.0; // d(theta_dot)/d(theta_dot) + double expected_A_21 = (pendulum->getGravity() / pendulum->getLength()) * std::cos(state(0)); // d(theta_ddot)/d(theta) + double expected_A_22 = -pendulum->getDamping() / (pendulum->getMass() * pendulum->getLength() * pendulum->getLength()); // d(theta_ddot)/d(theta_dot) + + EXPECT_NEAR(A(0, 0), expected_A_11, 1e-10); + EXPECT_NEAR(A(0, 1), expected_A_12, 1e-10); + EXPECT_NEAR(A(1, 0), expected_A_21, 1e-10); + EXPECT_NEAR(A(1, 1), expected_A_22, 1e-10); +} + +// Test pendulum control Jacobian +TEST_F(PendulumHessianTest, ControlJacobian) { + Eigen::MatrixXd B = pendulum->getControlJacobian(state, control); + + // Expected values for the control Jacobian + double expected_B_11 = 0.0; // d(theta_dot)/d(torque) + double expected_B_21 = 1.0 / (pendulum->getMass() * pendulum->getLength() * pendulum->getLength()); // d(theta_ddot)/d(torque) + + EXPECT_NEAR(B(0, 0), expected_B_11, 1e-10); + EXPECT_NEAR(B(1, 0), expected_B_21, 1e-10); +} + +// Test pendulum state Hessian +TEST_F(PendulumHessianTest, StateHessian) { + std::vector state_hessian = pendulum->getStateHessian(state, control); + + // Check dimensions + EXPECT_EQ(state_hessian.size(), 2); // Two state dimensions + EXPECT_EQ(state_hessian[0].rows(), 2); // 2x2 matrices + EXPECT_EQ(state_hessian[0].cols(), 2); + + // Expected values for the state Hessian + // For theta_dot dimension (index 0), all second derivatives should be zero + for (int i = 0; i < 2; ++i) { + for (int j = 0; j < 2; ++j) { + EXPECT_NEAR(state_hessian[0](i, j), 0.0, 1e-10); + } + } + + // For theta_ddot dimension (index 1), only d^2(theta_ddot)/d(theta)^2 is non-zero + double expected_H_100 = -(pendulum->getGravity() / pendulum->getLength()) * std::sin(state(0)); // d^2(theta_ddot)/d(theta)^2 + EXPECT_NEAR(state_hessian[1](0, 0), expected_H_100, 1e-10); + EXPECT_NEAR(state_hessian[1](0, 1), 0.0, 1e-10); + EXPECT_NEAR(state_hessian[1](1, 0), 0.0, 1e-10); + EXPECT_NEAR(state_hessian[1](1, 1), 0.0, 1e-10); +} + +// Test pendulum control Hessian +TEST_F(PendulumHessianTest, ControlHessian) { + std::vector control_hessian = pendulum->getControlHessian(state, control); + + // Check dimensions + EXPECT_EQ(control_hessian.size(), 2); // Two state dimensions + EXPECT_EQ(control_hessian[0].rows(), 1); // 1x1 matrices (control dimension is 1) + EXPECT_EQ(control_hessian[0].cols(), 1); + + // All second derivatives with respect to control should be zero + for (int k = 0; k < 2; ++k) { + EXPECT_NEAR(control_hessian[k](0, 0), 0.0, 1e-10); + } +} + +// Test Dubins car model parameters +TEST_F(DubinsCarHessianTest, ParametersAreCorrect) { + EXPECT_DOUBLE_EQ(dubins_car->getTimestep(), timestep); + EXPECT_EQ(dubins_car->getIntegrationType(), integration); +} + +// Test Dubins car dynamics calculation +TEST_F(DubinsCarHessianTest, DynamicsCalculation) { + Eigen::VectorXd xdot = dubins_car->getContinuousDynamics(state, control); + + // Expected values + double expected_x_dot = speed * std::cos(state(2)); + double expected_y_dot = speed * std::sin(state(2)); + double expected_theta_dot = control(0); + + EXPECT_NEAR(xdot(0), expected_x_dot, 1e-10); + EXPECT_NEAR(xdot(1), expected_y_dot, 1e-10); + EXPECT_NEAR(xdot(2), expected_theta_dot, 1e-10); +} + +// Test Dubins car state Jacobian +TEST_F(DubinsCarHessianTest, StateJacobian) { + Eigen::MatrixXd A = dubins_car->getStateJacobian(state, control); + + // Expected values for the state Jacobian + double expected_A_13 = -speed * std::sin(state(2)); // dx/dtheta + double expected_A_23 = speed * std::cos(state(2)); // dy/dtheta + + EXPECT_NEAR(A(0, 2), expected_A_13, 1e-10); + EXPECT_NEAR(A(1, 2), expected_A_23, 1e-10); +} + +// Test Dubins car control Jacobian +TEST_F(DubinsCarHessianTest, ControlJacobian) { + Eigen::MatrixXd B = dubins_car->getControlJacobian(state, control); + + // Expected values for the control Jacobian + double expected_B_31 = 1.0; // dtheta/domega + + EXPECT_NEAR(B(2, 0), expected_B_31, 1e-10); +} + +// Test Dubins car state Hessian +TEST_F(DubinsCarHessianTest, StateHessian) { + std::vector state_hessian = dubins_car->getStateHessian(state, control); + + // Check dimensions + EXPECT_EQ(state_hessian.size(), 3); // Three state dimensions + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(state_hessian[i].rows(), 3); // 3x3 matrices + EXPECT_EQ(state_hessian[i].cols(), 3); + } + + // Expected values for the state Hessian + // For x dimension (index 0), only d^2x/dtheta^2 is non-zero + double expected_H_022 = -speed * std::cos(state(2)); // d^2x/dtheta^2 + EXPECT_NEAR(state_hessian[0](2, 2), expected_H_022, 1e-10); + + // For y dimension (index 1), only d^2y/dtheta^2 is non-zero + double expected_H_122 = -speed * std::sin(state(2)); // d^2y/dtheta^2 + EXPECT_NEAR(state_hessian[1](2, 2), expected_H_122, 1e-10); + + // For theta dimension (index 2), all second derivatives should be zero + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + EXPECT_NEAR(state_hessian[2](i, j), 0.0, 1e-10); + } + } +} + +// Test Dubins car control Hessian +TEST_F(DubinsCarHessianTest, ControlHessian) { + std::vector control_hessian = dubins_car->getControlHessian(state, control); + + // Check dimensions + EXPECT_EQ(control_hessian.size(), 3); // Three state dimensions + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(control_hessian[i].rows(), 1); // 1x1 matrices (control dimension is 1) + EXPECT_EQ(control_hessian[i].cols(), 1); + } + + // All second derivatives with respect to control should be zero + for (int k = 0; k < 3; ++k) { + EXPECT_NEAR(control_hessian[k](0, 0), 0.0, 1e-10); + } +} + +} // namespace testing +} // namespace cddp \ No newline at end of file From 12119abb69b19800b2bce31b239b842dede00c8a Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 31 Mar 2025 18:41:48 -0400 Subject: [PATCH 02/18] Add autodiff library integration and tests - Configured CMake to include the autodiff library, disabling tests, examples, and Python bindings. - Added a new test executable for autodiff functionality, linking it with Google Test. - Implemented comprehensive tests for autodiff, covering scalar functions, multivariate functions, and gradient comparisons with finite differences. --- CMakeLists.txt | 12 ++++ tests/CMakeLists.txt | 5 ++ tests/test_autodiff.cpp | 128 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 145 insertions(+) create mode 100644 tests/test_autodiff.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e4e536..75e0426 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,6 +93,17 @@ if(NOT matplotplusplus_POPULATED) add_subdirectory(${matplotplusplus_SOURCE_DIR} ${matplotplusplus_BINARY_DIR} EXCLUDE_FROM_ALL) endif() +# autodiff +set(AUTODIFF_BUILD_TESTS OFF CACHE BOOL "Don't build autodiff tests") +set(AUTODIFF_BUILD_EXAMPLES OFF CACHE BOOL "Don't build autodiff examples") +set(AUTODIFF_BUILD_PYTHON OFF CACHE BOOL "Don't build autodiff Python bindings") +FetchContent_Declare( + autodiff + GIT_REPOSITORY https://github.com/autodiff/autodiff.git + GIT_TAG v1.1.2 # Use a stable version tag instead of main +) +FetchContent_MakeAvailable(autodiff) + # Googletest if (CDDP_CPP_BUILD_TESTS) enable_testing() @@ -256,6 +267,7 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} Eigen3::Eigen matplot + autodiff osqp-cpp ) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 79a7f13..ce139d1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -150,6 +150,11 @@ add_executable(test_eigen test_eigen.cpp) target_link_libraries(test_eigen gtest gmock gtest_main cddp) gtest_discover_tests(test_eigen) +# Test for autodiff +add_executable(test_autodiff test_autodiff.cpp) +target_link_libraries(test_autodiff gtest gmock gtest_main cddp) +gtest_discover_tests(test_autodiff) + # Test for gurobi if (CDDP_CPP_GUROBI) add_executable(test_gurobi test_gurobi.cpp) diff --git a/tests/test_autodiff.cpp b/tests/test_autodiff.cpp new file mode 100644 index 0000000..0e0c842 --- /dev/null +++ b/tests/test_autodiff.cpp @@ -0,0 +1,128 @@ + +/** + * @file test_autodiff.cpp + * @brief Test the autodiff library functionality + */ + +#include +#include +#include +#include +#include + +using namespace autodiff; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +// Simple scalar function: f(x) = x^2 + 3x + 2 +dual scalar_function(const dual& x) { + return x*x + 3*x + 2; +} + +// Multivariate function: f(x,y) = x^2 + 2xy + y^2 +dual multivariate_function(const VectorXdual& x) { + return x(0)*x(0) + 2*x(0)*x(1) + x(1)*x(1); +} + +// Function to compute with Eigen: f(x) = x^T.A.x + b^T.x +dual quadratic_form(const VectorXdual& x) { + MatrixXd A = MatrixXd::Identity(2, 2); + A(0, 1) = 1.0; + A(1, 0) = 1.0; + + VectorXd b(2); + b << 3.0, 2.0; + + return x.dot(A * x) + b.dot(x); +} + +// Equivalent using regular Eigen for numerical diff comparison +double quadratic_form_eigen(const Eigen::VectorXd& x) { + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(2, 2); + A(0, 1) = 1.0; + A(1, 0) = 1.0; + + Eigen::VectorXd b(2); + b << 3.0, 2.0; + + return x.dot(A * x) + b.dot(x); +} + +TEST(AutoDiffTest, ScalarDerivative) { + dual x = 2.0; + dual y = scalar_function(x); + double dydx = derivative(scalar_function, wrt(x), at(x)); + + // For f(x) = x^2 + 3x + 2, f'(x) = 2x + 3 + // At x = 2, f'(2) = 2*2 + 3 = 7 + EXPECT_DOUBLE_EQ(dydx, 7.0); +} + +TEST(AutoDiffTest, MultivariateGradient) { + VectorXdual x(2); + x << 1.0, 2.0; + + // Compute gradient using autodiff + VectorXd grad = gradient(multivariate_function, wrt(x), at(x)); + + // For f(x,y) = x^2 + 2xy + y^2 + // df/dx = 2x + 2y + // df/dy = 2x + 2y + // At x=1, y=2: df/dx = 2*1 + 2*2 = 6, df/dy = 2*1 + 2*2 = 6 + EXPECT_DOUBLE_EQ(grad(0), 6.0); + EXPECT_DOUBLE_EQ(grad(1), 6.0); +} + +TEST(AutoDiffTest, QuadraticFormGradient) { + VectorXdual x(2); + x << 1.0, 2.0; + + // Compute gradient using autodiff + VectorXd autodiff_grad = gradient(quadratic_form, wrt(x), at(x)); + + // Compute gradient using finite differences for comparison + Eigen::VectorXd x_eigen(2); + x_eigen << 1.0, 2.0; + Eigen::VectorXd finite_diff_grad = cddp::finite_difference_gradient(quadratic_form_eigen, x_eigen); + + // Print results for debugging + std::cout << "AutoDiff gradient: " << autodiff_grad.transpose() << std::endl; + std::cout << "Finite diff gradient: " << finite_diff_grad.transpose() << std::endl; + + // Check that gradients are close (within a small tolerance) + EXPECT_NEAR(autodiff_grad(0), finite_diff_grad(0), 1e-5); + EXPECT_NEAR(autodiff_grad(1), finite_diff_grad(1), 1e-5); +} + +TEST(AutoDiffTest, CompareWithFiniteDifference) { + // Define test point + Eigen::VectorXd x(2); + x << 2.0, 3.0; + + // Equivalent autodiff point + VectorXdual x_dual(2); + x_dual << 2.0, 3.0; + + // Compute gradient with autodiff + VectorXd autodiff_grad = gradient(quadratic_form, wrt(x_dual), at(x_dual)); + + // Compute using finite difference (with different methods) + Eigen::VectorXd finite_diff_central = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 0); + Eigen::VectorXd finite_diff_forward = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 1); + Eigen::VectorXd finite_diff_backward = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 2); + + // Print results + std::cout << "AutoDiff gradient: " << autodiff_grad.transpose() << std::endl; + std::cout << "Finite diff (central): " << finite_diff_central.transpose() << std::endl; + std::cout << "Finite diff (forward): " << finite_diff_forward.transpose() << std::endl; + std::cout << "Finite diff (backward): " << finite_diff_backward.transpose() << std::endl; + + // Compare results (central difference should be closer to autodiff) + EXPECT_NEAR(autodiff_grad(0), finite_diff_central(0), 1e-5); + EXPECT_NEAR(autodiff_grad(1), finite_diff_central(1), 1e-5); +} + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 7022cbad15ebad829e663b75f8b2f6572488ac95 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Mon, 31 Mar 2025 18:43:55 -0400 Subject: [PATCH 03/18] Update README.md to remove outdated dependency installation instructions and add autodiff library reference --- README.md | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/README.md b/README.md index 2c19d49..03d73d2 100644 --- a/README.md +++ b/README.md @@ -176,15 +176,6 @@ sudo apt-get install imagemagick # For Ubuntu brew install imagemagick # For macOS ``` -Although the library automatically finds and installs the following dependencies via [FetchContent](https://cmake.org/cmake/help/latest/module/FetchContent.html), if you do not have ones, here is how you can install on your own. - -* [OSQP](https://osqp.org/) (QP solver) and [osqp-cpp](https://github.com/google/osqp-cpp) (C++ Wrapper for OSQP) -```bash -conda install -c conda-forge osqp # Optional -``` -* [libtorch](https://pytorch.org/get-started/locally/) : This library utilizes Torch for its underlying computations. It will be automatically installed during the build process. - -* [CUDA](https://developer.nvidia.com/cuda-toolkit)(Optional): If you want to leverage GPU acceleration for torch, ensure you have CUDA installed. You can download it from the [NVIDIA website](https://developer.nvidia.com/cuda-12-4-0-download-archive?target_os=Linux&target_arch=x86_64&Distribution=Ubuntu&target_version=22.04&target_type=deb_local). ### Building ```bash @@ -215,6 +206,7 @@ This project uses the following open-source libraries: * [osqp-cpp](https://github.com/google/osqp-cpp) (Apache License 2.0) * [matplotplusplus](https://github.com/alandefreitas/matplotplusplus) (MIT License) * [libtorch](https://github.com/pytorch/pytorch) (BSD 3-Clause License) +* [autodiff](https://github.com/autodiff/autodiff) (MIT License) ## Citing If you use this work in an academic context, please cite this repository. From 611469d92fb3fc49e142b05993d96c8e23467729 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 10:40:05 -0400 Subject: [PATCH 04/18] Create autodiff base files --- CMakeLists.txt | 1 + include/cddp-cpp/cddp.hpp | 2 + .../cddp_core/autodiff_dynamical_system.hpp | 0 .../dynamics_model/autodiff_pendulum.hpp | 0 src/dynamics_model/autodiff_pendulum.cpp | 0 tests/CMakeLists.txt | 4 + .../dynamics_model/test_autodiff_pendulum.cpp | 0 tests/test_autodiff.cpp | 128 ------------------ 8 files changed, 7 insertions(+), 128 deletions(-) create mode 100644 include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp create mode 100644 include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp create mode 100644 src/dynamics_model/autodiff_pendulum.cpp create mode 100644 tests/dynamics_model/test_autodiff_pendulum.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 75e0426..96f7305 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -243,6 +243,7 @@ endif() set(dynamics_model_srcs src/dynamics_model/pendulum.cpp + src/dynamics_model/autodiff_pendulum.cpp src/dynamics_model/unicycle.cpp src/dynamics_model/bicycle.cpp src/dynamics_model/cartpole.cpp diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index 8a2334a..b8a7019 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -23,6 +23,7 @@ // #include "cddp-cpp/sdqp.hpp" #include "cddp_core/dynamical_system.hpp" +#include "cddp_core/autodiff_dynamical_system.hpp" #include "cddp_core/objective.hpp" #include "cddp_core/constraint.hpp" #include "cddp_core/barrier.hpp" @@ -37,6 +38,7 @@ // Models #include "dynamics_model/pendulum.hpp" +#include "dynamics_model/autodiff_pendulum.hpp" #include "dynamics_model/unicycle.hpp" #include "dynamics_model/bicycle.hpp" #include "dynamics_model/car.hpp" diff --git a/include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp b/include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp new file mode 100644 index 0000000..e69de29 diff --git a/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp b/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp new file mode 100644 index 0000000..e69de29 diff --git a/src/dynamics_model/autodiff_pendulum.cpp b/src/dynamics_model/autodiff_pendulum.cpp new file mode 100644 index 0000000..e69de29 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ce139d1..f09dcb9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -50,6 +50,10 @@ add_executable(test_pendulum dynamics_model/test_pendulum.cpp) target_link_libraries(test_pendulum gtest gmock gtest_main cddp) gtest_discover_tests(test_pendulum) +add_executable(test_autodiff_pendulum dynamics_model/test_autodiff_pendulum.cpp) +target_link_libraries(test_autodiff_pendulum gtest gmock gtest_main cddp) +gtest_discover_tests(test_autodiff_pendulum) + add_executable(test_quadrotor dynamics_model/test_quadrotor.cpp) target_link_libraries(test_quadrotor gtest gmock gtest_main cddp) gtest_discover_tests(test_quadrotor) diff --git a/tests/dynamics_model/test_autodiff_pendulum.cpp b/tests/dynamics_model/test_autodiff_pendulum.cpp new file mode 100644 index 0000000..e69de29 diff --git a/tests/test_autodiff.cpp b/tests/test_autodiff.cpp index 0e0c842..e69de29 100644 --- a/tests/test_autodiff.cpp +++ b/tests/test_autodiff.cpp @@ -1,128 +0,0 @@ - -/** - * @file test_autodiff.cpp - * @brief Test the autodiff library functionality - */ - -#include -#include -#include -#include -#include - -using namespace autodiff; -using Eigen::MatrixXd; -using Eigen::VectorXd; - -// Simple scalar function: f(x) = x^2 + 3x + 2 -dual scalar_function(const dual& x) { - return x*x + 3*x + 2; -} - -// Multivariate function: f(x,y) = x^2 + 2xy + y^2 -dual multivariate_function(const VectorXdual& x) { - return x(0)*x(0) + 2*x(0)*x(1) + x(1)*x(1); -} - -// Function to compute with Eigen: f(x) = x^T.A.x + b^T.x -dual quadratic_form(const VectorXdual& x) { - MatrixXd A = MatrixXd::Identity(2, 2); - A(0, 1) = 1.0; - A(1, 0) = 1.0; - - VectorXd b(2); - b << 3.0, 2.0; - - return x.dot(A * x) + b.dot(x); -} - -// Equivalent using regular Eigen for numerical diff comparison -double quadratic_form_eigen(const Eigen::VectorXd& x) { - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(2, 2); - A(0, 1) = 1.0; - A(1, 0) = 1.0; - - Eigen::VectorXd b(2); - b << 3.0, 2.0; - - return x.dot(A * x) + b.dot(x); -} - -TEST(AutoDiffTest, ScalarDerivative) { - dual x = 2.0; - dual y = scalar_function(x); - double dydx = derivative(scalar_function, wrt(x), at(x)); - - // For f(x) = x^2 + 3x + 2, f'(x) = 2x + 3 - // At x = 2, f'(2) = 2*2 + 3 = 7 - EXPECT_DOUBLE_EQ(dydx, 7.0); -} - -TEST(AutoDiffTest, MultivariateGradient) { - VectorXdual x(2); - x << 1.0, 2.0; - - // Compute gradient using autodiff - VectorXd grad = gradient(multivariate_function, wrt(x), at(x)); - - // For f(x,y) = x^2 + 2xy + y^2 - // df/dx = 2x + 2y - // df/dy = 2x + 2y - // At x=1, y=2: df/dx = 2*1 + 2*2 = 6, df/dy = 2*1 + 2*2 = 6 - EXPECT_DOUBLE_EQ(grad(0), 6.0); - EXPECT_DOUBLE_EQ(grad(1), 6.0); -} - -TEST(AutoDiffTest, QuadraticFormGradient) { - VectorXdual x(2); - x << 1.0, 2.0; - - // Compute gradient using autodiff - VectorXd autodiff_grad = gradient(quadratic_form, wrt(x), at(x)); - - // Compute gradient using finite differences for comparison - Eigen::VectorXd x_eigen(2); - x_eigen << 1.0, 2.0; - Eigen::VectorXd finite_diff_grad = cddp::finite_difference_gradient(quadratic_form_eigen, x_eigen); - - // Print results for debugging - std::cout << "AutoDiff gradient: " << autodiff_grad.transpose() << std::endl; - std::cout << "Finite diff gradient: " << finite_diff_grad.transpose() << std::endl; - - // Check that gradients are close (within a small tolerance) - EXPECT_NEAR(autodiff_grad(0), finite_diff_grad(0), 1e-5); - EXPECT_NEAR(autodiff_grad(1), finite_diff_grad(1), 1e-5); -} - -TEST(AutoDiffTest, CompareWithFiniteDifference) { - // Define test point - Eigen::VectorXd x(2); - x << 2.0, 3.0; - - // Equivalent autodiff point - VectorXdual x_dual(2); - x_dual << 2.0, 3.0; - - // Compute gradient with autodiff - VectorXd autodiff_grad = gradient(quadratic_form, wrt(x_dual), at(x_dual)); - - // Compute using finite difference (with different methods) - Eigen::VectorXd finite_diff_central = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 0); - Eigen::VectorXd finite_diff_forward = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 1); - Eigen::VectorXd finite_diff_backward = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 2); - - // Print results - std::cout << "AutoDiff gradient: " << autodiff_grad.transpose() << std::endl; - std::cout << "Finite diff (central): " << finite_diff_central.transpose() << std::endl; - std::cout << "Finite diff (forward): " << finite_diff_forward.transpose() << std::endl; - std::cout << "Finite diff (backward): " << finite_diff_backward.transpose() << std::endl; - - // Compare results (central difference should be closer to autodiff) - EXPECT_NEAR(autodiff_grad(0), finite_diff_central(0), 1e-5); - EXPECT_NEAR(autodiff_grad(1), finite_diff_central(1), 1e-5); -} - -int main(int argc, char **argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file From 0811f81ef4e3a07fc7ee454f9c2f22031500becd Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 11:09:00 -0400 Subject: [PATCH 05/18] Remove unused autodiff files and clean up --- include/cddp-cpp/cddp.hpp | 2 -- include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp | 0 include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp | 1 + 3 files changed, 1 insertion(+), 2 deletions(-) delete mode 100644 include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp diff --git a/include/cddp-cpp/cddp.hpp b/include/cddp-cpp/cddp.hpp index b8a7019..8a2334a 100644 --- a/include/cddp-cpp/cddp.hpp +++ b/include/cddp-cpp/cddp.hpp @@ -23,7 +23,6 @@ // #include "cddp-cpp/sdqp.hpp" #include "cddp_core/dynamical_system.hpp" -#include "cddp_core/autodiff_dynamical_system.hpp" #include "cddp_core/objective.hpp" #include "cddp_core/constraint.hpp" #include "cddp_core/barrier.hpp" @@ -38,7 +37,6 @@ // Models #include "dynamics_model/pendulum.hpp" -#include "dynamics_model/autodiff_pendulum.hpp" #include "dynamics_model/unicycle.hpp" #include "dynamics_model/bicycle.hpp" #include "dynamics_model/car.hpp" diff --git a/include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp b/include/cddp-cpp/cddp_core/autodiff_dynamical_system.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp b/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp index e69de29..0519ecb 100644 --- a/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp +++ b/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp @@ -0,0 +1 @@ + \ No newline at end of file From 275c5c2272814ee05a482cb8ba9c21b0205b08a7 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 11:59:48 -0400 Subject: [PATCH 06/18] Add comprehensive tests for autodiff functionality - Introduced a new test file `test_autodiff.cpp` to validate autodiff operations. - Implemented tests for scalar functions, multivariate functions, and quadratic forms. - Included comparisons of autodiff gradients with finite difference methods for accuracy verification. - Utilized Google Test framework for structured testing and output of results. --- tests/test_autodiff.cpp | 138 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 138 insertions(+) diff --git a/tests/test_autodiff.cpp b/tests/test_autodiff.cpp index e69de29..6b80aab 100644 --- a/tests/test_autodiff.cpp +++ b/tests/test_autodiff.cpp @@ -0,0 +1,138 @@ +/* + Copyright 2024 Tomo Sasaki + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + https://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +#include +#include +#include +#include +#include + +using namespace autodiff; +using Eigen::MatrixXd; +using Eigen::VectorXd; + +// Simple scalar function: f(x) = x^2 + 3x + 2 +dual scalar_function(const dual& x) { + return x*x + 3*x + 2; +} + +// Multivariate function: f(x,y) = x^2 + 2xy + y^2 +dual multivariate_function(const VectorXdual& x) { + return x(0)*x(0) + 2*x(0)*x(1) + x(1)*x(1); +} + +// Function to compute with Eigen: f(x) = x^T.A.x + b^T.x +dual quadratic_form(const VectorXdual& x) { + MatrixXd A = MatrixXd::Identity(2, 2); + A(0, 1) = 1.0; + A(1, 0) = 1.0; + + VectorXd b(2); + b << 3.0, 2.0; + + return x.dot(A * x) + b.dot(x); +} + +// Equivalent using regular Eigen for numerical diff comparison +double quadratic_form_eigen(const Eigen::VectorXd& x) { + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(2, 2); + A(0, 1) = 1.0; + A(1, 0) = 1.0; + + Eigen::VectorXd b(2); + b << 3.0, 2.0; + + return x.dot(A * x) + b.dot(x); +} + +TEST(AutoDiffTest, ScalarDerivative) { + dual x = 2.0; + dual y = scalar_function(x); + double dydx = derivative(scalar_function, wrt(x), at(x)); + + // For f(x) = x^2 + 3x + 2, f'(x) = 2x + 3 + // At x = 2, f'(2) = 2*2 + 3 = 7 + EXPECT_DOUBLE_EQ(dydx, 7.0); +} + +TEST(AutoDiffTest, MultivariateGradient) { + VectorXdual x(2); + x << 1.0, 2.0; + + // Compute gradient using autodiff + VectorXd grad = gradient(multivariate_function, wrt(x), at(x)); + + // For f(x,y) = x^2 + 2xy + y^2 + // df/dx = 2x + 2y + // df/dy = 2x + 2y + // At x=1, y=2: df/dx = 2*1 + 2*2 = 6, df/dy = 2*1 + 2*2 = 6 + EXPECT_DOUBLE_EQ(grad(0), 6.0); + EXPECT_DOUBLE_EQ(grad(1), 6.0); +} + +TEST(AutoDiffTest, QuadraticFormGradient) { + VectorXdual x(2); + x << 1.0, 2.0; + + // Compute gradient using autodiff + VectorXd autodiff_grad = gradient(quadratic_form, wrt(x), at(x)); + + // Compute gradient using finite differences for comparison + Eigen::VectorXd x_eigen(2); + x_eigen << 1.0, 2.0; + Eigen::VectorXd finite_diff_grad = cddp::finite_difference_gradient(quadratic_form_eigen, x_eigen); + + // Print results for debugging + std::cout << "AutoDiff gradient: " << autodiff_grad.transpose() << std::endl; + std::cout << "Finite diff gradient: " << finite_diff_grad.transpose() << std::endl; + + // Check that gradients are close (within a small tolerance) + EXPECT_NEAR(autodiff_grad(0), finite_diff_grad(0), 1e-5); + EXPECT_NEAR(autodiff_grad(1), finite_diff_grad(1), 1e-5); +} + +TEST(AutoDiffTest, CompareWithFiniteDifference) { + // Define test point + Eigen::VectorXd x(2); + x << 2.0, 3.0; + + // Equivalent autodiff point + VectorXdual x_dual(2); + x_dual << 2.0, 3.0; + + // Compute gradient with autodiff + VectorXd autodiff_grad = gradient(quadratic_form, wrt(x_dual), at(x_dual)); + + // Compute using finite difference (with different methods) + Eigen::VectorXd finite_diff_central = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 0); + Eigen::VectorXd finite_diff_forward = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 1); + Eigen::VectorXd finite_diff_backward = cddp::finite_difference_gradient(quadratic_form_eigen, x, 1e-6, 2); + + // Print results + std::cout << "AutoDiff gradient: " << autodiff_grad.transpose() << std::endl; + std::cout << "Finite diff (central): " << finite_diff_central.transpose() << std::endl; + std::cout << "Finite diff (forward): " << finite_diff_forward.transpose() << std::endl; + std::cout << "Finite diff (backward): " << finite_diff_backward.transpose() << std::endl; + + // Compare results (central difference should be closer to autodiff) + EXPECT_NEAR(autodiff_grad(0), finite_diff_central(0), 1e-5); + EXPECT_NEAR(autodiff_grad(1), finite_diff_central(1), 1e-5); +} + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From a6f686a4fcf1f0a1fb7b7a83a3d39d4c2eea7d96 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 12:47:08 -0400 Subject: [PATCH 07/18] Enhance DynamicalSystem with Autodiff Support - Added support for autodiff in the DynamicalSystem class, including methods for computing Jacobians and Hessians using second-order duals. - Implemented default versions of getStateJacobian, getControlJacobian, getStateHessian, getControlHessian, and getCrossHessian, leveraging autodiff for accurate derivative calculations. - Updated comments for clarity and consistency regarding the use of autodiff in dynamics computations. --- .../cddp-cpp/cddp_core/dynamical_system.hpp | 50 +++++--- src/cddp_core/dynamical_system.cpp | 114 +++++++++++++++++- 2 files changed, 145 insertions(+), 19 deletions(-) diff --git a/include/cddp-cpp/cddp_core/dynamical_system.hpp b/include/cddp-cpp/cddp_core/dynamical_system.hpp index 03fd76d..70596a3 100644 --- a/include/cddp-cpp/cddp_core/dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/dynamical_system.hpp @@ -19,8 +19,16 @@ #include #include #include "cddp_core/helper.hpp" +#include // Include autodiff (defines dual, dual2nd) +#include // Include autodiff Eigen support namespace cddp { + +// Define type alias for second-order dual vectors +using VectorXdual2nd = Eigen::Matrix; +// Keep first-order alias for convenience if needed elsewhere, although not strictly necessary now +using VectorXdual = Eigen::Matrix; + class DynamicalSystem { public: @@ -30,57 +38,63 @@ class DynamicalSystem { virtual ~DynamicalSystem() {} // Virtual destructor - // Core dynamics function: xdot = f(x_t, u_t) + // Core continuous dynamics function: xdot = f(x_t, u_t) + // This remains virtual, derived classes can implement it. + // The base implementation uses discrete dynamics + finite difference. virtual Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; + // Autodiff version of continuous dynamics using second-order duals + // Derived classes MUST implement this to use autodiff for derivatives. + virtual VectorXdual2nd getContinuousDynamicsAutodiff(const VectorXdual2nd& state, + const VectorXdual2nd& control) const = 0; // Takes and returns dual2nd + // Discrete dynamics function: x_{t+1} = f(x_t, u_t) + // Uses integration based on getContinuousDynamics virtual Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; // Jacobian of dynamics w.r.t state: df/dx + // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const = 0; + const Eigen::VectorXd& control) const; // Jacobian of dynamics w.r.t control: df/du + // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const = 0; + const Eigen::VectorXd& control) const; // Jacobians of dynamics w.r.t state and control: df/dx, df/du virtual std::tuple getJacobians(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + // This can now call the default implementations or overridden ones return {getStateJacobian(state, control), getControlJacobian(state, control)}; } // Hessian of dynamics w.r.t state: d^2f/dx^2 - // This is a tensor (state_dim x state_dim x state_dim), represented as a vector of matrices - // Each matrix is state_dim x state_dim, and corresponds to the Hessian of one state dimension + // Tensor (state_dim x state_dim x state_dim), vector (size state_dim) + // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual std::vector getStateHessian(const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const = 0; + const Eigen::VectorXd& control) const; // Hessian of dynamics w.r.t control: d^2f/du^2 - // This is a tensor (state_dim x control_dim x control_dim), represented as a vector of matrices - // Each matrix is control_dim x control_dim, and corresponds to the Hessian of one state dimension + // Tensor (state_dim x control_dim x control_dim), vector (size state_dim) + // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual std::vector getControlHessian(const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const = 0; + const Eigen::VectorXd& control) const; // Hessian of dynamics w.r.t state and control: d^2f/dudx - // This is a tensor (state_dim x control_dim x state_dim), represented as a vector of matrices - // Each matrix is control_dim x state_dim, and corresponds to the Hessian of one state dimension + // Tensor (state_dim x control_dim x state_dim), vector (size state_dim) + // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual std::vector getCrossHessian(const Eigen::VectorXd& state, - const Eigen::VectorXd& control) const { - std::vector cross_hessian(state_dim_); - for (int i = 0; i < state_dim_; ++i) { - cross_hessian[i] = Eigen::MatrixXd::Zero(control_dim_, state_dim_); - } - return cross_hessian; - } + const Eigen::VectorXd& control) const; // Hessian of dynamics w.r.t state and control: d^2f/dx^2, d^2f/du^2, d^2f/dudx virtual std::tuple, std::vector, std::vector> getHessians(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + // This can now call the default implementations or overridden ones return {getStateHessian(state, control), getControlHessian(state, control), getCrossHessian(state, control)}; diff --git a/src/cddp_core/dynamical_system.cpp b/src/cddp_core/dynamical_system.cpp index d330547..a01a70e 100644 --- a/src/cddp_core/dynamical_system.cpp +++ b/src/cddp_core/dynamical_system.cpp @@ -16,10 +16,13 @@ #include #include +#include // Include autodiff +#include // Include autodiff Eigen support -#include "cddp_core/dynamical_system.hpp" +#include "cddp_core/dynamical_system.hpp" using namespace cddp; +using namespace autodiff; // Use autodiff namespace // Implement integration methods Eigen::VectorXd DynamicalSystem::euler_step(const Eigen::VectorXd& state, const Eigen::VectorXd& control, double dt) const { @@ -76,3 +79,112 @@ Eigen::VectorXd DynamicalSystem::getContinuousDynamics( return continuous_dynamics; } + +// --- Autodiff Default Implementations for Jacobians --- + +Eigen::MatrixXd DynamicalSystem::getStateJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { + // Use second-order duals for consistency, jacobian works fine + VectorXdual2nd x = state; + VectorXdual2nd u = control; + + // Need to capture 'this' pointer for member function access + auto dynamics_wrt_x = [&](const VectorXdual2nd& x_ad) -> VectorXdual2nd { + return this->getContinuousDynamicsAutodiff(x_ad, u); + }; + + // Compute Jacobian w.r.t. state + Eigen::MatrixXd Jx = jacobian(dynamics_wrt_x, wrt(x), at(x)); + return Jx; +} + +Eigen::MatrixXd DynamicalSystem::getControlJacobian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { + VectorXdual2nd x = state; + VectorXdual2nd u = control; + + auto dynamics_wrt_u = [&](const VectorXdual2nd& u_ad) -> VectorXdual2nd { + return this->getContinuousDynamicsAutodiff(x, u_ad); + }; + + Eigen::MatrixXd Ju = jacobian(dynamics_wrt_u, wrt(u), at(u)); + return Ju; +} + +// --- Autodiff Default Implementations for Hessians --- + +std::vector DynamicalSystem::getStateHessian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { + int n = state_dim_; + int m = control_dim_; + std::vector state_hessian_tensor(state_dim_); + + // Create the combined state-control vector using second-order duals + VectorXdual2nd z(n + m); + z.head(n) = state; + z.tail(m) = control; + + // Compute Hessian for each output dimension + for (int i = 0; i < state_dim_; ++i) { + // Define a scalar function for the i-th output dimension + auto f_i = [&](const VectorXdual2nd& z_ad) -> autodiff::dual2nd { + VectorXdual2nd x_ad = z_ad.head(n); + VectorXdual2nd u_ad = z_ad.tail(m); + // Return the i-th component of the dynamics vector + return this->getContinuousDynamicsAutodiff(x_ad, u_ad)(i); + }; + + // Compute the full Hessian matrix for the i-th output w.r.t z = [x, u] + Eigen::MatrixXd H_i = hessian(f_i, wrt(z), at(z)); + + // Extract the top-left (n x n) block (d^2 f_i / dx^2) + state_hessian_tensor[i] = H_i.topLeftCorner(n, n); + } + return state_hessian_tensor; +} + +std::vector DynamicalSystem::getControlHessian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { + int n = state_dim_; + int m = control_dim_; + std::vector control_hessian_tensor(state_dim_); + + VectorXdual2nd z(n + m); + z.head(n) = state; + z.tail(m) = control; + + for (int i = 0; i < state_dim_; ++i) { + auto f_i = [&](const VectorXdual2nd& z_ad) -> autodiff::dual2nd { + VectorXdual2nd x_ad = z_ad.head(n); + VectorXdual2nd u_ad = z_ad.tail(m); + return this->getContinuousDynamicsAutodiff(x_ad, u_ad)(i); + }; + Eigen::MatrixXd H_i = hessian(f_i, wrt(z), at(z)); + // Extract the bottom-right (m x m) block (d^2 f_i / du^2) + control_hessian_tensor[i] = H_i.bottomRightCorner(m, m); + } + return control_hessian_tensor; +} + +std::vector DynamicalSystem::getCrossHessian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const { + int n = state_dim_; + int m = control_dim_; + std::vector cross_hessian_tensor(state_dim_); + + VectorXdual2nd z(n + m); + z.head(n) = state; + z.tail(m) = control; + + for (int i = 0; i < state_dim_; ++i) { + auto f_i = [&](const VectorXdual2nd& z_ad) -> autodiff::dual2nd { + VectorXdual2nd x_ad = z_ad.head(n); + VectorXdual2nd u_ad = z_ad.tail(m); + return this->getContinuousDynamicsAutodiff(x_ad, u_ad)(i); + }; + Eigen::MatrixXd H_i = hessian(f_i, wrt(z), at(z)); + // Extract the bottom-left (m x n) block (d^2 f_i / dudx) + cross_hessian_tensor[i] = H_i.bottomLeftCorner(m, n); + } + return cross_hessian_tensor; +} From d8b43f20d5403acaf0d43850488d1dcd76c4e9a7 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 12:47:17 -0400 Subject: [PATCH 08/18] Add autodiff support for Bicycle dynamics model - Implemented the `getContinuousDynamicsAutodiff` method in the Bicycle class to compute continuous dynamics using autodiff with dual2nd types. - Updated the header file to declare the new autodiff method. - Enhanced the dynamics calculations to leverage autodiff for accurate derivative computations. --- include/cddp-cpp/dynamics_model/bicycle.hpp | 4 ++++ src/dynamics_model/bicycle.cpp | 24 +++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/include/cddp-cpp/dynamics_model/bicycle.hpp b/include/cddp-cpp/dynamics_model/bicycle.hpp index 8068b15..484802b 100644 --- a/include/cddp-cpp/dynamics_model/bicycle.hpp +++ b/include/cddp-cpp/dynamics_model/bicycle.hpp @@ -42,6 +42,10 @@ class Bicycle : public DynamicalSystem { Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + // Add the autodiff version declaration + VectorXdual2nd getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + /** * Computes the discrete-time dynamics using the specified integration method * @param state Current state vector diff --git a/src/dynamics_model/bicycle.cpp b/src/dynamics_model/bicycle.cpp index c16e36a..ff01448 100644 --- a/src/dynamics_model/bicycle.cpp +++ b/src/dynamics_model/bicycle.cpp @@ -16,6 +16,8 @@ #include "dynamics_model/bicycle.hpp" #include +#include +#include namespace cddp { @@ -46,6 +48,28 @@ Eigen::VectorXd Bicycle::getContinuousDynamics( return state_dot; } +VectorXdual2nd Bicycle::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + // Extract state variables (now dual2nd types) + const autodiff::dual2nd theta = state(STATE_THETA); // heading angle + const autodiff::dual2nd v = state(STATE_V); // velocity + + // Extract control variables (now dual2nd types) + const autodiff::dual2nd a = control(CONTROL_ACC); // acceleration + const autodiff::dual2nd delta = control(CONTROL_DELTA); // steering angle + + // Kinematic bicycle model equations using ADL for math functions + state_dot(STATE_X) = v * cos(theta); // dx/dt + state_dot(STATE_Y) = v * sin(theta); // dy/dt + state_dot(STATE_THETA) = (v / wheelbase_) * tan(delta); // dtheta/dt + state_dot(STATE_V) = a; // dv/dt + + return state_dot; +} + Eigen::MatrixXd Bicycle::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { From 630f0ed2fc28cc585906aa77b74bd36d58c3dbb1 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 12:50:29 -0400 Subject: [PATCH 09/18] Add autodiff methods for Car dynamics model - Implemented `getDiscreteDynamicsAutodiff` and `getContinuousDynamicsAutodiff` methods in the Car class to support autodiff for discrete and continuous dynamics using dual2nd types. - Updated the header file to declare the new autodiff methods. - Included necessary headers for autodiff functionality and enhanced dynamics calculations for accurate derivative computations. --- include/cddp-cpp/dynamics_model/car.hpp | 10 ++++ src/dynamics_model/car.cpp | 64 +++++++++++++++++++++++++ 2 files changed, 74 insertions(+) diff --git a/include/cddp-cpp/dynamics_model/car.hpp b/include/cddp-cpp/dynamics_model/car.hpp index caf2ed2..d2b238c 100644 --- a/include/cddp-cpp/dynamics_model/car.hpp +++ b/include/cddp-cpp/dynamics_model/car.hpp @@ -111,6 +111,11 @@ class Car : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + // Add the required autodiff version declaration for continuous dynamics + // Use fully qualified type name cddp::VectorXdual2nd + cddp::VectorXdual2nd getContinuousDynamicsAutodiff( + const cddp::VectorXdual2nd& state, const cddp::VectorXdual2nd& control) const override; + private: double wheelbase_; ///< Distance between front and rear axles @@ -125,6 +130,11 @@ class Car : public DynamicalSystem { static constexpr int CONTROL_DELTA = 0; ///< steering angle index static constexpr int CONTROL_A = 1; ///< acceleration index static constexpr int CONTROL_DIM = 2; ///< total control dimension + + // Helper function for autodiff discrete dynamics + // Use fully qualified type name cddp::VectorXdual2nd + cddp::VectorXdual2nd getDiscreteDynamicsAutodiff( + const cddp::VectorXdual2nd& state, const cddp::VectorXdual2nd& control) const; }; } // namespace cddp diff --git a/src/dynamics_model/car.cpp b/src/dynamics_model/car.cpp index 8733e9d..f2c861c 100644 --- a/src/dynamics_model/car.cpp +++ b/src/dynamics_model/car.cpp @@ -1,5 +1,8 @@ #include "dynamics_model/car.hpp" #include +#include // Include dual types and math functions +#include // Include Eigen support for dual types +#include "cddp_core/helper.hpp" // For finite_difference_jacobian (used in old methods) namespace cddp { @@ -108,4 +111,65 @@ std::vector Car::getControlHessian( return hessians; } +// **** Autodiff Implementation **** + +// Helper: Autodiff version of discrete dynamics +cddp::VectorXdual2nd Car::getDiscreteDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + + // Extract states (dual2nd) + const autodiff::dual2nd theta = state(STATE_THETA); + const autodiff::dual2nd v = state(STATE_V); + + // Extract controls (dual2nd) + const autodiff::dual2nd delta = control(CONTROL_DELTA); + const autodiff::dual2nd a = control(CONTROL_A); + + // Constants + const double d_double = wheelbase_; + const double h = timestep_; + const autodiff::dual2nd d = wheelbase_; + + // Use ADL for math functions (remove 'autodiff::') + const autodiff::dual2nd cos_theta = cos(theta); + const autodiff::dual2nd sin_theta = sin(theta); + + const autodiff::dual2nd f = h * v; + + autodiff::dual2nd b; + autodiff::dual2nd f_sin_delta = f * sin(delta); + autodiff::dual2nd d_squared = d * d; + autodiff::dual2nd inside_sqrt = d_squared - f_sin_delta * f_sin_delta; + + if (val(inside_sqrt) < 0.0) { + inside_sqrt = 0.0; + } + b = d + f * cos(delta) - sqrt(inside_sqrt); + + + autodiff::dual2nd dtheta; + autodiff::dual2nd asin_arg = sin(delta) * f / d; + + if (std::abs(val(asin_arg)) > 1.0) { + asin_arg = (val(asin_arg) > 0.0) ? 1.0 : -1.0; + } + dtheta = asin(asin_arg); + + + VectorXdual2nd dy = VectorXdual2nd::Zero(STATE_DIM); + dy(STATE_X) = b * cos_theta; + dy(STATE_Y) = b * sin_theta; + dy(STATE_THETA) = dtheta; + dy(STATE_V) = h * a; + + return state + dy; +} + +// Required continuous dynamics using autodiff discrete dynamics +cddp::VectorXdual2nd Car::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + VectorXdual2nd next_state = this->getDiscreteDynamicsAutodiff(state, control); + return (next_state - state) / timestep_; +} + } // namespace cddp \ No newline at end of file From 0cf75fdcbc3692394edf21d7c5c532cc6f49db24 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 13:36:36 -0400 Subject: [PATCH 10/18] Add autodiff methods for various dynamics models --- .../cddp-cpp/cddp_core/dynamical_system.hpp | 7 +- include/cddp-cpp/dynamics_model/cartpole.hpp | 9 +++ .../dynamics_model/dreyfus_rocket.hpp | 4 ++ .../cddp-cpp/dynamics_model/dubins_car.hpp | 5 ++ .../cddp-cpp/dynamics_model/lti_system.hpp | 8 +++ .../cddp-cpp/dynamics_model/manipulator.hpp | 26 +++++++ include/cddp-cpp/dynamics_model/pendulum.hpp | 4 ++ include/cddp-cpp/dynamics_model/quadrotor.hpp | 21 ++++++ .../dynamics_model/spacecraft_landing2d.hpp | 4 ++ .../dynamics_model/spacecraft_linear.hpp | 9 +++ include/cddp-cpp/dynamics_model/unicycle.hpp | 4 ++ src/dynamics_model/bicycle.cpp | 10 +-- src/dynamics_model/car.cpp | 2 +- src/dynamics_model/cartpole.cpp | 34 +++++++++- src/dynamics_model/dreyfus_rocket.cpp | 15 +++++ src/dynamics_model/dubins_car.cpp | 21 ++++++ src/dynamics_model/lti_system.cpp | 20 ++++++ src/dynamics_model/manipulator.cpp | 67 +++++++++++++++++++ src/dynamics_model/pendulum.cpp | 18 +++++ src/dynamics_model/quadrotor.cpp | 16 +++++ src/dynamics_model/spacecraft_landing2d.cpp | 41 ++++++++++++ src/dynamics_model/spacecraft_linear.cpp | 36 ++++++++++ src/dynamics_model/unicycle.cpp | 22 ++++++ 23 files changed, 393 insertions(+), 10 deletions(-) diff --git a/include/cddp-cpp/cddp_core/dynamical_system.hpp b/include/cddp-cpp/cddp_core/dynamical_system.hpp index 70596a3..27ebcca 100644 --- a/include/cddp-cpp/cddp_core/dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/dynamical_system.hpp @@ -45,9 +45,12 @@ class DynamicalSystem { const Eigen::VectorXd& control) const; // Autodiff version of continuous dynamics using second-order duals - // Derived classes MUST implement this to use autodiff for derivatives. + // Derived classes MUST implement this to use the default autodiff-based derivative functions (getJacobians, getHessians). + // If not overridden, calling functions that depend on it will result in a runtime error. virtual VectorXdual2nd getContinuousDynamicsAutodiff(const VectorXdual2nd& state, - const VectorXdual2nd& control) const = 0; // Takes and returns dual2nd + const VectorXdual2nd& control) const { + throw std::logic_error("getContinuousDynamicsAutodiff must be overridden in the derived class to use Autodiff-based derivatives."); + } // Discrete dynamics function: x_{t+1} = f(x_t, u_t) // Uses integration based on getContinuousDynamics diff --git a/include/cddp-cpp/dynamics_model/cartpole.hpp b/include/cddp-cpp/dynamics_model/cartpole.hpp index ebfbf29..ccab386 100644 --- a/include/cddp-cpp/dynamics_model/cartpole.hpp +++ b/include/cddp-cpp/dynamics_model/cartpole.hpp @@ -63,6 +63,15 @@ class CartPole : public DynamicalSystem { Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + /** + * @brief Computes continuous-time system dynamics using autodiff + * @param state Current state vector + * @param control Current control input + * @return State derivative vector + */ + VectorXdual2nd getContinuousDynamicsAutodiff(const VectorXdual2nd& state, + const VectorXdual2nd& control) const override; + /** * @brief Computes discrete-time system dynamics * @param state Current state vector diff --git a/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp b/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp index d0a0e93..3cdd19a 100644 --- a/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp +++ b/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp @@ -100,6 +100,10 @@ class DreyfusRocket : public DynamicalSystem { double getThrustAcceleration() const { return thrust_acceleration_; } double getGravityAcceleration() const { return gravity_acceleration_; } + // Ensure declaration exists and matches + VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + private: // State indices static constexpr int STATE_X = 0; ///< Position index diff --git a/include/cddp-cpp/dynamics_model/dubins_car.hpp b/include/cddp-cpp/dynamics_model/dubins_car.hpp index f8758d1..720dae1 100644 --- a/include/cddp-cpp/dynamics_model/dubins_car.hpp +++ b/include/cddp-cpp/dynamics_model/dubins_car.hpp @@ -17,6 +17,7 @@ #ifndef CDDP_DUBINS_CAR_HPP #define CDDP_DUBINS_CAR_HPP +#include "cddp_core/dynamical_system.hpp" #include "cddp_core/dynamical_system.hpp" namespace cddp { @@ -101,6 +102,10 @@ class DubinsCar : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + // Add the required autodiff version declaration + VectorXdual2nd getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + private: // State indices static constexpr int STATE_X = 0; // x position diff --git a/include/cddp-cpp/dynamics_model/lti_system.hpp b/include/cddp-cpp/dynamics_model/lti_system.hpp index 92491d0..2090bb9 100644 --- a/include/cddp-cpp/dynamics_model/lti_system.hpp +++ b/include/cddp-cpp/dynamics_model/lti_system.hpp @@ -104,6 +104,10 @@ class LTISystem : public DynamicalSystem { const Eigen::MatrixXd& getA() const { return A_; } const Eigen::MatrixXd& getB() const { return B_; } + // Ensure declaration exists and matches base class + VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + private: Eigen::MatrixXd A_; ///< System matrix Eigen::MatrixXd B_; ///< Input matrix @@ -112,6 +116,10 @@ class LTISystem : public DynamicalSystem { * @brief Initialize random stable system matrices */ void initializeRandomSystem(); + + // Ensure helper declaration exists + VectorXdual2nd getDiscreteDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const; }; } // namespace cddp diff --git a/include/cddp-cpp/dynamics_model/manipulator.hpp b/include/cddp-cpp/dynamics_model/manipulator.hpp index 312d46f..e27223a 100644 --- a/include/cddp-cpp/dynamics_model/manipulator.hpp +++ b/include/cddp-cpp/dynamics_model/manipulator.hpp @@ -131,6 +131,17 @@ class Manipulator : public DynamicalSystem { } } + /** + * Computes the continuous-time dynamics of the manipulator + * State vector: [q1, q2, q3, dq1, dq2, dq3] + * Control vector: [tau1, tau2, tau3] + * @param state Current state vector + * @param control Current control input (joint torques) + * @return State derivative vector + */ + VectorXdual2nd getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + private: // Link lengths (match MATLAB example) const double la_{1.0}; // Link a length @@ -182,6 +193,21 @@ class Manipulator : public DynamicalSystem { * @return Gravity torque vector */ Eigen::VectorXd getGravityVector(const Eigen::VectorXd& q) const; + + /** + * Computes the mass matrix M(q) of the manipulator + * @param q Joint positions + * @return Mass matrix + */ + Eigen::Matrix getMassMatrixAutodiff( + const VectorXdual2nd& q) const; + + /** + * Computes the gravity compensation terms + * @param q Joint positions + * @return Gravity torque vector + */ + VectorXdual2nd getGravityVectorAutodiff(const VectorXdual2nd& q) const; }; } // namespace cddp diff --git a/include/cddp-cpp/dynamics_model/pendulum.hpp b/include/cddp-cpp/dynamics_model/pendulum.hpp index cf6efb8..6bfd03e 100644 --- a/include/cddp-cpp/dynamics_model/pendulum.hpp +++ b/include/cddp-cpp/dynamics_model/pendulum.hpp @@ -109,6 +109,10 @@ class Pendulum : public DynamicalSystem { double getDamping() const { return damping_; } double getGravity() const { return gravity_; } + // Ensure declaration exists and matches + VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + private: // Pendulum parameters double length_; // length [m] diff --git a/include/cddp-cpp/dynamics_model/quadrotor.hpp b/include/cddp-cpp/dynamics_model/quadrotor.hpp index ad7bb95..a63b129 100644 --- a/include/cddp-cpp/dynamics_model/quadrotor.hpp +++ b/include/cddp-cpp/dynamics_model/quadrotor.hpp @@ -92,6 +92,15 @@ class Quadrotor : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + /** + * Computes the continuous-time dynamics of the quadrotor model using autodiff + * @param state Current state vector + * @param control Current control input + * @return State derivative vector + */ + VectorXdual2nd getContinuousDynamicsAutodiff(const VectorXdual2nd& state, + const VectorXdual2nd& control) const override; + private: double mass_; // Mass of the quadrotor Eigen::Matrix3d inertia_; // Inertia matrix @@ -130,6 +139,18 @@ class Quadrotor : public DynamicalSystem { * @return 3x3 rotation matrix */ Eigen::Matrix3d getRotationMatrix(double qw, double qx, double qy, double qz) const; + + /** + * Helper function to compute rotation matrix from a quaternion using autodiff + * @param qw Scalar part of quaternion + * @param qx x component + * @param qy y component + * @param qz z component + * @return 3x3 rotation matrix + */ + Eigen::Matrix getRotationMatrixAutodiff( + const autodiff::dual2nd& qw, const autodiff::dual2nd& qx, + const autodiff::dual2nd& qy, const autodiff::dual2nd& qz) const; }; } // namespace cddp diff --git a/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp b/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp index 381c868..90ab521 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp @@ -52,6 +52,10 @@ class SpacecraftLanding2D : public DynamicalSystem { Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + // **** Add declaration **** + VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override { return DynamicalSystem::getDiscreteDynamics(state, control); diff --git a/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp b/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp index b386372..e7d7474 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_linear.hpp @@ -93,6 +93,15 @@ class HCW : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + /** + * Computes the continuous-time dynamics of the HCW model using autodiff + * @param state Current state vector + * @param control Current control input + * @return State derivative vector + */ + VectorXdual2nd getContinuousDynamicsAutodiff(const VectorXdual2nd& state, + const VectorXdual2nd& control) const override; + private: double mean_motion_; // Mean motion of reference orbit double mass_; // Mass of the chaser spacecraft diff --git a/include/cddp-cpp/dynamics_model/unicycle.hpp b/include/cddp-cpp/dynamics_model/unicycle.hpp index d01c483..e48bab3 100644 --- a/include/cddp-cpp/dynamics_model/unicycle.hpp +++ b/include/cddp-cpp/dynamics_model/unicycle.hpp @@ -97,6 +97,10 @@ class Unicycle : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + // Add declaration + VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const override; + private: // State indices static constexpr int STATE_X = 0; // x position diff --git a/src/dynamics_model/bicycle.cpp b/src/dynamics_model/bicycle.cpp index ff01448..b30d487 100644 --- a/src/dynamics_model/bicycle.cpp +++ b/src/dynamics_model/bicycle.cpp @@ -61,11 +61,11 @@ VectorXdual2nd Bicycle::getContinuousDynamicsAutodiff( const autodiff::dual2nd a = control(CONTROL_ACC); // acceleration const autodiff::dual2nd delta = control(CONTROL_DELTA); // steering angle - // Kinematic bicycle model equations using ADL for math functions - state_dot(STATE_X) = v * cos(theta); // dx/dt - state_dot(STATE_Y) = v * sin(theta); // dy/dt - state_dot(STATE_THETA) = (v / wheelbase_) * tan(delta); // dtheta/dt - state_dot(STATE_V) = a; // dv/dt + // Use ADL for math functions + state_dot(STATE_X) = v * cos(theta); + state_dot(STATE_Y) = v * sin(theta); + state_dot(STATE_THETA) = (v / wheelbase_) * tan(delta); + state_dot(STATE_V) = a; return state_dot; } diff --git a/src/dynamics_model/car.cpp b/src/dynamics_model/car.cpp index f2c861c..361a076 100644 --- a/src/dynamics_model/car.cpp +++ b/src/dynamics_model/car.cpp @@ -130,7 +130,7 @@ cddp::VectorXdual2nd Car::getDiscreteDynamicsAutodiff( const double h = timestep_; const autodiff::dual2nd d = wheelbase_; - // Use ADL for math functions (remove 'autodiff::') + // Use ADL for math functions const autodiff::dual2nd cos_theta = cos(theta); const autodiff::dual2nd sin_theta = sin(theta); diff --git a/src/dynamics_model/cartpole.cpp b/src/dynamics_model/cartpole.cpp index 8970b86..09910b3 100644 --- a/src/dynamics_model/cartpole.cpp +++ b/src/dynamics_model/cartpole.cpp @@ -19,6 +19,8 @@ #include #include #include "cddp_core/helper.hpp" +#include +#include namespace cddp { @@ -35,8 +37,6 @@ CartPole::CartPole(double timestep, std::string integration_type, Eigen::VectorXd CartPole::getContinuousDynamics( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - // TODO: Implement damping term - Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM); const double x = state(STATE_X); @@ -62,6 +62,36 @@ Eigen::VectorXd CartPole::getContinuousDynamics( return state_dot; } +cddp::VectorXdual2nd CartPole::getContinuousDynamicsAutodiff( + const cddp::VectorXdual2nd& state, const cddp::VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + const autodiff::dual2nd theta = state(STATE_THETA); + const autodiff::dual2nd x_dot = state(STATE_X_DOT); + const autodiff::dual2nd theta_dot = state(STATE_THETA_DOT); + + const autodiff::dual2nd force = control(CONTROL_FORCE); + + const double total_mass = cart_mass_ + pole_mass_; + + const autodiff::dual2nd sin_theta = sin(theta); + const autodiff::dual2nd cos_theta = cos(theta); + + const autodiff::dual2nd mp_sin2_theta = pole_mass_ * sin_theta * sin_theta; + const autodiff::dual2nd den = cart_mass_ + mp_sin2_theta; + + state_dot(STATE_X) = x_dot; + + state_dot(STATE_THETA) = theta_dot; + + state_dot(STATE_X_DOT) = (force + pole_mass_ * sin_theta * (pole_length_ * theta_dot * theta_dot + gravity_ * cos_theta)) / den; + + state_dot(STATE_THETA_DOT) = (-force * cos_theta - pole_mass_ * pole_length_ * theta_dot * theta_dot * cos_theta * sin_theta - total_mass * gravity_ * sin_theta - damping_ * theta_dot) / (pole_length_ * den); + + return state_dot; +} + Eigen::MatrixXd CartPole::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { diff --git a/src/dynamics_model/dreyfus_rocket.cpp b/src/dynamics_model/dreyfus_rocket.cpp index 12086cd..f3d28b9 100644 --- a/src/dynamics_model/dreyfus_rocket.cpp +++ b/src/dynamics_model/dreyfus_rocket.cpp @@ -17,6 +17,8 @@ #include "dynamics_model/dreyfus_rocket.hpp" #include #include "cddp_core/helper.hpp" +#include +#include namespace cddp { @@ -85,4 +87,17 @@ std::vector DreyfusRocket::getControlHessian( return hessians; } +VectorXdual2nd DreyfusRocket::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + const autodiff::dual2nd x_dot = state(STATE_X_DOT); + const autodiff::dual2nd theta = control(CONTROL_THETA); + + state_dot(STATE_X) = x_dot; + state_dot(STATE_X_DOT) = thrust_acceleration_ * cos(theta) - gravity_acceleration_; + + return state_dot; +} + } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/dubins_car.cpp b/src/dynamics_model/dubins_car.cpp index f3bdd56..bae7001 100644 --- a/src/dynamics_model/dubins_car.cpp +++ b/src/dynamics_model/dubins_car.cpp @@ -16,6 +16,8 @@ #include "dynamics_model/dubins_car.hpp" // Adjust include path as needed #include +#include // Include dual types and math functions +#include // Include Eigen support for dual types namespace cddp { @@ -47,6 +49,25 @@ Eigen::VectorXd DubinsCar::getContinuousDynamics( return state_dot; } +cddp::VectorXdual2nd DubinsCar::getContinuousDynamicsAutodiff( + const cddp::VectorXdual2nd& state, const cddp::VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + // Extract states (dual2nd) + const autodiff::dual2nd theta = state(STATE_THETA); + + // Extract controls (dual2nd) + const autodiff::dual2nd omega = control(CONTROL_OMEGA); + + // Use ADL for math functions + state_dot(STATE_X) = speed_ * cos(theta); + state_dot(STATE_Y) = speed_ * sin(theta); + state_dot(STATE_THETA) = omega; // dtheta/dt = omega + + return state_dot; +} + Eigen::MatrixXd DubinsCar::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const diff --git a/src/dynamics_model/lti_system.cpp b/src/dynamics_model/lti_system.cpp index 7acbd65..7321eb7 100644 --- a/src/dynamics_model/lti_system.cpp +++ b/src/dynamics_model/lti_system.cpp @@ -1,5 +1,9 @@ #include "dynamics_model/lti_system.hpp" #include +#include +#include +#include +#include namespace cddp { @@ -109,4 +113,20 @@ std::vector LTISystem::getControlHessian( return hessians; } +// **** Autodiff Implementation **** + +// Helper definition (should be present) +VectorXdual2nd LTISystem::getDiscreteDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + return A_ * state + B_ * control; +} + +// **** Ensure this definition exists and matches the header **** +VectorXdual2nd LTISystem::getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const { // MUST be const + VectorXdual2nd next_state = this->getDiscreteDynamicsAutodiff(state, control); + // Approximate continuous dynamics: x_dot = (x_{k+1} - x_k) / dt + return (next_state - state) / timestep_; +} + } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/manipulator.cpp b/src/dynamics_model/manipulator.cpp index 7b817a9..558a9eb 100644 --- a/src/dynamics_model/manipulator.cpp +++ b/src/dynamics_model/manipulator.cpp @@ -16,6 +16,9 @@ #include "dynamics_model/manipulator.hpp" #include +#include "cddp_core/helper.hpp" +#include +#include namespace cddp { @@ -210,4 +213,68 @@ Eigen::VectorXd Manipulator::getGravityVector(const Eigen::VectorXd& q) const { return G; } +Eigen::Matrix Manipulator::getMassMatrixAutodiff(const autodiff::VectorXdual2nd& q) const { + Eigen::Matrix M = Eigen::Matrix::Zero(NUM_JOINTS, NUM_JOINTS); + + // Simplified mass matrix - assuming point masses at the end of each link + double m1 = 1.0; // Mass of first link + double m2 = 1.0; // Mass of second link + double m3 = 0.5; // Mass of third link (end effector) + + // Use ADL for math functions + autodiff::dual2nd cos_q1 = cos(q(1)); + autodiff::dual2nd cos_q2 = cos(q(2)); + autodiff::dual2nd cos_q1_plus_q2 = cos(q(1) + q(2)); + + // Diagonal terms (simplified) + M(0,0) = (m1 + m2 + m3) * (la_ * la_); + M(1,1) = (m2 + m3) * (lb_ * lb_); + M(2,2) = m3 * (lc_ * lc_); + + // Off-diagonal terms (coupling between joints - simplified) + M(0,1) = M(1,0) = (m2 + m3) * la_ * lb_ * cos_q1; + M(1,2) = M(2,1) = m3 * lb_ * lc_ * cos_q2; + M(0,2) = M(2,0) = m3 * la_ * lc_ * cos_q1_plus_q2; + + return M; +} + +autodiff::VectorXdual2nd Manipulator::getGravityVectorAutodiff(const autodiff::VectorXdual2nd& q) const { + autodiff::VectorXdual2nd G = autodiff::VectorXdual2nd::Zero(NUM_JOINTS); + + double m1 = 1.0; + double m2 = 1.0; + double m3 = 0.5; + + G(0) = 0; // Base joint not affected by gravity + G(1) = -(m2 + m3) * gravity_ * lb_ * cos(q(1)) + - m3 * gravity_ * lc_ * cos(q(1) + q(2)); + G(2) = -m3 * gravity_ * lc_ * cos(q(1) + q(2)); + + return G; +} + +autodiff::VectorXdual2nd Manipulator::getContinuousDynamicsAutodiff( + const autodiff::VectorXdual2nd& state, const autodiff::VectorXdual2nd& control) const { + autodiff::VectorXdual2nd state_dot = autodiff::VectorXdual2nd::Zero(STATE_DIM); + + // Extract joint positions and velocities + autodiff::VectorXdual2nd q = state.segment(0, NUM_JOINTS); + autodiff::VectorXdual2nd dq = state.segment(NUM_JOINTS, NUM_JOINTS); + + // Get dynamic matrices + Eigen::Matrix M = getMassMatrixAutodiff(q); + autodiff::VectorXdual2nd G = getGravityVectorAutodiff(q); + + // Compute accelerations using simplified dynamics: + // M(q)ddq + G(q) = tau + autodiff::VectorXdual2nd ddq = M.inverse() * (control - G); + + // State derivative + state_dot.segment(0, NUM_JOINTS) = dq; + state_dot.segment(NUM_JOINTS, NUM_JOINTS) = ddq; + + return state_dot; +} + } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/pendulum.cpp b/src/dynamics_model/pendulum.cpp index 851c2b2..235e5bc 100644 --- a/src/dynamics_model/pendulum.cpp +++ b/src/dynamics_model/pendulum.cpp @@ -16,6 +16,8 @@ #include "dynamics_model/pendulum.hpp" #include +#include +#include namespace cddp { @@ -113,4 +115,20 @@ std::vector Pendulum::getControlHessian( return hessian; } +VectorXdual2nd Pendulum::getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + const autodiff::dual2nd theta = state(STATE_THETA); + const autodiff::dual2nd theta_dot = state(STATE_THETA_DOT); + const autodiff::dual2nd torque = control(CONTROL_TORQUE); + const double inertia = mass_ * length_ * length_; + + state_dot(STATE_THETA) = theta_dot; + // Corrected sign for gravity assumed (theta=0 down) + state_dot(STATE_THETA_DOT) = (torque - damping_ * theta_dot - mass_ * gravity_ * length_ * sin(theta)) / inertia; // Use ADL + + return state_dot; +} + } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/quadrotor.cpp b/src/dynamics_model/quadrotor.cpp index 180ffbe..ea1ba3d 100644 --- a/src/dynamics_model/quadrotor.cpp +++ b/src/dynamics_model/quadrotor.cpp @@ -16,6 +16,9 @@ #include "dynamics_model/quadrotor.hpp" #include +#include "cddp_core/helper.hpp" +#include +#include namespace cddp { @@ -160,4 +163,17 @@ std::vector Quadrotor::getControlHessian( return hessians; } +Eigen::Matrix Quadrotor::getRotationMatrixAutodiff( + const autodiff::dual2nd& qw, const autodiff::dual2nd& qx, + const autodiff::dual2nd& qy, const autodiff::dual2nd& qz) const { + // No trig functions here, just multiplication +} + +VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + // ... lots of state/control extraction ... + // No direct trig calls here, done in getRotationMatrixAutodiff (which doesn't use trig) + // Relies on Eigen operations (.cross, *, /) on dual types. +} + } // namespace cddp diff --git a/src/dynamics_model/spacecraft_landing2d.cpp b/src/dynamics_model/spacecraft_landing2d.cpp index e23a705..16f274a 100644 --- a/src/dynamics_model/spacecraft_landing2d.cpp +++ b/src/dynamics_model/spacecraft_landing2d.cpp @@ -16,6 +16,9 @@ #include "dynamics_model/spacecraft_landing2d.hpp" #include +#include +#include +#include namespace cddp { @@ -112,4 +115,42 @@ std::vector SpacecraftLanding2D::getControlHessian( return hessians; } +VectorXdual2nd SpacecraftLanding2D::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + // Extract state variables (dual2nd) + const autodiff::dual2nd theta = state(STATE_THETA); + const autodiff::dual2nd x_dot = state(STATE_X_DOT); + const autodiff::dual2nd y_dot = state(STATE_Y_DOT); + const autodiff::dual2nd theta_dot = state(STATE_THETA_DOT); + + // Extract control variables (dual2nd) - Assume valid inputs (no clamping here) + autodiff::dual2nd thrust_cmd = control(CONTROL_THRUST); + autodiff::dual2nd gimble_angle_cmd = control(CONTROL_ANGLE); + + // Total angle of thrust vector in world frame + const autodiff::dual2nd total_angle = gimble_angle_cmd + theta; + + // Calculate forces in world frame using ADL for math + const autodiff::dual2nd F_x = thrust_cmd * sin(total_angle); + const autodiff::dual2nd F_y = thrust_cmd * cos(total_angle); + + // Calculate torque using ADL for math + const autodiff::dual2nd T = -thrust_cmd * (length_ / 2.0) * sin(gimble_angle_cmd); + + // Position derivatives + state_dot(STATE_X) = x_dot; + state_dot(STATE_Y) = y_dot; + state_dot(STATE_THETA) = theta_dot; + + // Velocity derivatives + state_dot(STATE_X_DOT) = F_x / mass_; + state_dot(STATE_Y_DOT) = F_y / mass_ - gravity_; + state_dot(STATE_THETA_DOT) = T / inertia_; + + return state_dot; +} + } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/spacecraft_linear.cpp b/src/dynamics_model/spacecraft_linear.cpp index e2c0f0f..44d0dc1 100644 --- a/src/dynamics_model/spacecraft_linear.cpp +++ b/src/dynamics_model/spacecraft_linear.cpp @@ -16,6 +16,8 @@ #include "dynamics_model/spacecraft_linear.hpp" #include +#include +#include namespace cddp { @@ -123,4 +125,38 @@ std::vector HCW::getControlHessian( return hessians; } +VectorXdual2nd HCW::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + // Extract state variables (dual2nd) + const autodiff::dual2nd x = state(STATE_X); + const autodiff::dual2nd y = state(STATE_Y); + const autodiff::dual2nd z = state(STATE_Z); + const autodiff::dual2nd vx = state(STATE_VX); + const autodiff::dual2nd vy = state(STATE_VY); + const autodiff::dual2nd vz = state(STATE_VZ); + + // Extract control variables (dual2nd) + const autodiff::dual2nd Fx = control(CONTROL_FX); + const autodiff::dual2nd Fy = control(CONTROL_FY); + const autodiff::dual2nd Fz = control(CONTROL_FZ); + + // Constants + const double n = mean_motion_; + const double n2 = n * n; + + // Position derivatives + state_dot(STATE_X) = vx; + state_dot(STATE_Y) = vy; + state_dot(STATE_Z) = vz; + + // Velocity derivatives (HCW equations) + state_dot(STATE_VX) = 2.0 * n * vy + 3.0 * n2 * x + Fx/mass_; + state_dot(STATE_VY) = -2.0 * n * vx + Fy/mass_; + state_dot(STATE_VZ) = -n2 * z + Fz/mass_; + + return state_dot; +} + } // namespace cddp \ No newline at end of file diff --git a/src/dynamics_model/unicycle.cpp b/src/dynamics_model/unicycle.cpp index db300d1..73fa972 100644 --- a/src/dynamics_model/unicycle.cpp +++ b/src/dynamics_model/unicycle.cpp @@ -16,6 +16,8 @@ #include "dynamics_model/unicycle.hpp" #include +#include +#include namespace cddp { @@ -118,4 +120,24 @@ std::vector Unicycle::getControlHessian( return hessians; } +VectorXdual2nd Unicycle::getContinuousDynamicsAutodiff( // Use alias + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + // Extract state (dual2nd) + const autodiff::dual2nd theta = state(STATE_THETA); + + // Extract control (dual2nd) + const autodiff::dual2nd v = control(CONTROL_V); + const autodiff::dual2nd omega = control(CONTROL_OMEGA); + + // Dynamics using ADL for math + state_dot(STATE_X) = v * cos(theta); + state_dot(STATE_Y) = v * sin(theta); + state_dot(STATE_THETA) = omega; + + return state_dot; +} + } // namespace cddp \ No newline at end of file From 15da2ada13c0e692f1d1561ba74b994de1f79c1f Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 13:42:05 -0400 Subject: [PATCH 11/18] cleanup the codebase --- .../dynamics_model/autodiff_pendulum.hpp | 1 - include/cddp-cpp/dynamics_model/car.hpp | 3 +-- .../cddp-cpp/dynamics_model/dreyfus_rocket.hpp | 3 +-- include/cddp-cpp/dynamics_model/dubins_car.hpp | 1 - include/cddp-cpp/dynamics_model/hcw.hpp | 17 ----------------- include/cddp-cpp/dynamics_model/lti_system.hpp | 6 +++--- include/cddp-cpp/dynamics_model/pendulum.hpp | 4 ++-- .../dynamics_model/spacecraft_landing2d.hpp | 1 - include/cddp-cpp/dynamics_model/unicycle.hpp | 3 +-- 9 files changed, 8 insertions(+), 31 deletions(-) delete mode 100644 include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp delete mode 100644 include/cddp-cpp/dynamics_model/hcw.hpp diff --git a/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp b/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp deleted file mode 100644 index 0519ecb..0000000 --- a/include/cddp-cpp/dynamics_model/autodiff_pendulum.hpp +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file diff --git a/include/cddp-cpp/dynamics_model/car.hpp b/include/cddp-cpp/dynamics_model/car.hpp index d2b238c..a266a62 100644 --- a/include/cddp-cpp/dynamics_model/car.hpp +++ b/include/cddp-cpp/dynamics_model/car.hpp @@ -111,8 +111,7 @@ class Car : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; - // Add the required autodiff version declaration for continuous dynamics - // Use fully qualified type name cddp::VectorXdual2nd + cddp::VectorXdual2nd getContinuousDynamicsAutodiff( const cddp::VectorXdual2nd& state, const cddp::VectorXdual2nd& control) const override; diff --git a/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp b/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp index 3cdd19a..d137b06 100644 --- a/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp +++ b/include/cddp-cpp/dynamics_model/dreyfus_rocket.hpp @@ -100,8 +100,7 @@ class DreyfusRocket : public DynamicalSystem { double getThrustAcceleration() const { return thrust_acceleration_; } double getGravityAcceleration() const { return gravity_acceleration_; } - // Ensure declaration exists and matches - VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + VectorXdual2nd getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/dubins_car.hpp b/include/cddp-cpp/dynamics_model/dubins_car.hpp index 720dae1..0ee91e4 100644 --- a/include/cddp-cpp/dynamics_model/dubins_car.hpp +++ b/include/cddp-cpp/dynamics_model/dubins_car.hpp @@ -102,7 +102,6 @@ class DubinsCar : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; - // Add the required autodiff version declaration VectorXdual2nd getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const override; diff --git a/include/cddp-cpp/dynamics_model/hcw.hpp b/include/cddp-cpp/dynamics_model/hcw.hpp deleted file mode 100644 index 33a4312..0000000 --- a/include/cddp-cpp/dynamics_model/hcw.hpp +++ /dev/null @@ -1,17 +0,0 @@ -/* - Copyright 2024 Tomo Sasaki - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - https://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. -*/ - -// TODO: Implement the HCW class \ No newline at end of file diff --git a/include/cddp-cpp/dynamics_model/lti_system.hpp b/include/cddp-cpp/dynamics_model/lti_system.hpp index 2090bb9..9dcc530 100644 --- a/include/cddp-cpp/dynamics_model/lti_system.hpp +++ b/include/cddp-cpp/dynamics_model/lti_system.hpp @@ -104,8 +104,8 @@ class LTISystem : public DynamicalSystem { const Eigen::MatrixXd& getA() const { return A_; } const Eigen::MatrixXd& getB() const { return B_; } - // Ensure declaration exists and matches base class - VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + + VectorXdual2nd getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const override; private: @@ -118,7 +118,7 @@ class LTISystem : public DynamicalSystem { void initializeRandomSystem(); // Ensure helper declaration exists - VectorXdual2nd getDiscreteDynamicsAutodiff( // Use alias + VectorXdual2nd getDiscreteDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const; }; diff --git a/include/cddp-cpp/dynamics_model/pendulum.hpp b/include/cddp-cpp/dynamics_model/pendulum.hpp index 6bfd03e..c113ac1 100644 --- a/include/cddp-cpp/dynamics_model/pendulum.hpp +++ b/include/cddp-cpp/dynamics_model/pendulum.hpp @@ -109,8 +109,8 @@ class Pendulum : public DynamicalSystem { double getDamping() const { return damping_; } double getGravity() const { return gravity_; } - // Ensure declaration exists and matches - VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + + VectorXdual2nd getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const override; private: diff --git a/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp b/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp index 90ab521..95919c7 100644 --- a/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp +++ b/include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp @@ -52,7 +52,6 @@ class SpacecraftLanding2D : public DynamicalSystem { Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; - // **** Add declaration **** VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias const VectorXdual2nd& state, const VectorXdual2nd& control) const override; diff --git a/include/cddp-cpp/dynamics_model/unicycle.hpp b/include/cddp-cpp/dynamics_model/unicycle.hpp index e48bab3..80ce4a2 100644 --- a/include/cddp-cpp/dynamics_model/unicycle.hpp +++ b/include/cddp-cpp/dynamics_model/unicycle.hpp @@ -97,8 +97,7 @@ class Unicycle : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; - // Add declaration - VectorXdual2nd getContinuousDynamicsAutodiff( // Use alias + VectorXdual2nd getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const override; private: From 172c3f45346933247bf6e6d19cbba092df7e8b36 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 13:42:57 -0400 Subject: [PATCH 12/18] clearn up the code base --- include/cddp-cpp/cddp_core/dynamical_system.hpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/include/cddp-cpp/cddp_core/dynamical_system.hpp b/include/cddp-cpp/cddp_core/dynamical_system.hpp index 27ebcca..86b3ea6 100644 --- a/include/cddp-cpp/cddp_core/dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/dynamical_system.hpp @@ -58,12 +58,11 @@ class DynamicalSystem { const Eigen::VectorXd& control) const; // Jacobian of dynamics w.r.t state: df/dx - // Default implementation uses autodiff via getContinuousDynamicsAutodiff + virtual Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; // Jacobian of dynamics w.r.t control: df/du - // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; @@ -76,19 +75,17 @@ class DynamicalSystem { // Hessian of dynamics w.r.t state: d^2f/dx^2 // Tensor (state_dim x state_dim x state_dim), vector (size state_dim) - // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual std::vector getStateHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; // Hessian of dynamics w.r.t control: d^2f/du^2 // Tensor (state_dim x control_dim x control_dim), vector (size state_dim) - // Default implementation uses autodiff via getContinuousDynamicsAutodiff + virtual std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; // Hessian of dynamics w.r.t state and control: d^2f/dudx // Tensor (state_dim x control_dim x state_dim), vector (size state_dim) - // Default implementation uses autodiff via getContinuousDynamicsAutodiff virtual std::vector getCrossHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const; @@ -97,7 +94,6 @@ class DynamicalSystem { std::vector, std::vector> getHessians(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - // This can now call the default implementations or overridden ones return {getStateHessian(state, control), getControlHessian(state, control), getCrossHessian(state, control)}; From 4a9d1a3f076b447b8bf31afcd99a6ad30466a0a4 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 13:45:23 -0400 Subject: [PATCH 13/18] Clean up the codebase --- src/dynamics_model/autodiff_pendulum.cpp | 0 src/dynamics_model/dubins_car.cpp | 3 --- src/dynamics_model/lti_system.cpp | 10 +++------- src/dynamics_model/pendulum.cpp | 2 +- src/dynamics_model/quadrotor.cpp | 6 ++---- src/dynamics_model/unicycle.cpp | 2 +- 6 files changed, 7 insertions(+), 16 deletions(-) delete mode 100644 src/dynamics_model/autodiff_pendulum.cpp diff --git a/src/dynamics_model/autodiff_pendulum.cpp b/src/dynamics_model/autodiff_pendulum.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/dynamics_model/dubins_car.cpp b/src/dynamics_model/dubins_car.cpp index bae7001..92d9373 100644 --- a/src/dynamics_model/dubins_car.cpp +++ b/src/dynamics_model/dubins_car.cpp @@ -131,14 +131,11 @@ std::vector DubinsCar::getControlHessian( (void)state; (void)control; - // Initialize vector of matrices (one matrix per state dimension) std::vector hessian(STATE_DIM); for (int i = 0; i < STATE_DIM; ++i) { hessian[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); } - // For the Dubins car, all second derivatives with respect to control are zero - // No need to set any values as the matrices are already initialized to zero return hessian; } diff --git a/src/dynamics_model/lti_system.cpp b/src/dynamics_model/lti_system.cpp index 7321eb7..6509960 100644 --- a/src/dynamics_model/lti_system.cpp +++ b/src/dynamics_model/lti_system.cpp @@ -113,19 +113,15 @@ std::vector LTISystem::getControlHessian( return hessians; } -// **** Autodiff Implementation **** -// Helper definition (should be present) -VectorXdual2nd LTISystem::getDiscreteDynamicsAutodiff( // Use alias +VectorXdual2nd LTISystem::getDiscreteDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const { return A_ * state + B_ * control; } -// **** Ensure this definition exists and matches the header **** -VectorXdual2nd LTISystem::getContinuousDynamicsAutodiff( // Use alias - const VectorXdual2nd& state, const VectorXdual2nd& control) const { // MUST be const +VectorXdual2nd LTISystem::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const VectorXdual2nd next_state = this->getDiscreteDynamicsAutodiff(state, control); - // Approximate continuous dynamics: x_dot = (x_{k+1} - x_k) / dt return (next_state - state) / timestep_; } diff --git a/src/dynamics_model/pendulum.cpp b/src/dynamics_model/pendulum.cpp index 235e5bc..dff874d 100644 --- a/src/dynamics_model/pendulum.cpp +++ b/src/dynamics_model/pendulum.cpp @@ -115,7 +115,7 @@ std::vector Pendulum::getControlHessian( return hessian; } -VectorXdual2nd Pendulum::getContinuousDynamicsAutodiff( // Use alias +VectorXdual2nd Pendulum::getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const { VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); diff --git a/src/dynamics_model/quadrotor.cpp b/src/dynamics_model/quadrotor.cpp index ea1ba3d..ca92032 100644 --- a/src/dynamics_model/quadrotor.cpp +++ b/src/dynamics_model/quadrotor.cpp @@ -166,14 +166,12 @@ std::vector Quadrotor::getControlHessian( Eigen::Matrix Quadrotor::getRotationMatrixAutodiff( const autodiff::dual2nd& qw, const autodiff::dual2nd& qx, const autodiff::dual2nd& qy, const autodiff::dual2nd& qz) const { - // No trig functions here, just multiplication + // TODO: Implement this } VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const { - // ... lots of state/control extraction ... - // No direct trig calls here, done in getRotationMatrixAutodiff (which doesn't use trig) - // Relies on Eigen operations (.cross, *, /) on dual types. + // TODO: Implement this } } // namespace cddp diff --git a/src/dynamics_model/unicycle.cpp b/src/dynamics_model/unicycle.cpp index 73fa972..22b7326 100644 --- a/src/dynamics_model/unicycle.cpp +++ b/src/dynamics_model/unicycle.cpp @@ -120,7 +120,7 @@ std::vector Unicycle::getControlHessian( return hessians; } -VectorXdual2nd Unicycle::getContinuousDynamicsAutodiff( // Use alias +VectorXdual2nd Unicycle::getContinuousDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const { VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); From 0c8ace000ab79233fe317b8445372a5db9f3c488 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 13:48:00 -0400 Subject: [PATCH 14/18] Remove unused test file for autodiff pendulum --- CMakeLists.txt | 1 - tests/CMakeLists.txt | 4 ---- tests/dynamics_model/test_autodiff_pendulum.cpp | 0 3 files changed, 5 deletions(-) delete mode 100644 tests/dynamics_model/test_autodiff_pendulum.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 96f7305..75e0426 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -243,7 +243,6 @@ endif() set(dynamics_model_srcs src/dynamics_model/pendulum.cpp - src/dynamics_model/autodiff_pendulum.cpp src/dynamics_model/unicycle.cpp src/dynamics_model/bicycle.cpp src/dynamics_model/cartpole.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f09dcb9..ce139d1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -50,10 +50,6 @@ add_executable(test_pendulum dynamics_model/test_pendulum.cpp) target_link_libraries(test_pendulum gtest gmock gtest_main cddp) gtest_discover_tests(test_pendulum) -add_executable(test_autodiff_pendulum dynamics_model/test_autodiff_pendulum.cpp) -target_link_libraries(test_autodiff_pendulum gtest gmock gtest_main cddp) -gtest_discover_tests(test_autodiff_pendulum) - add_executable(test_quadrotor dynamics_model/test_quadrotor.cpp) target_link_libraries(test_quadrotor gtest gmock gtest_main cddp) gtest_discover_tests(test_quadrotor) diff --git a/tests/dynamics_model/test_autodiff_pendulum.cpp b/tests/dynamics_model/test_autodiff_pendulum.cpp deleted file mode 100644 index e69de29..0000000 From 0d8e0518b02c281c87ecabe3d9b84d7131f61d06 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 1 Apr 2025 13:48:10 -0400 Subject: [PATCH 15/18] Adding missing opening brace for function definition. --- src/dynamics_model/lti_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dynamics_model/lti_system.cpp b/src/dynamics_model/lti_system.cpp index 6509960..5f55055 100644 --- a/src/dynamics_model/lti_system.cpp +++ b/src/dynamics_model/lti_system.cpp @@ -120,7 +120,7 @@ VectorXdual2nd LTISystem::getDiscreteDynamicsAutodiff( } VectorXdual2nd LTISystem::getContinuousDynamicsAutodiff( - const VectorXdual2nd& state, const VectorXdual2nd& control) const + const VectorXdual2nd& state, const VectorXdual2nd& control) const { VectorXdual2nd next_state = this->getDiscreteDynamicsAutodiff(state, control); return (next_state - state) / timestep_; } From a1cb1600e104cd207a27204a362697222cbf10b2 Mon Sep 17 00:00:00 2001 From: Tomohiro Sasaki Date: Tue, 1 Apr 2025 15:28:26 -0400 Subject: [PATCH 16/18] Add autodiff methods for Quadrotor dynamics model --- include/cddp-cpp/dynamics_model/quadrotor.hpp | 9 + src/dynamics_model/quadrotor.cpp | 178 ++++++++++-- tests/dynamics_model/test_quadrotor.cpp | 273 +++++++++++++++++- 3 files changed, 442 insertions(+), 18 deletions(-) diff --git a/include/cddp-cpp/dynamics_model/quadrotor.hpp b/include/cddp-cpp/dynamics_model/quadrotor.hpp index a63b129..5125a0a 100644 --- a/include/cddp-cpp/dynamics_model/quadrotor.hpp +++ b/include/cddp-cpp/dynamics_model/quadrotor.hpp @@ -92,6 +92,15 @@ class Quadrotor : public DynamicalSystem { std::vector getControlHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const override; + /** + * Computes the cross Hessian of the dynamics with respect to both state and control + * @param state Current state vector + * @param control Current control input + * @return Vector of cross Hessian matrices, one per state dimension + */ + std::vector getCrossHessian(const Eigen::VectorXd& state, + const Eigen::VectorXd& control) const override; + /** * Computes the continuous-time dynamics of the quadrotor model using autodiff * @param state Current state vector diff --git a/src/dynamics_model/quadrotor.cpp b/src/dynamics_model/quadrotor.cpp index ca92032..33da4ad 100644 --- a/src/dynamics_model/quadrotor.cpp +++ b/src/dynamics_model/quadrotor.cpp @@ -126,52 +126,196 @@ Eigen::Matrix3d Quadrotor::getRotationMatrix(double qw, double qx, double qy, do Eigen::MatrixXd Quadrotor::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - auto f = [&](const Eigen::VectorXd& x) { - return getContinuousDynamics(x, control); + // Use autodiff to compute state Jacobian + VectorXdual2nd x = state; + VectorXdual2nd u = control; + + auto dynamics_wrt_x = [&](const VectorXdual2nd& x_ad) -> VectorXdual2nd { + return this->getContinuousDynamicsAutodiff(x_ad, u); }; - return finite_difference_jacobian(f, state); + return autodiff::jacobian(dynamics_wrt_x, wrt(x), at(x)); } Eigen::MatrixXd Quadrotor::getControlJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - auto f = [&](const Eigen::VectorXd& u) { - return getContinuousDynamics(state, u); + // Use autodiff to compute control Jacobian + VectorXdual2nd x = state; + VectorXdual2nd u = control; + + auto dynamics_wrt_u = [&](const VectorXdual2nd& u_ad) -> VectorXdual2nd { + return this->getContinuousDynamicsAutodiff(x, u_ad); }; - return finite_difference_jacobian(f, control); + + return autodiff::jacobian(dynamics_wrt_u, wrt(u), at(u)); +} + +Eigen::Matrix Quadrotor::getRotationMatrixAutodiff( + const autodiff::dual2nd& qw, const autodiff::dual2nd& qx, + const autodiff::dual2nd& qy, const autodiff::dual2nd& qz) const { + // Compute the rotation matrix from a unit quaternion. + // The quaternion is assumed to be normalized and in the form [qw, qx, qy, qz] + Eigen::Matrix R; + R(0, 0) = 1 - 2 * (qy * qy + qz * qz); + R(0, 1) = 2 * (qx * qy - qz * qw); + R(0, 2) = 2 * (qx * qz + qy * qw); + + R(1, 0) = 2 * (qx * qy + qz * qw); + R(1, 1) = 1 - 2 * (qx * qx + qz * qz); + R(1, 2) = 2 * (qy * qz - qx * qw); + + R(2, 0) = 2 * (qx * qz - qy * qw); + R(2, 1) = 2 * (qy * qz + qx * qw); + R(2, 2) = 1 - 2 * (qx * qx + qy * qy); + + return R; } +VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( + const VectorXdual2nd& state, const VectorXdual2nd& control) const { + VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM); + + // --- Position Derivative --- + // The derivative of the position is the linear velocity. + state_dot.segment<3>(STATE_X) = state.segment<3>(STATE_VX); + + // --- Quaternion Derivative --- + // Extract the quaternion (assumed to be [qw, qx, qy, qz]) + autodiff::dual2nd qw = state(STATE_QW); + autodiff::dual2nd qx = state(STATE_QX); + autodiff::dual2nd qy = state(STATE_QY); + autodiff::dual2nd qz = state(STATE_QZ); + + // Normalize the quaternion to enforce unit norm before further use. + autodiff::dual2nd norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz); + if (val(norm) > 1e-6) { + qw /= norm; + qx /= norm; + qy /= norm; + qz /= norm; + } else { + // In degenerate cases, default to the identity quaternion. + qw = 1.0; qx = 0.0; qy = 0.0; qz = 0.0; + } + + // Extract body angular velocity components + autodiff::dual2nd omega_x = state(STATE_OMEGA_X); + autodiff::dual2nd omega_y = state(STATE_OMEGA_Y); + autodiff::dual2nd omega_z = state(STATE_OMEGA_Z); + + // Compute quaternion derivative using: q_dot = 0.5 * q ⊗ [0, omega] + state_dot(STATE_QW) = -0.5 * (qx * omega_x + qy * omega_y + qz * omega_z); + state_dot(STATE_QX) = 0.5 * (qw * omega_x + qy * omega_z - qz * omega_y); + state_dot(STATE_QY) = 0.5 * (qw * omega_y - qx * omega_z + qz * omega_x); + state_dot(STATE_QZ) = 0.5 * (qw * omega_z + qx * omega_y - qy * omega_x); + + // --- Velocity Derivative --- + // Extract control variables (motor forces) + const autodiff::dual2nd f1 = control(CONTROL_F1); + const autodiff::dual2nd f2 = control(CONTROL_F2); + const autodiff::dual2nd f3 = control(CONTROL_F3); + const autodiff::dual2nd f4 = control(CONTROL_F4); + + // Compute total thrust and moments + const autodiff::dual2nd thrust = f1 + f2 + f3 + f4; + const autodiff::dual2nd tau_x = arm_length_ * (f1 - f3); + const autodiff::dual2nd tau_y = arm_length_ * (f2 - f4); + const autodiff::dual2nd tau_z = 0.1 * (f1 - f2 + f3 - f4); + + // Compute rotation matrix from the normalized quaternion + Eigen::Matrix R = getRotationMatrixAutodiff(qw, qx, qy, qz); + + // Thrust is applied along the body z-axis. + Eigen::Matrix F_thrust(0, 0, thrust); + Eigen::Matrix gravity(0, 0, gravity_); + Eigen::Matrix acceleration = (1.0/mass_) * (R * F_thrust) - gravity; + state_dot.segment<3>(STATE_VX) = acceleration; + + // --- Angular Velocity Derivative --- + Eigen::Matrix omega(omega_x, omega_y, omega_z); + Eigen::Matrix tau(tau_x, tau_y, tau_z); + + // Convert inertia matrix to autodiff type + Eigen::Matrix inertia = inertia_.cast(); + + // Calculate angular acceleration + Eigen::Matrix angular_acc = inertia.inverse() * (tau - omega.cross(inertia * omega)); + state_dot.segment<3>(STATE_OMEGA_X) = angular_acc; + + return state_dot; +} // TODO: Implement a more accurate version if needed std::vector Quadrotor::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + // We'll use autodiff to compute Hessians + VectorXdual2nd x = state; + VectorXdual2nd u = control; + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + // Define lambda for the ith component of the dynamics + auto fi = [&, i](const VectorXdual2nd& x_ad) -> autodiff::dual2nd { + return getContinuousDynamicsAutodiff(x_ad, u)(i); + }; + + // Compute Hessian of ith component w.r.t. state + hessians[i] = autodiff::hessian(fi, wrt(x), at(x)); } + return hessians; } // TODO: Implement a more accurate version if needed std::vector Quadrotor::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + // We'll use autodiff to compute Hessians + VectorXdual2nd x = state; + VectorXdual2nd u = control; + std::vector hessians(STATE_DIM); + for (int i = 0; i < STATE_DIM; ++i) { - hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); + // Define lambda for the ith component of the dynamics + auto fi = [&, i](const VectorXdual2nd& u_ad) -> autodiff::dual2nd { + return getContinuousDynamicsAutodiff(x, u_ad)(i); + }; + + // Compute Hessian of ith component w.r.t. control + hessians[i] = autodiff::hessian(fi, wrt(u), at(u)); } + return hessians; } -Eigen::Matrix Quadrotor::getRotationMatrixAutodiff( - const autodiff::dual2nd& qw, const autodiff::dual2nd& qx, - const autodiff::dual2nd& qy, const autodiff::dual2nd& qz) const { - // TODO: Implement this -} - -VectorXdual2nd Quadrotor::getContinuousDynamicsAutodiff( - const VectorXdual2nd& state, const VectorXdual2nd& control) const { - // TODO: Implement this +std::vector Quadrotor::getCrossHessian( + const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + // For mixed partial derivatives, we need a different approach + // We compute derivatives of Jacobian w.r.t. control + VectorXdual2nd x = state; + VectorXdual2nd u = control; + + std::vector cross_hessians(STATE_DIM); + + for (int i = 0; i < STATE_DIM; ++i) { + // Define a function that returns the gradient of the ith component w.r.t. state + auto gradient_i = [&, i](const VectorXdual2nd& u_ad) -> VectorXdual2nd { + // Capture the current u_ad in a lambda + auto fi_x = [&, u_ad, i](const VectorXdual2nd& x_ad) -> autodiff::dual2nd { + return getContinuousDynamicsAutodiff(x_ad, u_ad)(i); + }; + + // Return the gradient of fi with respect to x at the current x + return autodiff::gradient(fi_x, wrt(x), at(x)); + }; + + // Compute Jacobian of gradient w.r.t. control (this is the cross Hessian) + cross_hessians[i] = autodiff::jacobian(gradient_i, wrt(u), at(u)); + } + + return cross_hessians; } } // namespace cddp diff --git a/tests/dynamics_model/test_quadrotor.cpp b/tests/dynamics_model/test_quadrotor.cpp index 090db5b..b7048a5 100644 --- a/tests/dynamics_model/test_quadrotor.cpp +++ b/tests/dynamics_model/test_quadrotor.cpp @@ -25,7 +25,8 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" -#include "dynamics_model/quadrotor.hpp" +#include "cddp-cpp/dynamics_model/quadrotor.hpp" +#include "cddp-cpp/cddp_core/helper.hpp" using namespace cddp; // Helper: Convert quaternion [qw, qx, qy, qz] to Euler angles (roll, pitch, yaw) @@ -219,6 +220,276 @@ TEST(QuadrotorTest, ContinuousDynamics) { EXPECT_GT(std::abs(state_dot[10]), 0.0); } +TEST(QuadrotorTest, StateJacobianFiniteDifference) { + // Create quadrotor instance + double timestep = 0.01; + double mass = 1.0; + double arm_length = 0.2; + Eigen::Matrix3d inertia; + inertia << 0.01, 0, 0, + 0, 0.01, 0, + 0, 0, 0.02; + Quadrotor quadrotor(timestep, mass, inertia, arm_length, "euler"); + + // Test state and control + Eigen::VectorXd state = Eigen::VectorXd::Zero(13); + state(2) = 1.0; // 1m height + // Set identity quaternion [1, 0, 0, 0] + state(3) = 1.0; + state(4) = 0.0; + state(5) = 0.0; + state(6) = 0.0; + + double hover_thrust = mass * 9.81 / 4.0; + Eigen::VectorXd control(4); + control << hover_thrust, hover_thrust, hover_thrust, hover_thrust; + + // Get analytical Jacobian + Eigen::MatrixXd A_analytical = quadrotor.getStateJacobian(state, control); + + // Get numerical Jacobian + auto f_A = [&](const Eigen::VectorXd& x) { + return quadrotor.getContinuousDynamics(x, control); + }; + Eigen::MatrixXd A_numerical = finite_difference_jacobian(f_A, state); + + // Test dimensions + ASSERT_EQ(A_analytical.rows(), 13); + ASSERT_EQ(A_analytical.cols(), 13); + + // Compare analytical and numerical Jacobians + double tolerance = 1e-4; + EXPECT_NEAR((A_analytical - A_numerical).norm(), 0.0, tolerance); + + // Test with non-zero velocities and rotations + state(4) = 0.1; // non-zero quaternion components + state(5) = 0.1; + state(6) = 0.1; + state(7) = 0.2; // non-zero linear velocities + state(8) = 0.2; + state(9) = 0.2; + state(10) = 0.1; // non-zero angular velocities + state(11) = 0.1; + state(12) = 0.1; + + // Normalize quaternion + double qnorm = std::sqrt(state(3)*state(3) + state(4)*state(4) + + state(5)*state(5) + state(6)*state(6)); + state(3) /= qnorm; + state(4) /= qnorm; + state(5) /= qnorm; + state(6) /= qnorm; + + // Get Jacobians for non-zero state + A_analytical = quadrotor.getStateJacobian(state, control); + A_numerical = finite_difference_jacobian(f_A, state); + + // Compare again + EXPECT_NEAR((A_analytical - A_numerical).norm(), 0.0, tolerance); +} + +TEST(QuadrotorTest, ControlJacobianFiniteDifference) { + // Create quadrotor instance + double timestep = 0.01; + double mass = 1.0; + double arm_length = 0.2; + Eigen::Matrix3d inertia; + inertia << 0.01, 0, 0, + 0, 0.01, 0, + 0, 0, 0.02; + Quadrotor quadrotor(timestep, mass, inertia, arm_length, "euler"); + + // Test state and control + Eigen::VectorXd state = Eigen::VectorXd::Zero(13); + state(2) = 1.0; // 1m height + // Set identity quaternion [1, 0, 0, 0] + state(3) = 1.0; + state(4) = 0.0; + state(5) = 0.0; + state(6) = 0.0; + + double hover_thrust = mass * 9.81 / 4.0; + Eigen::VectorXd control(4); + control << hover_thrust, hover_thrust, hover_thrust, hover_thrust; + + // Get analytical Jacobian + Eigen::MatrixXd B_analytical = quadrotor.getControlJacobian(state, control); + + // Get numerical Jacobian + auto f_B = [&](const Eigen::VectorXd& u) { + return quadrotor.getContinuousDynamics(state, u); + }; + Eigen::MatrixXd B_numerical = finite_difference_jacobian(f_B, control); + + // Test dimensions + ASSERT_EQ(B_analytical.rows(), 13); + ASSERT_EQ(B_analytical.cols(), 4); + + // Compare analytical and numerical Jacobians + double tolerance = 1e-4; + EXPECT_NEAR((B_analytical - B_numerical).norm(), 0.0, tolerance); + + // Test with non-zero velocities and rotations + state(4) = 0.1; // non-zero quaternion components + state(5) = 0.1; + state(6) = 0.1; + state(7) = 0.2; // non-zero linear velocities + state(8) = 0.2; + state(9) = 0.2; + state(10) = 0.1; // non-zero angular velocities + state(11) = 0.1; + state(12) = 0.1; + + // Normalize quaternion + double qnorm = std::sqrt(state(3)*state(3) + state(4)*state(4) + + state(5)*state(5) + state(6)*state(6)); + state(3) /= qnorm; + state(4) /= qnorm; + state(5) /= qnorm; + state(6) /= qnorm; + + // Also try non-uniform control + control << hover_thrust*1.1, hover_thrust*0.9, hover_thrust*1.1, hover_thrust*0.9; + + // Get Jacobians for non-zero state and non-uniform control + B_analytical = quadrotor.getControlJacobian(state, control); + B_numerical = finite_difference_jacobian(f_B, control); + + // Compare again + EXPECT_NEAR((B_analytical - B_numerical).norm(), 0.0, tolerance); +} + +TEST(QuadrotorTest, StateJacobianAutodiff) { + // Create quadrotor instance + double timestep = 0.01; + double mass = 1.0; + double arm_length = 0.2; + Eigen::Matrix3d inertia; + inertia << 0.01, 0, 0, + 0, 0.01, 0, + 0, 0, 0.02; + Quadrotor quadrotor(timestep, mass, inertia, arm_length, "euler"); + + // Test state and control + Eigen::VectorXd state = Eigen::VectorXd::Zero(13); + state(2) = 1.0; // 1m height + // Set identity quaternion [1, 0, 0, 0] + state(3) = 1.0; + state(4) = 0.0; + state(5) = 0.0; + state(6) = 0.0; + + double hover_thrust = mass * 9.81 / 4.0; + Eigen::VectorXd control(4); + control << hover_thrust, hover_thrust, hover_thrust, hover_thrust; + + // Get Jacobian using autodiff (which is the default implementation) + Eigen::MatrixXd A_autodiff = quadrotor.getStateJacobian(state, control); + + // Get numerical Jacobian for comparison + auto f_A = [&](const Eigen::VectorXd& x) { + return quadrotor.getContinuousDynamics(x, control); + }; + Eigen::MatrixXd A_numerical = finite_difference_jacobian(f_A, state); + + // Compare autodiff and numerical Jacobians + double tolerance = 1e-4; + EXPECT_NEAR((A_autodiff - A_numerical).norm(), 0.0, tolerance); + + // Try non-zero state + state(4) = 0.1; // non-zero quaternion components + state(5) = 0.1; + state(6) = 0.1; + state(7) = 0.2; // non-zero linear velocities + state(8) = 0.2; + state(9) = 0.2; + state(10) = 0.1; // non-zero angular velocities + state(11) = 0.1; + state(12) = 0.1; + + // Normalize quaternion + double qnorm = std::sqrt(state(3)*state(3) + state(4)*state(4) + + state(5)*state(5) + state(6)*state(6)); + state(3) /= qnorm; + state(4) /= qnorm; + state(5) /= qnorm; + state(6) /= qnorm; + + // Get Jacobians for non-zero state + A_autodiff = quadrotor.getStateJacobian(state, control); + A_numerical = finite_difference_jacobian(f_A, state); + + // Compare again + EXPECT_NEAR((A_autodiff - A_numerical).norm(), 0.0, tolerance); +} + +TEST(QuadrotorTest, ControlJacobianAutodiff) { + // Create quadrotor instance + double timestep = 0.01; + double mass = 1.0; + double arm_length = 0.2; + Eigen::Matrix3d inertia; + inertia << 0.01, 0, 0, + 0, 0.01, 0, + 0, 0, 0.02; + Quadrotor quadrotor(timestep, mass, inertia, arm_length, "euler"); + + // Test state and control + Eigen::VectorXd state = Eigen::VectorXd::Zero(13); + state(2) = 1.0; // 1m height + // Set identity quaternion [1, 0, 0, 0] + state(3) = 1.0; + state(4) = 0.0; + state(5) = 0.0; + state(6) = 0.0; + + double hover_thrust = mass * 9.81 / 4.0; + Eigen::VectorXd control(4); + control << hover_thrust, hover_thrust, hover_thrust, hover_thrust; + + // Get Jacobian using autodiff (which is the default implementation) + Eigen::MatrixXd B_autodiff = quadrotor.getControlJacobian(state, control); + + // Get numerical Jacobian for comparison + auto f_B = [&](const Eigen::VectorXd& u) { + return quadrotor.getContinuousDynamics(state, u); + }; + Eigen::MatrixXd B_numerical = finite_difference_jacobian(f_B, control); + + // Compare autodiff and numerical Jacobians + double tolerance = 1e-4; + EXPECT_NEAR((B_autodiff - B_numerical).norm(), 0.0, tolerance); + + // Try non-zero state and non-uniform control + state(4) = 0.1; // non-zero quaternion components + state(5) = 0.1; + state(6) = 0.1; + state(7) = 0.2; // non-zero linear velocities + state(8) = 0.2; + state(9) = 0.2; + state(10) = 0.1; // non-zero angular velocities + state(11) = 0.1; + state(12) = 0.1; + + // Normalize quaternion + double qnorm = std::sqrt(state(3)*state(3) + state(4)*state(4) + + state(5)*state(5) + state(6)*state(6)); + state(3) /= qnorm; + state(4) /= qnorm; + state(5) /= qnorm; + state(6) /= qnorm; + + // Also try non-uniform control + control << hover_thrust*1.1, hover_thrust*0.9, hover_thrust*1.1, hover_thrust*0.9; + + // Get Jacobians for non-zero state and non-uniform control + B_autodiff = quadrotor.getControlJacobian(state, control); + B_numerical = finite_difference_jacobian(f_B, control); + + // Compare again + EXPECT_NEAR((B_autodiff - B_numerical).norm(), 0.0, tolerance); +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From 3a78f81fb8082a8742fb8910cf92f4faf633855f Mon Sep 17 00:00:00 2001 From: Tomohiro Sasaki Date: Tue, 1 Apr 2025 15:36:25 -0400 Subject: [PATCH 17/18] Implement autodiff-based Jacobian and Hessian calculations for Car dynamics model --- src/dynamics_model/car.cpp | 94 +++++++++++++++++++++++++------ tests/dynamics_model/test_car.cpp | 67 ++++++++++++++++++++++ 2 files changed, 145 insertions(+), 16 deletions(-) diff --git a/src/dynamics_model/car.cpp b/src/dynamics_model/car.cpp index 361a076..75d3e4b 100644 --- a/src/dynamics_model/car.cpp +++ b/src/dynamics_model/car.cpp @@ -59,16 +59,26 @@ Eigen::VectorXd Car::getDiscreteDynamics( Eigen::MatrixXd Car::getStateJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - // Use the finite difference Jacobian helper function - auto dynamics_func = [this, &control](const Eigen::VectorXd& s) { - return this->getDiscreteDynamics(s, control); - }; + // Convert inputs to autodiff types + VectorXdual2nd state_dual = state.cast(); + VectorXdual2nd control_dual = control.cast(); - // Get discretized Jacobian - Eigen::MatrixXd J = finite_difference_jacobian(dynamics_func, state); + // Initialize Jacobian + Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); + + // Calculate Jacobian using autodiff + for (int i = 0; i < STATE_DIM; ++i) { + // Create a lambda that returns the i-th component of the dynamics + auto dynamics_i = [this, i, &control_dual](const VectorXdual2nd& x) -> autodiff::dual2nd { + VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual); + return dynamics(i); + }; + + // Calculate gradient of the i-th output with respect to state + J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(state_dual), at(state_dual)); + } // Convert discrete Jacobian to continuous time Jacobian - // A = timestep_ * Fx + I -> Fx = (A - I)/timestep_ J.diagonal().array() -= 1.0; J /= timestep_; @@ -78,16 +88,26 @@ Eigen::MatrixXd Car::getStateJacobian( Eigen::MatrixXd Car::getControlJacobian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { - // Use the finite difference Jacobian helper function - auto dynamics_func = [this, &state](const Eigen::VectorXd& c) { - return this->getDiscreteDynamics(state, c); - }; + // Convert inputs to autodiff types + VectorXdual2nd state_dual = state.cast(); + VectorXdual2nd control_dual = control.cast(); - // Get discretized Jacobian - Eigen::MatrixXd J = finite_difference_jacobian(dynamics_func, control); + // Initialize Jacobian + Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM); + + // Calculate Jacobian using autodiff + for (int i = 0; i < STATE_DIM; ++i) { + // Create a lambda that returns the i-th component of the dynamics + auto dynamics_i = [this, i, &state_dual](const VectorXdual2nd& u) -> autodiff::dual2nd { + VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u); + return dynamics(i); + }; + + // Calculate gradient of the i-th output with respect to control + J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(control_dual), at(control_dual)); + } // Convert discrete Jacobian to continuous time Jacobian - // B = timestep_ * Fu -> Fu = B/timestep_ J /= timestep_; return J; @@ -95,24 +115,66 @@ Eigen::MatrixXd Car::getControlJacobian( std::vector Car::getStateHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + + // Convert inputs to autodiff types + VectorXdual2nd state_dual = state.cast(); + VectorXdual2nd control_dual = control.cast(); + + // Initialize Hessians std::vector hessians(STATE_DIM); for (int i = 0; i < STATE_DIM; ++i) { hessians[i] = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); } + + // Calculate Hessians using autodiff + for (int i = 0; i < STATE_DIM; ++i) { + // Create a lambda that returns the i-th component of the dynamics + auto dynamics_i = [this, i, &control_dual](const VectorXdual2nd& x) -> autodiff::dual2nd { + VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual); + return dynamics(i); + }; + + // Calculate Hessian of the i-th output with respect to state + hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(state_dual), at(state_dual)); + + // Convert discrete Hessian to continuous time Hessian + hessians[i] /= timestep_; + } + return hessians; } std::vector Car::getControlHessian( const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { + + // Convert inputs to autodiff types + VectorXdual2nd state_dual = state.cast(); + VectorXdual2nd control_dual = control.cast(); + + // Initialize Hessians std::vector hessians(STATE_DIM); for (int i = 0; i < STATE_DIM; ++i) { hessians[i] = Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); } + + // Calculate Hessians using autodiff + for (int i = 0; i < STATE_DIM; ++i) { + // Create a lambda that returns the i-th component of the dynamics + auto dynamics_i = [this, i, &state_dual](const VectorXdual2nd& u) -> autodiff::dual2nd { + VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u); + return dynamics(i); + }; + + // Calculate Hessian of the i-th output with respect to control + hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(control_dual), at(control_dual)); + + // Convert discrete Hessian to continuous time Hessian + hessians[i] /= timestep_; + } + return hessians; } -// **** Autodiff Implementation **** - // Helper: Autodiff version of discrete dynamics cddp::VectorXdual2nd Car::getDiscreteDynamicsAutodiff( const VectorXdual2nd& state, const VectorXdual2nd& control) const { diff --git a/tests/dynamics_model/test_car.cpp b/tests/dynamics_model/test_car.cpp index a387a37..7899b59 100644 --- a/tests/dynamics_model/test_car.cpp +++ b/tests/dynamics_model/test_car.cpp @@ -164,6 +164,73 @@ TEST(CarTest, JacobianTest) { } } +TEST(CarTest, HessianTest) { + // Create a car instance + double timestep = 0.03; + double wheelbase = 2.0; + std::string integration_type = "euler"; + cddp::Car car(timestep, wheelbase, integration_type); + + // Initial state and control for testing + Eigen::VectorXd state(4); + state << 1.0, 1.0, 3*M_PI/2, 1.0; + Eigen::VectorXd control(2); + control << 0.3, 0.1; + + // Get the state and control Hessians + std::vector state_hessians = car.getStateHessian(state, control); + std::vector control_hessians = car.getControlHessian(state, control); + + // Scale Hessians back since they were divided by timestep in implementation + for (auto& H : state_hessians) { + H *= timestep; + } + for (auto& H : control_hessians) { + H *= timestep; + } + + // Expected values from autodiff calculation + double expected_state_theta_v_v = 8.71e-08; // d²theta/dv² + double expected_control_theta_delta_delta = -0.00443; // d²theta/ddelta² + double expected_state_x_v_theta = 0.0287; // d²x/dv·dθ + + // Verify dimensions + ASSERT_EQ(state_hessians.size(), 4); + ASSERT_EQ(control_hessians.size(), 4); + + for (int i = 0; i < 4; ++i) { + ASSERT_EQ(state_hessians[i].rows(), 4); + ASSERT_EQ(state_hessians[i].cols(), 4); + ASSERT_EQ(control_hessians[i].rows(), 2); + ASSERT_EQ(control_hessians[i].cols(), 2); + } + + // Check specific values using autodiff-calculated expectations + EXPECT_NEAR(state_hessians[2](3, 3), expected_state_theta_v_v, 1e-4); + EXPECT_NEAR(control_hessians[2](0, 0), expected_control_theta_delta_delta, 1e-4); + EXPECT_NEAR(state_hessians[0](3, 2), expected_state_x_v_theta, 1e-4); + EXPECT_NEAR(control_hessians[0](0, 1), 0.0, 1e-4); // d²x/dδ·da + + // Test another state point + state << 1.0, 1.0, 3*M_PI/2, 0.0; + control << 0.01, 0.01; + + state_hessians = car.getStateHessian(state, control); + control_hessians = car.getControlHessian(state, control); + + // Scale Hessians back + for (auto& H : state_hessians) { + H *= timestep; + } + for (auto& H : control_hessians) { + H *= timestep; + } + + // For near-zero velocity, check that the non-zero Hessian components are smaller + EXPECT_LT(state_hessians[2](3, 3), expected_state_theta_v_v); + EXPECT_LT(std::abs(control_hessians[2](0, 0)), std::abs(expected_control_theta_delta_delta)); +} + namespace cddp { class CarParkingObjective : public NonlinearObjective { public: From 7e707fe4f1f20d29f47d25f3b272517acac095fe Mon Sep 17 00:00:00 2001 From: Tomohiro Sasaki Date: Tue, 1 Apr 2025 15:47:10 -0400 Subject: [PATCH 18/18] Add Hessian calculations for non-iLQR dynamics in ipddp_core --- src/cddp_core/ipddp_core.cpp | 104 +++++++++++++++++++++++++++++++++-- 1 file changed, 100 insertions(+), 4 deletions(-) diff --git a/src/cddp_core/ipddp_core.cpp b/src/cddp_core/ipddp_core.cpp index dcd8530..f9a6e5b 100644 --- a/src/cddp_core/ipddp_core.cpp +++ b/src/cddp_core/ipddp_core.cpp @@ -463,15 +463,51 @@ namespace cddp Eigen::MatrixXd A = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep_ * Fx; Eigen::MatrixXd B = timestep_ * Fu; + // Get dynamics hessians if not using iLQR + std::vector Fxx, Fuu, Fux; + if (!options_.is_ilqr) { + const auto hessians = system_->getHessians(x, u); + Fxx = std::get<0>(hessians); + Fuu = std::get<1>(hessians); + Fux = std::get<2>(hessians); + } + double l = objective_->running_cost(x, u, t); auto [l_x, l_u] = objective_->getRunningCostGradients(x, u, t); auto [l_xx, l_uu, l_ux] = objective_->getRunningCostHessians(x, u, t); Eigen::VectorXd Q_x = l_x + A.transpose() * V_x; Eigen::VectorXd Q_u = l_u + B.transpose() * V_x; + + // Standard Q_xx calculation Eigen::MatrixXd Q_xx = l_xx + A.transpose() * V_xx * A; + + // Add state hessian term if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_xx += timestep_ * V_x(i) * Fxx[i]; + } + } + + // Standard Q_ux calculation Eigen::MatrixXd Q_ux = l_ux + B.transpose() * V_xx * A; + + // Add cross hessian term if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_ux += timestep_ * V_x(i) * Fux[i]; + } + } + + // Standard Q_uu calculation Eigen::MatrixXd Q_uu = l_uu + B.transpose() * V_xx * B; + + // Add control hessian term if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_uu += timestep_ * V_x(i) * Fuu[i]; + } + } // Regularization Eigen::MatrixXd Q_ux_reg = Q_ux; @@ -480,8 +516,20 @@ namespace cddp if (options_.regularization_type == "state" || options_.regularization_type == "both") { - Q_ux_reg = l_ux + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * A; - Q_uu_reg = l_uu + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * B; + // Apply regularization to the value function Hessian + Eigen::MatrixXd V_xx_reg = V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim); + + // Recompute Q_ux and Q_uu with regularized V_xx + Q_ux_reg = l_ux + B.transpose() * V_xx_reg * A; + Q_uu_reg = l_uu + B.transpose() * V_xx_reg * B; + + // Add hessian terms with regularized V_xx if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_ux_reg += timestep_ * V_x(i) * Fux[i]; + Q_uu_reg += timestep_ * V_x(i) * Fuu[i]; + } + } } else { @@ -533,6 +581,15 @@ namespace cddp // Continuous dynamics const auto [Fx, Fu] = system_->getJacobians(x, u); + // Get dynamics hessians if not using iLQR + std::vector Fxx, Fuu, Fux; + if (!options_.is_ilqr) { + const auto hessians = system_->getHessians(x, u); + Fxx = std::get<0>(hessians); + Fuu = std::get<1>(hessians); + Fux = std::get<2>(hessians); + } + // Discretize Eigen::MatrixXd A = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep_ * Fx; Eigen::MatrixXd B = timestep_ * Fu; @@ -576,9 +633,36 @@ namespace cddp // Q expansions from cost Eigen::VectorXd Q_x = l_x + Q_yx.transpose() * y + A.transpose() * V_x; Eigen::VectorXd Q_u = l_u + Q_yu.transpose() * y + B.transpose() * V_x; + + // Standard Q_xx calculation Eigen::MatrixXd Q_xx = l_xx + A.transpose() * V_xx * A; + + // Add state hessian term if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_xx += timestep_ * V_x(i) * Fxx[i]; + } + } + + // Standard Q_ux calculation Eigen::MatrixXd Q_ux = l_ux + B.transpose() * V_xx * A; + + // Add cross hessian term if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_ux += timestep_ * V_x(i) * Fux[i]; + } + } + + // Standard Q_uu calculation Eigen::MatrixXd Q_uu = l_uu + B.transpose() * V_xx * B; + + // Add control hessian term if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_uu += timestep_ * V_x(i) * Fuu[i]; + } + } Eigen::MatrixXd Y = y.asDiagonal(); // Diagonal matrix with y as diagonal Eigen::MatrixXd S = s.asDiagonal(); // Diagonal matrix with s as diagonal @@ -603,8 +687,20 @@ namespace cddp if (options_.regularization_type == "state" || options_.regularization_type == "both") { - Q_ux_reg = l_ux + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * A; - Q_uu_reg = l_uu + B.transpose() * (V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim)) * B; + // Apply regularization to the value function Hessian + Eigen::MatrixXd V_xx_reg = V_xx + regularization_state_ * Eigen::MatrixXd::Identity(state_dim, state_dim); + + // Recompute Q_ux and Q_uu with regularized V_xx + Q_ux_reg = l_ux + B.transpose() * V_xx_reg * A; + Q_uu_reg = l_uu + B.transpose() * V_xx_reg * B; + + // Add hessian terms with regularized V_xx if not using iLQR + if (!options_.is_ilqr) { + for (int i = 0; i < state_dim; ++i) { + Q_ux_reg += timestep_ * V_x(i) * Fux[i]; + Q_uu_reg += timestep_ * V_x(i) * Fuu[i]; + } + } } else {