Test SpatialMomentum for Fused Links.

[mitiguy]

Ensure spatial momentum for a model having fused welds matches a model having unfused melds.

There is no end-user value here yet. This is a separately-reviewable piece of the larger fused-body project #24350.

---

This change is 

[mitiguy]

Feature review +a:@sherm1

This is the first PR for CalcSpatialMomentum() for fused links. Another will follow after your work on velocity calculations for fused links.

@mitiguy made 1 comment.
Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).

[sherm1]

Feature with a few minor comments

@sherm1 reviewed 2 files and all commit messages, and made 7 comments.
Reviewable status: 6 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

---

multibody/plant/test/fused_welds_test.cc line 3 at r1 (raw file):

/* Tests that mass properties are identical whether welded-together links are
modeled with unfused weld joints or whether links that are welded together
are fused into a mobilized body (fused Mobod). The test builds two identical

nit: into -> onto

---

multibody/plant/test/fused_welds_test.cc line 274 at r1 (raw file):

    SetState(unfused_model, angle, 2.0 /* rad/s */);
    SetState(fused_model, angle, 2.0 /* rad/s */);
    const Vector3<double> p_WoWo_W = Vector3<double>::Zero();

minor: would be better with a non-zero shift to make sure the zero doesn't hide a flaw

BTW do you know if there is a test somewhere that checks the correctness of the momentum calculation (should be, from when this was implemented)?

---

multibody/plant/test/fused_welds_test.cc line 282 at r1 (raw file):

            *fused_model.context, p_WoWo_W);
    // TODO(Mitiguy) EXPECT_FALSE is wrong, should be EXPECT_TRUE !
    EXPECT_TRUE(CompareMatrices(L_unfused.get_coeffs(), L_fused.get_coeffs(),

minor: comment out of date?

---

multibody/plant/test/fused_welds_test.cc line 284 at r1 (raw file):

    EXPECT_TRUE(CompareMatrices(L_unfused.get_coeffs(), L_fused.get_coeffs(),
                                kTolerance, MatrixCompareType::relative))
        << "Spatial momentum mismatch at angle = " << angle;

BTW consider adding a TODO noting the other signatures of CalcSpatialMomentumInWorldAboutPoint() that don't work yet when fused is enabled.

---

multibody/tree/multibody_tree.cc line 2846 at r1 (raw file):

SpatialMomentum<T> MultibodyTree<T>::CalcSpatialMomentumInWorldAboutPoint(
    const systems::Context<T>& context, const Vector3<T>& p_WoP_W) const {
  // Efficienctly evaluate all mobods' spatial inertias, velocities, and poses.

typo: efficienctly -> efficiently

---

multibody/tree/multibody_tree.cc line 2862 at r1 (raw file):

    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;

BTW my inclination with these "in place" things is to name the variable the way we want it to end up, i.e. L_WBWo_W and then get it right asap. That way the very last line makes the most sense and we minimize the amount of time it's wrong (and the number of comments about it that are needed). So something like this:

    const RigidTransform<T>& X_WB = pc.get_X_WB(mobod_index);
    const Vector3<T>& p_WoBo_W = X_WB.translation();
    // Here this is actually L_WBBo_W, we'll fix it immediately.
    SpatialMomentum<T> L_WBWo_W = M_BBo_W * V_WBo_W;
    L_WBWo_W.ShiftInPlace(-p_WoBo_W);  // Now it is correct.
    L_WSWo_W += L_WBWo_W;

An alternative would be just to make it right in the first place:

const SpatialMomentum<T> L_WBWo_W = (M_BBo_W * V_WBo_W).Shift(-p_WoBo_W);

[sherm1]

@sherm1 reviewed 2 files and all commit messages, made 2 comments, and resolved 6 discussions.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

---

multibody/plant/test/fused_welds_test.cc line 235 at r2 (raw file):

             unfused_model.link3->model_instance()},
            p_WoP_W);
    SpatialMomentum<double> L_WCP_W =

BTW shouldn't this symbol be L_WFP_W with "F" for fused?

---

multibody/plant/test/fused_welds_test.cc line 262 at r2 (raw file):

          << " at angle = " << angle;

      // Verify individual link's spatial momentum do not depend on fused links.

nit: link's -> links'

[mitiguy]

@mitiguy reviewed 2 files and all commit messages, and made 6 comments.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

---

multibody/plant/test/fused_welds_test.cc line 3 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: into -> onto

Done.

---

multibody/plant/test/fused_welds_test.cc line 274 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: would be better with a non-zero shift to make sure the zero doesn't hide a flaw

BTW do you know if there is a test somewhere that checks the correctness of the momentum calculation (should be, from when this was implemented)?

Done.
Yes, there are tests in multibody_plant_momentum_energy_test.cc

---

multibody/plant/test/fused_welds_test.cc line 282 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: comment out of date?

Done.

---

multibody/plant/test/fused_welds_test.cc line 284 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider adding a TODO noting the other signatures of CalcSpatialMomentumInWorldAboutPoint() that don't work yet when fused is enabled.

Done. Added TODO() above tests that should pass, but do not.

---

multibody/tree/multibody_tree.cc line 2846 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: efficienctly -> efficiently

Done.

---

multibody/tree/multibody_tree.cc line 2862 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW my inclination with these "in place" things is to name the variable the way we want it to end up, i.e. L_WBWo_W and then get it right asap. That way the very last line makes the most sense and we minimize the amount of time it's wrong (and the number of comments about it that are needed). So something like this:

    const RigidTransform<T>& X_WB = pc.get_X_WB(mobod_index);
    const Vector3<T>& p_WoBo_W = X_WB.translation();
    // Here this is actually L_WBBo_W, we'll fix it immediately.
    SpatialMomentum<T> L_WBWo_W = M_BBo_W * V_WBo_W;
    L_WBWo_W.ShiftInPlace(-p_WoBo_W);  // Now it is correct.
    L_WSWo_W += L_WBWo_W;

An alternative would be just to make it right in the first place:

const SpatialMomentum<T> L_WBWo_W = (M_BBo_W * V_WBo_W).Shift(-p_WoBo_W);

Done. I like the alternative -- much shorter.

[mitiguy]

@mitiguy reviewed 1 file and all commit messages, made 2 comments, and resolved 2 discussions.
Reviewable status: needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).

---

multibody/plant/test/fused_welds_test.cc line 235 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW shouldn't this symbol be L_WFP_W with "F" for fused?

Done. Good catch. Removed here and elsewhere.
C (leftover from composite) should be removed in favor of F (fused).

---

multibody/plant/test/fused_welds_test.cc line 262 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: link's -> links'

Done.