Calc hessian of potential energy

multibody/plant/multibody_plant.cc

@@ -1287,6 +1287,46 @@ void MultibodyPlant<T>::RegisterRigidBodyWithSceneGraph(
   }
 }
 
+template <typename T>
+MatrixX<T> MultibodyPlant<T>::CalcHessianOfPotentialEnergy(
+    const systems::Context<T>& context) const {
+  // Let's make the matrix of n*n where n is the number of generalized
+  // coordinates.
+  int n = num_positions();
+  MatrixX<T> hessian(n, n);
+  // We will use CalcBiasCenterOfMassTranslationalAcceleration
+  const auto& frame_world = world_frame();
+  VectorX<T> bias_diagonal(n);
+  auto context_copy = context.Clone();
+  auto wrt = JacobianWrtVariable::kV;
+  const Vector3<T>& 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<T>::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<T>::Unit(n, i) + VectorX<T>::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 <typename T>
 void MultibodyPlant<T>::SetFloatingBaseBodyPoseInWorldFrame(
     systems::Context<T>* context, const RigidBody<T>& body,

multibody/plant/multibody_plant.h

@@ -5114,6 +5114,9 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
         context, with_respect_to, frame_A, frame_E);
   }
 
+  MatrixX<T> CalcHessianOfPotentialEnergy(
+      const systems::Context<T>& 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

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 <chrono>
 #include <limits>
 #include <memory>
+#include <string>
 #include <vector>
 
 #include <gmock/gmock.h>
@@ -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<double> plant(0.0);
+  const SpatialInertia<double> link_inertia =
+      SpatialInertia<double>::PointMass(
+          kLinkMass, 0.5 * kLinkLength * Vector3<double>::UnitX());
+
+  std::vector<const RigidBody<double>*> 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<RevoluteJoint>(
+      "Joint0", plant.world_body(), std::nullopt, *bodies[0],
+      math::RigidTransformd(-0.5 * kLinkLength * Vector3<double>::UnitX()),
+      Vector3<double>::UnitZ());
+
+  // Connect each subsequent link.
+  for (int i = 1; i < kNumLinks; ++i) {
+    plant.AddJoint<RevoluteJoint>(
+        "Joint" + std::to_string(i), *bodies[i - 1],
+        math::RigidTransformd(0.5 * kLinkLength * Vector3<double>::UnitX()),
+        *bodies[i],
+        math::RigidTransformd(-0.5 * kLinkLength * Vector3<double>::UnitX()),
+        Vector3<double>::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<double, std::milli>(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<double>::ToAutoDiffXd(plant);
+  const auto& plant_ad =
+      dynamic_cast<const MultibodyPlant<AutoDiffXd>&>(*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<AutoDiffXd> 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<AutoDiffXd> 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<double, std::milli>(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