From 3d154a5eaaa27ef71d6f4203a7e3bcaec24035a5 Mon Sep 17 00:00:00 2001 From: sadraddini Date: Sun, 10 May 2026 01:22:57 +0000 Subject: [PATCH 1/4] first pass --- multibody/plant/multibody_plant.cc | 33 ++++++++++++ multibody/plant/multibody_plant.h | 2 + .../test/multibody_plant_kinematics_test.cc | 53 +++++++++++++++++++ 3 files changed, 88 insertions(+) diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index cc7c986af608..8944642c63ae 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1287,6 +1287,39 @@ void MultibodyPlant::RegisterRigidBodyWithSceneGraph( } } +template +MatrixX MultibodyPlant::CalcHessianOfPotentialEnergy(const systems::Context& context) const{ + // Let's make the matrix of n*n where n is the number of generalized coordinates. + int n = num_positions(); + MatrixX hessian(n, n); + // We will use CalcBiasCenterOfMassTranslationalAcceleration + const auto& frame_world = world_frame(); + VectorX bias_diagonal(n); + auto context_copy = context.Clone(); + auto wrt = JacobianWrtVariable::kV; + const Vector3& gravity_vector = this->gravity_field().gravity_vector(); + const T total_mass = CalcTotalMass(context); + for (int i = 0; i < n; ++i) { + SetVelocities(context_copy.get(), VectorX::Unit(n, i)); + auto bias_acc = CalcBiasCenterOfMassTranslationalAcceleration(*context_copy, wrt, frame_world, frame_world); + // Dot product of bias acceleration with gravity vector gives us the second derivative of potential energy with respect to the i-th generalized coordinate. + bias_diagonal(i) = bias_acc.dot(gravity_vector) * total_mass; + hessian(i, i) = bias_diagonal(i); + } + // Now let's do the off-dioagonal terms. + for (int i = 0; i < n; ++i) { + for (int j = i + 1; j < n; ++j) { + SetVelocities(context_copy.get(), VectorX::Unit(n, i) + VectorX::Unit(n, j)); + auto bias_acc = CalcBiasCenterOfMassTranslationalAcceleration(*context_copy, wrt, frame_world, frame_world); + T off_diagonal = bias_acc.dot(gravity_vector) * total_mass; + hessian(i, j) = (off_diagonal - bias_diagonal(i) - bias_diagonal(j)) / 2.0; + hessian(j, i) = hessian(i, j); // Symmetric matrix + } + } + return hessian; +} + + template void MultibodyPlant::SetFloatingBaseBodyPoseInWorldFrame( systems::Context* context, const RigidBody& body, diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 9cae1bb6affa..e358baeca4da 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -5114,6 +5114,8 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { context, with_respect_to, frame_A, frame_E); } + MatrixX CalcHessianOfPotentialEnergy(const systems::Context& context) const; + /// For the system S containing the selected model instances, calculates /// a𝑠Bias_AScm_E, Scm's translational acceleration bias in frame A with /// respect to "speeds" 𝑠, expressed in frame E, where Scm is the center of diff --git a/multibody/plant/test/multibody_plant_kinematics_test.cc b/multibody/plant/test/multibody_plant_kinematics_test.cc index 77c69081538b..451be575a817 100644 --- a/multibody/plant/test/multibody_plant_kinematics_test.cc +++ b/multibody/plant/test/multibody_plant_kinematics_test.cc @@ -514,6 +514,59 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcCenterOfMassAccelerationForwardDynamics) { EXPECT_TRUE(CompareMatrices(a_WScm_W, a_WScm_W_expected, kTolerance)); } +TEST_F(TwoDOFPlanarPendulumTest, CalcHessianOfPotentialEnergy) { + // Verify that the Hessian of potential energy is zero for this test since + // the gravity is in z-direction, and the mechanism is planar in the xy-plane. + const MatrixXd H = plant_.CalcHessianOfPotentialEnergy(*context_); + EXPECT_TRUE(CompareMatrices(H, MatrixXd::Zero(2, 2), kTolerance)); + double V_zero = plant_.CalcPotentialEnergy(*context_); +std::cout << "Potential energy with gravity in z: " << V_zero << std::endl; +EXPECT_TRUE(std::abs(V_zero) < kTolerance); + // Now, let's add gravity. + plant_.mutable_gravity_field().set_gravity_vector(Vector3d(-9.81, 0, 0)); + const MatrixXd H_with_gravity = plant_.CalcHessianOfPotentialEnergy(*context_); + // log the value of the Hessian for debugging purposes. + std::cout << "Hessian of potential energy with gravity:" << std::endl; + for (int i = 0; i < H_with_gravity.rows(); ++i) { + for (int j = 0; j < H_with_gravity.cols(); ++j) { + std::cout << H_with_gravity(i, j) << " "; + } + std::cout << std::endl; + } + // Verify that the Hessian of potential energy is not zero when gravity is present. + EXPECT_FALSE(CompareMatrices(H_with_gravity, MatrixXd::Zero(2, 2), kTolerance)); + // Now let's compare this with finite difference approximation of the Hessian. + const double epsilon = 1e-5; + int n = plant_.num_positions(); + MatrixXd H_fd = MatrixXd::Zero(n, n); + // We need to compute the gradient of CalcGravityGeneralizedForces + const auto q_original = plant_.GetPositions(*context_); + for (int i = 0; i < n; ++i) { + // Perturb the i-th and j-th positions by epsilon. + Eigen::VectorXd q_plus = q_original; + Eigen::VectorXd q_minus = q_original; + q_plus(i) += epsilon; + q_minus(i) -= epsilon; + // Set the perturbed positions in the context. + plant_.SetPositions(context_.get(), q_plus); + Eigen::VectorXd g_plus = plant_.CalcGravityGeneralizedForces(*context_); + // Let's also compute CalcPotentialEnergy for the gravity + plant_.SetPositions(context_.get(), q_minus); + Eigen::VectorXd g_minus = plant_.CalcGravityGeneralizedForces(*context_); + // Compute the finite difference approximation of the Hessian. + H_fd.col(i) = (g_plus - g_minus) / (2 * epsilon); + } + // Print the finite difference Hessian for debugging purposes. + std::cout << "Finite difference Hessian of potential energy with gravity:" << std::endl; + for (int i = 0; i < H_fd.rows(); ++i) { + for (int j = 0; j < H_fd.cols(); ++j) { + std::cout << H_fd(i, j) << " "; + } std::cout << std::endl; + } + // Compare the finite difference Hessian with the analytical Hessian. + EXPECT_TRUE(CompareMatrices(H_with_gravity, H_fd, 2e-3)); +} + } // namespace } // namespace multibody } // namespace drake From 4255dd09ad73e4f48501bfd8fd0337f037d602d6 Mon Sep 17 00:00:00 2001 From: sadraddini Date: Sun, 10 May 2026 01:26:02 +0000 Subject: [PATCH 2/4] clanged --- multibody/plant/multibody_plant.cc | 23 +++-- multibody/plant/multibody_plant.h | 3 +- .../test/multibody_plant_kinematics_test.cc | 91 ++++++++++--------- 3 files changed, 65 insertions(+), 52 deletions(-) diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 8944642c63ae..203e0369d3e1 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1288,8 +1288,10 @@ void MultibodyPlant::RegisterRigidBodyWithSceneGraph( } template -MatrixX MultibodyPlant::CalcHessianOfPotentialEnergy(const systems::Context& context) const{ - // Let's make the matrix of n*n where n is the number of generalized coordinates. +MatrixX MultibodyPlant::CalcHessianOfPotentialEnergy( + const systems::Context& context) const { + // Let's make the matrix of n*n where n is the number of generalized + // coordinates. int n = num_positions(); MatrixX hessian(n, n); // We will use CalcBiasCenterOfMassTranslationalAcceleration @@ -1301,25 +1303,30 @@ MatrixX MultibodyPlant::CalcHessianOfPotentialEnergy(const systems::Contex const T total_mass = CalcTotalMass(context); for (int i = 0; i < n; ++i) { SetVelocities(context_copy.get(), VectorX::Unit(n, i)); - auto bias_acc = CalcBiasCenterOfMassTranslationalAcceleration(*context_copy, wrt, frame_world, frame_world); - // Dot product of bias acceleration with gravity vector gives us the second derivative of potential energy with respect to the i-th generalized coordinate. + auto bias_acc = CalcBiasCenterOfMassTranslationalAcceleration( + *context_copy, wrt, frame_world, frame_world); + // Dot product of bias acceleration with gravity vector gives us the second + // derivative of potential energy with respect to the i-th generalized + // coordinate. bias_diagonal(i) = bias_acc.dot(gravity_vector) * total_mass; hessian(i, i) = bias_diagonal(i); } // Now let's do the off-dioagonal terms. for (int i = 0; i < n; ++i) { for (int j = i + 1; j < n; ++j) { - SetVelocities(context_copy.get(), VectorX::Unit(n, i) + VectorX::Unit(n, j)); - auto bias_acc = CalcBiasCenterOfMassTranslationalAcceleration(*context_copy, wrt, frame_world, frame_world); + SetVelocities(context_copy.get(), + VectorX::Unit(n, i) + VectorX::Unit(n, j)); + auto bias_acc = CalcBiasCenterOfMassTranslationalAcceleration( + *context_copy, wrt, frame_world, frame_world); T off_diagonal = bias_acc.dot(gravity_vector) * total_mass; - hessian(i, j) = (off_diagonal - bias_diagonal(i) - bias_diagonal(j)) / 2.0; + hessian(i, j) = + (off_diagonal - bias_diagonal(i) - bias_diagonal(j)) / 2.0; hessian(j, i) = hessian(i, j); // Symmetric matrix } } return hessian; } - template void MultibodyPlant::SetFloatingBaseBodyPoseInWorldFrame( systems::Context* context, const RigidBody& body, diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index e358baeca4da..9f405c985925 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -5114,7 +5114,8 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { context, with_respect_to, frame_A, frame_E); } - MatrixX CalcHessianOfPotentialEnergy(const systems::Context& context) const; + MatrixX CalcHessianOfPotentialEnergy( + const systems::Context& context) const; /// For the system S containing the selected model instances, calculates /// a𝑠Bias_AScm_E, Scm's translational acceleration bias in frame A with diff --git a/multibody/plant/test/multibody_plant_kinematics_test.cc b/multibody/plant/test/multibody_plant_kinematics_test.cc index 451be575a817..468ce4897356 100644 --- a/multibody/plant/test/multibody_plant_kinematics_test.cc +++ b/multibody/plant/test/multibody_plant_kinematics_test.cc @@ -519,52 +519,57 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcHessianOfPotentialEnergy) { // the gravity is in z-direction, and the mechanism is planar in the xy-plane. const MatrixXd H = plant_.CalcHessianOfPotentialEnergy(*context_); EXPECT_TRUE(CompareMatrices(H, MatrixXd::Zero(2, 2), kTolerance)); - double V_zero = plant_.CalcPotentialEnergy(*context_); -std::cout << "Potential energy with gravity in z: " << V_zero << std::endl; -EXPECT_TRUE(std::abs(V_zero) < kTolerance); + double V_zero = plant_.CalcPotentialEnergy(*context_); + std::cout << "Potential energy with gravity in z: " << V_zero << std::endl; + EXPECT_TRUE(std::abs(V_zero) < kTolerance); // Now, let's add gravity. - plant_.mutable_gravity_field().set_gravity_vector(Vector3d(-9.81, 0, 0)); - const MatrixXd H_with_gravity = plant_.CalcHessianOfPotentialEnergy(*context_); - // log the value of the Hessian for debugging purposes. - std::cout << "Hessian of potential energy with gravity:" << std::endl; - for (int i = 0; i < H_with_gravity.rows(); ++i) { - for (int j = 0; j < H_with_gravity.cols(); ++j) { - std::cout << H_with_gravity(i, j) << " "; - } - std::cout << std::endl; + plant_.mutable_gravity_field().set_gravity_vector(Vector3d(-9.81, 0, 0)); + const MatrixXd H_with_gravity = + plant_.CalcHessianOfPotentialEnergy(*context_); + // log the value of the Hessian for debugging purposes. + std::cout << "Hessian of potential energy with gravity:" << std::endl; + for (int i = 0; i < H_with_gravity.rows(); ++i) { + for (int j = 0; j < H_with_gravity.cols(); ++j) { + std::cout << H_with_gravity(i, j) << " "; } - // Verify that the Hessian of potential energy is not zero when gravity is present. - EXPECT_FALSE(CompareMatrices(H_with_gravity, MatrixXd::Zero(2, 2), kTolerance)); - // Now let's compare this with finite difference approximation of the Hessian. - const double epsilon = 1e-5; - int n = plant_.num_positions(); - MatrixXd H_fd = MatrixXd::Zero(n, n); - // We need to compute the gradient of CalcGravityGeneralizedForces - const auto q_original = plant_.GetPositions(*context_); - for (int i = 0; i < n; ++i) { - // Perturb the i-th and j-th positions by epsilon. - Eigen::VectorXd q_plus = q_original; - Eigen::VectorXd q_minus = q_original; - q_plus(i) += epsilon; - q_minus(i) -= epsilon; - // Set the perturbed positions in the context. - plant_.SetPositions(context_.get(), q_plus); - Eigen::VectorXd g_plus = plant_.CalcGravityGeneralizedForces(*context_); - // Let's also compute CalcPotentialEnergy for the gravity - plant_.SetPositions(context_.get(), q_minus); - Eigen::VectorXd g_minus = plant_.CalcGravityGeneralizedForces(*context_); - // Compute the finite difference approximation of the Hessian. - H_fd.col(i) = (g_plus - g_minus) / (2 * epsilon); - } - // Print the finite difference Hessian for debugging purposes. - std::cout << "Finite difference Hessian of potential energy with gravity:" << std::endl; - for (int i = 0; i < H_fd.rows(); ++i) { - for (int j = 0; j < H_fd.cols(); ++j) { - std::cout << H_fd(i, j) << " "; - } std::cout << std::endl; + std::cout << std::endl; + } + // Verify that the Hessian of potential energy is not zero when gravity is + // present. + EXPECT_FALSE( + CompareMatrices(H_with_gravity, MatrixXd::Zero(2, 2), kTolerance)); + // Now let's compare this with finite difference approximation of the Hessian. + const double epsilon = 1e-5; + int n = plant_.num_positions(); + MatrixXd H_fd = MatrixXd::Zero(n, n); + // We need to compute the gradient of CalcGravityGeneralizedForces + const auto q_original = plant_.GetPositions(*context_); + for (int i = 0; i < n; ++i) { + // Perturb the i-th and j-th positions by epsilon. + Eigen::VectorXd q_plus = q_original; + Eigen::VectorXd q_minus = q_original; + q_plus(i) += epsilon; + q_minus(i) -= epsilon; + // Set the perturbed positions in the context. + plant_.SetPositions(context_.get(), q_plus); + Eigen::VectorXd g_plus = plant_.CalcGravityGeneralizedForces(*context_); + // Let's also compute CalcPotentialEnergy for the gravity + plant_.SetPositions(context_.get(), q_minus); + Eigen::VectorXd g_minus = plant_.CalcGravityGeneralizedForces(*context_); + // Compute the finite difference approximation of the Hessian. + H_fd.col(i) = (g_plus - g_minus) / (2 * epsilon); + } + // Print the finite difference Hessian for debugging purposes. + std::cout << "Finite difference Hessian of potential energy with gravity:" + << std::endl; + for (int i = 0; i < H_fd.rows(); ++i) { + for (int j = 0; j < H_fd.cols(); ++j) { + std::cout << H_fd(i, j) << " "; } - // Compare the finite difference Hessian with the analytical Hessian. - EXPECT_TRUE(CompareMatrices(H_with_gravity, H_fd, 2e-3)); + std::cout << std::endl; + } + // Compare the finite difference Hessian with the analytical Hessian. + EXPECT_TRUE(CompareMatrices(H_with_gravity, H_fd, 2e-3)); } } // namespace From 0bb91a71197f405f97d2a57d1c4bb2f4044d4c9e Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 12 May 2026 00:21:27 +0000 Subject: [PATCH 3/4] Add N-DOF pendulum chain test comparing CalcHessianOfPotentialEnergy with AutoDiff Agent-Logs-Url: https://github.com/sadraddini/drake/sessions/83ff32df-c1a9-4f27-8309-dea7b6d8260e Co-authored-by: sadraddini <26656757+sadraddini@users.noreply.github.com> --- .../test/multibody_plant_kinematics_test.cc | 99 +++++++++++++++++++ 1 file changed, 99 insertions(+) diff --git a/multibody/plant/test/multibody_plant_kinematics_test.cc b/multibody/plant/test/multibody_plant_kinematics_test.cc index 468ce4897356..dfdab5c4f54a 100644 --- a/multibody/plant/test/multibody_plant_kinematics_test.cc +++ b/multibody/plant/test/multibody_plant_kinematics_test.cc @@ -2,8 +2,10 @@ /// This file contains tests for kinematics methods in the MultibodyPlant class. /// There are similar tests in frame_kinematics_test.cc which test /// kinematics methods in the Frame class. +#include #include #include +#include #include #include @@ -572,6 +574,103 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcHessianOfPotentialEnergy) { EXPECT_TRUE(CompareMatrices(H_with_gravity, H_fd, 2e-3)); } +// Test CalcHessianOfPotentialEnergy on an N-DOF chain of planar pendulums and +// compare its result and runtime against an AutoDiff-based computation. +GTEST_TEST(NDOFPendulumHessianTest, CompareMethodAndAutoDiff) { + const int kNumLinks = 10; + const double kLinkLength = 1.0; // m + const double kLinkMass = 1.0; // kg + + // Build an N-DOF chain: each link is a point mass at its distal end, + // connected to the previous link (or world) via a z-axis revolute joint. + MultibodyPlant plant(0.0); + const SpatialInertia link_inertia = + SpatialInertia::PointMass( + kLinkMass, 0.5 * kLinkLength * Vector3::UnitX()); + + std::vector*> bodies; + for (int i = 0; i < kNumLinks; ++i) { + bodies.push_back( + &plant.AddRigidBody("Link" + std::to_string(i), link_inertia)); + } + + // Connect world to the first link at its proximal end. + plant.AddJoint( + "Joint0", plant.world_body(), std::nullopt, *bodies[0], + math::RigidTransformd(-0.5 * kLinkLength * Vector3::UnitX()), + Vector3::UnitZ()); + + // Connect each subsequent link. + for (int i = 1; i < kNumLinks; ++i) { + plant.AddJoint( + "Joint" + std::to_string(i), *bodies[i - 1], + math::RigidTransformd(0.5 * kLinkLength * Vector3::UnitX()), + *bodies[i], + math::RigidTransformd(-0.5 * kLinkLength * Vector3::UnitX()), + Vector3::UnitZ()); + } + + // Use gravity in the x-direction so the planar (x-y) pendulum chain has a + // non-trivial potential-energy Hessian. + plant.mutable_gravity_field().set_gravity_vector(Vector3d(-9.81, 0, 0)); + plant.Finalize(); + + auto context = plant.CreateDefaultContext(); + const int n = plant.num_positions(); + + // Set non-trivial joint angles. + Eigen::VectorXd q = Eigen::VectorXd::LinSpaced(n, 0.1, 0.5); + plant.SetPositions(context.get(), q); + + // ----------------------------------------------------------------------- + // Method 1: CalcHessianOfPotentialEnergy (analytical / bias-acceleration). + // ----------------------------------------------------------------------- + const int kReps = 50; + MatrixXd H_method; + auto t0 = std::chrono::high_resolution_clock::now(); + for (int rep = 0; rep < kReps; ++rep) { + H_method = plant.CalcHessianOfPotentialEnergy(*context); + } + auto t1 = std::chrono::high_resolution_clock::now(); + const double time_method_ms = + std::chrono::duration(t1 - t0).count() / kReps; + + // ----------------------------------------------------------------------- + // Method 2: AutoDiff – differentiate CalcGravityGeneralizedForces w.r.t. q. + // tau_g = -dV/dq => H = d²V/dq² = -d(tau_g)/dq + // ----------------------------------------------------------------------- + auto plant_ad_sys = systems::System::ToAutoDiffXd(plant); + const auto& plant_ad = + dynamic_cast&>(*plant_ad_sys); + auto context_ad = plant_ad.CreateDefaultContext(); + context_ad->SetTimeStateAndParametersFrom(*context); + + // q_ad has values equal to q and gradient equal to the identity matrix. + const VectorX q_ad = math::InitializeAutoDiff(q); + + MatrixXd H_autodiff; + auto t2 = std::chrono::high_resolution_clock::now(); + for (int rep = 0; rep < kReps; ++rep) { + plant_ad.SetPositions(context_ad.get(), q_ad); + const VectorX tau_g_ad = + plant_ad.CalcGravityGeneralizedForces(*context_ad); + H_autodiff = -math::ExtractGradient(tau_g_ad); + } + auto t3 = std::chrono::high_resolution_clock::now(); + const double time_autodiff_ms = + std::chrono::duration(t3 - t2).count() / kReps; + + std::cout << "N=" << kNumLinks << " DOF pendulum chain Hessian benchmark " + << "(averaged over " << kReps << " reps):\n" + << " CalcHessianOfPotentialEnergy : " << time_method_ms + << " ms\n" + << " AutoDiff Hessian : " << time_autodiff_ms + << " ms\n"; + + // The two methods should agree to near machine precision. + EXPECT_TRUE(CompareMatrices(H_method, H_autodiff, 1e-10)); +} + } // namespace } // namespace multibody } // namespace drake From 1489f49a5c5385337acc9eff3c2f0b8990263996 Mon Sep 17 00:00:00 2001 From: sadraddini Date: Tue, 12 May 2026 00:42:22 +0000 Subject: [PATCH 4/4] make the negative Hessian --- multibody/plant/test/multibody_plant_kinematics_test.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/multibody/plant/test/multibody_plant_kinematics_test.cc b/multibody/plant/test/multibody_plant_kinematics_test.cc index dfdab5c4f54a..7527662c6471 100644 --- a/multibody/plant/test/multibody_plant_kinematics_test.cc +++ b/multibody/plant/test/multibody_plant_kinematics_test.cc @@ -577,7 +577,7 @@ TEST_F(TwoDOFPlanarPendulumTest, CalcHessianOfPotentialEnergy) { // Test CalcHessianOfPotentialEnergy on an N-DOF chain of planar pendulums and // compare its result and runtime against an AutoDiff-based computation. GTEST_TEST(NDOFPendulumHessianTest, CompareMethodAndAutoDiff) { - const int kNumLinks = 10; + const int kNumLinks = 7; const double kLinkLength = 1.0; // m const double kLinkMass = 1.0; // kg @@ -625,7 +625,7 @@ GTEST_TEST(NDOFPendulumHessianTest, CompareMethodAndAutoDiff) { // ----------------------------------------------------------------------- // Method 1: CalcHessianOfPotentialEnergy (analytical / bias-acceleration). // ----------------------------------------------------------------------- - const int kReps = 50; + const int kReps = 50; // Number of repetitions for timing. MatrixXd H_method; auto t0 = std::chrono::high_resolution_clock::now(); for (int rep = 0; rep < kReps; ++rep) { @@ -654,7 +654,7 @@ GTEST_TEST(NDOFPendulumHessianTest, CompareMethodAndAutoDiff) { plant_ad.SetPositions(context_ad.get(), q_ad); const VectorX tau_g_ad = plant_ad.CalcGravityGeneralizedForces(*context_ad); - H_autodiff = -math::ExtractGradient(tau_g_ad); + H_autodiff = math::ExtractGradient(tau_g_ad); } auto t3 = std::chrono::high_resolution_clock::now(); const double time_autodiff_ms =