diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index cc7c986af608..203e0369d3e1 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1287,6 +1287,46 @@ 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..9f405c985925 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -5114,6 +5114,9 @@ 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..7527662c6471 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 @@ -514,6 +516,161 @@ 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)); +} + +// 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 = 7; + 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; // Number of repetitions for timing. + 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