Skip to content

Use discrete dynamics Jacobians in solvers #179

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
astomodynamics merged 1 commit into from
Jul 8, 2026
Merged

Use discrete dynamics Jacobians in solvers #179

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 17 additions & 11 deletions include/cddp-cpp/cddp_core/dynamical_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,10 @@

#include "cddp_core/helper.hpp"
#include <Eigen/Dense>
#include <autodiff/forward/dual.hpp> // Include autodiff (defines dual, dual2nd)
#include <autodiff/forward/dual/eigen.hpp> // Include autodiff Eigen support
#include <autodiff/forward/dual.hpp>
#include <autodiff/forward/dual/eigen.hpp>
#include <string>
#include <tuple>
#include <vector>

namespace cddp {
Expand Down Expand Up @@ -66,25 +68,24 @@ class DynamicalSystem {
const Eigen::VectorXd &control,
double time) const;

// Jacobian of dynamics w.r.t state: df/dx
virtual VectorXdual2nd
getDiscreteDynamicsAutodiff(const VectorXdual2nd &state,
const VectorXdual2nd &control, double time) const;

// Jacobian of continuous dynamics w.r.t state: df/dx
virtual Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &state,
const Eigen::VectorXd &control,
double time) const;

// Jacobian of dynamics w.r.t control: df/du
// Jacobian of continuous dynamics w.r.t control: df/du
virtual Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd &state,
const Eigen::VectorXd &control,
double time) const;

// Jacobians of dynamics w.r.t state and control: df/dx, df/du
// Jacobians of discrete dynamics w.r.t state and control: dF/dx, dF/du.
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd>
getJacobians(const Eigen::VectorXd &state, const Eigen::VectorXd &control,
double time) const {
// This can now call the default implementations or overridden ones
return {getStateJacobian(state, control, time),
getControlJacobian(state, control, time)};
}
double time) const;

// Hessian of dynamics w.r.t state: d^2f/dx^2
// Tensor (state_dim x state_dim x state_dim), vector<MatrixXd> (size
Expand Down Expand Up @@ -136,6 +137,11 @@ class DynamicalSystem {
double timestep_;
std::string integration_type_; // Integration type: Euler, Heun, RK3, RK4

private:
// 0 = unknown, 1 = autodiff, -1 = finite differences.
mutable int discrete_autodiff_status_ = 0;

protected:
// Integration step functions
Eigen::VectorXd euler_step(const Eigen::VectorXd &state,
const Eigen::VectorXd &control, double dt,
Expand All @@ -151,4 +157,4 @@ class DynamicalSystem {
double time) const;
};
} // namespace cddp
#endif // CDDP_DYNAMICAL_SYSTEM_HPP
#endif // CDDP_DYNAMICAL_SYSTEM_HPP
6 changes: 2 additions & 4 deletions src/cddp_core/cddp_solver_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -338,10 +338,8 @@ void CDDPSolverBase::precomputeDynamicsDerivatives(
double time = t * context.getTimestep();

auto [Fx, Fu] = context.getSystem().getJacobians(x, u, time);
// Convert to discrete time
F_x_[t] = context.getTimestep() * Fx;
F_x_[t].diagonal().array() += 1.0;
F_u_[t] = context.getTimestep() * Fu;
F_x_[t] = Fx;
F_u_[t] = Fu;

if (!options.use_ilqr) {
auto [Fxx, Fuu, Fux] = context.getSystem().getHessians(x, u, time);
Expand Down
5 changes: 2 additions & 3 deletions src/cddp_core/clddp_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,9 +113,8 @@ bool CLDDPSolver::backwardPass(CDDP &context) {
const auto [Fx, Fu] =
context.getSystem().getJacobians(x, u, t * context.getTimestep());

A = context.getTimestep() * Fx;
A.diagonal().array() += 1.0;
B = context.getTimestep() * Fu;
A = Fx;
B = Fu;

auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t);
auto [l_xx, l_uu, l_ux] =
Expand Down
136 changes: 112 additions & 24 deletions src/cddp_core/dynamical_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,16 +15,15 @@
*/

#include <Eigen/Dense>
#include <autodiff/forward/dual.hpp> // Include autodiff
#include <autodiff/forward/dual/eigen.hpp> // Include autodiff Eigen support
#include <autodiff/forward/dual.hpp>
#include <autodiff/forward/dual/eigen.hpp>
#include <iostream>

#include "cddp_core/dynamical_system.hpp"

using namespace cddp;
using namespace autodiff; // Use autodiff namespace
using namespace autodiff;

// Implement integration methods
Eigen::VectorXd DynamicalSystem::euler_step(const Eigen::VectorXd &state,
const Eigen::VectorXd &control,
double dt, double time) const {
Expand Down Expand Up @@ -86,33 +85,23 @@ Eigen::VectorXd
DynamicalSystem::getContinuousDynamics(const Eigen::VectorXd &state,
const Eigen::VectorXd &control,
double time) const {

// Get next state using discrete dynamics
Eigen::VectorXd next_state = getDiscreteDynamics(state, control, time);

// Compute continuous dynamics using finite difference
// dx/dt ≈ (x_{k+1} - x_k) / dt
Eigen::VectorXd continuous_dynamics = (next_state - state) / timestep_;

return continuous_dynamics;
}

// --- Autodiff Default Implementations for Jacobians ---

Eigen::MatrixXd
DynamicalSystem::getStateJacobian(const Eigen::VectorXd &state,
const Eigen::VectorXd &control,
double time) 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, time);
};

// Compute Jacobian w.r.t. state
Eigen::MatrixXd Jx = jacobian(dynamics_wrt_x, wrt(x), at(x));
return Jx;
}
Expand All @@ -132,7 +121,115 @@ DynamicalSystem::getControlJacobian(const Eigen::VectorXd &state,
return Ju;
}

// --- Autodiff Default Implementations for Hessians ---
VectorXdual2nd
DynamicalSystem::getDiscreteDynamicsAutodiff(const VectorXdual2nd &state,
const VectorXdual2nd &control,
double time) const {
const double dt = timestep_;
if (integration_type_ == "euler") {
return state + dt * getContinuousDynamicsAutodiff(state, control, time);
} else if (integration_type_ == "heun") {
VectorXdual2nd k1 = getContinuousDynamicsAutodiff(state, control, time);
VectorXdual2nd k2 =
getContinuousDynamicsAutodiff(state + dt * k1, control, time + dt);
return state + 0.5 * dt * (k1 + k2);
} else if (integration_type_ == "rk3") {
VectorXdual2nd k1 = getContinuousDynamicsAutodiff(state, control, time);
VectorXdual2nd k2 = getContinuousDynamicsAutodiff(state + 0.5 * dt * k1,
control, time + 0.5 * dt);
VectorXdual2nd k3 = getContinuousDynamicsAutodiff(
state - dt * k1 + 2 * dt * k2, control, time + dt);
return state + (dt / 6) * (k1 + 4 * k2 + k3);
} else if (integration_type_ == "rk4") {
VectorXdual2nd k1 = getContinuousDynamicsAutodiff(state, control, time);
VectorXdual2nd k2 = getContinuousDynamicsAutodiff(state + 0.5 * dt * k1,
control, time + 0.5 * dt);
VectorXdual2nd k3 = getContinuousDynamicsAutodiff(state + 0.5 * dt * k2,
control, time + 0.5 * dt);
VectorXdual2nd k4 =
getContinuousDynamicsAutodiff(state + dt * k3, control, time + dt);
return state + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4);
}
throw std::runtime_error("Integration type not supported for autodiff "
"discrete dynamics: " +
integration_type_);
}

std::tuple<Eigen::MatrixXd, Eigen::MatrixXd>
DynamicalSystem::getJacobians(const Eigen::VectorXd &state,
const Eigen::VectorXd &control,
double time) const {
if (discrete_autodiff_status_ == 0) {
try {
VectorXdual2nd x = state;
VectorXdual2nd u = control;
VectorXdual2nd next_ad = getDiscreteDynamicsAutodiff(x, u, time);
Eigen::VectorXd next_ad_val(next_ad.size());
for (int i = 0; i < next_ad.size(); ++i) {
next_ad_val(i) = val(next_ad(i));
}
const Eigen::VectorXd next = getDiscreteDynamics(state, control, time);
const double value_scale = 1.0 + next.cwiseAbs().maxCoeff();
bool consistent =
(next_ad_val - next).cwiseAbs().maxCoeff() <= 1e-9 * value_scale;

if (consistent) {
auto fd_wrt_x = [&](const Eigen::VectorXd &x_in) {
return getDiscreteDynamics(x_in, control, time);
};
auto fd_wrt_u = [&](const Eigen::VectorXd &u_in) {
return getDiscreteDynamics(state, u_in, time);
};
auto ad_wrt_x = [&](const VectorXdual2nd &x_in) -> VectorXdual2nd {
return getDiscreteDynamicsAutodiff(x_in, u, time);
};
auto ad_wrt_u = [&](const VectorXdual2nd &u_in) -> VectorXdual2nd {
return getDiscreteDynamicsAutodiff(x, u_in, time);
};
const Eigen::MatrixXd A_ad = jacobian(ad_wrt_x, wrt(x), at(x));
const Eigen::MatrixXd B_ad = jacobian(ad_wrt_u, wrt(u), at(u));
const Eigen::MatrixXd A_fd = finite_difference_jacobian(fd_wrt_x, state);
const Eigen::MatrixXd B_fd = finite_difference_jacobian(fd_wrt_u, control);
const double jac_scale = 1.0 + std::max(A_fd.cwiseAbs().maxCoeff(),
B_fd.cwiseAbs().maxCoeff());
consistent = (A_ad - A_fd).cwiseAbs().maxCoeff() <= 1e-5 * jac_scale &&
(B_ad - B_fd).cwiseAbs().maxCoeff() <= 1e-5 * jac_scale;
}
discrete_autodiff_status_ = consistent ? 1 : -1;

@chatgpt-codex-connector chatgpt-codex-connector Bot Jul 7, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 Badge Synchronize the autodiff status cache

When enable_parallel is set, the solver precompute paths call getJacobians() from multiple std::async workers on the same DynamicalSystem instance. This new mutable cache is read and then written during first-use initialization without an atomic, mutex, or std::once_flag, so the first parallel backward pass can data-race while deciding between autodiff and finite-difference Jacobians; that is undefined behavior and can produce inconsistent derivative paths or be flagged by thread sanitizers.

Useful? React with 👍 / 👎.

if (!consistent) {
std::cerr << "Warning: autodiff discrete dynamics disagree with "
"getDiscreteDynamics; falling back to finite-difference "
"Jacobians. Check the model's autodiff implementation."
<< std::endl;
}
} catch (const std::exception &) {
discrete_autodiff_status_ = -1;
}
}

if (discrete_autodiff_status_ == 1) {
VectorXdual2nd x = state;
VectorXdual2nd u = control;
auto discrete_wrt_x = [&](const VectorXdual2nd &x_ad) -> VectorXdual2nd {
return getDiscreteDynamicsAutodiff(x_ad, u, time);
};
auto discrete_wrt_u = [&](const VectorXdual2nd &u_ad) -> VectorXdual2nd {
return getDiscreteDynamicsAutodiff(x, u_ad, time);
};
return {jacobian(discrete_wrt_x, wrt(x), at(x)),
jacobian(discrete_wrt_u, wrt(u), at(u))};
}

auto dynamics_wrt_x = [&](const Eigen::VectorXd &x) {
return getDiscreteDynamics(x, control, time);
};
auto dynamics_wrt_u = [&](const Eigen::VectorXd &u) {
return getDiscreteDynamics(state, u, time);
};

return {finite_difference_jacobian(dynamics_wrt_x, state),
finite_difference_jacobian(dynamics_wrt_u, control)};
}

std::vector<Eigen::MatrixXd>
DynamicalSystem::getStateHessian(const Eigen::VectorXd &state,
Expand All @@ -142,25 +239,18 @@ DynamicalSystem::getStateHessian(const Eigen::VectorXd &state,
int m = control_dim_;
std::vector<Eigen::MatrixXd> 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, time)(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;
Expand All @@ -185,7 +275,6 @@ DynamicalSystem::getControlHessian(const Eigen::VectorXd &state,
return this->getContinuousDynamicsAutodiff(x_ad, u_ad, time)(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;
Expand All @@ -210,7 +299,6 @@ DynamicalSystem::getCrossHessian(const Eigen::VectorXd &state,
return this->getContinuousDynamicsAutodiff(x_ad, u_ad, time)(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;
Expand Down
8 changes: 2 additions & 6 deletions src/cddp_core/logddp_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -483,12 +483,8 @@ bool LogDDPSolver::backwardPass(CDDP &context) {
const Eigen::VectorXd &x = context.X_[t];
const Eigen::VectorXd &u = context.U_[t];

// Use pre-computed continuous-time dynamics Jacobians
const Eigen::MatrixXd &Fx = F_x_[t];
const Eigen::MatrixXd &Fu = F_u_[t];
const Eigen::MatrixXd &A =
timestep * Fx + Eigen::MatrixXd::Identity(state_dim, state_dim);
const Eigen::MatrixXd &B = timestep * Fu;
const Eigen::MatrixXd &A = F_x_[t];
const Eigen::MatrixXd &B = F_u_[t];

// Cost derivatives at (x_t, u_t)
auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t);
Expand Down
24 changes: 8 additions & 16 deletions src/cddp_core/msipddp_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1125,13 +1125,10 @@ namespace cddp
d = f - context.X_[t + 1];
}

const Eigen::MatrixXd &Fx = F_x_[t];
const Eigen::MatrixXd &Fu = F_u_[t];

Eigen::MatrixXd &A = workspace_.A_matrices[t];
Eigen::MatrixXd &B = workspace_.B_matrices[t];
A.noalias() = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep * Fx;
B.noalias() = timestep * Fu;
A = F_x_[t];
B = F_u_[t];

auto [l_x, l_u] = context.getObjective().getRunningCostGradients(x, u, t);
auto [l_xx, l_uu, l_ux] =
Expand Down Expand Up @@ -1235,13 +1232,10 @@ namespace cddp
d = f - context.X_[t + 1];
}

const Eigen::MatrixXd &Fx = F_x_[t];
const Eigen::MatrixXd &Fu = F_u_[t];

Eigen::MatrixXd &A = workspace_.A_matrices[t];
Eigen::MatrixXd &B = workspace_.B_matrices[t];
A.noalias() = Eigen::MatrixXd::Identity(state_dim, state_dim) + timestep * Fx;
B.noalias() = timestep * Fu;
A = F_x_[t];
B = F_u_[t];

Eigen::VectorXd &y = workspace_.y_combined;
Eigen::VectorXd &s = workspace_.s_combined;
Expand Down Expand Up @@ -1493,9 +1487,8 @@ namespace cddp
{
const auto [Fx, Fu] = context.getSystem().getJacobians(
context.X_[t], context.U_[t], t * context.getTimestep());
const double timestep = context.getTimestep();
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(context.getStateDim(), context.getStateDim()) + timestep * Fx;
Eigen::MatrixXd B = timestep * Fu;
Eigen::MatrixXd A = Fx;
Eigen::MatrixXd B = Fu;

result.state_trajectory[t + 1] = context.X_[t + 1] +
(A + B * K_u_[t]) * delta_x +
Expand Down Expand Up @@ -1591,9 +1584,8 @@ namespace cddp
{
const auto [Fx, Fu] = context.getSystem().getJacobians(
context.X_[t], context.U_[t], t * context.getTimestep());
const double timestep = context.getTimestep();
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(context.getStateDim(), context.getStateDim()) + timestep * Fx;
Eigen::MatrixXd B = timestep * Fu;
Eigen::MatrixXd A = Fx;
Eigen::MatrixXd B = Fu;

result.state_trajectory[t + 1] = context.X_[t + 1] +
(A + B * K_u_[t]) * delta_x +
Expand Down
5 changes: 3 additions & 2 deletions src/dynamics_model/pendulum.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,8 +93,9 @@ VectorXdual2nd Pendulum::getContinuousDynamicsAutodiff(
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
state_dot(STATE_THETA_DOT) =
(torque - damping_ * theta_dot + mass_ * gravity_ * length_ * sin(theta)) /
inertia;

return state_dot;
}
Expand Down
Loading
Loading