Inverse Dynamics in M proto + cassiebench test

Author: sherm1

math/rotation_matrix.h

@@ -101,20 +101,14 @@ class RotationMatrix {
   /// @throws std::exception in debug builds if the rotation matrix
   /// R that is built from `theta_lambda` fails IsValid(R).  For example, an
   /// exception is thrown if `lambda` is zero or contains a NaN or infinity.
-  explicit RotationMatrix(const Eigen::AngleAxis<T>& theta_lambda) {
-    using std::cos;
-    using std::sin;
-    // TODO(mitiguy) Consider adding an optional second argument if lambda is
-    // known to be normalized apriori or the caller does not want normalization.
-    const T& theta = theta_lambda.angle();
-    const Vector3<T>& lambda = theta_lambda.axis();
-    // We won't use AngleAxis<T>::toRotationMatrix because somtimes it is
-    // miscompiled by Clang 15. Instead, we'll follow the derivation here:
-    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
-    const T norm = lambda.norm();
-    const T x = lambda.x() / norm;
-    const T y = lambda.y() / norm;
-    const T z = lambda.z() / norm;
+  explicit RotationMatrix(const Eigen::AngleAxis<T>& theta_lambda)
+      : RotationMatrix(theta_lambda.angle(), theta_lambda.axis().normalized()) {
+  }
+
+  RotationMatrix(const T& theta, const Eigen::Vector3<T>& unit_lambda) {
+    const T x = unit_lambda.x();
+    const T y = unit_lambda.y();
+    const T z = unit_lambda.z();
     const T s = sin(theta);
     const T c = cos(theta);
     const T t = 1 - c;     // 1 - cos(θ)
@@ -131,15 +125,15 @@ class RotationMatrix {
     const T txz = tx * z;  // (1 - cos(θ)) x z
     const T tyz = ty * z;  // (1 - cos(θ)) y z
     Matrix3<T> R;
-    R.coeffRef(0, 0) = txx + c;   // (1 - cos(θ)) x² + cos(θ)
-    R.coeffRef(1, 1) = tyy + c;   // (1 - cos(θ)) y² + cos(θ)
-    R.coeffRef(2, 2) = tzz + c;   // (1 - cos(θ)) z² + cos(θ)
-    R.coeffRef(0, 1) = txy - sz;  // (1 - cos(θ)) x y - sin(θ) z
-    R.coeffRef(1, 0) = txy + sz;  // (1 - cos(θ)) x y + sin(θ) z
-    R.coeffRef(0, 2) = txz + sy;  // (1 - cos(θ)) x z + sin(θ) y
-    R.coeffRef(2, 0) = txz - sy;  // (1 - cos(θ)) x z - sin(θ) y
-    R.coeffRef(1, 2) = tyz - sx;  // (1 - cos(θ)) y z - sin(θ) x
-    R.coeffRef(2, 1) = tyz + sx;  // (1 - cos(θ)) y z + sin(θ) x
+    R(0, 0) = txx + c;   // (1 - cos(θ)) x² + cos(θ)
+    R(1, 1) = tyy + c;   // (1 - cos(θ)) y² + cos(θ)
+    R(2, 2) = tzz + c;   // (1 - cos(θ)) z² + cos(θ)
+    R(0, 1) = txy - sz;  // (1 - cos(θ)) x y - sin(θ) z
+    R(1, 0) = txy + sz;  // (1 - cos(θ)) x y + sin(θ) z
+    R(0, 2) = txz + sy;  // (1 - cos(θ)) x z + sin(θ) y
+    R(2, 0) = txz - sy;  // (1 - cos(θ)) x z - sin(θ) y
+    R(1, 2) = tyz - sx;  // (1 - cos(θ)) y z - sin(θ) x
+    R(2, 1) = tyz + sx;  // (1 - cos(θ)) y z + sin(θ) x
     set(R);
   }
 

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, InverseDynamics2)(benchmark::State& state) {
+  DoInverseDynamicsInM(state);
+}
+BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics2)
+    ->Unit(benchmark::kMicrosecond)
+    ->Arg(kWantNoGrad);
+
 // NOLINTNEXTLINE(runtime/references)
 BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) {
   DoForwardDynamics(state);
@@ -383,6 +402,21 @@ BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics)
   ->Arg(kWantGradV|kWantGradVdot)
   ->Arg(kWantGradX|kWantGradVdot);
 
+// NOLINTNEXTLINE(runtime/references)
+BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamics2)(benchmark::State& state) {
+  DoInverseDynamicsInM(state);
+}
+BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics2)
+    ->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

@@ -3868,6 +3868,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_id2 =
+      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_id2, tau_id,
+                              16 * std::numeric_limits<double>::epsilon(),
+                              MatrixCompareType::relative));
 }
 
 TEST_F(KukaIiwaModelTests, BodyForceApi) {

multibody/tree/body_node.h

@@ -241,6 +241,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
@@ -290,6 +294,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
@@ -373,6 +381,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
@@ -432,6 +446,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.