Skip to content

[WIP] Correct spatial momentum calculations for fused links. #24830

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
mitiguy wants to merge 1 commit into
base: master
Choose a base branch
from
+96 −68
Open

[WIP] Correct spatial momentum calculations for fused links. #24830

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 21 additions & 14 deletions multibody/plant/test/fused_welds_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,11 +84,16 @@ TestModel MakeModel(bool fuse_welded_links) {
const SpatialInertia<double> M =
SpatialInertia<double>::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<RevoluteJoint>(
Expand Down Expand Up @@ -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.
Expand All @@ -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.
Expand All @@ -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<double>::Zero(), kTolerance,
MatrixCompareType::relative));
EXPECT_TRUE(CompareMatrices(L_WFP_W.get_coeffs(),
Vector6<double>::Zero(), kTolerance,
MatrixCompareType::relative));
}
}

Expand Down
121 changes: 71 additions & 50 deletions multibody/tree/multibody_tree.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2549,64 +2549,63 @@ Vector3<T> MultibodyTree<T>::CalcCenterOfMassPositionInWorld(
template <typename T>
SpatialInertia<T> MultibodyTree<T>::CalcSpatialInertia(
const systems::Context<T>& context, const Frame<T>& frame_F,
const std::vector<BodyIndex>& body_indexes) const {
// Check if there are repeated BodyIndex in body_indexes by converting the
const std::vector<LinkIndex>& 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<BodyIndex> without_duplicate_bodies(body_indexes.begin(),
body_indexes.end());
if (body_indexes.size() != without_duplicate_bodies.size()) {
const std::set<LinkIndex> 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<SpatialInertia<T>>& M_Bi_W =
// For efficiency, evaluate all mobods' spatial inertia and pose.
const std::vector<SpatialInertia<T>>& M_BiBo_W =
EvalSpatialInertiaInWorldCache(context);
const FrameBodyPoseCache<T>& frame_body_poses = EvalFrameBodyPoses(context);
const FrameBodyPoseCache<T>& fbpc = EvalFrameBodyPoses(context);
const PositionKinematicsCache<T>& 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<T> M_SWo_W = SpatialInertia<T>::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<T>& 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<T>& 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<T>& X_WB = pc.get_X_WB(mobod_index);
const Vector3<T>& 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<T>& 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<T>& M_LLo_L = fbpc.get_M_LLo_L(link_ordinal);
const RigidTransform<T>& X_WL = pc.get_X_WL(link_ordinal);
const RotationMatrix<T>& R_WL = X_WL.rotation();
const SpatialInertia<T> 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<T> 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<T>& 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.
}
Expand Down Expand Up @@ -2904,38 +2903,60 @@ SpatialMomentum<T> MultibodyTree<T>::CalcBodiesSpatialMomentumInWorldAboutWo(
const systems::Context<T>& context,
const std::vector<LinkIndex>& link_indexes) const {
// Efficiently evaluate all mobods' spatial inertias, poses, and velocities.
const std::vector<SpatialInertia<T>>& M_Bi_W =
const std::vector<SpatialInertia<T>>& M_BiBo_W =
EvalSpatialInertiaInWorldCache(context);
const PositionKinematicsCache<T>& pc = EvalPositionKinematics(context);
const VelocityKinematicsCache<T>& vc = EvalVelocityKinematics(context);
const FrameBodyPoseCache<T>& 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<T> L_WS_W = SpatialMomentum<T>::Zero();
SpatialMomentum<T> L_WSWo_W = SpatialMomentum<T>::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<T>& M_BBo_W = M_Bi_W[mobod_index];
const SpatialVelocity<T>& V_WBo_W = vc.get_V_WB(mobod_index);
SpatialMomentum<T> 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<T>& X_WB = pc.get_X_WB(mobod_index);
const Vector3<T>& 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<T>& M_BBo_W = M_BiBo_W[mobod_index];
const SpatialVelocity<T>& V_WBo_W = vc.get_V_WB(mobod_index);
const RigidTransform<T>& X_WB = pc.get_X_WB(mobod_index);
const Vector3<T>& p_WoBo_W = X_WB.translation();
const SpatialMomentum<T> 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<T>& M_LLo_L = fbpc.get_M_LLo_L(link_ordinal);
const RigidTransform<T>& X_WL = pc.get_X_WL(link_ordinal);
const RotationMatrix<T>& R_WL = X_WL.rotation();
const SpatialInertia<T> 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<T>& link = get_link(link_index);
const SpatialVelocity<T>& V_WLo_W =
link.EvalSpatialVelocityInWorld(context);
const Vector3<T>& p_WoLo_W = X_WL.translation();
const SpatialMomentum<T> 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 <typename T>
Expand Down
8 changes: 4 additions & 4 deletions multibody/tree/multibody_tree.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2661,16 +2661,16 @@ class MultibodyTree {
const systems::Context<T>& context, JacobianWrtVariable with_respect_to,
std::vector<SpatialAcceleration<T>>* 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<T> CalcBodiesSpatialMomentumInWorldAboutWo(
const systems::Context<T>& context,
const std::vector<BodyIndex>& body_indexes) const;
const std::vector<LinkIndex>& 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
Expand Down