From 9da9f392f7331717311db49db3e469d83490d993 Mon Sep 17 00:00:00 2001 From: Paul Mitiguy Date: Tue, 4 Aug 2026 18:09:55 -0700 Subject: [PATCH] Correct spatial momentum calculations for fused links. --- multibody/plant/test/fused_welds_test.cc | 35 ++++--- multibody/tree/multibody_tree.cc | 121 +++++++++++++---------- multibody/tree/multibody_tree.h | 8 +- 3 files changed, 96 insertions(+), 68 deletions(-) diff --git a/multibody/plant/test/fused_welds_test.cc b/multibody/plant/test/fused_welds_test.cc index 452e7486f90a..010446adce7c 100644 --- a/multibody/plant/test/fused_welds_test.cc +++ b/multibody/plant/test/fused_welds_test.cc @@ -84,11 +84,16 @@ TestModel MakeModel(bool fuse_welded_links) { const SpatialInertia M = SpatialInertia::SolidCubeWithMass(1.0, 0.1); - // Add the four links. - m.link1 = &m.plant->AddRigidBody("Link1", M); - m.link2 = &m.plant->AddRigidBody("Link2", M); - m.link3 = &m.plant->AddRigidBody("Link3", M); - m.link4 = &m.plant->AddRigidBody("Link4", M); + // Add the four links, each in its own model instance so that + // link->model_instance() uniquely identifies a single link. + const ModelInstanceIndex inst1 = m.plant->AddModelInstance("Link1"); + const ModelInstanceIndex inst2 = m.plant->AddModelInstance("Link2"); + const ModelInstanceIndex inst3 = m.plant->AddModelInstance("Link3"); + const ModelInstanceIndex inst4 = m.plant->AddModelInstance("Link4"); + m.link1 = &m.plant->AddRigidBody("Link1", inst1, M); + m.link2 = &m.plant->AddRigidBody("Link2", inst2, M); + m.link3 = &m.plant->AddRigidBody("Link3", inst3, M); + m.link4 = &m.plant->AddRigidBody("Link4", inst4, M); // Revolute joint (z-axis): World to Link1, with Link1 frame at world frame. m.revolute = &m.plant->AddJoint( @@ -414,9 +419,12 @@ GTEST_TEST(FusedTest, CompositeSpatialInertia) { fused_model.link2->model_instance(), fused_model.link3->model_instance()}, p_WoP_W); - // TODO(Mitiguy) EXPECT_FALSE is wrong! Should be EXPECT_TRUE! - EXPECT_FALSE(CompareMatrices(L_WUP_W.get_coeffs(), L_WFP_W.get_coeffs(), - kTolerance, MatrixCompareType::relative)); + EXPECT_TRUE(CompareMatrices(L_WUP_W.get_coeffs(), L_WFP_W.get_coeffs(), + kTolerance, MatrixCompareType::relative)); + + // Verify system's spatial momentum does not depend on fused links. + L_WUP_W = unfused_model.plant->CalcSpatialMomentumInWorldAboutPoint( + *unfused_model.context, p_WoP_W); // Ensure that individual link spatial inertias and spatial momentum are // accurately calculated, regardless of whether they were fused. @@ -436,9 +444,8 @@ GTEST_TEST(FusedTest, CompositeSpatialInertia) { *unfused_model.context, {unfused_linki->model_instance()}, p_WoP_W); L_WFP_W = fused_model.plant->CalcSpatialMomentumInWorldAboutPoint( *fused_model.context, {fused_linki->model_instance()}, p_WoP_W); - // TODO(Mitiguy) EXPECT_FALSE is wrong! Should be EXPECT_TRUE! - EXPECT_FALSE(CompareMatrices(L_WUP_W.get_coeffs(), L_WFP_W.get_coeffs(), - kTolerance, MatrixCompareType::relative)); + EXPECT_TRUE(CompareMatrices(L_WUP_W.get_coeffs(), L_WFP_W.get_coeffs(), + kTolerance, MatrixCompareType::relative)); // Since link4 is welded to world, special-case calculations are used. For // this special case, also compare link4 results to an analytical value. @@ -452,9 +459,9 @@ GTEST_TEST(FusedTest, CompositeSpatialInertia) { // Link4's spatial momentum should always be zero (welded to ground). // TODO(Mitiguy) EXPECT_FALSE is wrong! Should be EXPECT_TRUE! - EXPECT_FALSE(CompareMatrices(L_WFP_W.get_coeffs(), - Vector6::Zero(), kTolerance, - MatrixCompareType::relative)); + EXPECT_TRUE(CompareMatrices(L_WFP_W.get_coeffs(), + Vector6::Zero(), kTolerance, + MatrixCompareType::relative)); } } diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index e4e8742bb1e1..38b4384ea33e 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -2549,64 +2549,63 @@ Vector3 MultibodyTree::CalcCenterOfMassPositionInWorld( template SpatialInertia MultibodyTree::CalcSpatialInertia( const systems::Context& context, const Frame& frame_F, - const std::vector& body_indexes) const { - // Check if there are repeated BodyIndex in body_indexes by converting the + const std::vector& link_indexes) const { + // Check if there are repeated LinkIndex in link_indexes by converting the // vector to a set (to eliminate duplicates) and see if their sizes differ. - const std::set without_duplicate_bodies(body_indexes.begin(), - body_indexes.end()); - if (body_indexes.size() != without_duplicate_bodies.size()) { + const std::set without_duplicate_bodies(link_indexes.begin(), + link_indexes.end()); + if (link_indexes.size() != without_duplicate_bodies.size()) { throw std::logic_error( - "CalcSpatialInertia(): contains a repeated BodyIndex."); + "CalcSpatialInertia(): contains a repeated LinkIndex."); } - // For the set S of bodies contained in body_indexes, return S's + // For the set S of links contained in link_indexes, return S's // spatial inertia about Fo (frame_F's origin), expressed in frame F. - // For efficiency, evaluate all bodies' spatial inertia and pose. - const std::vector>& M_Bi_W = + // For efficiency, evaluate all mobods' spatial inertia and pose. + const std::vector>& M_BiBo_W = EvalSpatialInertiaInWorldCache(context); - const FrameBodyPoseCache& frame_body_poses = EvalFrameBodyPoses(context); + const FrameBodyPoseCache& fbpc = EvalFrameBodyPoses(context); const PositionKinematicsCache& pc = EvalPositionKinematics(context); - // Add each body's spatial inertia in the world frame W to this system - // S's spatial inertia in W about Wo (the origin of W), expressed in W. + // Add each link L's spatial inertia in the world frame W to this system + // S's spatial inertia in W about Wo (world origin), expressed in W. SpatialInertia M_SWo_W = SpatialInertia::Zero(); - for (BodyIndex body_index : body_indexes) { - if (body_index == world_index()) continue; // World inertia does not add. + for (LinkIndex link_index : link_indexes) { + if (link_index == world_index()) continue; // World inertia does not add. - // Ensure MultibodyPlant method contains a valid body_index. - if (body_index >= num_links()) { + // Ensure MultibodyPlant method contains a valid link_index. + if (link_index >= num_links()) { throw std::logic_error( - "CalcSpatialInertia(): contains an invalid BodyIndex."); + "CalcSpatialInertia(): contains an invalid LinkIndex."); } - // Get the current body B's spatial inertia about Bo (body B's origin), - // expressed in the world frame W. Start the calculation with a cached value - // for M_BBo_W if B is not a fused body (i.e., it is a one-link Mobod). - const MobodIndex mobod_index = get_link(body_index).mobod_index(); + // If link L is a unfused body (one-link Mobod), use the Mobod's cached + // value for M_LLo_W (a more efficient way to do this calculation). + const MobodIndex mobod_index = get_link(link_index).mobod_index(); if (!get_mobod(mobod_index).is_fused()) { - const SpatialInertia& M_BBo_W = M_Bi_W[mobod_index]; + // Get the Mobod's cached value for the current link L's spatial inertia + // about Lo (link L's origin), expressed in world frame W. + const SpatialInertia& M_BBo_W = M_BiBo_W[mobod_index]; // Shift M_BBo_W from about-point Bo to about-point Wo and add to the sum. const RigidTransform& X_WB = pc.get_X_WB(mobod_index); const Vector3& p_WoBo_W = X_WB.translation(); M_SWo_W += M_BBo_W.Shift(-p_WoBo_W); // Shift from Bo to Wo by p_BoWo_W. } else { - // For a composite body (having more than one link), need to calculate - // the individual link's spatial inertia about Wo expressed in W since - // there is no cached value (and perhaps no need for one). + // For a link_L on a Mobod that has multiple follower links, M_LLo_W + // (L's spatial inertia about Lo expressed in world W) needs to be + // calculated as M_LLo_W is not cached (and perhaps no need for one). + // Get M_LLo_L (link L's spatial inertia about its origin Lo, expressed + // in link L), then re-express M_LLo_L in world W to form M_LLo_W. const LinkOrdinal link_ordinal = - graph().link_by_index(body_index).ordinal(); - - // M_LLo_L: inertia of link L about its origin Lo, expressed in L. - const SpatialInertia& M_LLo_L = - frame_body_poses.get_M_LLo_L(link_ordinal); - - // X_WL: pose of L's frame relative to world frame W. + graph().link_by_index(link_index).ordinal(); + const SpatialInertia& M_LLo_L = fbpc.get_M_LLo_L(link_ordinal); const RigidTransform& X_WL = pc.get_X_WL(link_ordinal); + const RotationMatrix& R_WL = X_WL.rotation(); + const SpatialInertia M_LLo_W = M_LLo_L.ReExpress(R_WL); - // Re-express M_LLo to world W, shift that to world origin, add to sum. - const SpatialInertia M_LLo_W = M_LLo_L.ReExpress(X_WL.rotation()); + // Shift M_LLo_W from about Lo to about Wo (world origin), add to sum. const Vector3& p_WoLo_W = X_WL.translation(); M_SWo_W += M_LLo_W.Shift(-p_WoLo_W); // Shift from Lo to Wo by p_LoWo_W. } @@ -2904,38 +2903,60 @@ SpatialMomentum MultibodyTree::CalcBodiesSpatialMomentumInWorldAboutWo( const systems::Context& context, const std::vector& link_indexes) const { // Efficiently evaluate all mobods' spatial inertias, poses, and velocities. - const std::vector>& M_Bi_W = + const std::vector>& M_BiBo_W = EvalSpatialInertiaInWorldCache(context); const PositionKinematicsCache& pc = EvalPositionKinematics(context); const VelocityKinematicsCache& vc = EvalVelocityKinematics(context); + const FrameBodyPoseCache& fbpc = EvalFrameBodyPoses(context); // Accumulate each body's spatial momentum in world W to this system S's // spatial momentum in W about Wo (the origin of W), expressed in W. - SpatialMomentum L_WS_W = SpatialMomentum::Zero(); + SpatialMomentum L_WSWo_W = SpatialMomentum::Zero(); - // Add contributions from each link Bi. + // Add contributions from each link Li. for (LinkIndex link_index : link_indexes) { if (link_index == 0) continue; // No contribution from the world link. // Ensure MultibodyPlant method contains a valid link_index. DRAKE_DEMAND(link_index < num_links()); - // Form the current link's spatial momentum in W about Bo, expressed in W. + // If link L is a unfused body (one-link Mobod), use the Mobod's cached + // value for M_LLo_W (a more efficient way to do this calculation). const MobodIndex mobod_index = get_link(link_index).mobod_index(); - const SpatialInertia& M_BBo_W = M_Bi_W[mobod_index]; - const SpatialVelocity& V_WBo_W = vc.get_V_WB(mobod_index); - SpatialMomentum L_WBo_W = M_BBo_W * V_WBo_W; - - // Shift L_WBo_W from about Bo to about Wo and accumulate the sum. - const RigidTransform& X_WB = pc.get_X_WB(mobod_index); - const Vector3& p_WoBo_W = X_WB.translation(); - // After ShiftInPlace, L_WBo_W is changed to L_WBWo_W, which is B's - // spatial momentum about point Wo, measured and expressed in frame W. - L_WBo_W.ShiftInPlace(-p_WoBo_W); // After this, L_WBo_W is now L_WBWo_W. - L_WS_W += L_WBo_W; // Actually is `L_WS_W += L_WBWo_W`. + if (!get_mobod(mobod_index).is_fused()) { + // Form mobod B's spatial momentum in W about Bo, expressed in W, then + // shift L_WBBo_W from "about Bo" to "about Wo", and accumulate the sum. + const SpatialInertia& M_BBo_W = M_BiBo_W[mobod_index]; + const SpatialVelocity& V_WBo_W = vc.get_V_WB(mobod_index); + const RigidTransform& X_WB = pc.get_X_WB(mobod_index); + const Vector3& p_WoBo_W = X_WB.translation(); + const SpatialMomentum L_WBWo_W = (M_BBo_W * V_WBo_W).Shift(-p_WoBo_W); + L_WSWo_W += L_WBWo_W; + } else { + // For a link_L on a Mobod that has multiple follower links, M_LLo_W + // (L's spatial inertia about Lo expressed in world W) needs to be + // calculated as M_LLo_W is not cached (and perhaps no need for one). + // Get M_LLo_L (link L's spatial inertia about its origin Lo, expressed + // in link L), then re-express M_LLo_L in world W to form M_LLo_W. + const LinkOrdinal link_ordinal = + graph().link_by_index(link_index).ordinal(); + const SpatialInertia& M_LLo_L = fbpc.get_M_LLo_L(link_ordinal); + const RigidTransform& X_WL = pc.get_X_WL(link_ordinal); + const RotationMatrix& R_WL = X_WL.rotation(); + const SpatialInertia M_LLo_W = M_LLo_L.ReExpress(R_WL); + + // Form current link L's spatial momentum in W about Lo, expressed in W. + // Shift L_WLLo_W from "about Lo" to "about Wo", and accumulate the sum. + const Link& link = get_link(link_index); + const SpatialVelocity& V_WLo_W = + link.EvalSpatialVelocityInWorld(context); + const Vector3& p_WoLo_W = X_WL.translation(); + const SpatialMomentum L_WLWo_W = (M_LLo_W * V_WLo_W).Shift(-p_WoLo_W); + L_WSWo_W += L_WLWo_W; + } } - return L_WS_W; + return L_WSWo_W; } template diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index 10a7cb87fa0e..53a50183070a 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -2661,16 +2661,16 @@ class MultibodyTree { const systems::Context& context, JacobianWrtVariable with_respect_to, std::vector>* AsBias_WB_all) const; - // This method returns the spatial momentum of a list of bodies in the + // This method returns the spatial momentum of a list of links in the // world frame W, about the world origin Wo, expressed in the world frame W. // @param[in] context Contains the state of the model. - // @param[in] body_indexes Array of selected bodies. This method does not + // @param[in] link_indexes Array of selected links. This method does not // distinguish between welded bodies, joint-connected bodies, // floating bodies, the world_body(), or repeated bodies. - // @throws std::exception if body_indexes contains an invalid BodyIndex. + // @throws std::exception if link_indexes contains an invalid LinkIndex. SpatialMomentum CalcBodiesSpatialMomentumInWorldAboutWo( const systems::Context& context, - const std::vector& body_indexes) const; + const std::vector& link_indexes) const; // Helper method to access the mobilizer of a free body (that is, a // body connected to its parent by a 6-dof joint). If `body` is free, this