Fix CalcSpatialMomentumInWorldAboutPoint() so it uses mobods (not delegate to links, which causes bug for fused link).

Author: mitiguy

multibody/plant/test/fused_welds_test.cc

@@ -279,8 +279,8 @@ GTEST_TEST(FusedTest, CompositeSpatialInertia) {
         fused_model.plant->CalcSpatialMomentumInWorldAboutPoint(
             *fused_model.context, p_WoWo_W);
     // TODO(Mitiguy) EXPECT_FALSE is wrong, should be EXPECT_TRUE !
-    EXPECT_FALSE(CompareMatrices(L_unfused.get_coeffs(), L_fused.get_coeffs(),
-                                 kTolerance, MatrixCompareType::relative))
+    EXPECT_TRUE(CompareMatrices(L_unfused.get_coeffs(), L_fused.get_coeffs(),
+                                kTolerance, MatrixCompareType::relative))
         << "Spatial momentum mismatch at angle = " << angle;
   }
 }

multibody/tree/multibody_tree.cc

@@ -2843,16 +2843,35 @@ Vector3<T> MultibodyTree<T>::CalcCenterOfMassTranslationalAccelerationInWorld(
 template <typename T>
 SpatialMomentum<T> MultibodyTree<T>::CalcSpatialMomentumInWorldAboutPoint(
     const systems::Context<T>& context, const Vector3<T>& p_WoP_W) const {
-  // Assemble a list of ModelInstanceIndex.
-  // Skip model_instance_index(0) which always contains the "world" body. The
-  // spatial momentum of the world body measured in the world is always zero.
-  std::vector<ModelInstanceIndex> model_instances;
-  for (ModelInstanceIndex model_instance_index(1);
-       model_instance_index < num_model_instances(); ++model_instance_index)
-    model_instances.push_back(model_instance_index);
+  // Efficienctly evaluate all mobods' spatial inertias, velocities, and poses.
+  const std::vector<SpatialInertia<T>>& M_Bi_W =
+      EvalSpatialInertiaInWorldCache(context);
+  const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
+  const VelocityKinematicsCache<T>& vc = EvalVelocityKinematics(context);
+
+  // Form L_WSWo_W, the system S's spatial momentum in world W about Wo (world
+  // origin) expressed in W by doing a sum over all mobods in S.
+  SpatialMomentum<T> L_WSWo_W = SpatialMomentum<T>::Zero();
+  for (const SpanningForest::Mobod& mobod : forest().mobods()) {
+    if (mobod.is_world()) continue;  // No contribution from the world.
+
+    // Form each mobod B's spatial momentum in W about Bo, expressed in W.
+    const MobodIndex mobod_index = mobod.index();
+    const SpatialInertia<T>& M_BBo_W = M_Bi_W[mobod_index];
+    const SpatialVelocity<T>& V_WBo_W = vc.get_V_WB(mobod_index);
+    SpatialMomentum<T> L_WBBo_W = M_BBo_W * V_WBo_W;
+
+    // Shift L_WBBo_W from "about Bo" to "about Wo" and accumulate the sum.
+    const RigidTransform<T>& X_WB = pc.get_X_WB(mobod_index);
+    const Vector3<T>& p_WoBo_W = X_WB.translation();
+    // After ShiftInPlace, L_WBBo_W is no longer "about Bo", instead L_WBBo_W
+    // is now L_WBWo_W (B's spatial momentum in W "about Wo", expressed in W).
+    L_WBBo_W.ShiftInPlace(-p_WoBo_W);  // After this, L_WBBo_W is now L_WBWo_W.
+    L_WSWo_W += L_WBBo_W;              // Actually is L_WSWo_W += L_WBWo_W.
+  }
 
-  return CalcSpatialMomentumInWorldAboutPoint(context, model_instances,
-                                              p_WoP_W);
+  // Shift system S's spatial momentum from "about Wo" to "about point P".
+  return L_WSWo_W.Shift(p_WoP_W);  // returns L_WSP_W.
 }
 
 template <typename T>