Inverse Dynamics in M proto + cassiebench test

Author: sherm1

multibody/benchmarking/cassie.cc

@@ -146,6 +146,17 @@ class Cassie : public benchmark::Fixture {
     }
   }
 
+  // Runs the InverseDynamicsInM benchmark.
+  // NOLINTNEXTLINE(runtime/references)
+  void DoInverseDynamicsInM(benchmark::State& state) {
+    DRAKE_DEMAND(want_grad_u(state) == false);
+    for (auto _ : state) {
+      InvalidateState();
+      plant_->CalcInverseDynamicsInM(*context_, desired_vdot_,
+                                     external_forces_);
+    }
+  }
+
   // Runs the ForwardDynamics benchmark.
   // NOLINTNEXTLINE(runtime/references)
   void DoForwardDynamics(benchmark::State& state) {
@@ -327,6 +338,14 @@ BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics)
   ->Unit(benchmark::kMicrosecond)
   ->Arg(kWantNoGrad);
 
+// NOLINTNEXTLINE(runtime/references)
+BENCHMARK_DEFINE_F(CassieDouble, InverseDynamicsInM)(benchmark::State& state) {
+  DoInverseDynamicsInM(state);
+}
+BENCHMARK_REGISTER_F(CassieDouble, InverseDynamicsInM)
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
+
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) {
   DoForwardDynamics(state);
@@ -383,6 +402,22 @@ BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics)
   ->Arg(kWantGradV|kWantGradVdot)
   ->Arg(kWantGradX|kWantGradVdot);
 
+BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamicsInM)
+// NOLINTNEXTLINE(runtime/references)
+(benchmark::State& state) {
+  DoInverseDynamicsInM(state);
+}
+BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamicsInM)
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad)
+    ->Arg(kWantGradQ)
+    ->Arg(kWantGradV)
+    ->Arg(kWantGradX)
+    ->Arg(kWantGradVdot)
+    ->Arg(kWantGradQ|kWantGradVdot)
+    ->Arg(kWantGradV|kWantGradVdot)
+    ->Arg(kWantGradX|kWantGradVdot);
+
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieAutoDiff, ForwardDynamics)(benchmark::State& state) {
   DoForwardDynamics(state);

multibody/math/spatial_acceleration.h

@@ -134,7 +134,7 @@ class SpatialAcceleration : public SpatialVector<SpatialAcceleration, T> {
     const Vector3<T>& alpha_MB_E = this->rotational();
     // Calculate point Co's translational acceleration measured in M.
     Vector3<T>& a_MCo_E = this->translational();
-    a_MCo_E += (alpha_MB_E.cross(p_BoCo_E)
+    a_MCo_E += (alpha_MB_E.cross(p_BoCo_E)  // 33 flops total
             +   w_MB_E.cross(w_MB_E.cross(p_BoCo_E)));
   }
 

multibody/plant/multibody_plant.h

@@ -4036,6 +4036,14 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
                                                external_forces);
   }
 
+  VectorX<T> CalcInverseDynamicsInM(
+      const systems::Context<T>& context, const VectorX<T>& known_vdot,
+      const MultibodyForces<T>& external_forces) const {
+    this->ValidateContext(context);
+    return internal_tree().CalcInverseDynamicsInM(context, known_vdot,
+                                                  external_forces);
+  }
+
 #ifdef DRAKE_DOXYGEN_CXX
   // MultibodyPlant uses the NVI implementation of
   // CalcImplicitTimeDerivativesResidual from

multibody/plant/test/external_forces_test.cc

@@ -1,14 +1,12 @@
 #include <limits>
 
-#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
-#include "drake/common/autodiff.h"
+#include "drake/common/fmt_eigen.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/limit_malloc.h"
 #include "drake/multibody/plant/test/kuka_iiwa_model_tests.h"
 #include "drake/multibody/tree/frame.h"
-#include "drake/multibody/tree/rigid_body.h"
 
 namespace drake {
 
@@ -41,6 +39,9 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
                                  end_effector_link_->body_frame(), &forces);
   const VectorX<double> tau_id =
       plant_->CalcInverseDynamics(*context_, vdot, forces);
+  const VectorX<double> tau_id_in_m =
+      plant_->CalcInverseDynamicsInM(*context_, vdot, forces);
+
   {  // Repeat the computation to confirm the heap behavior.  We allow the
     // method to heap-allocate 2 temporaries and 1 return value.
     drake::test::LimitMalloc guard({.max_num_allocations = 3});
@@ -75,6 +76,10 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
   const double kTolerance = 50 * std::numeric_limits<double>::epsilon();
   EXPECT_TRUE(CompareMatrices(tau_id, tau_id_expected, kTolerance,
                               MatrixCompareType::relative));
+  // W & M frame ID should be very close.
+  EXPECT_TRUE(CompareMatrices(tau_id_in_m, tau_id,
+                              16 * std::numeric_limits<double>::epsilon(),
+                              MatrixCompareType::relative));
 }
 
 TEST_F(KukaIiwaModelTests, BodyForceApi) {

multibody/tree/body_node.h

@@ -242,6 +242,10 @@ class BodyNode : public MultibodyElement<T> {
       const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
       PositionKinematicsCache<T>* pc) const = 0;
 
+  virtual void CalcPositionKinematicsCacheInM_BaseToTip(
+      const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
+      PositionKinematicsCacheInM<T>* pcm) const = 0;
+
   // Calculates the hinge matrix H_PB_W, the `6 x nm` hinge matrix that relates
   // V_PB_W`(body B's spatial velocity in its parent body P, expressed in world
   // W) to this node's nm generalized velocities (or mobilities) v_B as
@@ -291,6 +295,10 @@ class BodyNode : public MultibodyElement<T> {
       const std::vector<Vector6<T>>& H_PB_W_cache, const T* velocities,
       VelocityKinematicsCache<T>* vc) const = 0;
 
+  virtual void CalcVelocityKinematicsCacheInM_BaseToTip(
+      const T* positions, const PositionKinematicsCacheInM<T>& pcm,
+      const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const = 0;
+
   // The CalcMassMatrix() algorithm invokes this on each body k, serving
   // as the composite body R(k) in the outer loop of Jain's algorithm 9.3.
   // This node must fill in its nv x nv diagonal block in M, and then
@@ -374,6 +382,12 @@ class BodyNode : public MultibodyElement<T> {
       const VelocityKinematicsCache<T>* vc, const T* accelerations,
       std::vector<SpatialAcceleration<T>>* A_WB_array) const = 0;
 
+  virtual void CalcSpatialAccelerationInM_BaseToTip(
+      const T* positions, const PositionKinematicsCacheInM<T>& pcm,
+      const T* velocities, const VelocityKinematicsCacheInM<T>& vcm,
+      const T* accelerations,
+      std::vector<SpatialAcceleration<T>>* A_WM_M_array) const = 0;
+
   // Computes the generalized forces `tau` for a single BodyNode.
   // This method is used by MultibodyTree within a tip-to-base loop to compute
   // the vector of generalized forces `tau` that would correspond with a known
@@ -433,6 +447,17 @@ class BodyNode : public MultibodyElement<T> {
       std::vector<SpatialForce<T>>* F_BMo_W_array,
       EigenPtr<VectorX<T>> tau_array) const = 0;
 
+  virtual void CalcInverseDynamicsInM_TipToBase(
+      const FrameBodyPoseCache<T>& frame_body_pose_cache,  // M_BMo_M, X_BM
+      const T* positions,
+      const PositionKinematicsCacheInM<T>& pc,  // X_MpM, X_WB
+      const VelocityKinematicsCacheInM<T>& vc,  // V_WM_M
+      const std::vector<SpatialAcceleration<T>>& A_WM_M_array,
+      const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array,  // Bo, W !
+      const Eigen::Ref<const VectorX<T>>& tau_applied_array,
+      std::vector<SpatialForce<T>>* F_BMo_M_array,
+      EigenPtr<VectorX<T>> tau_array) const = 0;
+
   // This method is used by MultibodyTree within a tip-to-base loop to compute
   // this node's articulated body inertia quantities that depend only on the
   // generalized positions.

multibody/tree/body_node_impl.cc

@@ -119,6 +119,41 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcPositionKinematicsCache_BaseToTip(
   p_PoBo_W = R_WP * p_PoBo_P;
 }
 
+// Calculate and save X_FM, X_MpM, X_WM
+template <typename T, class ConcreteMobilizer>
+void BodyNodeImpl<T, ConcreteMobilizer>::
+    CalcPositionKinematicsCacheInM_BaseToTip(
+        const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
+        PositionKinematicsCacheInM<T>* pcm) const {
+  DRAKE_ASSERT(pcm != nullptr);
+  const MobodIndex index = mobod_index();
+  DRAKE_ASSERT(index != world_mobod_index());
+
+  const T* q = get_q(positions);
+
+  // Calculate and store X_FM.
+  math::RigidTransform<T>& X_FM = pcm->get_mutable_X_FM(mobod_index());
+  X_FM = mobilizer_->calc_X_FM(q);
+
+  // Get precalculated frame info.
+  // This mobilizer's inboard frame, fixed on inboard (parent) body.
+  const FrameIndex index_F = inboard_frame().index();
+  const bool X_MpF_is_identity =
+      frame_body_pose_cache.is_X_MbF_identity(index_F);
+  const math::RigidTransform<T>& X_MpF =
+      frame_body_pose_cache.get_X_MbF(index_F);
+
+  // Calculate and store X_MpM.
+  math::RigidTransform<T>& X_MpM = pcm->get_mutable_X_MpM(mobod_index());
+  X_MpM = X_MpF_is_identity ? X_FM : X_MpF * X_FM;  // 0 or 63 flops
+
+  // Calculate and store X_WM.
+  // Inboard's M frame (Mp) pose is known since we're going base to tip.
+  const math::RigidTransform<T>& X_WMp = pcm->get_X_WM(inboard_mobod_index());
+  math::RigidTransform<T>& X_WM = pcm->get_mutable_X_WM(mobod_index());
+  X_WM = X_WMp * X_MpM;  // 63 flops
+}
+
 // TODO(sherm1) Consider combining this with VelocityCache computation
 //  so that we don't have to make a separate pass. Or better, get rid of this
 //  computation altogether by working in better frames.
@@ -282,6 +317,43 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcVelocityKinematicsCache_BaseToTip(
   get_mutable_V_WB(vc) = V_WP.ComposeWithMovingFrameVelocity(p_PB_W, V_PB_W);
 }
 
+// Calculate V_FM_M and V_WM_M.
+template <typename T, class ConcreteMobilizer>
+void BodyNodeImpl<T, ConcreteMobilizer>::
+    CalcVelocityKinematicsCacheInM_BaseToTip(
+        const T* positions, const PositionKinematicsCacheInM<T>& pcm,
+        const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const {
+  DRAKE_ASSERT(vcm != nullptr);
+  const MobodIndex index = mobod_index();
+  DRAKE_ASSERT(index != world_mobod_index());
+
+  // Generalized coordinates local to this node's mobilizer.
+  const T* q = get_q(positions);
+  const T* v = get_v(velocities);
+
+  const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
+
+  SpatialVelocity<T>& V_FM_M = vcm->get_mutable_V_FM_M(index);
+  V_FM_M = mobilizer_->calc_V_FM_M(X_FM, q, v);  // 3 flops: revolute/prismatic
+                                                 // 30 flops: floating
+
+  const math::RigidTransform<T>& X_MpM = pcm.get_X_MpM(index);
+  const Vector3<T>& p_MpM_Mp = X_MpM.translation();  // Shift vector
+  const math::RotationMatrix<T> R_MMp = X_MpM.rotation().inverse();
+
+  // We're going base to tip so we know the inboard body's M-frame velocity.
+  const SpatialVelocity<T>& V_WMp_Mp = vcm->get_V_WM_M(inboard_mobod_index());
+
+  // Let Mc be a frame fixed to parent body P but coincident with the child
+  // body's M frame. Then this is the spatial velocity of P, shifted to Mc,
+  // but expressed in the Mp frame.
+  const SpatialVelocity<T> V_WMc_Mp = V_WMp_Mp.Shift(p_MpM_Mp);  // 15 flops
+
+  SpatialVelocity<T>& V_WM_M = vcm->get_mutable_V_WM_M(index);
+  V_WM_M = R_MMp * V_WMc_Mp  // 6 SIMD flops
+           + V_FM_M;         // 6 flops
+}
+
 // As a guideline for developers, a summary of the computations performed in
 // this method is provided:
 // Notation:
@@ -411,6 +483,60 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAcceleration_BaseToTip(
                                                  V_PB_W, A_PB_W);
 }
 
+template <typename T, class ConcreteMobilizer>
+void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAccelerationInM_BaseToTip(
+    const T* positions, const PositionKinematicsCacheInM<T>& pcm,
+    const T* velocities, const VelocityKinematicsCacheInM<T>& vcm,
+    const T* accelerations,
+    std::vector<SpatialAcceleration<T>>* A_WM_M_array) const {
+  const MobodIndex index = mobod_index();  // mobod B
+
+  // Collect the inputs.
+  const math::RigidTransform<T>& X_MpM = pcm.get_X_MpM(index);
+  const SpatialVelocity<T>& V_WM_M = vcm.get_V_WM_M(index);
+  const T* q = get_q(positions);
+  const T* v = get_v(velocities);
+  const T* vdot = get_v(accelerations);
+  // Inboard (parent) body's M frame spatial velocity and spatial acceleration
+  // in World (already computed in base-to-tip ordering).
+  const SpatialVelocity<T>& V_WMp_Mp = vcm.get_V_WM_M(inboard_mobod_index());
+  const SpatialAcceleration<T>& A_WMp_Mp =
+      (*A_WM_M_array)[inboard_mobod_index()];
+
+  // This is going to be the output.
+  SpatialAcceleration<T>& A_WM_M = (*A_WM_M_array)[index];
+
+  // Shift and re-express parent contribution.
+  const Vector3<T>& p_MpM_Mp = X_MpM.translation();
+  const math::RotationMatrix<T> R_MMp = X_MpM.rotation().inverse();
+  const Vector3<T>& w_WP_Mp = V_WMp_Mp.rotational();  // all frames on P same ω
+
+  // Start with parent contribution. shift: 33 flops, reexpress: 6 SIMD flops
+  A_WM_M = R_MMp * A_WMp_Mp.Shift(p_MpM_Mp, w_WP_Mp);  // parent contribution
+
+  // Next add in the velocity-dependent bias acceleration:
+  // Ab_M =     ω_WP x ω_FM        note: ω_WF == ω_WP
+  //        2 * ω_WP x v_FM
+  const SpatialVelocity<T>& V_FM_M = vcm.get_V_FM_M(index);
+  const Vector3<T>& w_FM_M = V_FM_M.rotational();
+  const Vector3<T>& v_FM_M = V_FM_M.translational();
+  const Vector3<T>& w_WM_M = V_WM_M.rotational();
+
+  // To avoid re-expressing w_WP_Mp in M, we can use this identity:
+  //   w_WM = w_WP + w_FM, so w_WP = w_WM - w_FM.
+  const Vector3<T> w_WP_M = w_WM_M - w_FM_M;                       // 3 flops
+  const SpatialAcceleration<T> Ab_WM_M(w_WP_M.cross(w_FM_M),       // 12 flops
+                                       2 * w_WP_M.cross(v_FM_M));  // 15 flops
+  A_WM_M += Ab_WM_M;                                               // 6 flops
+
+  // Calculate cross-mobilizer spatial acceleration.
+  const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
+  const SpatialAcceleration<T> A_FM_M =
+      mobilizer_->calc_A_FM_M(X_FM, q, v, vdot);  // 3 flops revolute/prismatic
+                                                  // 30 flops floating
+  A_WM_M += A_FM_M;                               // 6 flops
+}
+
 // As a guideline for developers, a summary of the computations performed in
 // this method is provided:
 // Notation:
@@ -586,6 +712,79 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcInverseDynamics_TipToBase(
   }
 }
 
+template <typename T, class ConcreteMobilizer>
+void BodyNodeImpl<T, ConcreteMobilizer>::CalcInverseDynamicsInM_TipToBase(
+    const FrameBodyPoseCache<T>& frame_body_pose_cache,  // M_BMo_M, X_BM
+    const T* positions,
+    const PositionKinematicsCacheInM<T>& pcm,  // X_MpM, X_WM
+    const VelocityKinematicsCacheInM<T>& vcm,  // V_WM_M
+    const std::vector<SpatialAcceleration<T>>& A_WM_M_array,
+    const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array,  // Bo, W !
+    const Eigen::Ref<const VectorX<T>>& tau_applied_array,
+    std::vector<SpatialForce<T>>* F_BMo_M_array,
+    EigenPtr<VectorX<T>> tau_array) const {
+  const MobodIndex index = mobod_index();  // mobod B
+
+  // Model parameters: mass properties, frame locations.
+  const SpatialInertia<T>& M_BMo_M = frame_body_pose_cache.get_M_BMo_M(index);
+  const T& mass = M_BMo_M.get_mass();
+  const Vector3<T>& p_MoBcm_M = M_BMo_M.get_com();
+  const UnitInertia<T>& G_BMo_M = M_BMo_M.get_unit_inertia();
+
+  const math::RigidTransform<T>& X_MB =
+      outboard_frame().get_X_FB(frame_body_pose_cache);  // F==M
+  const Vector3<T>& p_MoBo_M = X_MB.translation();
+
+  // Kinematics.
+  const math::RotationMatrix<T> R_MW = pcm.get_X_WM(index).rotation().inverse();
+  const SpatialVelocity<T>& V_WM_M = vcm.get_V_WM_M(index);
+  const Vector3<T>& w_WM_M = V_WM_M.rotational();
+  const SpatialAcceleration<T>& A_WM_M = A_WM_M_array[index];
+
+  // Unfortunately the applied force is given at Bo and expressed in W. We
+  // need it applied at Mo and expressed in M.
+  const SpatialForce<T>& Fapp_BBo_W = Fapplied_Bo_W_array[index];
+  const SpatialForce<T> Fapp_BBo_M = R_MW * Fapp_BBo_W;  // 6 SIMD flops
+  const SpatialForce<T> Fapp_BMo_M = Fapp_BBo_M.Shift(-p_MoBo_M);  // 15 flops
+
+  // Calculate the velocity-dependent bias force on B (48 flops).
+  const SpatialForce<T> Fbias_BMo_M(
+      mass * w_WM_M.cross(G_BMo_M * w_WM_M),          // bias moment, 27 flops
+      mass * w_WM_M.cross(w_WM_M.cross(p_MoBcm_M)));  // bias force, 21 flops
+
+  // F_BMo_M is an output and will be the total force on B.
+  SpatialForce<T>& F_BMo_M = (*F_BMo_M_array)[index];
+  F_BMo_M = M_BMo_M * A_WM_M             // 45 flops
+            + Fbias_BMo_M - Fapp_BMo_M;  // 12 flops
+
+  // Next, account for forces applied to B through its outboard joints, treated
+  // as though they were locked. Since we're going tip to base we already have
+  // the total force F_CMc_Mc on each outboard body C. 51 flops per
+  for (const BodyNode<T>* outboard_node : child_nodes()) {
+    const MobodIndex outboard_index = outboard_node->mobod_index();
+
+    // Outboard body's kinematics.
+    const math::RigidTransform<T>& X_MMc = pcm.get_X_MpM(outboard_index);
+    const math::RotationMatrix<T>& R_MMc = X_MMc.rotation();
+    const Vector3<T>& p_MoMc_M = X_MMc.translation();
+
+    const SpatialForce<T>& F_CMc_Mc = (*F_BMo_M_array)[outboard_index];
+    const SpatialForce<T> F_CMc_M = R_MMc * F_CMc_Mc;          // 6 SIMD flops
+    const SpatialForce<T> F_CMo_M = F_CMc_M.Shift(-p_MoMc_M);  // 15 flops
+    F_BMo_M += F_CMo_M;                                        // 6 flops
+  }
+
+  // tau is the primary output.
+  Eigen::Map<VVector<T>> tau(get_mutable_v(tau_array->data()));
+  const Eigen::Map<const VVector<T>> tau_app(get_v(tau_applied_array.data()));
+  VVector<T> tau_projection;  // = Hᵀ_FM_M ⋅ F_BMo_M
+  const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
+  // Next line is 5 flops for revolute/prismatic, 30 flops for floating
+  mobilizer_->calc_tau_from_M(X_FM, get_q(positions), F_BMo_M,
+                              tau_projection.data());
+  tau = tau_projection - tau_app;  // 1-6 flops
+}
+
 template <typename T, class ConcreteMobilizer>
 void BodyNodeImpl<T, ConcreteMobilizer>::
     CalcArticulatedBodyInertiaCache_TipToBase(