From 3020bbb607afe46b531d625fb25f6be364e5c7f6 Mon Sep 17 00:00:00 2001 From: Tomo Sasaki Date: Tue, 7 Jul 2026 12:31:37 -0400 Subject: [PATCH] fix: use discrete dynamics Jacobians in solvers Differentiate the integrated discrete dynamics map for first-order Jacobians and pass the resulting STM directly through the DDP solvers. Add RK4 regressions that compare getJacobians against finite differences of getDiscreteDynamics and guard against Euler-style rescaling. --- .../cddp-cpp/cddp_core/dynamical_system.hpp | 28 ++-- src/cddp_core/cddp_solver_base.cpp | 6 +- src/cddp_core/clddp_solver.cpp | 5 +- src/cddp_core/dynamical_system.cpp | 136 ++++++++++++++---- src/cddp_core/logddp_solver.cpp | 8 +- src/cddp_core/msipddp_solver.cpp | 24 ++-- src/dynamics_model/pendulum.cpp | 5 +- tests/cddp_core/test_cddp_core.cpp | 66 +++++++++ tests/cddp_core/test_finite_difference.cpp | 38 +++++ tests/dynamics_model/test_pendulum.cpp | 38 ++++- 10 files changed, 287 insertions(+), 67 deletions(-) diff --git a/include/cddp-cpp/cddp_core/dynamical_system.hpp b/include/cddp-cpp/cddp_core/dynamical_system.hpp index 0d886c77..3460e3c6 100644 --- a/include/cddp-cpp/cddp_core/dynamical_system.hpp +++ b/include/cddp-cpp/cddp_core/dynamical_system.hpp @@ -18,8 +18,10 @@ #include "cddp_core/helper.hpp" #include -#include // Include autodiff (defines dual, dual2nd) -#include // Include autodiff Eigen support +#include +#include +#include +#include #include namespace cddp { @@ -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 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 (size @@ -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, @@ -151,4 +157,4 @@ class DynamicalSystem { double time) const; }; } // namespace cddp -#endif // CDDP_DYNAMICAL_SYSTEM_HPP \ No newline at end of file +#endif // CDDP_DYNAMICAL_SYSTEM_HPP diff --git a/src/cddp_core/cddp_solver_base.cpp b/src/cddp_core/cddp_solver_base.cpp index 870a5e46..d1a1e32e 100644 --- a/src/cddp_core/cddp_solver_base.cpp +++ b/src/cddp_core/cddp_solver_base.cpp @@ -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); diff --git a/src/cddp_core/clddp_solver.cpp b/src/cddp_core/clddp_solver.cpp index 1ae342d0..640442fe 100644 --- a/src/cddp_core/clddp_solver.cpp +++ b/src/cddp_core/clddp_solver.cpp @@ -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] = diff --git a/src/cddp_core/dynamical_system.cpp b/src/cddp_core/dynamical_system.cpp index 7821cb19..a13a2ada 100644 --- a/src/cddp_core/dynamical_system.cpp +++ b/src/cddp_core/dynamical_system.cpp @@ -15,16 +15,15 @@ */ #include -#include // Include autodiff -#include // Include autodiff Eigen support +#include +#include #include #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 { @@ -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; } @@ -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 +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; + 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 DynamicalSystem::getStateHessian(const Eigen::VectorXd &state, @@ -142,25 +239,18 @@ DynamicalSystem::getStateHessian(const Eigen::VectorXd &state, 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, 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; @@ -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; @@ -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; diff --git a/src/cddp_core/logddp_solver.cpp b/src/cddp_core/logddp_solver.cpp index 1809d2ac..d6a24f5c 100644 --- a/src/cddp_core/logddp_solver.cpp +++ b/src/cddp_core/logddp_solver.cpp @@ -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); diff --git a/src/cddp_core/msipddp_solver.cpp b/src/cddp_core/msipddp_solver.cpp index 42fe040e..f7d4e54d 100644 --- a/src/cddp_core/msipddp_solver.cpp +++ b/src/cddp_core/msipddp_solver.cpp @@ -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] = @@ -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; @@ -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 + @@ -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 + diff --git a/src/dynamics_model/pendulum.cpp b/src/dynamics_model/pendulum.cpp index 94b20f7d..5577ee1e 100644 --- a/src/dynamics_model/pendulum.cpp +++ b/src/dynamics_model/pendulum.cpp @@ -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; } diff --git a/tests/cddp_core/test_cddp_core.cpp b/tests/cddp_core/test_cddp_core.cpp index d0d37c49..4db968ea 100644 --- a/tests/cddp_core/test_cddp_core.cpp +++ b/tests/cddp_core/test_cddp_core.cpp @@ -170,6 +170,38 @@ class ThrowingJacobianSystem : public DynamicalSystem { } }; +class FixedDiscreteJacobianSystem : public DynamicalSystem { +public: + explicit FixedDiscreteJacobianSystem(double timestep) + : DynamicalSystem(2, 1, timestep, "rk4") {} + + Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd &state, + const Eigen::VectorXd &control, + double time) const override { + return Eigen::VectorXd::Zero(2); + } + + std::tuple + getJacobians(const Eigen::VectorXd &state, const Eigen::VectorXd &control, + double time) const override { + return {stateJacobian(), controlJacobian()}; + } + + static Eigen::MatrixXd stateJacobian() { + Eigen::MatrixXd A(2, 2); + A << 1.2, 0.3, + -0.4, 0.8; + return A; + } + + static Eigen::MatrixXd controlJacobian() { + Eigen::MatrixXd B(2, 1); + B << 0.7, + -0.2; + return B; + } +}; + class ThrowingPrecomputeSolver : public CDDPSolverBase { public: void initialize(CDDP &context) override {} @@ -178,6 +210,9 @@ class ThrowingPrecomputeSolver : public CDDPSolverBase { precomputeDynamicsDerivatives(context, min_horizon_for_parallel); } + const std::vector& stateJacobians() const { return F_x_; } + const std::vector& controlJacobians() const { return F_u_; } + std::string getSolverName() const override { return "ThrowingPrecomputeSolver"; } @@ -459,6 +494,37 @@ TEST_F(CDDPCoreTest, ParallelPrecomputeDynamicsDerivativesPropagatesExceptions) std::runtime_error); } +TEST_F(CDDPCoreTest, PrecomputeDynamicsDerivativesKeepsDiscreteJacobians) { + Eigen::VectorXd local_initial_state = Eigen::VectorXd::Zero(2); + Eigen::VectorXd local_goal_state = Eigen::VectorXd::Zero(2); + cddp::CDDPOptions local_options = options; + local_options.use_ilqr = true; + + cddp::CDDP cddp_solver( + local_initial_state, local_goal_state, 2, timestep, + std::make_unique(timestep), + std::make_unique( + Eigen::MatrixXd::Identity(2, 2), Eigen::MatrixXd::Identity(1, 1), + Eigen::MatrixXd::Identity(2, 2), local_goal_state, + std::vector(), timestep), + local_options); + + cddp_solver.X_.assign(3, Eigen::VectorXd::Zero(2)); + cddp_solver.U_.assign(2, Eigen::VectorXd::Zero(1)); + + cddp::ThrowingPrecomputeSolver solver; + ASSERT_NO_THROW(solver.runPrecomputeDynamicsDerivatives(cddp_solver, 50)); + ASSERT_EQ(solver.stateJacobians().size(), 2); + ASSERT_EQ(solver.controlJacobians().size(), 2); + + for (int t = 0; t < 2; ++t) { + EXPECT_TRUE(solver.stateJacobians()[t].isApprox( + cddp::FixedDiscreteJacobianSystem::stateJacobian(), 1e-12)); + EXPECT_TRUE(solver.controlJacobians()[t].isApprox( + cddp::FixedDiscreteJacobianSystem::controlJacobian(), 1e-12)); + } +} + // Test solver precedence (external over built-in) TEST_F(CDDPCoreTest, SolverPrecedence) { // Register a solver with the same name as a built-in solver diff --git a/tests/cddp_core/test_finite_difference.cpp b/tests/cddp_core/test_finite_difference.cpp index dac78791..2649a1f0 100644 --- a/tests/cddp_core/test_finite_difference.cpp +++ b/tests/cddp_core/test_finite_difference.cpp @@ -67,3 +67,41 @@ TEST(JacobianTest , Pendulum) { std::cout << "A = \n" << A << std::endl; std::cout << "B = \n" << B << std::endl; } + +TEST(JacobianTest, PendulumRK4GetJacobiansUsesDiscreteDynamics) { + const double length = 1.0; + const double mass = 1.0; + const double damping = 0.05; + const double timestep = 0.2; + cddp::Pendulum pendulum(timestep, length, mass, damping, "rk4"); + + Eigen::VectorXd state(2); + state << 0.7, -0.25; + Eigen::VectorXd control(1); + control << 0.3; + + const auto [A, B] = pendulum.getJacobians(state, control, 0.0); + + auto discrete_wrt_x = [&](const Eigen::VectorXd& x) { + return pendulum.getDiscreteDynamics(x, control, 0.0); + }; + auto discrete_wrt_u = [&](const Eigen::VectorXd& u) { + return pendulum.getDiscreteDynamics(state, u, 0.0); + }; + + const Eigen::MatrixXd A_expected = + finite_difference_jacobian(discrete_wrt_x, state); + const Eigen::MatrixXd B_expected = + finite_difference_jacobian(discrete_wrt_u, control); + + Eigen::MatrixXd A_euler = + Eigen::MatrixXd::Identity(state.size(), state.size()) + + timestep * pendulum.getStateJacobian(state, control, 0.0); + Eigen::MatrixXd B_euler = + timestep * pendulum.getControlJacobian(state, control, 0.0); + + EXPECT_TRUE(A.isApprox(A_expected, 1e-10)); + EXPECT_TRUE(B.isApprox(B_expected, 1e-10)); + EXPECT_GT((A - A_euler).norm(), 1e-3); + EXPECT_GT((B - B_euler).norm(), 1e-3); +} diff --git a/tests/dynamics_model/test_pendulum.cpp b/tests/dynamics_model/test_pendulum.cpp index 41492ec2..08b2efe4 100644 --- a/tests/dynamics_model/test_pendulum.cpp +++ b/tests/dynamics_model/test_pendulum.cpp @@ -71,4 +71,40 @@ TEST(PendulumTest, DiscreteDynamics) { double initial_energy = 9.81 * (1.0 + std::cos(M_PI/4)); // mgl(1+cos(theta)) double final_energy = 9.81 * (1.0 + std::cos(theta_data.back())); ASSERT_LT(final_energy, initial_energy); -} \ No newline at end of file +} +TEST(PendulumTest, DiscreteJacobiansMatchFiniteDifference) { + // getJacobians differentiates the discrete RK4 step via autodiff; verify + // it agrees with finite differences of the same discrete map. + double timestep = 0.05; + cddp::Pendulum pendulum(timestep, 1.0, 1.0, 0.1, "rk4"); + + Eigen::VectorXd state(2); + state << M_PI / 3.0, -0.4; + Eigen::VectorXd control(1); + control << 0.7; + + auto [A, B] = pendulum.getJacobians(state, control, 0.0); + + const double h = 1e-6; + Eigen::MatrixXd A_fd(2, 2), B_fd(2, 1); + for (int i = 0; i < 2; ++i) { + Eigen::VectorXd xp = state, xm = state; + xp(i) += h; + xm(i) -= h; + A_fd.col(i) = (pendulum.getDiscreteDynamics(xp, control, 0.0) - + pendulum.getDiscreteDynamics(xm, control, 0.0)) / (2 * h); + } + Eigen::VectorXd up = control, um = control; + up(0) += h; + um(0) -= h; + B_fd.col(0) = (pendulum.getDiscreteDynamics(state, up, 0.0) - + pendulum.getDiscreteDynamics(state, um, 0.0)) / (2 * h); + + ASSERT_LT((A - A_fd).cwiseAbs().maxCoeff(), 1e-7); + ASSERT_LT((B - B_fd).cwiseAbs().maxCoeff(), 1e-7); + // The autodiff Jacobian of the discrete step must differ from the + // continuous-dynamics Jacobian scaled naively (i.e., it is not Euler). + Eigen::MatrixXd A_euler = Eigen::MatrixXd::Identity(2, 2) + + timestep * pendulum.getStateJacobian(state, control, 0.0); + ASSERT_GT((A - A_euler).cwiseAbs().maxCoeff(), 1e-9); +}