Skip to content

Commit 1f327cf

Browse files
astomodynamicsastomodynamics
authored
Add Second-Order Derivatives and Autodiff Support (#95)
This PR adds support for second-order derivatives (Hessians) and automatic differentiation across the codebase: * Added autodiff library (v1.1.2) as a dependency for automatic differentiation * Refactored state and control Hessian functions to return vectors of matrices instead of single matrices * Added autodiff versions of continuous dynamics functions in all dynamics models * Implemented functions to compute Hessians using autodiff for improved accuracy * Updated core algorithm calculations in ipddp_core.cpp to use second-order information * Added examples demonstrating Hessian calculations for pendulum and Dubins car models * Added comprehensive tests for Hessian calculations and autodiff functionality This refactoring improves accuracy of second-order derivatives while providing a consistent interface across all dynamics models.
1 parent 7562bb4 commit 1f327cf

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

45 files changed

+2301
-263
lines changed

‎CMakeLists.txt

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -93,6 +93,17 @@ if(NOT matplotplusplus_POPULATED)
9393
add_subdirectory(${matplotplusplus_SOURCE_DIR} ${matplotplusplus_BINARY_DIR} EXCLUDE_FROM_ALL)
9494
endif()
9595

96+
# autodiff
97+
set(AUTODIFF_BUILD_TESTS OFF CACHE BOOL "Don't build autodiff tests")
98+
set(AUTODIFF_BUILD_EXAMPLES OFF CACHE BOOL "Don't build autodiff examples")
99+
set(AUTODIFF_BUILD_PYTHON OFF CACHE BOOL "Don't build autodiff Python bindings")
100+
FetchContent_Declare(
101+
autodiff
102+
GIT_REPOSITORY https://github.com/autodiff/autodiff.git
103+
GIT_TAG v1.1.2 # Use a stable version tag instead of main
104+
)
105+
FetchContent_MakeAvailable(autodiff)
106+
96107
# Googletest
97108
if (CDDP_CPP_BUILD_TESTS)
98109
enable_testing()
@@ -256,6 +267,7 @@ add_library(${PROJECT_NAME}
256267
target_link_libraries(${PROJECT_NAME}
257268
Eigen3::Eigen
258269
matplot
270+
autodiff
259271
osqp-cpp
260272
)
261273

‎README.md

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -176,15 +176,6 @@ sudo apt-get install imagemagick # For Ubuntu
176176
brew install imagemagick # For macOS
177177
```
178178

179-
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.
180-
181-
* [OSQP](https://osqp.org/) (QP solver) and [osqp-cpp](https://github.com/google/osqp-cpp) (C++ Wrapper for OSQP)
182-
```bash
183-
conda install -c conda-forge osqp # Optional
184-
```
185-
* [libtorch](https://pytorch.org/get-started/locally/) : This library utilizes Torch for its underlying computations. It will be automatically installed during the build process.
186-
187-
* [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).
188179

189180
### Building
190181
```bash
@@ -215,6 +206,7 @@ This project uses the following open-source libraries:
215206
* [osqp-cpp](https://github.com/google/osqp-cpp) (Apache License 2.0)
216207
* [matplotplusplus](https://github.com/alandefreitas/matplotplusplus) (MIT License)
217208
* [libtorch](https://github.com/pytorch/pytorch) (BSD 3-Clause License)
209+
* [autodiff](https://github.com/autodiff/autodiff) (MIT License)
218210

219211
## Citing
220212
If you use this work in an academic context, please cite this repository.

‎examples/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,9 @@ target_link_libraries(cddp_manipulator cddp)
3737
add_executable(cddp_pendulum cddp_pendulum.cpp)
3838
target_link_libraries(cddp_pendulum cddp)
3939

40+
add_executable(hessian_example hessian_example.cpp)
41+
target_link_libraries(hessian_example cddp)
42+
4043
add_executable(cddp_quadrotor_circle cddp_quadrotor_circle.cpp)
4144
target_link_libraries(cddp_quadrotor_circle cddp)
4245

‎examples/hessian_example.cpp

Lines changed: 182 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,182 @@
1+
/*
2+
Copyright 2024 Tomo Sasaki
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
https://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
*/
16+
17+
#include <iostream>
18+
#include <Eigen/Dense>
19+
#include <vector>
20+
#include <iomanip>
21+
22+
#include "dynamics_model/pendulum.hpp"
23+
#include "dynamics_model/dubins_car.hpp"
24+
25+
using namespace cddp;
26+
27+
// Helper function to print a matrix with a label
28+
void printMatrix(const std::string& label, const Eigen::MatrixXd& matrix) {
29+
std::cout << label << " (" << matrix.rows() << "x" << matrix.cols() << ")" << std::endl;
30+
std::cout << std::fixed << std::setprecision(6) << matrix << std::endl << std::endl;
31+
}
32+
33+
// Helper function to print the Hessian tensor
34+
void printHessianTensor(const std::string& label, const std::vector<Eigen::MatrixXd>& hessian) {
35+
std::cout << label << " (Tensor with " << hessian.size() << " matrices)" << std::endl;
36+
37+
for (size_t i = 0; i < hessian.size(); ++i) {
38+
std::cout << "Matrix for state dimension " << i << " ("
39+
<< hessian[i].rows() << "x" << hessian[i].cols() << "):" << std::endl;
40+
std::cout << std::fixed << std::setprecision(6) << hessian[i] << std::endl;
41+
}
42+
std::cout << std::endl;
43+
}
44+
45+
// Function to demonstrate pendulum Hessian
46+
void demonstratePendulumHessian() {
47+
std::cout << "\n========== PENDULUM MODEL EXAMPLE ==========" << std::endl;
48+
49+
// Create a pendulum model
50+
double timestep = 0.01;
51+
double length = 1.0; // Length of the pendulum [m]
52+
double mass = 1.0; // Mass [kg]
53+
double damping = 0.1; // Damping coefficient
54+
std::string integration = "rk4";
55+
56+
Pendulum pendulum(timestep, length, mass, damping, integration);
57+
58+
// Define a state and control
59+
Eigen::VectorXd state = Eigen::VectorXd::Zero(2);
60+
state << M_PI / 4.0, 0.0; // 45-degree angle, zero velocity
61+
62+
Eigen::VectorXd control = Eigen::VectorXd::Zero(1);
63+
control << 1.0; // Apply a torque of 1 Nm
64+
65+
// Print system information
66+
std::cout << "Pendulum parameters:" << std::endl;
67+
std::cout << "Length: " << pendulum.getLength() << " m" << std::endl;
68+
std::cout << "Mass: " << pendulum.getMass() << " kg" << std::endl;
69+
std::cout << "Damping: " << pendulum.getDamping() << std::endl;
70+
std::cout << "Gravity: " << pendulum.getGravity() << " m/s²" << std::endl;
71+
std::cout << "Timestep: " << pendulum.getTimestep() << " s" << std::endl;
72+
std::cout << "Integration: " << pendulum.getIntegrationType() << std::endl << std::endl;
73+
74+
// Print state and control
75+
std::cout << "State: [theta, theta_dot] = [" << state.transpose() << "]" << std::endl;
76+
std::cout << "Control: [torque] = [" << control.transpose() << "]" << std::endl << std::endl;
77+
78+
// Compute dynamics
79+
Eigen::VectorXd xdot = pendulum.getContinuousDynamics(state, control);
80+
std::cout << "Continuous Dynamics (xdot): [" << xdot.transpose() << "]" << std::endl;
81+
82+
Eigen::VectorXd next_state = pendulum.getDiscreteDynamics(state, control);
83+
std::cout << "Discrete Dynamics (next state): [" << next_state.transpose() << "]" << std::endl << std::endl;
84+
85+
// Compute Jacobians
86+
Eigen::MatrixXd A = pendulum.getStateJacobian(state, control);
87+
Eigen::MatrixXd B = pendulum.getControlJacobian(state, control);
88+
89+
printMatrix("State Jacobian (A)", A);
90+
printMatrix("Control Jacobian (B)", B);
91+
92+
// Compute Hessians
93+
std::vector<Eigen::MatrixXd> state_hessian = pendulum.getStateHessian(state, control);
94+
std::vector<Eigen::MatrixXd> control_hessian = pendulum.getControlHessian(state, control);
95+
std::vector<Eigen::MatrixXd> cross_hessian = pendulum.getCrossHessian(state, control);
96+
97+
printHessianTensor("State Hessian (d²f/dx²)", state_hessian);
98+
printHessianTensor("Control Hessian (d²f/du²)", control_hessian);
99+
printHessianTensor("Cross Hessian (d²f/dudx)", cross_hessian);
100+
101+
// Demonstrate how to access specific elements of the Hessian
102+
// For example, accessing d²theta_dot/dtheta² (second derivative of theta_dot with respect to theta)
103+
int state_idx = 1; // theta_dot is state index 1
104+
int wrt_state_idx1 = 0; // first derivative with respect to theta (index 0)
105+
int wrt_state_idx2 = 0; // second derivative with respect to theta (index 0)
106+
107+
double d2f_dx2 = state_hessian[state_idx](wrt_state_idx1, wrt_state_idx2);
108+
std::cout << "d²theta_dot/dtheta² = " << d2f_dx2 << std::endl;
109+
}
110+
111+
// Function to demonstrate Dubins car Hessian
112+
void demonstrateDubinsCarHessian() {
113+
std::cout << "\n========== DUBINS CAR MODEL EXAMPLE ==========" << std::endl;
114+
115+
// Create a Dubins car model
116+
double speed = 1.0; // Constant forward speed [m/s]
117+
double timestep = 0.01; // Time step [s]
118+
std::string integration = "rk4";
119+
120+
DubinsCar dubins_car(speed, timestep, integration);
121+
122+
// Define a state and control
123+
Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
124+
state << 0.0, 0.0, M_PI / 4.0; // (x, y, theta) = (0, 0, 45°)
125+
126+
Eigen::VectorXd control = Eigen::VectorXd::Zero(1);
127+
control << 0.5; // Turn rate of 0.5 rad/s
128+
129+
// Print system information
130+
std::cout << "Dubins Car parameters:" << std::endl;
131+
std::cout << "Speed: " << speed << " m/s" << std::endl;
132+
std::cout << "Timestep: " << dubins_car.getTimestep() << " s" << std::endl;
133+
std::cout << "Integration: " << dubins_car.getIntegrationType() << std::endl << std::endl;
134+
135+
// Print state and control
136+
std::cout << "State: [x, y, theta] = [" << state.transpose() << "]" << std::endl;
137+
std::cout << "Control: [omega] = [" << control.transpose() << "]" << std::endl << std::endl;
138+
139+
// Compute dynamics
140+
Eigen::VectorXd xdot = dubins_car.getContinuousDynamics(state, control);
141+
std::cout << "Continuous Dynamics (xdot): [" << xdot.transpose() << "]" << std::endl;
142+
143+
Eigen::VectorXd next_state = dubins_car.getDiscreteDynamics(state, control);
144+
std::cout << "Discrete Dynamics (next state): [" << next_state.transpose() << "]" << std::endl << std::endl;
145+
146+
// Compute Jacobians
147+
Eigen::MatrixXd A = dubins_car.getStateJacobian(state, control);
148+
Eigen::MatrixXd B = dubins_car.getControlJacobian(state, control);
149+
150+
printMatrix("State Jacobian (A)", A);
151+
printMatrix("Control Jacobian (B)", B);
152+
153+
// Compute Hessians
154+
std::vector<Eigen::MatrixXd> state_hessian = dubins_car.getStateHessian(state, control);
155+
std::vector<Eigen::MatrixXd> control_hessian = dubins_car.getControlHessian(state, control);
156+
std::vector<Eigen::MatrixXd> cross_hessian = dubins_car.getCrossHessian(state, control);
157+
158+
printHessianTensor("State Hessian (d²f/dx²)", state_hessian);
159+
printHessianTensor("Control Hessian (d²f/du²)", control_hessian);
160+
printHessianTensor("Cross Hessian (d²f/dudx)", cross_hessian);
161+
162+
// Demonstrate how to access specific elements of the Hessian
163+
// For example, accessing d²x/dtheta² (second derivative of x with respect to theta)
164+
int state_idx = 0; // x is state index 0
165+
int wrt_state_idx1 = 2; // first derivative with respect to theta (index 2)
166+
int wrt_state_idx2 = 2; // second derivative with respect to theta (index 2)
167+
168+
double d2f_dx2 = state_hessian[state_idx](wrt_state_idx1, wrt_state_idx2);
169+
std::cout << "d²x/dtheta² = " << d2f_dx2 << std::endl;
170+
}
171+
172+
int main() {
173+
std::cout << "Hessian Examples for Dynamical Systems" << std::endl;
174+
std::cout << "=====================================" << std::endl;
175+
176+
// Demonstrate pendulum Hessian
177+
demonstratePendulumHessian();
178+
179+
// Demonstrate Dubins car Hessian
180+
demonstrateDubinsCarHessian();
181+
return 0;
182+
}

‎include/cddp-cpp/cddp_core/dynamical_system.hpp

Lines changed: 42 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,18 @@
1717
#define CDDP_DYNAMICAL_SYSTEM_HPP
1818

1919
#include <Eigen/Dense>
20+
#include <vector>
2021
#include "cddp_core/helper.hpp"
22+
#include <autodiff/forward/dual.hpp> // Include autodiff (defines dual, dual2nd)
23+
#include <autodiff/forward/dual/eigen.hpp> // Include autodiff Eigen support
2124

2225
namespace cddp {
26+
27+
// Define type alias for second-order dual vectors
28+
using VectorXdual2nd = Eigen::Matrix<autodiff::dual2nd, Eigen::Dynamic, 1>;
29+
// Keep first-order alias for convenience if needed elsewhere, although not strictly necessary now
30+
using VectorXdual = Eigen::Matrix<autodiff::dual, Eigen::Dynamic, 1>;
31+
2332
class DynamicalSystem {
2433
public:
2534

@@ -29,51 +38,65 @@ class DynamicalSystem {
2938

3039
virtual ~DynamicalSystem() {} // Virtual destructor
3140

32-
// Core dynamics function: xdot = f(x_t, u_t)
41+
// Core continuous dynamics function: xdot = f(x_t, u_t)
42+
// This remains virtual, derived classes can implement it.
43+
// The base implementation uses discrete dynamics + finite difference.
3344
virtual Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state,
3445
const Eigen::VectorXd& control) const;
3546

47+
// Autodiff version of continuous dynamics using second-order duals
48+
// Derived classes MUST implement this to use the default autodiff-based derivative functions (getJacobians, getHessians).
49+
// If not overridden, calling functions that depend on it will result in a runtime error.
50+
virtual VectorXdual2nd getContinuousDynamicsAutodiff(const VectorXdual2nd& state,
51+
const VectorXdual2nd& control) const {
52+
throw std::logic_error("getContinuousDynamicsAutodiff must be overridden in the derived class to use Autodiff-based derivatives.");
53+
}
54+
3655
// Discrete dynamics function: x_{t+1} = f(x_t, u_t)
56+
// Uses integration based on getContinuousDynamics
3757
virtual Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd& state,
3858
const Eigen::VectorXd& control) const;
3959

4060
// Jacobian of dynamics w.r.t state: df/dx
61+
4162
virtual Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state,
42-
const Eigen::VectorXd& control) const = 0;
63+
const Eigen::VectorXd& control) const;
4364

4465
// Jacobian of dynamics w.r.t control: df/du
4566
virtual Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state,
46-
const Eigen::VectorXd& control) const = 0;
67+
const Eigen::VectorXd& control) const;
4768

4869
// Jacobians of dynamics w.r.t state and control: df/dx, df/du
4970
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> getJacobians(const Eigen::VectorXd& state,
5071
const Eigen::VectorXd& control) const {
72+
// This can now call the default implementations or overridden ones
5173
return {getStateJacobian(state, control), getControlJacobian(state, control)};
5274
}
5375

54-
// TODO: Add methods for Hessian calculations
5576
// Hessian of dynamics w.r.t state: d^2f/dx^2
56-
// Note: This is a tensor, but we represent it as a matrix for now.
57-
// Each row corresponds to the Hessian for one state dimension
58-
virtual Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state,
59-
const Eigen::VectorXd& control) const = 0;
77+
// Tensor (state_dim x state_dim x state_dim), vector<MatrixXd> (size state_dim)
78+
virtual std::vector<Eigen::MatrixXd> getStateHessian(const Eigen::VectorXd& state,
79+
const Eigen::VectorXd& control) const;
6080

6181
// Hessian of dynamics w.r.t control: d^2f/du^2
62-
// Similar representation as state Hessian
63-
virtual Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state,
64-
const Eigen::VectorXd& control) const = 0;
82+
// Tensor (state_dim x control_dim x control_dim), vector<MatrixXd> (size state_dim)
83+
84+
virtual std::vector<Eigen::MatrixXd> getControlHessian(const Eigen::VectorXd& state,
85+
const Eigen::VectorXd& control) const;
6586

6687
// Hessian of dynamics w.r.t state and control: d^2f/dudx
67-
// Similar representation
68-
virtual Eigen::MatrixXd getCrossHessian(const Eigen::VectorXd& state,
69-
const Eigen::VectorXd& control) const {
70-
return Eigen::MatrixXd::Zero(state.size() * control.size(), state.size());
71-
}
88+
// Tensor (state_dim x control_dim x state_dim), vector<MatrixXd> (size state_dim)
89+
virtual std::vector<Eigen::MatrixXd> getCrossHessian(const Eigen::VectorXd& state,
90+
const Eigen::VectorXd& control) const;
7291

7392
// Hessian of dynamics w.r.t state and control: d^2f/dx^2, d^2f/du^2, d^2f/dudx
74-
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> getHessians(const Eigen::VectorXd& state,
75-
const Eigen::VectorXd& control) const {
76-
return {getStateHessian(state, control), getControlHessian(state, control), getCrossHessian(state, control)};
93+
virtual std::tuple<std::vector<Eigen::MatrixXd>,
94+
std::vector<Eigen::MatrixXd>,
95+
std::vector<Eigen::MatrixXd>> getHessians(const Eigen::VectorXd& state,
96+
const Eigen::VectorXd& control) const {
97+
return {getStateHessian(state, control),
98+
getControlHessian(state, control),
99+
getCrossHessian(state, control)};
77100
}
78101

79102
// Accessor methods

‎include/cddp-cpp/cddp_core/neural_dynamical_system.hpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -97,33 +97,33 @@ class NeuralDynamicalSystem : public DynamicalSystem {
9797
const Eigen::VectorXd& control) const override;
9898

9999
/**
100-
* @brief Hessian of the dynamics w.r.t. state (flattened or block representation).
100+
* @brief Hessian of the dynamics w.r.t. state
101101
*
102102
* @param state Current state (Eigen vector)
103103
* @param control Current control (Eigen vector)
104-
* @return Eigen::MatrixXd Hessian d^2f/dx^2
104+
* @return std::vector<Eigen::MatrixXd> Vector of Hessian matrices, one per state dimension
105105
*/
106-
Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state,
106+
std::vector<Eigen::MatrixXd> getStateHessian(const Eigen::VectorXd& state,
107107
const Eigen::VectorXd& control) const override;
108108

109109
/**
110-
* @brief Hessian of the dynamics w.r.t. control (flattened or block representation).
110+
* @brief Hessian of the dynamics w.r.t. control
111111
*
112112
* @param state Current state (Eigen vector)
113113
* @param control Current control (Eigen vector)
114-
* @return Eigen::MatrixXd Hessian d^2f/du^2
114+
* @return std::vector<Eigen::MatrixXd> Vector of Hessian matrices, one per state dimension
115115
*/
116-
Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state,
116+
std::vector<Eigen::MatrixXd> getControlHessian(const Eigen::VectorXd& state,
117117
const Eigen::VectorXd& control) const override;
118118

119119
/**
120-
* @brief Hessian of the dynamics w.r.t. state and control (flattened or block representation).
120+
* @brief Hessian of the dynamics w.r.t. state and control
121121
*
122122
* @param state Current state (Eigen vector)
123123
* @param control Current control (Eigen vector)
124-
* @return Eigen::MatrixXd Hessian d^2f/dudx
124+
* @return std::vector<Eigen::MatrixXd> Vector of Hessian matrices, one per state dimension
125125
*/
126-
Eigen::MatrixXd getCrossHessian(const Eigen::VectorXd& state,
126+
std::vector<Eigen::MatrixXd> getCrossHessian(const Eigen::VectorXd& state,
127127
const Eigen::VectorXd& control) const override;
128128

129129
private:

0 commit comments

Comments
 (0)