[multibody] Implement kinematic results for fused links (pos & vel)

[sherm1]

Separates per-mobod kinematics from per-link kinematics so that we get correct link kinematics when fusing welded-together links onto a single mobod.

Changes here:

• position and velocity cache entries gain some new fields, and methods that were incorrectly implemented in the header are moved to the cc file
• position and velocity kinematics calculations fill in the new fields (see body_node_impl.cc)
• new unit tests in fused_welds_test.cc
• cassie bench gets a --fuse flag to support timing comparisons between fused/unfused.
• RigidBody class gets some terminology cleanup to support wider use of "link" internally, and methods that were incorrectly implemented in the header are moved to the cc file
• Two test tolerances that were too tight for Mac are reduced

There are no user-visible changes here unless fusing is enabled (which is "internal only" for now). Also, there are likely to be several independent kinematics-dependent computations that are written in terms of mobods that need to be in terms of links instead, and possibly vice versa. Those will be addressed in their own PRs.

---

This change is 

[sherm1]

+a:@mitiguy for feature review, please

@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).

[Copilot]

Pull request overview

This PR extends MultibodyTree kinematics to correctly report per-link poses and spatial velocities when multiple welded-together links are fused onto a single mobilized body (mobod). It updates the position/velocity cache structures and fills the new link-indexed results during kinematics recursion, adds tests to validate fused vs explicit-weld equivalence, and adds a benchmarking toggle for timing comparisons.

Changes:

• Refactors kinematics caches to separate mobod vs link results (e.g., V_WL, X_WL, and new p_BoLo_W).
• Updates kinematics computations to populate link kinematics for fused mobods (position + velocity).
• Adds fused-weld kinematics unit tests and a Cassie benchmark --fuse flag; includes terminology cleanup (Link vs RigidBody) and minor tolerance updates.

Reviewed changes

Copilot reviewed 16 out of 16 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
multibody/tree/velocity_kinematics_cache.h	Refactors cache API (ctor + SetToZero) and updates documentation for link-aware velocity results.
multibody/tree/velocity_kinematics_cache.cc	Implements moved-out ctor / allocation / zeroing logic; initializes world and world-fixed link velocities.
multibody/tree/test/tree_from_mobilizers_test.cc	Updates to new velocity-cache API (SetToZero).
multibody/tree/rigid_body.h	Introduces Link synonym, renames attorney, updates link-vs-mobod wording, and moves inline methods to .cc.
multibody/tree/rigid_body.cc	Adds out-of-line implementations for Lock/Unlock, topology setup, parameter plumbing, and finalize checks.
multibody/tree/position_kinematics_cache.h	Adds p_BoLo_W storage/accessors for link origins relative to mobod origins.
multibody/tree/position_kinematics_cache.cc	Allocates/initializes p_BoLo_W and fills world-composite precomputations.
multibody/tree/multibody_tree.cc	Uses LinkAttorney, updates velocity zeroing, and adjusts frame-body pose / inertia bookkeeping for link ordinals.
multibody/tree/joint.h	Makes mobilizer clone lookup robust to mobilizer_ == nullptr.
multibody/tree/frame_body_pose_cache.h	Documents world-composite precalculation expectations.
multibody/tree/body_node_impl.cc	Populates per-link X_WL and p_BoLo_W for fused mobods; computes per-link V_WL via shifts.
multibody/plant/test/fused_welds_test.cc	Adds unit tests comparing link poses and spatial velocities for fused vs explicit welds.
multibody/optimization/test/toppra_test.cc	Relaxes an overly-tight numeric tolerance (Mac stability).
multibody/benchmarking/cassie.cc	Adds --fuse flag and updates Jacobian benchmark loop to iterate mobods / active links.
multibody/benchmarking/BUILD.bazel	Adds direct @gflags dependency for new flag include.
bindings/generated_docstrings/multibody_tree.h	Updates generated docstrings to reflect terminology and doc changes.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[mitiguy]

Just starting. Consider changing "explicit" to "unfused" -- some examples below.

@mitiguy partially reviewed 3 files and made 4 comments.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/benchmarking/cassie.cc line 139 at r2 (raw file):

        // The active link's link frame is also the Mobod's frame.
        const Frame<double>& active_link_frame =
            plant_->get_body(active_link_index).link_frame();

nit Hmmm. One line of code was replaced by many. If this pattern (or one like it for getting the body) repeats often, perhaps there should be a shortcut?

---

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

GTEST_TEST(CompositeTest, LinkKinematicsMatchExplicitWelds) {
  const TestModel explicit_model = MakeModel(false /* unfused */);
  const TestModel fused_model = MakeModel(true /* fuse welds */);

nit Consider using "unfused" rather than "explicit". It is shorter, and then the comment after false is redundant.

Suggestion:

  const TestModel unfused_model = MakeModel(false);
  const TestModel fused_model = MakeModel(true);

---

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

             {"Link3", explicit_model.link3, fused_model.link3},
             {"Link4", explicit_model.link4, fused_model.link4},
         }) {

nit Consider unfused -- which is what is used in later tests.

Suggestion:

    for (const auto& [link_name, unfused_link, fused_link] :
         std::vector<std::tuple<const char*, const RigidBody<double>*,
                                const RigidBody<double>*>>{
             {"Link1", unfused_model.link1, fused_model.link1},
             {"Link2", unfused_model.link2, fused_model.link2},
             {"Link3", unfused_model.link3, fused_model.link3},
             {"Link4", unfused_model.link4, fused_model.link4},
         }) {

[sherm1]

Resolved all the feature review comments so far.

@sherm1 made 6 comments and resolved 5 discussions.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).

---

multibody/benchmarking/cassie.cc line 139 at r2 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Hmmm. One line of code was replaced by many. If this pattern (or one like it for getting the body) repeats often, perhaps there should be a shortcut?

Done. I added an internal helper function to MultibodyTree.

---

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

Previously, mitiguy (Mitiguy) wrote…

nit Consider using "unfused" rather than "explicit". It is shorter, and then the comment after false is redundant.

Done

---

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

Previously, mitiguy (Mitiguy) wrote…

nit Consider unfused -- which is what is used in later tests.

Done

[sherm1]

@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).

[mitiguy]

@sherm1 More suggestions below. Will continue tomorrow at body_node_impl.cc

@mitiguy partially reviewed 3 files and made 23 comments.
Reviewable status: 22 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/plant/test/fused_welds_test.cc line 80 at r4 (raw file):

  TestModel m;
  m.plant = std::make_unique<MultibodyPlant<double>>(0.0 /* continuous */);
  m.plant->SetFuseWeldedLinks(fuse_welded_links);

BTW Sometimes, something like "is" for bools is helpful. Other times less so.
This is not a suggestion to change this, more to consider it.

Suggestion:

  m.plant->SetFuseWeldedLinks(is_fuse_welded_links);

---

multibody/plant/test/fused_welds_test.cc line 204 at r4 (raw file):

      EXPECT_TRUE(CompareMatrices(V_WL_unfused.get_coeffs(),
                                  V_WL_fused.get_coeffs(), kTolerance,
                                  MatrixCompareType::relative))

nit For next lines, consider instead SCOPE_TRACE. This was suggested by Jeremy for one of my last commits in PR #24731. Also suggested to remove the extra text: << "Velocity mismatch for " ...

---

multibody/plant/test/fused_welds_test.cc line 212 at r4 (raw file):

/* Tests that at the zero configuration, Link1 is at the World origin, Link2 is
1 m in +x, and Link3 is 1 m in +x and 1 m in +y. */

nit Comments about link locations, etc. are better below (not here).

Suggestion:

/* Test the pose of each link at the zero configuration. */

---

multibody/plant/test/fused_welds_test.cc line 219 at r4 (raw file):

    SetState(m, 0.0, 0.0);

    // Link1 at origin.

nit More detailed comment?

Suggestion:

    // Verify Link1 is at World origin, no rotation.

---

multibody/plant/test/fused_welds_test.cc line 223 at r4 (raw file):

    EXPECT_TRUE(X_WL1.IsNearlyEqualTo(RigidTransformd::Identity(), kTolerance));

    // Link2 at (1, 0, 0), no rotation.

nit More detailed comment?

Suggestion:

  // Verify Link2's position in World is (1, 0, 0), no rotation.

---

multibody/plant/test/fused_welds_test.cc line 228 at r4 (raw file):

        RigidTransformd(Vector3<double>(1.0, 0.0, 0.0)), kTolerance));

    // Link3 at (1, 1, 0)

nit More detailed comment?

Suggestion:

    // Verify Link3's position in World is (1, 1, 0), no rotation.

---

multibody/plant/test/fused_welds_test.cc line 233 at r4 (raw file):

        RigidTransformd(Vector3<double>(1.0, 1.0, 0.0)), kTolerance));

    // Link4 at (4, 0, 0)

nit More detailed comment?

Suggestion:

    // Verify Link4's position in World is (4, 0, 0), no rotation.

---

multibody/plant/test/fused_welds_test.cc line 248 at r4 (raw file):

  Link2: cos(π/2)·x̂ + sin(π/2)·ŷ = (0, 1, 0)
  Link3: Link2 origin + R_WL2 * (0,1,0) = (0,1,0) + (-1,0,0) = (-1, 1, 0)
  Link4: stays at (4, 0, 0) */

nit Comments about link locations, etc. are better below (not here)??

Suggestion:

/* Test the pose of each link when Link1 is rotated 90°. */

---

multibody/plant/test/fused_welds_test.cc line 255 at r4 (raw file):

    SetState(m, M_PI / 2, 0.0);

    // All links share Link1's rotation: 90° about z.

Hmmmm. Link4 does not share Link1's rotation ?

Suggestion:

    // Link2 and Link3 share Link1's 90° z-rotation.

---

multibody/plant/test/fused_welds_test.cc line 259 at r4 (raw file):

        math::RotationMatrixd::MakeZRotation(M_PI / 2);

    // Link1's origin stays at the world origin.

nit More detailed comment?

Suggestion:

    // Verify Link1 has a 90° z-rotation relative to World
    // and Link1's origin is at World origin.

---

multibody/plant/test/fused_welds_test.cc line 264 at r4 (raw file):

        RigidTransformd(R_W90z, Vector3<double>::Zero()), kTolerance));

    // Link2's origin is 1 m along Link1's x-axis → (0, 1, 0) in World.

nit More detailed comment?

Suggestion:

    // Verify Link1 has a 90° z-rotation relative to World. Link1's 
    // origin is (1, 0, 0) from World origin (expressed in World).
    // Link1's +x direction points in World's +y direction, so
    // verify Link1's origin from World is (0, 1, 0) in World.

---

multibody/plant/test/fused_welds_test.cc line 270 at r4 (raw file):

    // Link3's origin is 1 m along Link2's y-axis. After 90° rotation, Link2's
    // y-axis points in World -x, so Link3 is at (0,1,0) + (-1,0,0) = (-1,1,0).

nit More detailed comment?

Suggestion:

    // Verify Link3 has a 90° z-rotation relative to World. Link3's 
    // origin is (0, 1, 0) from Link2's origin (expressed in Link2). 
    // Link2's +y direction points in World's -x direction, so
    // verify Link3 is (0,1,0) + (-1,0,0) = (-1,1,0) in World.

---

multibody/plant/test/fused_welds_test.cc line 275 at r4 (raw file):

        RigidTransformd(R_W90z, Vector3<double>(-1.0, 1.0, 0.0)), kTolerance));

    // Link4 at (4, 0, 0)

nit More detailed comment?

Suggestion:

    // Verify Link4's position in World is (4, 0, 0), no rotation.

---

multibody/plant/test/fused_welds_test.cc line 287 at r4 (raw file):

  rotational velocity: ω * ẑ (World)
  translational velocity at its origin: ω * ẑ × p_WLi
where p_WLi is the world position of the iᵗʰ link's origin. */

This comment seems misleading. I think it better to leave the math for latter. Link4 does not share the same attributes as Links123.

Suggestion:

/* Verify spatial velocities for individual links, regardless of whether 
welded links are fused. */

---

multibody/plant/test/fused_welds_test.cc line 297 at r4 (raw file):

    SetState(m, angle, omega);

    // The angular velocity of all links is ω * ẑ.

Hmmmm. Link4 does not share Link1's angular velocity ?

Suggestion:

    // Link2 and Link3 share Link1's angular velocity in world of ω * ẑ.

---

multibody/plant/test/fused_welds_test.cc line 310 at r4 (raw file):

        m.link1->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL1.translational(), Vector3<double>::Zero(),
                                kTolerance, MatrixCompareType::relative));

nit More detailed comment/notation ?

Suggestion:

    // Verify that the translational velocity of L1o (Link1's origin) in World
    // is zero (L1o is at World origin).
    const SpatialVelocity<double>& V_WL1o =
        m.link1->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL1o.translational(), Vector3<double>::Zero(),
                                kTolerance, MatrixCompareType::relative));

---

multibody/plant/test/fused_welds_test.cc line 314 at r4 (raw file):

    // The translational velocity of Link2's origin is ω × p_WL2.
    // p_WL2 = R_W_L1 * [1, 0, 0] = [cos θ, sin θ, 0].
    // v_WL2 = ω ẑ × [cos θ, sin θ, 0] = ω[-sin θ, cos θ, 0].

nit More detailed comment ?

Suggestion:

    // Verify that the translational velocity of L2o (Link2's origin) in World
    // is ω × p_WL2o.
    // p_WL2o = R_W_L1 * [1, 0, 0] = [cos θ, sin θ, 0].
    // v_WL2o = ω ẑ × [cos θ, sin θ, 0] = ω[-sin θ, cos θ, 0].

---

multibody/plant/test/fused_welds_test.cc line 320 at r4 (raw file):

        m.link2->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL2.translational(), v_WL2o_expected,
                                kTolerance, MatrixCompareType::relative));

nit Fully explicit

Code quote:

    const Vector3<double> v_WL2o_expected =
        omega * Vector3<double>(-std::sin(angle), std::cos(angle), 0.0);
    const SpatialVelocity<double>& V_WL2 =
        m.link2->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL2.translational(), v_WL2o_expected,
                                kTolerance, MatrixCompareType::relative));

---

multibody/plant/test/fused_welds_test.cc line 320 at r4 (raw file):

        m.link2->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL2.translational(), v_WL2o_expected,
                                kTolerance, MatrixCompareType::relative));

nit Continue to use L2o (as was used in the v_WL2o_expected).

Suggestion:

    const SpatialVelocity<double>& V_WL2o =
        m.link2->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL2o.translational(), v_WL2o_expected,
                                kTolerance, MatrixCompareType::relative));

---

multibody/plant/test/fused_welds_test.cc line 326 at r4 (raw file):

    // v_WL3 = ω ẑ × p_WL3
    //       = ω ẑ × [cos θ - sin θ, sin θ + cos θ, 0]
    //       = ω[-(sin θ + cos θ), cos θ - sin θ, 0].

nit More detailed comment with language that parallels earlier comments.

Suggestion:

    // Verify that the translational velocity of L3o (Link3's origin) in World
    // is ω × p_WL3o.
    // p_WL3o = R_WL1 * [1,0,0] + R_WL1 * [0,1,0] 
    //        = [cos θ - sin θ, sin θ + cos θ, 0].
    // v_WL3o = ω ẑ × p_WL3o
    //        = ω ẑ × [cos θ - sin θ, sin θ + cos θ, 0]
    //        = ω[-(sin θ + cos θ), cos θ - sin θ, 0].

---

multibody/plant/test/fused_welds_test.cc line 333 at r4 (raw file):

        m.link3->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL3.translational(), v_WL3o_expected,
                                kTolerance, MatrixCompareType::relative));

nit Continue to use L3o (as was used in the v_WL3o_expected).

Suggestion:

    const SpatialVelocity<double>& V_WL3o =
        m.link3->EvalSpatialVelocityInWorld(*m.context);
    EXPECT_TRUE(CompareMatrices(V_WL3o.translational(), v_WL3o_expected,
                                kTolerance, MatrixCompareType::relative));

---

multibody/plant/test/fused_welds_test.cc line 335 at r4 (raw file):

                                kTolerance, MatrixCompareType::relative));

    // Link4 doesn't move.

nit More detailed comment.

Suggestion:

    // Verify Link4's is welded to World (zero spatial velocity).

[mitiguy]

@mitiguy reviewed 1 file and made 5 comments.
Reviewable status: 27 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/tree/body_node_impl.cc line 236 at r5 (raw file):

// - V_WB(q(W:P), q_B, v_B) spatial velocity of B in W, expressed in W.
// - V_WL(q(W:P), q_B, v_B) spatial velocity in W of each link L that follows
//   body B, expressed in W (for point Lo, the link frame's origin).

nit Change "across mobilizer" to "across-mobilizer".
The hyphen version seems significantly more common in Drake.
Consider explicit notation consistently.

Suggestion:

// - V_FM_F(q_B, v_B) across-mobilizer spatial velocity of M in F.
// - V_PB_W(q_B, v_B) B's spatial velocity in P, expressed in W.
// - V_WB_W(q(W:P), q_B, v_B) B's spatial velocity in W, expressed in W.
// - V_WL_W(q(W:P), q_B, v_B) For each link L that follows body B, link
//   L's spatial velocity in W, expressed in W.

---

multibody/tree/body_node_impl.cc line 241 at r5 (raw file):

// - the already-calculated position kinematics in pc.
// - the parent's spatial velocity V_WP which has already been calculated
//   since we are in a base-to-tip recursion.

nit More explicit comment.
Explicit monogram is nice, but less important.

Suggestion:

// - V_WP_W (parent frame P's spatial velocity in world W, expressed in W)
//   has already been calculated since we are in a base-to-tip recursion.

---

multibody/tree/body_node_impl.cc line 242 at r5 (raw file):

// - the parent's spatial velocity V_WP which has already been calculated
//   since we are in a base-to-tip recursion.
// - this mobilizer's generalized velocities v_B.

Hmmm. It seems body_node_imple.cc uses vm as a mobilizer's generalized velocities. Since B is a body and v_B could also mean a vector expressed in frame B, I think it worthwhile to use vm instead of v_B. There seems to only be 3 occurrences of v_B in this file. Similarly, consider replacing q_B with qm (shorter, more consistent)?

Code quote:

// - this mobilizer's generalized velocities v_B.

---

multibody/tree/body_node_impl.cc line 285 at r5 (raw file):

  // Generalized coordinates local to this node's mobilizer.
  const T* q_B = get_q(positions);

Consider qm ??

Code quote:

  const T* q_B = get_q(positions);

---

multibody/tree/body_node_impl.cc line 337 at r5 (raw file):

    const LinkOrdinal link_ordinal = followers[i];
    const Vector3<T>& p_BL_W = pc.get_p_BoLo_W(link_ordinal);
    vc->SetV_WL(link_ordinal, V_WB.Shift(p_BL_W));

nit Consistent notation -- easier to follow?

Suggestion:

    const Vector3<T>& p_BoLo_W = pc.get_p_BoLo_W(link_ordinal);
    vc->SetV_WL(link_ordinal, V_WB.Shift(p_BoLo_W));

[mitiguy]

@mitiguy reviewed 7 files and all commit messages, made 8 comments, and resolved 1 discussion.
Reviewable status: 34 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/tree/body_node_impl.cc line 70 at r5 (raw file):

  // - X_WL(q(W:P), q_B)  (for all links following mobod B)
  // - p_BoLo_W(q(W:P), q_B)  (for all links following mobod B)
  // - p_PoBo_W(q_B)

nit Prefer qm to q_B.
More consistent with its use elsewhere.
Add links "L" to help communication its use, e.g., X_WL.

Suggestion:

  // - X_FM(qm)
  // - X_PB(qm)
  // - X_WB(q(W:P), qm)
  // - X_WL(q(W:P), qm)  (for all links L following mobod B)
  // - p_BoLo_W(q(W:P), qm)  (for all links L following mobod B)
  // - p_PoBo_W(q_B)

---

multibody/tree/body_node_impl.cc line 125 at r5 (raw file):

  // For fused mobods, also set X_WLₒ and p_BₒLₒ_W for each follower
  // (non-active) link Lₒ. Because Lₒ is rigidly offset from B by X_BLₒ

nit Error Lₒ -> L

Suggestion:

  // (non-active) link L. Because Lₒ is rigidly offset from B by X_BLₒ

---

multibody/tree/body_node_impl.cc line 286 at r5 (raw file):

  // Generalized coordinates local to this node's mobilizer.
  const T* q_B = get_q(positions);
  const T* v_B = get_v(velocities);

nit Update comment.
Also, consider qm, vm.

Suggestion:

  // This node mobilizer's generalized coordinates and velocities.
  const T* qm = get_q(positions);
  const T* vm = get_v(velocities);

---

multibody/tree/body_node_impl.cc line 293 at r5 (raw file):

  // Update V_FM using the operator V_FM = H_FM * vm:
  SpatialVelocity<T>& V_FM = get_mutable_V_FM(vc);
  V_FM = mobilizer_->calc_V_FM(q_B, v_B);

nit q_B -> qm, v_B -> vm

Suggestion:

  V_FM = mobilizer_->calc_V_FM(qm, vm);

---

multibody/tree/body_node_impl.cc line 306 at r5 (raw file):

    // number of mobilities for this node.
    const auto H_PB_W = get_H(H_PB_W_cache);  // 6 x kNv fixed-size Map.
    const Eigen::Map<const VVector<T>> v(v_B);

nit v_B -> vm

Suggestion:

    const Eigen::Map<const VVector<T>> v(vm);

---

multibody/tree/body_node_impl.cc line 329 at r5 (raw file):

  V_WB = V_WP.ComposeWithMovingFrameVelocity(p_PB_W, V_PB_W);

  // We have V_WB, now fill in V_WL for all links following body B.

nit add "L"

Suggestion:

  // We have V_WB, now fill in V_WL for all links L following body B.

---

multibody/tree/body_node_impl.cc line 332 at r5 (raw file):

  const SpanningForest::Mobod& mobod_B = this->mobod();
  const std::vector<LinkOrdinal>& followers = mobod_B.follower_link_ordinals();
  // The active link L₀ is always the first follower and L₀=B.

nit Since Lo is the origin of link L and Lo is used a few lines down, I think it better to avoid Lo. Consider instead Lₐ (active link).

Suggestion:

  // The active link Lₐ is always the first follower and Lₐ=B.

---

multibody/tree/rigid_body.h line 657 at r5 (raw file):

  /// @param[in] pc position kinematics cache.
  /// @retval R_WL rotation matrix giving the orientation of this Link L in
  /// the world frame W.

nit Shorter and more standard (based on AI count of our terminology in Drake code-base).

Suggestion:

  /// @retval R_WL rotation matrix relating world frame W and Link L.

[sherm1]

All comments addressed, PTAL.

@sherm1 made 35 comments and resolved 23 discussions.
Reviewable status: 11 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).

---

multibody/plant/test/fused_welds_test.cc line 80 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

BTW Sometimes, something like "is" for bools is helpful. Other times less so.
This is not a suggestion to change this, more to consider it.

"is" seems wrong since this is a command rather than a report. I though "should_fuse_welded_links" would be better but then at the call sites it still looks wrong: SetFuseWeldedLinks(should_fuse_welded_links) still looks like we're telling it to fuse when really we're just passiing along either "fuse" or "don't fuse". I think it would have to be something like whether_to_fuse_welded_links.

---

multibody/plant/test/fused_welds_test.cc line 204 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit For next lines, consider instead SCOPE_TRACE. This was suggested by Jeremy for one of my last commits in PR #24731. Also suggested to remove the extra text: << "Velocity mismatch for " ...

Done. Just removed Claude's test failure details altogether.

---

multibody/plant/test/fused_welds_test.cc line 212 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Comments about link locations, etc. are better below (not here).

Done

---

multibody/plant/test/fused_welds_test.cc line 219 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done

---

multibody/plant/test/fused_welds_test.cc line 223 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done

---

multibody/plant/test/fused_welds_test.cc line 228 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done

---

multibody/plant/test/fused_welds_test.cc line 233 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done

---

multibody/plant/test/fused_welds_test.cc line 248 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Comments about link locations, etc. are better below (not here)??

Done.

---

multibody/plant/test/fused_welds_test.cc line 255 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Hmmmm. Link4 does not share Link1's rotation ?

Done.

---

multibody/plant/test/fused_welds_test.cc line 259 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done.

---

multibody/plant/test/fused_welds_test.cc line 264 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done.

---

multibody/plant/test/fused_welds_test.cc line 270 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

The existing comment was equally detailed and less confusing.

---

multibody/plant/test/fused_welds_test.cc line 275 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment?

Done.

---

multibody/plant/test/fused_welds_test.cc line 287 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

This comment seems misleading. I think it better to leave the math for latter. Link4 does not share the same attributes as Links123.

Done.

---

multibody/plant/test/fused_welds_test.cc line 297 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Hmmmm. Link4 does not share Link1's angular velocity ?

Done.

---

multibody/plant/test/fused_welds_test.cc line 310 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment/notation ?

Done

---

multibody/plant/test/fused_welds_test.cc line 314 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment ?

Done

---

multibody/plant/test/fused_welds_test.cc line 320 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Fully explicit

Done

---

multibody/plant/test/fused_welds_test.cc line 320 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Continue to use L2o (as was used in the v_WL2o_expected).

Done

---

multibody/plant/test/fused_welds_test.cc line 326 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment with language that parallels earlier comments.

Done

---

multibody/plant/test/fused_welds_test.cc line 333 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Continue to use L3o (as was used in the v_WL3o_expected).

Done

---

multibody/plant/test/fused_welds_test.cc line 335 at r4 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More detailed comment.

Done

---

multibody/tree/body_node_impl.cc line 70 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Prefer qm to q_B.
More consistent with its use elsewhere.
Add links "L" to help communication its use, e.g., X_WL.

Done

---

multibody/tree/body_node_impl.cc line 125 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Error Lₒ -> L

Done. I decided to be less sloppy and use Lᵢ for the generic link, PTAL.

---

multibody/tree/body_node_impl.cc line 236 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Change "across mobilizer" to "across-mobilizer".
The hyphen version seems significantly more common in Drake.
Consider explicit notation consistently.

Done, in part. The redundant frame in V_WL_W is bad for several reasons: it makes every expression harder to read, and it obscures the important distinction between V_WB and V_PB_W where one of those has been re-expressed away from its obvious frame and the other hasn't.

---

multibody/tree/body_node_impl.cc line 241 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit More explicit comment.
Explicit monogram is nice, but less important.

Done

---

multibody/tree/body_node_impl.cc line 242 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Hmmm. It seems body_node_imple.cc uses vm as a mobilizer's generalized velocities. Since B is a body and v_B could also mean a vector expressed in frame B, I think it worthwhile to use vm instead of v_B. There seems to only be 3 occurrences of v_B in this file. Similarly, consider replacing q_B with qm (shorter, more consistent)?

Done

---

multibody/tree/body_node_impl.cc line 286 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Update comment.
Also, consider qm, vm.

Done

---

multibody/tree/body_node_impl.cc line 293 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit q_B -> qm, v_B -> vm

Done

---

multibody/tree/body_node_impl.cc line 306 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit v_B -> vm

Done

---

multibody/tree/body_node_impl.cc line 329 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit add "L"

Done

---

multibody/tree/body_node_impl.cc line 332 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Since Lo is the origin of link L and Lo is used a few lines down, I think it better to avoid Lo. Consider instead Lₐ (active link).

I think this is OK now locally that I called the generic link Lᵢ so the active link L₀ is Lᵢ₌₀, PTAL

---

multibody/tree/body_node_impl.cc line 337 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Consistent notation -- easier to follow?

Done

---

multibody/tree/rigid_body.h line 657 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Shorter and more standard (based on AI count of our terminology in Drake code-base).

Done

[mitiguy]

Completed next pass. Ready for another look.

@mitiguy reviewed 9 files and all commit messages, made 12 comments, and resolved 11 discussions.
Reviewable status: 11 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

---

multibody/tree/position_kinematics_cache.cc line 17 at r6 (raw file):

  Allocate();

  // Set known values.

nit Add a comment to help with why world is separate.

Suggestion:

  // Set known values for world-related quantities.

---

multibody/tree/position_kinematics_cache.cc line 21 at r6 (raw file):

  X_WL_pool_[world_link_ordinal()] = RigidTransform<T>::Identity();
  p_BoLo_W_pool_[world_link_ordinal()].setZero();
  // p_BoLo_W is always zero for each mobod's active link (since B=L₀).

nit Add another comment here.
Also. Remove reference to L₀ (unnecessary and share too much in common with Lo).

Suggestion:

  // Set known values for active links 
  // p_BoLo_W is always zero for each mobod's active link.

---

multibody/tree/position_kinematics_cache.cc line 42 at r6 (raw file):

    SetX_WL(link_ordinal, X_WL);
    // For World's followers, B is the World mobod so R_WB is identity.
    // Thus p_BoLo_W = R_WB * p_BoLo_B = p_BoLo_B = X_BL.translation().

nit -- Could this be made more concise/clearer ?

Suggestion:

    // When B is world, R_WB = R_BB is identity, thus
    // p_BoLo_W = R_WB * p_BoLo_B = p_BoLo_B = X_BL.translation().

---

multibody/tree/rigid_body.h line 31 at r6 (raw file):

class RigidBody;

/// Link is a synonym for the mis-named RigidBody class; prefer Link in

nit Spelling "mis-named" is for gen-older.

Suggestion:

/// Link is a synonym for the misnamed RigidBody class; prefer Link in

---

multibody/tree/rigid_body.h line 266 at r6 (raw file):

  /// Returns a const reference to the associated LinkFrame (RigidBodyFrame).
  /// This is a synonym for body_frame().

nit An improved "This" .
A little marketing pitch for "link"?

Suggestion:

  /// link_frame() is synonymous with body_frame().
  /// @note "link" is the terminology used in urdf/sdf.

---

multibody/tree/rigid_body.h line 270 at r6 (raw file):

  /// Returns a const reference to the associated RigidBodyFrame (LinkFrame).
  /// This is a synonym for link_frame().

nit An improved "This" .
A little marketing pitch for "link"?

Suggestion:

  /// body_frame() is synonymous with link_frame().
  /// @note "link" is the terminology used in urdf/sdf.

---

multibody/tree/rigid_body.h line 298 at r6 (raw file):

  /// followed by this %RigidBody (Link). This serves as the BodyNode index
  /// and the index into all associated quantities. More than one link
  /// may follow the same mobod.

nit Wordsmithing. Please check to ensure I understand what was intended.

Suggestion:

  /// (Advanced) Returns the index of the mobilized body ("mobod") that 
  /// this this %RigidBody (Link) follows. This index serves as the 
  /// BodyNode index and the index into all associated quantities. 
  /// More than one link may follow the same mobod.

---

multibody/tree/rigid_body.h line 701 at r6 (raw file):

  /// @param[in] vc velocity kinematics cache.
  /// @retval v_WLo_W velocity of Lo (link frame origin) in world W, expressed
  /// in W.

nit -- more of a question.
Is there a difference between link origin and link frame origin.
If not, perhaps skip frame here?

Suggestion:

  /// @retval v_WLo_W velocity of Lo (link origin) in world W, expressed in W.

---

multibody/tree/rigid_body.h line 845 at r6 (raw file):

/// (Compatibility) Prefer Link or RigidBody to Body, however this dispreferred
/// alias is available to permit older code to continue working.

nit Stronger wording here?

Suggestion:

/// (Compatibility) Use Link or RigidBody instead of Body. An 
/// alias is provided to permit older code to continue working.

---

multibody/tree/velocity_kinematics_cache.h line 117 at r6 (raw file):

  // Allocates resources for this velocity kinematics cache. Sets most
  // quantities to NaN except for V_FM which is set to zero since mobilizers
  // are entitled to assume that.

nit -- Is zero the default value for mobilizers?
I am unsure what is meant by "since mobilizers are entitled to assume that."

Code quote:

  // quantities to NaN except for V_FM which is set to zero since mobilizers
  // are entitled to assume that.

---

multibody/tree/velocity_kinematics_cache.cc line 13 at r6 (raw file):

  Allocate();

  // Set values that won't ever change.

nit Reminder that these values are specific to world_mobod.

Suggestion:

  // For world mobod, set values that never change.

[sherm1]

All comments addressed, PTAL.

@sherm1 made 12 comments and resolved 11 discussions.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).

---

multibody/tree/position_kinematics_cache.cc line 17 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Add a comment to help with why world is separate.

Done

---

multibody/tree/position_kinematics_cache.cc line 21 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Add another comment here.
Also. Remove reference to L₀ (unnecessary and share too much in common with Lo).

Done

---

multibody/tree/position_kinematics_cache.cc line 42 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit -- Could this be made more concise/clearer ?

Done

---

multibody/tree/rigid_body.h line 31 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Spelling "mis-named" is for gen-older.

Done :)

---

multibody/tree/rigid_body.h line 266 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit An improved "This" .
A little marketing pitch for "link"?

Done

---

multibody/tree/rigid_body.h line 270 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit An improved "This" .
A little marketing pitch for "link"?

Done

---

multibody/tree/rigid_body.h line 298 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Wordsmithing. Please check to ensure I understand what was intended.

Done

---

multibody/tree/rigid_body.h line 701 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit -- more of a question.
Is there a difference between link origin and link frame origin.
If not, perhaps skip frame here?

Done. Link origin and link frame origin are the same thing.

---

multibody/tree/rigid_body.h line 845 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Stronger wording here?

Done

---

multibody/tree/velocity_kinematics_cache.h line 117 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit -- Is zero the default value for mobilizers?
I am unsure what is meant by "since mobilizers are entitled to assume that."

Done

---

multibody/tree/velocity_kinematics_cache.cc line 13 at r6 (raw file):

Previously, mitiguy (Mitiguy) wrote…

nit Reminder that these values are specific to world_mobod.

Done

[mitiguy]

@mitiguy reviewed 5 files and all commit messages, and made 1 comment.
Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).

[sherm1]

+a:@jwnimmer-tri for platform review per rotation, please

@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform) (waiting on jwnimmer-tri).

[jwnimmer-tri]

Checkpoint on the easy stuff. I'll return to review the main changes after lunch.

methods that were incorrectly implemented in the header are moved to the cc file

I'm a fan of the check, but not mixing it in with other logic changes. This really should have been its own pull request. Making me review logic changes alongside code-relocation is too hard. It would have taken no time at all for a single-review platform stamp from someone as an ahead-of-time PR. Mixed in here, it's extra work for me to try to tease it all apart during review.

@jwnimmer-tri reviewed 16 files and all commit messages, and made 14 comments.
Reviewable status: 13 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform) (waiting on sherm1).

---

-- commits line 2 at r7:
nit No period at end of git subject line (nor PR title).

---

multibody/plant/test/fused_welds_test.cc line 368 at r7 (raw file):

GTEST_TEST(FusedTest, CompositeSpatialInertia) {
  const TestModel unfused_model = MakeModel(false /* no combining */);
  const TestModel fused_model = MakeModel(true /* fuse welds */);

nit These inline comments are both gratuitous on their own, and are also inconsistent with the other newly-added parts of the test which call MakeModel using literals with no extra commenting.

Code quote:

  const TestModel unfused_model = MakeModel(false /* no combining */);
  const TestModel fused_model = MakeModel(true /* fuse welds */);

---

multibody/tree/frame_body_pose_cache.h line 53 at r7 (raw file):

         center of mass of L, expressed in B. Indexed by LinkOrdinal.

World composite precalculations

It's not clear to me why this documentation changed. Were these precalculated values already implemented here but we missed adding the doc earlier? But I don't see any position kinematics caching happening here.

---

multibody/tree/joint.h line 1037 at r7 (raw file):

  // cloned Mobilizer corresponding to this Joint's Mobilizer.
  template <typename ToScalar>
  internal::Mobilizer<ToScalar>* FindMobilizerToScalarClone(

BTW This (pre-existing) method seems a bit silly to me? It's one line long, called from exactly one place, with vapid documentation -- what value is there to this being carved out into a separate method?

---

multibody/tree/joint.h line 1040 at r7 (raw file):

      internal::MultibodyTree<ToScalar>* tree_clone) const {
    internal::Mobilizer<ToScalar>* const mobilizer_clone =
        mobilizer_ == nullptr ? nullptr

If I revert this change, no multibody tests fail (not even in a debug build).

I also couldn't understand how it's relevant to this pull request.

---

multibody/tree/multibody_tree.cc line 1761 at r7 (raw file):

        const math::RotationMatrix<T>& R_BL = X_BL.rotation();
        const Vector3<T> p_BoLcm_B = X_BL * p_LoLcm_L;
        frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_BoLcm_B);

I do not understand this change. Why did one argument change spelling to link.ordinal() but not the other? It seems link_ordinal was better anyway?

Code quote:

        const RigidTransform<T>& X_BL =
            frame_body_poses->get_X_BL(link_ordinal);
        const math::RotationMatrix<T>& R_BL = X_BL.rotation();
        const Vector3<T> p_BoLcm_B = X_BL * p_LoLcm_L;
        frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_BoLcm_B);

---

multibody/tree/multibody_tree.h line 963 at r7 (raw file):

  }

  [[nodiscard]] const Link<T>& get_active_link(MobodIndex index) const {

This method is in the public section, so needs (developer-facing) API documentation.

---

multibody/tree/position_kinematics_cache.h line 201 at r7 (raw file):

  // This allow us to quickly verify some of the values stored in the pools are
  // never used (however we store them anyway to simplify the indexing).
  static RigidTransform<T> NaNPose() {

BTW This is silly as a private static in the header file. It could be anonymous in the cc file.

Or really, since it's only used in a single method (but is called multiple times), it could just as well be a local variable like we did with nan_vec there.

---

multibody/tree/rigid_body.cc line 67 at r7 (raw file):

template <typename T>
void RigidBody<T>::Lock(systems::Context<T>* context) const {
  ThrowIfNotFinalized(__func__);

This code relocation lost a TODO.

Ditto for Unlock.

---

multibody/tree/position_kinematics_cache.cc line 53 at r7 (raw file):

  const Vector3<T> nan_vec =
      Vector3<T>::Constant(std::numeric_limits<double>::quiet_NaN());
  X_WB_pool_.resize(num_mobods_, NaNPose());

There appears to be no rhyme nor reason to the order things are initialized in this method.

---

multibody/tree/velocity_kinematics_cache.cc line 11 at r7 (raw file):

    const SpanningForest& forest)
    : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) {
  Allocate();

When trying to convince myself about whether & how this constructor does the right thing, I found that I had to jump back and forth between the header list of members, the Allocate definition, and this constructor body.

It seems to me that we could make the reader's life a lot simpler by getting rid of Allocate and just placing its contents inline here. Is there any reason why it's split apart right now?

I ended up rewriting it locally to this, for my sanity. (Keeping in the same order as the header also helps keep the mobod vs link indexing easier to follow.)

template <typename T>
VelocityKinematicsCache<T>::VelocityKinematicsCache(
    const SpanningForest& forest)
    : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) {
  V_WB_pool_.resize(num_mobods_, SpatialVelocity<T>::NaN());
  // Mobilizers are entitled to assume V_FM has been initialized to zero. For
  // example, a Weld mobilizer can simply not write to V_FM at all, and others
  // can take advantage of known zeroes.
  V_FM_pool_.resize(num_mobods_, SpatialVelocity<T>::Zero());
  V_PB_W_pool_.resize(num_mobods_, SpatialVelocity<T>::NaN());
  V_WL_pool_.resize(num_links_, SpatialVelocity<T>::NaN());

  // For the World mobod, set values that won't ever change.
  V_WB_pool_[world_mobod_index()].SetZero();   // World's velocity is zero.
  V_FM_pool_[world_mobod_index()].SetNaN();    // It must never be used.
  V_PB_W_pool_[world_mobod_index()].SetNaN();  // It must never be used.
  V_WL_pool_[world_link_ordinal()].SetZero();  // World's velocity is zero.

  // Any links that are part of the World composite also have known zero
  // velocities in World.
  const SpanningForest::Mobod& world_mobod = forest.mobods(MobodIndex(0));
  const std::vector<LinkOrdinal>& world_followers =
      world_mobod.follower_link_ordinals();
  DRAKE_DEMAND(world_followers[0] == LinkOrdinal(0));  // Already done above.
  for (size_t i = 1; i < world_followers.size(); ++i) {
    const LinkOrdinal link_ordinal = world_followers[i];
    V_WL_pool_[link_ordinal].SetZero();
  }
}

---

multibody/tree/velocity_kinematics_cache.cc line 17 at r7 (raw file):

  V_WL_pool_[world_link_ordinal()].SetZero();  //           "
  V_FM_pool_[world_mobod_index()].SetNaN();    // It must never be used.
  V_PB_W_pool_[world_mobod_index()].SetNaN();  // It must never be used.

Setting the world's V_PB_W to NaN is redundant, since we already set all V_PB_W to NaN a few lines before. If we still want to mention it as reminder, we could do that either with a comment or with an assertion.

---

multibody/tree/velocity_kinematics_cache.cc line 24 at r7 (raw file):

  const std::vector<LinkOrdinal>& world_followers =
      world_mobod.follower_link_ordinals();
  DRAKE_DEMAND(world_followers[0] == LinkOrdinal(0));  // Already done above.

Shouldn't this be world_link_ordinal()? That's the specific one that got "done above".

Code quote:

LinkOrdinal(0)

[jwnimmer-tri]

First pass complete.

@jwnimmer-tri reviewed 1 file and made 6 comments.
Reviewable status: 17 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform) (waiting on sherm1).

---

multibody/tree/rigid_body.h line 646 at r7 (raw file):

RigidBody class gets some terminology cleanup to support wider use of "link" internally

I don't understand what dividing line was here for terminology changes, which makes it difficult to review for consistency.

The new comments on Link say to prefer it for internal code. That leaves open the question of what to do with public code, like this method.

Here we change this method, but e.g. on the very next method "get_rotation_matrix_in_world" we still say "this body's frame" even while changing its return value to say "link".

What is the objective here? Can we make a more consistent change, all at once?

---

multibody/tree/rigid_body.cc line 67 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This code relocation lost a TODO.

Ditto for Unlock.

Aha! I just found its new home in the header file. That's a worse location. Having the TODO next to the if-check it's TODOing was better.

---

multibody/tree/rigid_body.h line 201 at r7 (raw file):

/// added using Constraint objects to remove more degrees of freedom.
///
/// @note This object corresponds to a "link" in urdf/sdf terminology. We

nit Here and throughout (eg line 272) -- sdf means "signed distance field". I believe what you intend here is "SDFormat" (or "sdformat").

---

multibody/tree/rigid_body.h line 298 at r7 (raw file):

  /// (Advanced) Returns the index of the mobilized body ("mobod") that
  /// this %RigidBody (Link) follows. This index serves as the

nit Since this is an internal method (by virtual of using an internal-use-only return value), I don't think we need to belabor the parenthetical.

Suggestion:

Link

---

multibody/tree/rigid_body.h line 680 at r7 (raw file):

  ///@{

  /// (Advanced) Returns V_WL, this Link (%RigidBody) L's SpatialVelocity in

nit Since this is an internal method (by virtual of using an internal-use-only argument type), I don't think we need to belabor the parenthetical.

Suggestion:

Link L's

[sherm1]

review[ing] logic changes alongside code-relocation is too hard

Agreed. By way of explanation but not excuse: I had to modify some of the existing misplaced functions and add some new ones (e.g. velocity_kinematics_cache). I didn't want to make things worse by defining those in the header. But then it looked bad to have some of the functions properly in .cc and others in .h so I moved the rest over. That would have been a good time to make a small PR that contained only the relocation of the existing functions, some of which would get modified or removed in this PR. But I didn't think of that at the time, unnecessarily burdening my reviewers -- sorry!

Checkpoint: most comments addressed, a few more to go.

@sherm1 made 12 comments and resolved 7 discussions.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform) (waiting on jwnimmer-tri and mitiguy).

---

multibody/plant/test/fused_welds_test.cc line 368 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit These inline comments are both gratuitous on their own, and are also inconsistent with the other newly-added parts of the test which call MakeModel using literals with no extra commenting.

Done

---

multibody/tree/joint.h line 1037 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This (pre-existing) method seems a bit silly to me? It's one line long, called from exactly one place, with vapid documentation -- what value is there to this being carved out into a separate method?

Done, see below.

---

multibody/tree/joint.h line 1040 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If I revert this change, no multibody tests fail (not even in a debug build).

I also couldn't understand how it's relevant to this pull request.

Done. This slithered over from my larger working PR when I split out the kinematics. It is necessary for proper cloning of plants with fused welds (because those welds are not modeled with any mobilizer). However, that's not part of the current PR so shouldn't be here. Instead I've added a nullptr check (can't happen w/o fusing) and eliminated the method altogether per your suggestion (see line 778).

---

multibody/tree/multibody_tree.h line 963 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This method is in the public section, so needs (developer-facing) API documentation.

Done. Docs were missing from a few methods above this one also (leading to my cargo culting) so I added those too.

---

multibody/tree/multibody_tree.cc line 1761 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I do not understand this change. Why did one argument change spelling to link.ordinal() but not the other? It seems link_ordinal was better anyway?

Done. It was the other way around -- I added a bunch of code using link.ordinal() then realized I already had it in a local link_ordinal. Looks like I missed one spot!

---

multibody/tree/position_kinematics_cache.h line 201 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This is silly as a private static in the header file. It could be anonymous in the cc file.

Or really, since it's only used in a single method (but is called multiple times), it could just as well be a local variable like we did with nan_vec there.

Done (it's a local now).

---

multibody/tree/position_kinematics_cache.cc line 53 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

There appears to be no rhyme nor reason to the order things are initialized in this method.

Done. There was some organization in my head that made sense to me at the time but I can't remember it now! Switched to declaration order.

---

multibody/tree/rigid_body.h line 201 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Here and throughout (eg line 272) -- sdf means "signed distance field". I believe what you intend here is "SDFormat" (or "sdformat").

Done

---

multibody/tree/rigid_body.h line 298 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Since this is an internal method (by virtual of using an internal-use-only return value), I don't think we need to belabor the parenthetical.

Done

---

multibody/tree/rigid_body.h line 646 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

RigidBody class gets some terminology cleanup to support wider use of "link" internally

I don't understand what dividing line was here for terminology changes, which makes it difficult to review for consistency.

The new comments on Link say to prefer it for internal code. That leaves open the question of what to do with public code, like this method.

Here we change this method, but e.g. on the very next method "get_rotation_matrix_in_world" we still say "this body's frame" even while changing its return value to say "link".

What is the objective here? Can we make a more consistent change, all at once?

Done, sort of. There is no great solution unless we deprecate RigidBody altogether and rename a lot of methods and change a lot of documentation. For now I've elected to make the switch to Link internally but leave it possible for an API user to continue thinking "Body" or "RigidBody" when they mean "Link". Luckily when fusing is properly implemented there is very little leakage into the public behavior so API users shouldn't really have to think about it much. That said there is some leakage and also much internal use of these public APIs. I think it's important to rub in the Link==RigidBody equivalence in the public API so that (a) any leakage makes sense, and (b) the internals make sense when they work with the public RigidBody APIs.

I made a pass through for better consistency. BTW I also found a bunch of internal methods that were unmarked or just marked "advanced". (They were clearly internal since they used internal:: data structures in their parameter lists.) I clarified those and made them just use the link terminology.

---

multibody/tree/rigid_body.h line 680 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Since this is an internal method (by virtual of using an internal-use-only argument type), I don't think we need to belabor the parenthetical.

Done

[sherm1]

@sherm1 made 1 comment and resolved 1 discussion.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform) (waiting on jwnimmer-tri and mitiguy).

---

-- commits line 2 at r7:

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit No period at end of git subject line (nor PR title).

Done in both places

[sherm1]

All comments addressed, PTAL.

@sherm1 made 6 comments and resolved 3 discussions.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform) (waiting on jwnimmer-tri and mitiguy).

---

multibody/tree/frame_body_pose_cache.h line 53 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

It's not clear to me why this documentation changed. Were these precalculated values already implemented here but we missed adding the doc earlier? But I don't see any position kinematics caching happening here.

Right, the feature was already implemented in a previous PR but I missed moving over this bit of documentation from the working branch. The precalculation it's referring to is in MultibodyTree::CalcPositionKinematicsCache() beginning at multibody_tree.cc:1412.

---

multibody/tree/rigid_body.cc line 67 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Aha! I just found its new home in the header file. That's a worse location. Having the TODO next to the if-check it's TODOing was better.

Are you referring to Rico's TODO at rigid_body.h:277 // TODO(rpoyner-tri): consider extending the design to allow locking on non-floating bodies. ?
Since that refers to the API contract (currently only floating bodies) it seems better placed near the public API docs. Or did you mean something else?

---

multibody/tree/velocity_kinematics_cache.cc line 11 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

When trying to convince myself about whether & how this constructor does the right thing, I found that I had to jump back and forth between the header list of members, the Allocate definition, and this constructor body.

It seems to me that we could make the reader's life a lot simpler by getting rid of Allocate and just placing its contents inline here. Is there any reason why it's split apart right now?

I ended up rewriting it locally to this, for my sanity. (Keeping in the same order as the header also helps keep the mobod vs link indexing easier to follow.)

template <typename T>
VelocityKinematicsCache<T>::VelocityKinematicsCache(
    const SpanningForest& forest)
    : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) {
  V_WB_pool_.resize(num_mobods_, SpatialVelocity<T>::NaN());
  // Mobilizers are entitled to assume V_FM has been initialized to zero. For
  // example, a Weld mobilizer can simply not write to V_FM at all, and others
  // can take advantage of known zeroes.
  V_FM_pool_.resize(num_mobods_, SpatialVelocity<T>::Zero());
  V_PB_W_pool_.resize(num_mobods_, SpatialVelocity<T>::NaN());
  V_WL_pool_.resize(num_links_, SpatialVelocity<T>::NaN());

  // For the World mobod, set values that won't ever change.
  V_WB_pool_[world_mobod_index()].SetZero();   // World's velocity is zero.
  V_FM_pool_[world_mobod_index()].SetNaN();    // It must never be used.
  V_PB_W_pool_[world_mobod_index()].SetNaN();  // It must never be used.
  V_WL_pool_[world_link_ordinal()].SetZero();  // World's velocity is zero.

  // Any links that are part of the World composite also have known zero
  // velocities in World.
  const SpanningForest::Mobod& world_mobod = forest.mobods(MobodIndex(0));
  const std::vector<LinkOrdinal>& world_followers =
      world_mobod.follower_link_ordinals();
  DRAKE_DEMAND(world_followers[0] == LinkOrdinal(0));  // Already done above.
  for (size_t i = 1; i < world_followers.size(); ++i) {
    const LinkOrdinal link_ordinal = world_followers[i];
    V_WL_pool_[link_ordinal].SetZero();
  }
}

Done. I debated about doing this to position_kinematics_cache also but its Allocate() method is much bigger so I left it as is.

---

multibody/tree/velocity_kinematics_cache.cc line 17 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Setting the world's V_PB_W to NaN is redundant, since we already set all V_PB_W to NaN a few lines before. If we still want to mention it as reminder, we could do that either with a comment or with an assertion.

Done

---

multibody/tree/velocity_kinematics_cache.cc line 24 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Shouldn't this be world_link_ordinal()? That's the specific one that got "done above".

Done

[jwnimmer-tri]

@jwnimmer-tri reviewed 11 files and all commit messages, made 10 comments, and resolved 4 discussions.
Reviewable status: 7 unresolved discussions (waiting on sherm1).

---

multibody/tree/frame_body_pose_cache.h line 53 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Right, the feature was already implemented in a previous PR but I missed moving over this bit of documentation from the working branch. The precalculation it's referring to is in MultibodyTree::CalcPositionKinematicsCache() beginning at multibody_tree.cc:1412.

Okay, I see the code you're referring to now. The problem was that this documentation is confusing.

The opening paragraph atop this class says "[This class] holds the following items" and then lists "Frame & Link poses", "Mass properties", and "World composite precalculations".

I interpreted that to mean this class holds the world composite precalculations. Reading more closely I can see that it's saying the information is stored in the position kinematics cache, but the way the section titles are laid out goes against that.

---

multibody/tree/multibody_tree.h line 963 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done. Docs were missing from a few methods above this one also (leading to my cargo culting) so I added those too.

(I'm fine having those added, but I was mildly OK with those since they were pretty obvious. This one in particular though involves novelty about what "active" means.)

---

multibody/tree/rigid_body.h line 298 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done

This one didn't get changed? Per the other thread, internal-only methods should just use "Link".

---

multibody/tree/rigid_body.h line 646 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done, sort of. There is no great solution unless we deprecate RigidBody altogether and rename a lot of methods and change a lot of documentation. For now I've elected to make the switch to Link internally but leave it possible for an API user to continue thinking "Body" or "RigidBody" when they mean "Link". Luckily when fusing is properly implemented there is very little leakage into the public behavior so API users shouldn't really have to think about it much. That said there is some leakage and also much internal use of these public APIs. I think it's important to rub in the Link==RigidBody equivalence in the public API so that (a) any leakage makes sense, and (b) the internals make sense when they work with the public RigidBody APIs.

I made a pass through for better consistency. BTW I also found a bunch of internal methods that were unmarked or just marked "advanced". (They were clearly internal since they used internal:: data structures in their parameter lists.) I clarified those and made them just use the link terminology.

Much better now!

---

multibody/tree/rigid_body.cc line 67 at r7 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Are you referring to Rico's TODO at rigid_body.h:277 // TODO(rpoyner-tri): consider extending the design to allow locking on non-floating bodies. ?
Since that refers to the API contract (currently only floating bodies) it seems better placed near the public API docs. Or did you mean something else?

Yes, that's the TODO.

My intuition was that putting it at the point where the exception comes out (error message) was superior, because that's where the user would find it when they call it erroneously and then get an error.

Previously it didn't matter much since the error message and API spec were right next to each other in the near. I guess I can see the case for having it near the API spec also.

---

multibody/tree/rigid_body.h line 215 at r10 (raw file):

  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RigidBody);

  /// Constructs a %RigidBody (Link) named `body_name` with the given default

nit In places where we're going to suppress the Doxygen automatic linking, we need to do it consistency for both names. Otherwise, only the "Link" ends up as a hyperlink, which looks weird.

Ditto throughout this file.

Suggestion:

Constructs a %RigidBody (%Link) named `body_name` with the given default

---

multibody/tree/multibody_tree.h line 973 at r10 (raw file):

  // Every mobod has a unique "active link", the link that connects it to
  // its parent mobod (or the world link in the case of the world mobod).
  // This returns a reference to the Drake Link (aka RigidBody) corresponding

nit This is an internal method, so we can stick with the internal-preferred terminology without all the fluff.

Suggestion:

Link

---

multibody/tree/rigid_body.h line 250 at r10 (raw file):

  LinkIndex index() const { return this->template index_impl<LinkIndex>(); }

  /// (Internal use only) Returns this Link's unique ordinal. Currently

nit This is public, so for consistency should stick with the %RigidBody (%Link) phrasing used everywhere else.

Code quote:

Link's

---

multibody/tree/rigid_body.h line 838 at r10 (raw file):

  const std::string name_;

  // This link's LinkFrame (a.k.a. RigidBodyFrame).

nit consistency with the above

Suggestion:

aka

[jwnimmer-tri]

@jwnimmer-tri made 1 comment.
Reviewable status: 8 unresolved discussions (waiting on sherm1).

---

multibody/tree/rigid_body.cc line 65 at r10 (raw file):

      name_);
}
template <typename T>

nit Missing blank line between methods.

[sherm1]

Done. Two comments are blocking, PTAL.

@sherm1 made 8 comments and resolved 6 discussions.
Reviewable status: 2 unresolved discussions (waiting on jwnimmer-tri).

---

multibody/tree/frame_body_pose_cache.h line 53 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Okay, I see the code you're referring to now. The problem was that this documentation is confusing.

The opening paragraph atop this class says "[This class] holds the following items" and then lists "Frame & Link poses", "Mass properties", and "World composite precalculations".

I interpreted that to mean this class holds the world composite precalculations. Reading more closely I can see that it's saying the information is stored in the position kinematics cache, but the way the section titles are laid out goes against that.

Done. It's just a note now.

---

multibody/tree/multibody_tree.h line 973 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit This is an internal method, so we can stick with the internal-preferred terminology without all the fluff.

Done

---

multibody/tree/rigid_body.h line 298 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This one didn't get changed? Per the other thread, internal-only methods should just use "Link".

Done.

---

multibody/tree/rigid_body.h line 215 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit In places where we're going to suppress the Doxygen automatic linking, we need to do it consistency for both names. Otherwise, only the "Link" ends up as a hyperlink, which looks weird.

Ditto throughout this file.

Done

---

multibody/tree/rigid_body.h line 250 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit This is public, so for consistency should stick with the %RigidBody (%Link) phrasing used everywhere else.

This one is internal so gets just Link like the other internal methods.

---

multibody/tree/rigid_body.h line 838 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit consistency with the above

Done

---

multibody/tree/rigid_body.cc line 65 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Missing blank line between methods.

Done