From 747216f341cdc129161cf503518f5516c36ddc82 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Tue, 21 Apr 2026 17:50:45 -0700 Subject: [PATCH] Name changes & infrastructure in support of composites to come Also miscellaneous opportunistic comment cleanups. --- .../generated_docstrings/multibody_math.h | 7 + .../generated_docstrings/multibody_plant.h | 33 +- .../generated_docstrings/multibody_tree.h | 337 +++++---- bindings/pydrake/multibody/BUILD.bazel | 1 + bindings/pydrake/multibody/plant_py.cc | 6 +- .../kuka_iiwa_robot/drake_kuka_iiwa_robot.h | 20 +- .../kuka_iiwa_robot/make_kuka_iiwa_model.cc | 18 +- multibody/math/spatial_vector.h | 8 + multibody/math/test/spatial_algebra_test.cc | 21 + multibody/plant/deformable_model.cc | 12 +- multibody/plant/multibody_plant.cc | 48 +- multibody/plant/multibody_plant.h | 53 +- .../multibody_plant_forward_dynamics_test.cc | 12 +- multibody/plant/test/multibody_plant_test.cc | 2 +- .../rational/rational_forward_kinematics.cc | 8 +- .../rational_forward_kinematics_internal.cc | 12 +- .../test/rational_forward_kinematics_test.cc | 2 +- multibody/topology/link_joint_graph.cc | 8 +- multibody/topology/link_joint_graph_debug.cc | 4 +- multibody/topology/link_joint_graph_defs.h | 27 +- multibody/topology/spanning_forest.cc | 40 +- multibody/topology/spanning_forest.h | 17 +- multibody/topology/spanning_forest_debug.cc | 24 +- multibody/topology/spanning_forest_inlines.h | 4 +- multibody/topology/spanning_forest_mobod.h | 43 +- .../topology/test/spanning_forest_test.cc | 51 +- multibody/tree/body_node.h | 10 +- multibody/tree/body_node_impl.cc | 34 +- multibody/tree/deformable_body.cc | 2 +- multibody/tree/element_collection.cc | 9 +- multibody/tree/element_collection.h | 12 +- multibody/tree/frame.h | 327 +++++---- multibody/tree/frame_body_pose_cache.h | 174 +++-- multibody/tree/joint.h | 5 +- multibody/tree/linear_spring_damper.cc | 6 +- multibody/tree/linear_spring_damper.h | 2 +- multibody/tree/multibody_element.h | 3 + multibody/tree/multibody_forces.cc | 4 +- multibody/tree/multibody_tree.cc | 511 +++++++------ multibody/tree/multibody_tree.h | 681 +++++++++--------- multibody/tree/multibody_tree_indexes.h | 48 +- multibody/tree/multibody_tree_system.cc | 54 +- multibody/tree/multibody_tree_system.h | 5 +- multibody/tree/position_kinematics_cache.h | 101 ++- multibody/tree/rigid_body.cc | 8 +- multibody/tree/rigid_body.h | 65 +- .../test/articulated_body_algorithm_test.cc | 16 +- multibody/tree/test/ball_rpy_joint_test.cc | 4 +- multibody/tree/test/curvilinear_joint_test.cc | 4 +- .../tree/test/curvilinear_mobilizer_test.cc | 2 +- .../tree/test/element_collection_test.cc | 21 +- multibody/tree/test/frames_test.cc | 4 +- .../tree/test/free_rotating_body_plant.cc | 4 +- multibody/tree/test/joint_actuator_test.cc | 28 +- .../linear_bushing_roll_pitch_yaw_test.cc | 4 +- multibody/tree/test/mobilizer_tester.h | 2 +- multibody/tree/test/model_instance_test.cc | 12 +- multibody/tree/test/multibody_forces_test.cc | 8 +- .../tree/test/multibody_tree_creation_test.cc | 147 ++-- multibody/tree/test/multibody_tree_test.cc | 116 ++- multibody/tree/test/planar_joint_test.cc | 4 +- multibody/tree/test/planar_mobilizer_test.cc | 2 +- multibody/tree/test/prismatic_joint_test.cc | 4 +- .../tree/test/prismatic_mobilizer_test.cc | 2 +- multibody/tree/test/prismatic_spring_test.cc | 6 +- .../test/quaternion_floating_joint_test.cc | 4 +- .../quaternion_floating_mobilizer_test.cc | 2 +- multibody/tree/test/revolute_joint_test.cc | 4 +- .../tree/test/revolute_mobilizer_test.cc | 2 +- multibody/tree/test/revolute_spring_test.cc | 8 +- multibody/tree/test/rigid_body_test.cc | 2 +- .../tree/test/rpy_ball_mobilizer_test.cc | 2 +- .../tree/test/rpy_floating_joint_test.cc | 4 +- .../tree/test/rpy_floating_mobilizer_test.cc | 2 +- multibody/tree/test/screw_joint_test.cc | 4 +- multibody/tree/test/screw_mobilizer_test.cc | 4 +- multibody/tree/test/spatial_inertia_test.cc | 5 +- multibody/tree/test/tree_from_joints_test.cc | 10 +- .../tree/test/tree_from_mobilizers_test.cc | 24 +- multibody/tree/test/universal_joint_test.cc | 4 +- .../tree/test/universal_mobilizer_test.cc | 2 +- multibody/tree/test/weld_joint_test.cc | 10 +- multibody/tree/test/weld_mobilizer_test.cc | 2 +- .../tree/uniform_gravity_field_element.cc | 67 +- multibody/tree/velocity_kinematics_cache.h | 92 ++- 85 files changed, 1935 insertions(+), 1588 deletions(-) diff --git a/bindings/generated_docstrings/multibody_math.h b/bindings/generated_docstrings/multibody_math.h index c83d4284a98a..cf4898761d20 100644 --- a/bindings/generated_docstrings/multibody_math.h +++ b/bindings/generated_docstrings/multibody_math.h @@ -1005,6 +1005,13 @@ Parameter ``translational_tolerance``: elements of ``this`` and ``other`` are equal within translational_tolerance.)"""; } IsNearlyEqualWithinAbsoluteTolerance; + // Symbol: drake::multibody::SpatialVector::NaN + struct /* NaN */ { + // Source: drake/multibody/math/spatial_vector.h + const char* doc = +R"""(Factory to create a *NaN* spatial vector, i.e., a SpatialVector whose +rotational and translational components are all NaN.)"""; + } NaN; // Symbol: drake::multibody::SpatialVector::SetNaN struct /* SetNaN */ { // Source: drake/multibody/math/spatial_vector.h diff --git a/bindings/generated_docstrings/multibody_plant.h b/bindings/generated_docstrings/multibody_plant.h index 94127137f0d0..40ff388ffe71 100644 --- a/bindings/generated_docstrings/multibody_plant.h +++ b/bindings/generated_docstrings/multibody_plant.h @@ -186,32 +186,6 @@ This method must be called pre-Finalize. Raises: RuntimeError if ``plant`` is finalized or if time_step is changed.)"""; } ApplyMultibodyPlantConfig; - // Symbol: drake::multibody::BaseBodyJointType - struct /* BaseBodyJointType */ { - // Source: drake/multibody/plant/multibody_plant.h - const char* doc = -R"""(The kind of joint to be used to connect base bodies to world at -Finalize(). See mbp_working_with_free_bodies "Working with free -bodies" for definitions and discussion. - -See also: - SetBaseBodyJointType() for details.)"""; - // Symbol: drake::multibody::BaseBodyJointType::kQuaternionFloatingJoint - struct /* kQuaternionFloatingJoint */ { - // Source: drake/multibody/plant/multibody_plant.h - const char* doc = R"""(6 dofs, unrestricted orientation.)"""; - } kQuaternionFloatingJoint; - // Symbol: drake::multibody::BaseBodyJointType::kRpyFloatingJoint - struct /* kRpyFloatingJoint */ { - // Source: drake/multibody/plant/multibody_plant.h - const char* doc = R"""(6 dofs using 3 angles; has singularity.)"""; - } kRpyFloatingJoint; - // Symbol: drake::multibody::BaseBodyJointType::kWeldJoint - struct /* kWeldJoint */ { - // Source: drake/multibody/plant/multibody_plant.h - const char* doc = R"""(0 dofs, fixed to World.)"""; - } kWeldJoint; - } BaseBodyJointType; // Symbol: drake::multibody::CalcContactFrictionFromSurfaceProperties struct /* CalcContactFrictionFromSurfaceProperties */ { // Source: drake/multibody/plant/coulomb_friction.h @@ -4968,7 +4942,7 @@ be used by Finalize(); post-finalize it returns the joint type that *was* used if there were any base bodies in need of a joint. See also: - SetBaseBodyJointType())"""; + SetBaseBodyJointType(), Finalize())"""; } GetBaseBodyJointType; // Symbol: drake::multibody::MultibodyPlant::GetBodiesKinematicallyAffectedBy struct /* GetBodiesKinematicallyAffectedBy */ { @@ -6625,7 +6599,10 @@ Parameter ``model_instance``: is to be applied. Raises: - RuntimeError if called after Finalize().)"""; + RuntimeError if called after Finalize(). + +See also: + GetBaseBodyJointType(), Finalize())"""; } SetBaseBodyJointType; // Symbol: drake::multibody::MultibodyPlant::SetConstraintActiveStatus struct /* SetConstraintActiveStatus */ { diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index 54aba9a14629..1b785b905485 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -589,6 +589,32 @@ representation.)"""; const char* doc = R"""()"""; } type_name; } BallRpyJoint; + // Symbol: drake::multibody::BaseBodyJointType + struct /* BaseBodyJointType */ { + // Source: drake/multibody/tree/multibody_tree.h + const char* doc = +R"""(The kind of joint to be used to connect base bodies to world at +Finalize(). See mbp_working_with_free_bodies "Working with free +bodies" for definitions and discussion. + +See also: + SetBaseBodyJointType() for details.)"""; + // Symbol: drake::multibody::BaseBodyJointType::kQuaternionFloatingJoint + struct /* kQuaternionFloatingJoint */ { + // Source: drake/multibody/tree/multibody_tree.h + const char* doc = R"""(6 dofs, unrestricted orientation.)"""; + } kQuaternionFloatingJoint; + // Symbol: drake::multibody::BaseBodyJointType::kRpyFloatingJoint + struct /* kRpyFloatingJoint */ { + // Source: drake/multibody/tree/multibody_tree.h + const char* doc = R"""(6 dofs using 3 angles; has singularity.)"""; + } kRpyFloatingJoint; + // Symbol: drake::multibody::BaseBodyJointType::kWeldJoint + struct /* kWeldJoint */ { + // Source: drake/multibody/tree/multibody_tree.h + const char* doc = R"""(0 dofs, fixed to World.)"""; + } kWeldJoint; + } BaseBodyJointType; // Symbol: drake::multibody::BodyIndex struct /* BodyIndex */ { // Source: drake/multibody/tree/multibody_tree_indexes.h @@ -2113,30 +2139,39 @@ plant.)"""; // Source: drake/multibody/tree/frame.h const char* doc = R"""(%Frame is an abstract class representing a *material frame* (also -called a *physical frame*) of its underlying RigidBody. The Frame's -origin is a material point of its RigidBody, and its axes have fixed -directions in that body. A Frame's pose (position and orientation) -with respect to its RigidBodyFrame may be parameterized, but is fixed -(not time or state dependent) once parameters have been set. +called a *physical frame*) of its underlying RigidBody (Link). The +Frame's origin is a material point of its RigidBody, and its axes have +fixed directions in that body. A Frame's pose (position and +orientation) with respect to its RigidBodyFrame (LinkFrame) may be +parameterized, but is fixed (not time or state dependent) once +parameters have been set. An important characteristic of a Frame is that forces or torques -applied to a Frame are applied to the Frame's underlying RigidBody. +applied to a Frame are applied to the Frame's underlying body. Force-producing elements like joints, actuators, and constraints usually employ two Frames, with one Frame connected to one body and the other connected to a different body. Every Frame F can report the -RigidBody B to which it is attached and its pose X_BF with respect to -B's RigidBodyFrame. +Link (RigidBody) L to which it is attached and its pose X_LF with +respect to L's LinkFrame (RigidBodyFrame). A Frame's pose in World (or relative to other frames) is always calculated starting with its pose relative to its underlying -RigidBodyFrame. Subclasses derived from Frame differ in how kinematic +LinkFrame. Subclasses derived from Frame differ in how kinematic calculations are performed. For example, the angular velocity of a -FixedOffsetFrame or RigidBodyFrame is identical to the angular -velocity of its underlying body, whereas the translational velocity of -a FixedOffsetFrame differs from that of a RigidBodyFrame. +FixedOffsetFrame or LinkFrame (RigidBodyFrame) is identical to the +angular velocity of its underlying body, whereas the translational +velocity of a FixedOffsetFrame differs from that of a LinkFrame. Frame provides methods for obtaining its current orientation, -position, motion, etc. from a Context passed to those methods.)"""; +position, motion, etc. from a Context passed to those methods. + +Note: + For historical reasons, many of the method names here use "Body" + to mean "Link". The distinction matters when we form composite + bodies, which consist of multiple links welded together. Those + composites form a single *rigid body* in the physics sense. Frames + only know about their Links, not how they may have been combined + into a composite body.)"""; // Symbol: drake::multibody::Frame::CalcAngularVelocity struct /* CalcAngularVelocity */ { // Source: drake/multibody/tree/frame.h @@ -2169,23 +2204,23 @@ See also: // Source: drake/multibody/tree/frame.h const char* doc = R"""(Given the offset pose ``X_FQ`` of a frame Q in ``this`` frame F, this -method computes the pose ``X_BQ`` of frame Q in the body frame B to -which this frame is attached. In other words, if the pose of ``this`` -frame F in the body frame B is ``X_BF``, this method computes the pose -``X_BQ`` of frame Q in the body frame B as ``X_BQ = X_BF * X_FQ``. In -particular, if ``this`` **is** the body frame B, i.e. ``X_BF`` is the -identity transformation, this method directly returns ``X_FQ``. -Specific frame subclasses can override this method to provide faster -implementations if needed.)"""; +method computes the pose ``X_LQ`` of frame Q in the link frame L of +the Link (RigidBody) to which this frame is attached. In other words, +if the pose of ``this`` frame F in the link frame L is ``X_LF``, this +method computes the pose ``X_LQ`` of frame Q in the link frame L as +``X_LQ = X_LF * X_FQ``. In particular, if ``this`` **is** the link +frame L, i.e. ``X_LF`` is identically the identity transform, this +method directly returns ``X_FQ``. Specific frame subclasses can +override this method to provide faster implementations if needed.)"""; } CalcOffsetPoseInBody; // Symbol: drake::multibody::Frame::CalcOffsetRotationMatrixInBody struct /* CalcOffsetRotationMatrixInBody */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Calculates and returns the rotation matrix ``R_BQ`` that relates body -frame B to frame Q via ``this`` intermediate frame F, i.e., ``R_BQ = -R_BF * R_FQ`` (B is the body frame to which ``this`` frame F is -attached). +R"""(Calculates and returns the rotation matrix ``R_LQ`` that relates link +frame L to frame Q via ``this`` intermediate frame F, i.e., ``R_LQ = +R_LF * R_FQ`` (L is the link frame of the Link (RigidBody) to which +``this`` frame F is attached). Parameter ``R_FQ``: rotation matrix that relates frame F to frame Q.)"""; @@ -2205,11 +2240,11 @@ See also: struct /* CalcPoseInBodyFrame */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Returns the pose ``X_BF`` of ``this`` frame F in the body frame B -associated with this frame. In particular, if ``this`` **is** the body -frame B, this method directly returns the identity transformation. -Note that this ONLY depends on the Parameters in the context; it does -not depend on time, input, state, etc.)"""; +R"""(Returns the pose ``X_LF`` of ``this`` frame F in the LinkFrame +(RigidBodyFrame) L of this Frame's Link (RigidBody). In particular, if +``this`` **is** the link frame L, this method directly returns the +identity transformation. Note that this ONLY depends on the Parameters +in the context; it does not depend on time, input, state, etc.)"""; } CalcPoseInBodyFrame; // Symbol: drake::multibody::Frame::CalcPoseInWorld struct /* CalcPoseInWorld */ { @@ -2221,13 +2256,13 @@ world frame W as a function of the state of the model stored in Note: RigidBody∷EvalPoseInWorld() provides a more efficient way to - obtain the pose for a body frame.)"""; + obtain the pose for a RigidBodyFrame (LinkFrame).)"""; } CalcPoseInWorld; // Symbol: drake::multibody::Frame::CalcRelativeSpatialAcceleration struct /* CalcRelativeSpatialAcceleration */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Calculates ``this`` frame C's spatial acceleration relative to another +R"""(Calculates ``this`` frame F's spatial acceleration relative to another frame B, measured in a frame M, expressed in a frame E. Parameter ``context``: @@ -2243,14 +2278,14 @@ Parameter ``expressed_in_frame``: which is frame E. Returns: - A_M_BC_E = A_MC_E - A_MB_E, frame C's spatial acceleration + A_M_BF_E = A_MF_E - A_MB_E, frame F's spatial acceleration relative to frame B, measured in frame M, expressed in frame E. -In general, A_M_BC = DtW(V_M_BC), the time-derivative in frame M of -frame C's spatial velocity relative to frame B. The rotational part of -the returned quantity is α_MC_E - α_MB_E = DtM(ω_BC)_E. Note: For 3D -analysis, DtM(ω_BC) ≠ α_BC. The translational part of the returned -quantity is a_M_BoCo_E (Co's translational acceleration relative to +In general, A_M_BF = DtW(V_M_BF), the time-derivative in frame M of +frame F's spatial velocity relative to frame B. The rotational part of +the returned quantity is α_MF_E - α_MB_E = DtM(ω_BF)_E. Note: For 3D +analysis, DtM(ω_BF) ≠ α_BF. The translational part of the returned +quantity is a_M_BoFo_E (Fo's translational acceleration relative to Bo, measured in frame M, expressed in frame E). @@ -2260,20 +2295,20 @@ Bo, measured in frame M, expressed in frame E). .. code-block:: c++ - α_MC_E - α_MB_E = DtM(ω_MC)_E - DtM(ω_MB)_E = DtM(ω_BC)_E - a_M_BoCo_E = a_MCo_E - a_MBo_E = DtM(v_MCo) - DtM(v_MBo) = Dt²M(p_BoCo)_E + α_MF_E - α_MB_E = DtM(ω_MF)_E - DtM(ω_MB)_E = DtM(ω_BF)_E + a_M_BoFo_E = a_MFo_E - a_MBo_E = DtM(v_MFo) - DtM(v_MBo) = Dt²M(p_BoFo)_E .. raw:: html -where Dt²M(p_BoCo)_E is the 2ⁿᵈ time-derivative in frame M of p_BoCo -(the position vector from Bo to Co), and this result is expressed in +where Dt²M(p_BoFo)_E is the 2ⁿᵈ time-derivative in frame M of p_BoFo +(the position vector from Bo to Fo), and this result is expressed in frame E. Note: The calculation of the 2ⁿᵈ time-derivative of the distance between - Bo and Co can be done with relative translational acceleration, + Bo and Fo can be done with relative translational acceleration, but this calculation does not depend on the measured-in-frame, hence in this case, consider CalcRelativeSpatialAccelerationInWorld() since it is faster. @@ -2286,7 +2321,7 @@ See also: struct /* CalcRelativeSpatialAccelerationInWorld */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Calculates ``this`` frame C's spatial acceleration relative to another +R"""(Calculates ``this`` frame F's spatial acceleration relative to another frame B, measured and expressed in the world frame W. Parameter ``context``: @@ -2296,14 +2331,14 @@ Parameter ``other_frame``: which is frame B. Returns: - A_W_BC_W = A_WC_W - A_WB_W, frame C's spatial acceleration + A_W_BF_W = A_WF_W - A_WB_W, frame F's spatial acceleration relative to frame B, measured and expressed in the world frame W. -In general, A_W_BC = DtW(V_W_BC), the time-derivative in the world -frame W of frame C's spatial velocity relative to frame B. The -rotational part of the returned quantity is α_WC_W - α_WB_W = -DtW(ω_BC)_W. For 3D analysis, DtW(ω_BC) ≠ α_BC. The translational part -of the returned quantity is a_W_BoCo_W (Co's translational +In general, A_W_BF = DtW(V_W_BF), the time-derivative in the world +frame W of frame F's spatial velocity relative to frame B. The +rotational part of the returned quantity is α_WF_W - α_WB_W = +DtW(ω_BF)_W. For 3D analysis, DtW(ω_BF) ≠ α_BF. The translational part +of the returned quantity is a_W_BoFo_W (Fo's translational acceleration relative to Bo, measured and expressed in world frame W). @@ -2313,15 +2348,15 @@ acceleration relative to Bo, measured and expressed in world frame W). .. code-block:: c++ - α_WC_W - α_WB_W = DtW(ω_WC)_W - DtW(ω_WB)_W = DtW(ω_BC)_W - a_W_BoCo_W = a_WCo_W - a_WBo_W = DtW(v_WCo) - DtW(v_WBo) = Dt²W(p_BoCo)_W + α_WF_W - α_WB_W = DtW(ω_WF)_W - DtW(ω_WB)_W = DtW(ω_BF)_W + a_W_BoFo_W = a_WFo_W - a_WBo_W = DtW(v_WFo) - DtW(v_WBo) = Dt²W(p_BoFo)_W .. raw:: html -where Dt²W(p_BoCo)_W is the 2ⁿᵈ time-derivative in frame W of p_BoCo -(the position vector from Bo to Co), and this result is expressed in +where Dt²W(p_BoFo)_W is the 2ⁿᵈ time-derivative in frame W of p_BoFo +(the position vector from Bo to Fo), and this result is expressed in frame W. Note: @@ -2337,7 +2372,7 @@ See also: struct /* CalcRelativeSpatialVelocity */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Calculates ``this`` frame C's spatial velocity relative to another +R"""(Calculates ``this`` frame F's spatial velocity relative to another frame B, measured in a frame M, expressed in a frame E. Parameter ``context``: @@ -2353,11 +2388,11 @@ Parameter ``expressed_in_frame``: which is frame E. Returns: - V_M_BC_E = V_MC_E - V_MB_E, frame C's spatial velocity relative to + V_M_BF_E = V_MF_E - V_MB_E, frame F's spatial velocity relative to frame B, measured in frame M, expressed in frame E. The rotational - part of the returned quantity is ω_BC_E (C's angular velocity + part of the returned quantity is ω_BF_E (F's angular velocity measured in B and expressed in E). The translational part is - v_M_BoCo_E (Co's translational velocity relative to Bo, measured + v_M_BoFo_E (Fo's translational velocity relative to Bo, measured in M, and expressed in E). @@ -2367,25 +2402,25 @@ Parameter ``expressed_in_frame``: .. code-block:: c++ - ω_BC_E = ω_MC_E - ω_MB_E - v_M_BoCo_E = v_MCo_E - v_MBo_E = DtM(p_BoCo) + ω_BF_E = ω_MF_E - ω_MB_E + v_M_BoFo_E = v_MFo_E - v_MBo_E = DtM(p_BoFo) .. raw:: html -where DtM(p_BoCo) is the time-derivative in frame M of p_BoCo -(position vector from Bo to Co), and this vector is expressed in frame +where DtM(p_BoFo) is the time-derivative in frame M of p_BoFo +(position vector from Bo to Fo), and this vector is expressed in frame E. Note: The method CalcSpatialVelocity() is more efficient and coherent if any of ``this``, other_frame, or measured_in_frame are the same. - Also, the value of V_M_BoCo does not depend on the - measured_in_frame if Bo and Co are coincident (i.e., p_BoCo = 0), + Also, the value of V_M_BoFo does not depend on the + measured_in_frame if Bo and Fo are coincident (i.e., p_BoFo = 0), in which case consider the more efficient method CalcRelativeSpatialVelocityInWorld(). Lastly, the calculation of - elongation between Bo and Co can be done with relative + elongation between Bo and Fo can be done with relative translational velocity, but elongation does not depend on the measured-in-frame (hence consider CalcRelativeSpatialVelocityInWorld()). @@ -2398,7 +2433,7 @@ See also: struct /* CalcRelativeSpatialVelocityInWorld */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Calculates ``this`` frame C's spatial velocity relative to another +R"""(Calculates ``this`` frame F's spatial velocity relative to another frame B, measured and expressed in the world frame W. Parameter ``context``: @@ -2408,11 +2443,11 @@ Parameter ``other_frame``: which is frame B. Returns: - V_W_BC_W = V_WC_W - V_WB_W, frame C's spatial velocity relative to + V_W_BF_W = V_WF_W - V_WB_W, frame F's spatial velocity relative to frame B, measured and expressed in the world frame W. The - rotational part of the returned quantity is ω_BC_W (C's angular + rotational part of the returned quantity is ω_BF_W (F's angular velocity measured in B and expressed in W). The translational part - is v_W_BoCo_W (Co's translational velocity relative to Bo, + is v_W_BoFo_W (Fo's translational velocity relative to Bo, measured and expressed in world frame W). @@ -2422,15 +2457,15 @@ Parameter ``other_frame``: .. code-block:: c++ - ω_BC_W = ω_WC_W - ω_WB_W - v_W_BoCo_W = v_WCo_W - v_WBo_W = DtW(p_BoCo) + ω_BF_W = ω_WF_W - ω_WB_W + v_W_BoFo_W = v_WFo_W - v_WBo_W = DtW(p_BoFo) .. raw:: html -where DtW(p_BoCo) is the time-derivative in frame W of p_BoCo -(position vector from Bo to Co), and this vector is expressed in frame +where DtW(p_BoFo) is the time-derivative in frame W of p_BoFo +(position vector from Bo to Fo), and this vector is expressed in frame W. Note: @@ -2453,12 +2488,12 @@ R"""(Calculates and returns the rotation matrix ``R_MF`` that relates struct /* CalcRotationMatrixInBodyFrame */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Returns the rotation matrix ``R_BF`` that relates body frame B to -``this`` frame F (B is the body frame to which ``this`` frame F is -attached). +R"""(Returns the rotation matrix ``R_LF`` that relates link frame L to +``this`` frame F (L is the LinkFrame of the Link (RigidBody) to which +``this`` frame F is attached). Note: - If ``this`` is B, this method returns the identity RotationMatrix. + If ``this`` is L, this method returns the identity RotationMatrix. Note that this ONLY depends on the Parameters in the context; it does not depend on time, input, state, etc.)"""; } CalcRotationMatrixInBodyFrame; @@ -2594,8 +2629,8 @@ Parameter ``context``: Note: RigidBody∷EvalSpatialVelocityInWorld() provides a more efficient - way to obtain a body frame's spatial velocity measured in the - world frame. + way to obtain a RigidBodyFrame (LinkFrame) spatial velocity + measured in the world frame. See also: CalcSpatialVelocity(), CalcRelativeSpatialVelocityInWorld(), and @@ -2682,19 +2717,19 @@ See also: struct /* EvalPoseInBodyFrame */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Returns a reference to the body-relative pose X_BF giving the pose of -this Frame with respect to its body's RigidBodyFrame. This may depend -on parameters in the Context but not on time or state. The first time -this is called after a parameter change will precalculate offset poses -for all Frames into the Context's cache; subsequent calls on any Frame -are very fast.)"""; +R"""(Returns a reference to the link-relative pose X_LF giving the pose of +this Frame with respect to its link's LinkFrame (RigidBodyFrame). This +may depend on parameters in the Context but not on time or state. The +first time this is called after a parameter change will precalculate +offset poses for all Frames into the Context's cache; subsequent calls +on any Frame are very fast.)"""; } EvalPoseInBodyFrame; // Symbol: drake::multibody::Frame::Frame struct /* ctor */ { // Source: drake/multibody/tree/frame.h const char* doc = R"""(Only derived classes can use this constructor. It creates a Frame -object attached to ``body`` and puts the frame in the body's model +object attached to ``link`` and puts the frame in the link's model instance.)"""; } ctor; // Symbol: drake::multibody::Frame::GetFixedOffsetPoseInBody @@ -2702,8 +2737,8 @@ instance.)"""; // Source: drake/multibody/tree/frame.h const char* doc = R"""(Variant of CalcOffsetPoseInBody() that given the offset pose ``X_FQ`` -of a frame Q in ``this`` frame F, returns the pose ``X_BQ`` of frame Q -in the body frame B to which this frame is attached. +of a frame Q in ``this`` frame F, returns the pose ``X_LQ`` of frame Q +in the link frame L to which this frame is attached. Raises: RuntimeError if called on a Frame that does not have a fixed @@ -2713,8 +2748,8 @@ in the body frame B to which this frame is attached. struct /* GetFixedPoseInBodyFrame */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Variant of CalcPoseInBodyFrame() that returns the fixed pose ``X_BF`` -of ``this`` frame F in the body frame B associated with this frame. +R"""(Variant of CalcPoseInBodyFrame() that returns the fixed pose ``X_LF`` +of ``this`` frame F in the link frame L associated with this frame. Raises: RuntimeError if called on a Frame that does not have a fixed @@ -2724,33 +2759,31 @@ of ``this`` frame F in the body frame B associated with this frame. struct /* GetFixedRotationMatrixInBody */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Calculates and returns the rotation matrix ``R_BQ`` that relates body -frame B to frame Q via ``this`` intermediate frame F, i.e., ``R_BQ = -R_BF * R_FQ`` (B is the body frame to which ``this`` frame F is -attached). +R"""(Calculates and returns the rotation matrix ``R_LQ`` that relates link +frame L to frame Q via ``this`` intermediate frame F, i.e., ``R_LQ = +R_LF * R_FQ`` (L is the link frame of the Link (RigidBody) to which +``this`` frame F is attached). Parameter ``R_FQ``: rotation matrix that relates frame F to frame Q. Raises: RuntimeError if ``this`` frame F is a Frame that does not have a - fixed offset in the body frame B (i.e., ``R_BF`` is not constant).)"""; + fixed offset in the link frame L (i.e., ``R_LF`` is not constant).)"""; } GetFixedRotationMatrixInBody; // Symbol: drake::multibody::Frame::GetFixedRotationMatrixInBodyFrame struct /* GetFixedRotationMatrixInBodyFrame */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Returns the rotation matrix ``R_BF`` that relates body frame B to -``this`` frame F (B is the body frame to which ``this`` frame F is -attached). +R"""(Returns the rotation matrix ``R_LF`` that relates link frame L to +``this`` frame F (L is the LinkFrame of the Link (RigidBody) to which +``this`` frame F is attached). Raises: RuntimeError if ``this`` frame F is a Frame that does not have a - fixed offset in the body frame B (i.e., ``R_BF`` is not constant). - Frame sub-classes that have a constant ``R_BF`` must override this - method. An example of a frame sub-class not implementing this - method would be that of a frame on a soft body, for which its pose - in the body frame depends on the state of deformation of the body.)"""; + fixed offset in the link frame L (i.e., ``R_LF`` is not constant). + Frame sub-classes that have a constant ``R_LF`` must override this + method.)"""; } GetFixedRotationMatrixInBodyFrame; // Symbol: drake::multibody::Frame::ShallowClone struct /* ShallowClone */ { @@ -2764,21 +2797,23 @@ any MbT (so the assigned index, if any, is discarded).)"""; struct /* body */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Returns a const reference to the body associated to this Frame.)"""; +R"""(Returns a const reference to the RigidBody (Link) to which this Frame +is attached (synonym for link()).)"""; } body; // Symbol: drake::multibody::Frame::get_X_BF struct /* get_X_BF */ { // Source: drake/multibody/tree/frame.h const char* doc = R"""((Internal use only) Given an already up-to-date frame body pose cache, -extract X_BF for this Frame from it. +extract X_BF for this Frame from it. Note that X_BF is F's pose on its +mobilized body B which might not be the same as its link L. Note: Be sure you have called MultibodyTreeSystem∷EvalFrameBodyPoses() since the last parameter change; we can't check here. Returns ``X_BF``: - pose of this frame in its body's frame)"""; + pose of this frame in its Mobod's frame)"""; } get_X_BF; // Symbol: drake::multibody::Frame::get_X_FB struct /* get_X_FB */ { @@ -2792,15 +2827,22 @@ extract X_FB (=X_BF⁻¹) for this Frame from it. since the last parameter change; we can't check here. Returns ``X_FB``: - inverse of this frame's pose in its body's frame)"""; + inverse of this frame's pose in its Mobod's frame)"""; } get_X_FB; - // Symbol: drake::multibody::Frame::get_body_pose_index_in_cache - struct /* get_body_pose_index_in_cache */ { + // Symbol: drake::multibody::Frame::get_X_LF + struct /* get_X_LF */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""((Internal use only) Retrieve this Frame's body pose index in the -cache.)"""; - } get_body_pose_index_in_cache; +R"""((Internal use only) Given an already up-to-date frame body pose cache, +extract X_LF for this Frame from it. + +Note: + Be sure you have called MultibodyTreeSystem∷EvalFrameBodyPoses() + since the last parameter change; we can't check here. + +Returns ``X_LF``: + pose of this frame in its Link's frame)"""; + } get_X_LF; // Symbol: drake::multibody::Frame::index struct /* index */ { // Source: drake/multibody/tree/frame.h @@ -2819,20 +2861,35 @@ precomputed in the cache so is very fast to check. since the last parameter change; we can't check here. See also: - get_X_BF())"""; + get_X_BF(), get_X_FB())"""; } is_X_BF_identity; // Symbol: drake::multibody::Frame::is_body_frame struct /* is_body_frame */ { // Source: drake/multibody/tree/frame.h const char* doc = -R"""(Returns true if ``this`` is the body frame.)"""; +R"""(Returns true if ``this`` is the RigidBodyFrame (LinkFrame) of the +associated RigidBody (Link). This is a synonym for is_link_frame().)"""; } is_body_frame; + // Symbol: drake::multibody::Frame::is_link_frame + struct /* is_link_frame */ { + // Source: drake/multibody/tree/frame.h + const char* doc = +R"""(Returns true if ``this`` is the LinkFrame (RigidBodyFrame) of the +associated Link (RigidBody). This is a synonym for is_body_frame().)"""; + } is_link_frame; // Symbol: drake::multibody::Frame::is_world_frame struct /* is_world_frame */ { // Source: drake/multibody/tree/frame.h const char* doc = R"""(Returns true if ``this`` is the world frame.)"""; } is_world_frame; + // Symbol: drake::multibody::Frame::link + struct /* link */ { + // Source: drake/multibody/tree/frame.h + const char* doc = +R"""(Returns a const reference to the Link (RigidBody) to which this Frame +is attached (synonym for body()).)"""; + } link; // Symbol: drake::multibody::Frame::name struct /* name */ { // Source: drake/multibody/tree/frame.h @@ -2850,26 +2907,6 @@ name will be empty (the scope name and the element name). RuntimeError if this element is not associated with a MultibodyPlant.)"""; } scoped_name; - // Symbol: drake::multibody::Frame::set_body_pose_index_in_cache - struct /* set_body_pose_index_in_cache */ { - // Source: drake/multibody/tree/frame.h - const char* doc = -R"""((Internal use only) A Frame's pose-in-parent X_PF can be -parameterized, the parent's pose may also be parameterized, and so on. -Thus the calculation of this frame's pose in its body (X_BF) can be -expensive. There is a cache entry that holds the calculated X_BF, -evaluated whenever parameters change. This allows us to grab X_BF as a -const reference rather than having to extract and reformat parameters, -and compose with parent and ancestor poses at runtime. - -At the time parameters are allocated we assign a slot in the body pose -cache entry to each Frame and record its index using this function. -(The index for a RigidBodyFrame will refer to an identity transform.) -Note that the body pose index is not necessarily the same as the Frame -index because all RigidBodyFrames can share an entry. (Of course if -you know you are working with a RigidBodyFrame you don't need to ask -about its body pose!))"""; - } set_body_pose_index_in_cache; } Frame; // Symbol: drake::multibody::FrameIndex struct /* FrameIndex */ { @@ -4979,8 +5016,8 @@ to choose torque stiffness and damping constants" for more details.)"""; // Source: drake/multibody/tree/linear_spring_damper.h const char* doc = R"""(This ForceElement models a spring-damper attached between two points -on two different bodies. Given a point P on a body A and a point Q on -a body B with positions p_AP and p_BQ, respectively, this +on two different bodies (links). Given a point P on a body A and a +point Q on a body B with positions p_AP and p_BQ, respectively, this spring-damper applies equal and opposite forces on bodies A and B according to: @@ -5115,6 +5152,17 @@ body frame B.)"""; const char* doc = R"""()"""; } stiffness; } LinearSpringDamper; + // Symbol: drake::multibody::LinkIndex + struct /* LinkIndex */ { + // Source: drake/multibody/tree/multibody_tree_indexes.h + const char* doc = R"""(This is a synonym for BodyIndex.)"""; + } LinkIndex; + // Symbol: drake::multibody::LinkOrdinal + struct /* LinkOrdinal */ { + // Source: drake/multibody/tree/multibody_tree_indexes.h + const char* doc = +R"""(Type used to identify links by ordinal within a multibody plant.)"""; + } LinkOrdinal; // Symbol: drake::multibody::ModelInstanceIndex struct /* ModelInstanceIndex */ { // Source: drake/multibody/tree/multibody_tree_indexes.h @@ -7322,7 +7370,7 @@ R"""(For a floating base RigidBody, unlock its inboard joint. struct /* body_frame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Returns a const reference to the associated BodyFrame.)"""; +R"""((Compatibility) A synonym for link_frame().)"""; } body_frame; // Symbol: drake::multibody::RigidBody::default_com struct /* default_com */ { @@ -7660,6 +7708,13 @@ generally Joint∷is_locked() is preferable otherwise. Returns: true if the body is locked, false otherwise.)"""; } is_locked; + // Symbol: drake::multibody::RigidBody::link_frame + struct /* link_frame */ { + // Source: drake/multibody/tree/rigid_body.h + const char* doc = +R"""(Returns a const reference to the associated LinkFrame +(RigidBodyFrame).)"""; + } link_frame; // Symbol: drake::multibody::RigidBody::mobod_index struct /* mobod_index */ { // Source: drake/multibody/tree/rigid_body.h @@ -7676,6 +7731,14 @@ the index into all associated quantities.)"""; R"""(Gets the ``name`` associated with this rigid body. The name will never be empty.)"""; } name; + // Symbol: drake::multibody::RigidBody::ordinal + struct /* ordinal */ { + // Source: drake/multibody/tree/rigid_body.h + const char* doc = +R"""((Internal use only) Returns this Link's (RigidBody's) unique ordinal. +Currently identical to the index but will differ when we permit +removal of Links as we do for Joints.)"""; + } ordinal; // Symbol: drake::multibody::RigidBody::scoped_name struct /* scoped_name */ { // Source: drake/multibody/tree/rigid_body.h diff --git a/bindings/pydrake/multibody/BUILD.bazel b/bindings/pydrake/multibody/BUILD.bazel index 77351ec82c83..7080e12c300f 100644 --- a/bindings/pydrake/multibody/BUILD.bazel +++ b/bindings/pydrake/multibody/BUILD.bazel @@ -156,6 +156,7 @@ drake_pybind_library( name = "plant_py", cc_deps = [ "//bindings/generated_docstrings:multibody_plant", + "//bindings/generated_docstrings:multibody_tree", "//bindings/pydrake/common:cpp_template_pybind", "//bindings/pydrake/common:default_scalars_pybind", "//bindings/pydrake/common:deprecation_pybind", diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index 1476fa9e8857..27a5d23b8b62 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -5,6 +5,7 @@ #include #include "drake/bindings/generated_docstrings/multibody_plant.h" +#include "drake/bindings/generated_docstrings/multibody_tree.h" #include "drake/bindings/pydrake/common/cpp_template_pybind.h" #include "drake/bindings/pydrake/common/default_scalars_pybind.h" #include "drake/bindings/pydrake/common/deprecation_pybind.h" @@ -1776,8 +1777,11 @@ PYBIND11_MODULE(plant, m) { } { + // This enum comes from a MultibodyTree header but for historical + // reasons we've bound it in plant. using Class = BaseBodyJointType; - constexpr auto& cls_doc = doc.BaseBodyJointType; + constexpr auto& tree_doc = pydrake_doc_multibody_tree.drake.multibody; + constexpr auto& cls_doc = tree_doc.BaseBodyJointType; py::enum_ cls(m, "BaseBodyJointType", cls_doc.doc); cls.value("kQuaternionFloatingJoint", Class::kQuaternionFloatingJoint, cls_doc.kQuaternionFloatingJoint.doc) diff --git a/multibody/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h b/multibody/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h index da5e9c84a996..0ef9dfef8392 100644 --- a/multibody/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h +++ b/multibody/benchmarks/kuka_iiwa_robot/drake_kuka_iiwa_robot.h @@ -85,16 +85,16 @@ class DrakeKukaIIwaRobot { MakeKukaIiwaModel(false /* finalized model */, gravity /* acceleration of gravity */)); - linkN_ = &tree().world_body(); + linkN_ = &tree().world_link(); // Get this robot's seven links. - linkA_ = &tree().GetRigidBodyByName("iiwa_link_1"); - linkB_ = &tree().GetRigidBodyByName("iiwa_link_2"); - linkC_ = &tree().GetRigidBodyByName("iiwa_link_3"); - linkD_ = &tree().GetRigidBodyByName("iiwa_link_4"); - linkE_ = &tree().GetRigidBodyByName("iiwa_link_5"); - linkF_ = &tree().GetRigidBodyByName("iiwa_link_6"); - linkG_ = &tree().GetRigidBodyByName("iiwa_link_7"); + linkA_ = &tree().GetLinkByName("iiwa_link_1"); + linkB_ = &tree().GetLinkByName("iiwa_link_2"); + linkC_ = &tree().GetLinkByName("iiwa_link_3"); + linkD_ = &tree().GetLinkByName("iiwa_link_4"); + linkE_ = &tree().GetLinkByName("iiwa_link_5"); + linkF_ = &tree().GetLinkByName("iiwa_link_6"); + linkG_ = &tree().GetLinkByName("iiwa_link_7"); // Get this robot's seven joints. NA_joint_ = &tree().template GetJointByName("iiwa_joint_1"); @@ -111,7 +111,7 @@ class DrakeKukaIIwaRobot { /// This method gets the number of rigid bodies in this robot. /// @returns the number of rigid bodies in this robot. - int get_number_of_rigid_bodies() const { return tree().num_bodies(); } + int get_number_of_rigid_bodies() const { return tree().num_links(); } /// This method calculates kinematic properties of the end-effector (herein /// denoted as rigid body G) of a 7-DOF KUKA LBR iiwa robot (14 kg payload). @@ -145,7 +145,7 @@ class DrakeKukaIIwaRobot { const SpatialVelocity& V_NG_N = vc.get_V_WB(linkG_->mobod_index()); // Retrieve end-effector spatial acceleration from acceleration cache. - std::vector> A_WB(tree().num_bodies()); + std::vector> A_WB(tree().num_links()); // TODO(eric.cousineau): For this model, the end effector's BodyIndex // matches its MobodIndex, thus we're not really checking the difference // between MultibodyPlant and MultibodyTree's ordering. diff --git a/multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.cc b/multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.cc index dd17bb2b4324..8155326df123 100644 --- a/multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.cc +++ b/multibody/benchmarks/kuka_iiwa_robot/make_kuka_iiwa_model.cc @@ -75,13 +75,13 @@ KukaIiwaModelBuilder::Build() const { I_GGcm_G_); // Add this robot's seven links. - const RigidBody& linkA = model->AddRigidBody("iiwa_link_1", M_AAo_A); - const RigidBody& linkB = model->AddRigidBody("iiwa_link_2", M_BBo_B); - const RigidBody& linkC = model->AddRigidBody("iiwa_link_3", M_CCo_C); - const RigidBody& linkD = model->AddRigidBody("iiwa_link_4", M_DDo_D); - const RigidBody& linkE = model->AddRigidBody("iiwa_link_5", M_EEo_E); - const RigidBody& linkF = model->AddRigidBody("iiwa_link_6", M_FFo_F); - const RigidBody& linkG = model->AddRigidBody("iiwa_link_7", M_GGo_G); + const Link& linkA = model->AddLink("iiwa_link_1", M_AAo_A); + const Link& linkB = model->AddLink("iiwa_link_2", M_BBo_B); + const Link& linkC = model->AddLink("iiwa_link_3", M_CCo_C); + const Link& linkD = model->AddLink("iiwa_link_4", M_DDo_D); + const Link& linkE = model->AddLink("iiwa_link_5", M_EEo_E); + const Link& linkF = model->AddLink("iiwa_link_6", M_FFo_F); + const Link& linkG = model->AddLink("iiwa_link_7", M_GGo_G); // Create a revolute joint between linkN (Newtonian frame/world) and linkA // using two joint-frames, namely "Na" and "An". The "inboard frame" Na is @@ -91,7 +91,7 @@ KukaIiwaModelBuilder::Build() const { // angles and a position vector. Alternately, frame An is regarded as // coincident with linkA. const Joint* joint{nullptr}; - const RigidBody& linkN = model->world_body(); + const Link& linkN = model->world_link(); joint = &AddRevoluteJointFromSpaceXYZAnglesAndXYZ( "iiwa_joint_1", linkN, joint_1_rpy_, joint_1_xyz_, linkA, Eigen::Vector3d::UnitZ(), model.get()); @@ -109,7 +109,7 @@ KukaIiwaModelBuilder::Build() const { Eigen::Vector3d::UnitZ(), model.get()); model->AddJointActuator("iiwa_actuator_3", *joint); - // Create a revolute joint between linkB and linkC. + // Create a revolute joint between linkC and linkD. joint = &AddRevoluteJointFromSpaceXYZAnglesAndXYZ( "iiwa_joint_4", linkC, joint_4_rpy_, joint_4_xyz_, linkD, Eigen::Vector3d::UnitZ(), model.get()); diff --git a/multibody/math/spatial_vector.h b/multibody/math/spatial_vector.h index 642a10cc4197..087ac1221662 100644 --- a/multibody/math/spatial_vector.h +++ b/multibody/math/spatial_vector.h @@ -291,6 +291,14 @@ class SpatialVector { /// rotational and translational components are both zero. static SpatialQuantity Zero() { return SpatialQuantity{}.SetZero(); } + /// Factory to create a _NaN_ spatial vector, i.e., a %SpatialVector whose + /// rotational and translational components are all NaN. + static SpatialQuantity NaN() { + SpatialQuantity quantity; + quantity.SetNaN(); + return quantity; + } + private: // Helper method to return a mutable reference to the derived spatial // quantity. diff --git a/multibody/math/test/spatial_algebra_test.cc b/multibody/math/test/spatial_algebra_test.cc index 16944cfdce13..029c8e35406a 100644 --- a/multibody/math/test/spatial_algebra_test.cc +++ b/multibody/math/test/spatial_algebra_test.cc @@ -110,6 +110,27 @@ TYPED_TEST(SpatialQuantityTest, ZeroFactory) { EXPECT_TRUE(V.translational() == Vector3::Zero()); } +// Construction of a "NaN" spatial vector. +TYPED_TEST(SpatialQuantityTest, NaNFactory) { + typedef typename TestFixture::SpatialQuantityType SpatialQuantity; + typedef typename TestFixture::ScalarType T; + SpatialQuantity V = SpatialQuantity::NaN(); + auto is_nan = [](const T& x) -> bool { + if constexpr (std::is_same_v) { + return symbolic::is_nan(x); + } + if constexpr (std::is_same_v) { + return std::isnan(x.value()); + } + if constexpr (std::is_same_v) { + return std::isnan(x); + } + }; + for (int i = 0; i < V.size(); ++i) { + EXPECT_TRUE(is_nan(V[i])); + } +} + // Tests: // - Construction from a Eigen expressions. // - SetZero() method. diff --git a/multibody/plant/deformable_model.cc b/multibody/plant/deformable_model.cc index af011e5be3a7..87a9910acaee 100644 --- a/multibody/plant/deformable_model.cc +++ b/multibody/plant/deformable_model.cc @@ -333,7 +333,7 @@ template void DeformableModel::SetParallelism(Parallelism parallelism) { parallelism_ = parallelism; const std::vector& body_indices = - deformable_bodies_.indices(); + deformable_bodies_.valid_indices(); for (const DeformableBodyIndex& index : body_indices) { deformable_bodies_.get_mutable_element(index).set_parallelism(parallelism); } @@ -346,7 +346,8 @@ void DeformableModel::SetDefaultState(const systems::Context& context, DRAKE_DEMAND(is_empty()); return; } else { - for (const DeformableBodyIndex& index : deformable_bodies_.indices()) { + for (const DeformableBodyIndex& index : + deformable_bodies_.valid_indices()) { const DeformableBody& body = deformable_bodies_.get_element(index); body.SetDefaultState(context, state); } @@ -372,7 +373,8 @@ std::unique_ptr> DeformableModel::CloneToDouble( /* Copy over deformable_bodies_. */ result->deformable_bodies_.ResizeToMatch(deformable_bodies_); - for (const DeformableBodyIndex& index : deformable_bodies_.indices()) { + for (const DeformableBodyIndex& index : + deformable_bodies_.valid_indices()) { const DeformableBody& body = deformable_bodies_.get_element(index); result->deformable_bodies_.Add(body.CloneToDouble()); } @@ -430,7 +432,7 @@ void DeformableModel::DoDeclareSystemResources() { /* Declare discrete states and parameters. */ const std::vector& body_indices = - deformable_bodies_.indices(); + deformable_bodies_.valid_indices(); for (const DeformableBodyIndex& index : body_indices) { DeformableBody& body = deformable_bodies_.get_mutable_element(index); body.DeclareDiscreteState(static_cast*>( @@ -482,7 +484,7 @@ void DeformableModel::CopyVertexPositions(const systems::Context& context, output->get_mutable_value>(); output_value.clear(); const std::vector& body_indices = - deformable_bodies_.indices(); + deformable_bodies_.valid_indices(); for (const DeformableBodyIndex& index : body_indices) { const DeformableBody& body = deformable_bodies_.get_element(index); const GeometryId geometry_id = body.geometry_id(); diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 0dea92dfbff2..cc7c986af608 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1224,8 +1224,8 @@ std::vector MultibodyPlant::GetBodiesKinematicallyAffectedBy( __func__, joint)); } } - const std::set links = - internal_tree().GetBodiesKinematicallyAffectedBy(joint_indexes); + const std::set links = + internal_tree().GetLinksKinematicallyAffectedBy(joint_indexes); // TODO(sherm1) Change the return type to set to avoid this copy. return std::vector(links.cbegin(), links.cend()); } @@ -1350,7 +1350,7 @@ void MultibodyPlant::CalcSpatialAccelerationsFromVdot( // Make sure there aren't any inactives for now. DRAKE_DEMAND(ssize(mobod.follower_link_ordinals()) == 1); const BodyIndex active_link_index = - forest.links(mobod.link_ordinal()).index(); + forest.links(mobod.active_link_ordinal()).index(); (*A_WB_array)[active_link_index] = A_WB_array_mobod[mobod.index()]; } } @@ -1418,43 +1418,13 @@ template void MultibodyPlant::SetBaseBodyJointType( BaseBodyJointType joint_type, std::optional model_instance) { - std::optional options; - switch (joint_type) { - case BaseBodyJointType::kQuaternionFloatingJoint: - options = internal::ForestBuildingOptions::kDefault; - break; - case BaseBodyJointType::kRpyFloatingJoint: - options = internal::ForestBuildingOptions::kUseRpyFloatingJoints; - break; - case BaseBodyJointType::kWeldJoint: - options = internal::ForestBuildingOptions::kUseFixedBase; - break; - } - DRAKE_DEMAND(options.has_value()); - DRAKE_THROW_UNLESS(!is_finalized()); - internal::LinkJointGraph& graph = mutable_tree().mutable_graph(); - if (model_instance.has_value()) { - graph.SetForestBuildingOptions(*model_instance, *options); - } else { - graph.SetGlobalForestBuildingOptions(*options); - } + mutable_tree().SetBaseBodyJointType(joint_type, model_instance); } template BaseBodyJointType MultibodyPlant::GetBaseBodyJointType( std::optional model_instance) const { - const internal::LinkJointGraph& graph = internal_tree().graph(); - const internal::ForestBuildingOptions options = - model_instance.has_value() - ? graph.get_forest_building_options_in_use(*model_instance) - : graph.get_global_forest_building_options(); - if (static_cast(options & - internal::ForestBuildingOptions::kUseRpyFloatingJoints)) - return BaseBodyJointType::kRpyFloatingJoint; - if (static_cast(options & - internal::ForestBuildingOptions::kUseFixedBase)) - return BaseBodyJointType::kWeldJoint; - return BaseBodyJointType::kQuaternionFloatingJoint; + return internal_tree().GetBaseBodyJointType(model_instance); } template @@ -2690,7 +2660,7 @@ void MultibodyPlant::AddAppliedExternalSpatialForces( HasNaN(spatial_force.translational())) { throw std::runtime_error(fmt::format( "Spatial force applied on body {} contains NaN.", - internal_tree().get_body(external_spatial_force.body_index).name())); + internal_tree().get_link(external_spatial_force.body_index).name())); } }; // Loop over all forces. @@ -4060,13 +4030,13 @@ void MultibodyPlant::DeclareSceneGraphPorts() { template void MultibodyPlant::CalcBodyPosesOutput( const Context& context, - std::vector>* outupt) const { + std::vector>* output) const { DRAKE_MBP_THROW_IF_NOT_FINALIZED(); this->ValidateContext(context); - outupt->resize(num_bodies()); + output->resize(num_bodies()); for (BodyIndex body_index(0); body_index < this->num_bodies(); ++body_index) { const RigidBody& body = get_body(body_index); - outupt->at(body_index) = EvalBodyPoseInWorld(context, body); + output->at(body_index) = EvalBodyPoseInWorld(context, body); } } diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 3156ca522194..9cae1bb6affa 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -245,16 +245,6 @@ enum class DiscreteContactApproximation { kLagged, }; -/// The kind of joint to be used to connect base bodies to world at Finalize(). -/// See @ref mbp_working_with_free_bodies "Working with free bodies" -/// for definitions and discussion. -/// @see SetBaseBodyJointType() for details. -enum class BaseBodyJointType { - kQuaternionFloatingJoint, ///< 6 dofs, unrestricted orientation. - kRpyFloatingJoint, ///< 6 dofs using 3 angles; has singularity. - kWeldJoint, ///< 0 dofs, fixed to World. -}; - /// @cond // Helper macro to throw an exception within methods that should not be called // post-finalize. @@ -1267,6 +1257,10 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { const systems::OutputPort& get_generalized_contact_forces_output_port( ModelInstanceIndex model_instance) const; + // TODO(sherm1) Modify the next comment to explain that unmodeled joints will + // have NaN reaction force entries here. (Joints can be unmodeled because + // they were removed or because they are internal to a composite body.) + /// Reports joint reaction forces as an @ref AbstractValue "abstract-valued" /// output port containing an `std::vector>` of size /// num_joints(). @@ -1424,9 +1418,9 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { const std::string& name, ModelInstanceIndex model_instance, const SpatialInertia& M_BBo_B = SpatialInertia::Zero()) { DRAKE_MBP_THROW_IF_FINALIZED(); - // Add the actual rigid body to the model. + // Add the actual RigidBody (Link) to the model. const RigidBody& body = - this->mutable_tree().AddRigidBody(name, model_instance, M_BBo_B); + this->mutable_tree().AddLink(name, model_instance, M_BBo_B); // Each entry of visual_geometries_, ordered by body index, contains a // std::vector of geometry ids for that body. The emplace_back() below // resizes visual_geometries_ to store the geometry ids for the body we @@ -1766,6 +1760,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// @param[in] model_instance (optional) the index of the model instance to /// which `joint_type` is to be applied. /// @throws std::exception if called after Finalize(). + /// @see GetBaseBodyJointType(), Finalize() void SetBaseBodyJointType( BaseBodyJointType joint_type, std::optional model_instance = {}); @@ -1778,7 +1773,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// This can be called any time -- pre-finalize it returns the joint type /// that will be used by Finalize(); post-finalize it returns the joint type /// that _was_ used if there were any base bodies in need of a joint. - /// @see SetBaseBodyJointType() + /// @see SetBaseBodyJointType(), Finalize() BaseBodyJointType GetBaseBodyJointType( std::optional model_instance = {}) const; @@ -2476,7 +2471,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { const RigidBody* GetBodyFromFrameId(geometry::FrameId frame_id) const { const auto it = frame_id_to_body_index_.find(frame_id); if (it == frame_id_to_body_index_.end()) return nullptr; - return &internal_tree().get_body(it->second); + return &internal_tree().get_link(it->second); } /// If the body with `body_index` belongs to the called plant, it returns @@ -2499,7 +2494,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { const auto it = body_index_to_frame_id_.find(body_index); if (it == body_index_to_frame_id_.end()) { throw std::logic_error("Body '" + - internal_tree().get_body(body_index).name() + + internal_tree().get_link(body_index).name() + "' does not have geometry registered with it."); } return it->second; @@ -3764,7 +3759,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { const math::RigidTransform& EvalBodyPoseInWorld( const systems::Context& context, const RigidBody& body_B) const { this->ValidateContext(context); - return internal_tree().EvalBodyPoseInWorld(context, body_B); + return internal_tree().EvalLinkPoseInWorld(context, body_B); } /// Evaluates V_WB, body B's spatial velocity in the world frame W. @@ -3777,7 +3772,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { const SpatialVelocity& EvalBodySpatialVelocityInWorld( const systems::Context& context, const RigidBody& body_B) const { this->ValidateContext(context); - return internal_tree().EvalBodySpatialVelocityInWorld(context, body_B); + return internal_tree().EvalLinkSpatialVelocityInWorld(context, body_B); } /// Evaluates A_WB, body B's spatial acceleration in the world frame W. @@ -5195,7 +5190,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// Returns a constant reference to the *world* body. const RigidBody& world_body() const { - return internal_tree().world_body(); + return internal_tree().world_link(); } /// Returns a constant reference to the *world* frame. @@ -5206,18 +5201,18 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// Returns the number of RigidBody elements in the model, including the /// "world" RigidBody, which is always part of the model. /// @see AddRigidBody(). - int num_bodies() const { return internal_tree().num_bodies(); } + int num_bodies() const { return internal_tree().num_links(); } /// Returns `true` if plant has a rigid body with unique index `body_index`. bool has_body(BodyIndex body_index) const { - return internal_tree().has_body(body_index); + return internal_tree().has_link(body_index); } /// Returns a constant reference to the body with unique index `body_index`. /// @throws std::exception if `body_index` does not correspond to a body in /// this model. const RigidBody& get_body(BodyIndex body_index) const { - return internal_tree().get_body(body_index); + return internal_tree().get_link(body_index); } /// Returns `true` if @p body is anchored (i.e. the kinematic path between @@ -5234,13 +5229,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// @throws std::exception if the body name occurs in multiple model /// instances. bool HasBodyNamed(std::string_view name) const { - return internal_tree().HasBodyNamed(name); + return internal_tree().HasLinkNamed(name); } /// @returns The total number of bodies (across all model instances) with the /// given name. int NumBodiesWithName(std::string_view name) const { - return internal_tree().NumBodiesWithName(name); + return internal_tree().NumLinksWithName(name); } /// @returns `true` if a body named `name` was added to the %MultibodyPlant @@ -5250,7 +5245,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// @throws std::exception if @p model_instance is not valid for this model. bool HasBodyNamed(std::string_view name, ModelInstanceIndex model_instance) const { - return internal_tree().HasBodyNamed(name, model_instance); + return internal_tree().HasLinkNamed(name, model_instance); } /// Returns a constant reference to a body that is identified @@ -5261,7 +5256,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// @see HasBodyNamed() to query if there exists a body in `this` /// %MultibodyPlant with a given specified name. const RigidBody& GetBodyByName(std::string_view name) const { - return internal_tree().GetRigidBodyByName(name); + return internal_tree().GetLinkByName(name); } /// Returns a constant reference to the body that is uniquely identified @@ -5271,13 +5266,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// %MultibodyPlant with a given specified name. const RigidBody& GetBodyByName(std::string_view name, ModelInstanceIndex model_instance) const { - return internal_tree().GetRigidBodyByName(name, model_instance); + return internal_tree().GetLinkByName(name, model_instance); } /// Returns a list of body indices associated with `model_instance`. std::vector GetBodyIndices( ModelInstanceIndex model_instance) const { - return internal_tree().GetBodyIndices(model_instance); + return internal_tree().GetLinkIndices(model_instance); } /// Returns a constant reference to a rigid body that is identified @@ -5289,7 +5284,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// @see HasBodyNamed() to query if there exists a body in `this` model with a /// given specified name. const RigidBody& GetRigidBodyByName(std::string_view name) const { - return internal_tree().GetRigidBodyByName(name); + return internal_tree().GetLinkByName(name); } /// Returns a constant reference to the rigid body that is uniquely identified @@ -5302,7 +5297,7 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// given specified name. const RigidBody& GetRigidBodyByName( std::string_view name, ModelInstanceIndex model_instance) const { - return internal_tree().GetRigidBodyByName(name, model_instance); + return internal_tree().GetLinkByName(name, model_instance); } /// Returns all bodies that are transitively welded, or rigidly affixed, to diff --git a/multibody/plant/test/multibody_plant_forward_dynamics_test.cc b/multibody/plant/test/multibody_plant_forward_dynamics_test.cc index a4d502cc117f..ca2702b09a78 100644 --- a/multibody/plant/test/multibody_plant_forward_dynamics_test.cc +++ b/multibody/plant/test/multibody_plant_forward_dynamics_test.cc @@ -116,8 +116,8 @@ class KukaIiwaModelForwardDynamicsTests : public test::KukaIiwaModelTests { plant_->CalcMassMatrixViaInverseDynamics(*context_, &M); const double kappa = 1.0 / M.llt().rcond(); - // Compare expected results against actual vdot. - const double kRelativeTolerance = kappa * kEpsilon; + // Compare expected results against actual vdot (with a little slop). + const double kRelativeTolerance = 2 * kappa * kEpsilon; EXPECT_TRUE(CompareMatrices(vdot, vdot_expected, kRelativeTolerance, MatrixCompareType::relative)); } @@ -189,6 +189,14 @@ GTEST_TEST(MultibodyPlantForwardDynamics, AtlasRobot) { .FixValue(context.get(), VectorX::Zero(num_actuators)); auto derivatives = plant.AllocateTimeDerivatives(); { + // Make sure to get global and parameter-change heap allocations done + // before we check CalcTimeDerivatives() heap use. + + // Initialization of Highway may use heap on the first call. + const math::RotationMatrix R_AB, R_BC; // Both identity. + EXPECT_TRUE((R_AB * R_BC).IsExactlyIdentity()); + plant.EvalFrameBodyPoses(*context); // Updated when parameters change. + // CalcTimeDerivatives should not be allocating, but for now we have a few // remaining fixes before it's down to zero: // 2 temps in MbTS::CalcArticulatedBodyForceCache (F_B_W_, tau_). diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index 69eb512692ef..331aed5d5205 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -3080,7 +3080,7 @@ TEST_F(SplitPendulum, GetMultibodyPlantFromElement) { struct MyMBSystem : public internal::MultibodyTreeSystem { MyMBSystem() { rigid_body = - &mutable_tree().AddRigidBody("Body", SpatialInertia::NaN()); + &mutable_tree().AddLink("Body", SpatialInertia::NaN()); Finalize(); } const RigidBody* rigid_body{}; diff --git a/multibody/rational/rational_forward_kinematics.cc b/multibody/rational/rational_forward_kinematics.cc index c401d8755167..3c3f6e196c06 100644 --- a/multibody/rational/rational_forward_kinematics.cc +++ b/multibody/rational/rational_forward_kinematics.cc @@ -245,12 +245,12 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial( internal::MobodIndex mobilizer_index; bool is_order_reversed{}; - if (parent_mobod.inboard().is_valid() && - parent_mobod.inboard() == child_mobod_index) { + if (parent_mobod.inboard_mobod().is_valid() && + parent_mobod.inboard_mobod() == child_mobod_index) { is_order_reversed = true; mobilizer_index = parent_mobod_index; - } else if (child_mobod.inboard().is_valid() && - child_mobod.inboard() == parent_mobod_index) { + } else if (child_mobod.inboard_mobod().is_valid() && + child_mobod.inboard_mobod() == parent_mobod_index) { is_order_reversed = false; mobilizer_index = child_mobod_index; } else { diff --git a/multibody/rational/rational_forward_kinematics_internal.cc b/multibody/rational/rational_forward_kinematics_internal.cc index 72e97f7e765a..69cdc1d1e53b 100644 --- a/multibody/rational/rational_forward_kinematics_internal.cc +++ b/multibody/rational/rational_forward_kinematics_internal.cc @@ -46,16 +46,17 @@ std::vector FindPath(const MultibodyPlant& plant, forest.mobods(current_link.mobod_index()); if (current != world_index()) { const SpanningForest::Mobod& parent_mobod = - forest.mobods(current_mobod.inboard()); + forest.mobods(current_mobod.inboard_mobod()); const BodyIndex parent = - tree.forest().links(parent_mobod.link_ordinal()).index(); + tree.forest().links(parent_mobod.active_link_ordinal()).index(); visit_edge(current, parent); } - for (const MobodIndex& child_mobod_index : current_mobod.outboards()) { + for (const MobodIndex& child_mobod_index : + current_mobod.outboard_mobods()) { const SpanningForest::Mobod& child_mobod = forest.mobods(child_mobod_index); const BodyIndex child = - tree.forest().links(child_mobod.link_ordinal()).index(); + tree.forest().links(child_mobod.active_link_ordinal()).index(); visit_edge(current, child); } } @@ -86,7 +87,8 @@ std::vector FindMobilizersOnPath( const LinkJointGraph::Link& link_i = forest.link_by_index(path[i]); const LinkJointGraph::Link& link_ip1 = forest.link_by_index(path[i + 1]); const SpanningForest::Mobod& mobod_i = forest.mobods(link_i.mobod_index()); - if (!mobod_i.is_world() && mobod_i.inboard() == link_ip1.mobod_index()) { + if (!mobod_i.is_world() && + mobod_i.inboard_mobod() == link_ip1.mobod_index()) { // path[i] is the child of path[i+1] in MultibodyTreeTopology, they are // connected by path[i]'s inboard mobilizer. mobilizers_on_path.push_back(link_i.mobod_index()); diff --git a/multibody/rational/test/rational_forward_kinematics_test.cc b/multibody/rational/test/rational_forward_kinematics_test.cc index 41c0307e8743..fd646d5c0a8d 100644 --- a/multibody/rational/test/rational_forward_kinematics_test.cc +++ b/multibody/rational/test/rational_forward_kinematics_test.cc @@ -45,7 +45,7 @@ void CheckBodyKinematics(const RationalForwardKinematics& dut, const internal::MultibodyTree& tree = internal::GetInternalTree(dut.plant()); - tree.CalcAllBodyPosesInWorld(*context, &X_WB_expected); + tree.CalcAllLinkPosesInWorld(*context, &X_WB_expected); symbolic::Environment env; for (int i = 0; i < s_val.rows(); ++i) { diff --git a/multibody/topology/link_joint_graph.cc b/multibody/topology/link_joint_graph.cc index 07029ec98d32..2337f793d449 100644 --- a/multibody/topology/link_joint_graph.cc +++ b/multibody/topology/link_joint_graph.cc @@ -739,10 +739,10 @@ std::vector LinkJointGraph::FindPathFromWorld( const SpanningForest::Mobod* mobod = &forest().mobods()[link_to_mobod(link_index)]; std::vector path(mobod->level() + 1); - while (mobod->inboard().is_valid()) { - const Link& link = links(mobod->link_ordinal()); + while (mobod->inboard_mobod().is_valid()) { + const Link& link = links(mobod->active_link_ordinal()); path[mobod->level()] = link.index(); // Active Link if optimized assembly. - mobod = &forest().mobods(mobod->inboard()); + mobod = &forest().mobods(mobod->inboard_mobod()); } DRAKE_DEMAND(mobod->is_world()); path[0] = LinkIndex(0); @@ -755,7 +755,7 @@ LinkIndex LinkJointGraph::FindFirstCommonAncestor(LinkIndex link1_index, const MobodIndex mobod_ancestor = forest().FindFirstCommonAncestor( link_to_mobod(link1_index), link_to_mobod(link2_index)); const Link& ancestor_link = - links(forest().mobod_to_link_ordinal(mobod_ancestor)); + links(forest().mobod_to_active_link_ordinal(mobod_ancestor)); return ancestor_link.index(); } diff --git a/multibody/topology/link_joint_graph_debug.cc b/multibody/topology/link_joint_graph_debug.cc index 8b66e268410d..f5a676950157 100644 --- a/multibody/topology/link_joint_graph_debug.cc +++ b/multibody/topology/link_joint_graph_debug.cc @@ -97,9 +97,9 @@ std::string LinkJointGraph::GenerateGraphvizString( if (show_as_modeled && joint.mobod_index().is_valid()) { const SpanningForest::Mobod& mobod = forest().mobods(joint.mobod_index()); if (mobod.is_reversed()) { - revised_parent_ordinal = mobod.link_ordinal(); + revised_parent_ordinal = mobod.active_link_ordinal(); } else { - revised_child_ordinal = mobod.link_ordinal(); + revised_child_ordinal = mobod.active_link_ordinal(); } } diff --git a/multibody/topology/link_joint_graph_defs.h b/multibody/topology/link_joint_graph_defs.h index eedb5e79a013..ae56f8b13016 100644 --- a/multibody/topology/link_joint_graph_defs.h +++ b/multibody/topology/link_joint_graph_defs.h @@ -13,18 +13,16 @@ namespace multibody { // TODO(sherm1) Promote from internal once API has stabilized: issue #11307. namespace internal { -using LinkIndex = BodyIndex; - class SpanningForest; -using LinkOrdinal = TypeSafeIndex; - using JointTraitsIndex = TypeSafeIndex; using WeldedLinksAssemblyIndex = TypeSafeIndex; using LoopConstraintIndex = TypeSafeIndex; -/* Link properties that can affect how the forest model gets built. Or-ing -these also produces a LinkFlags object. */ +/* Link properties that can affect how the forest model gets built. This enum +is emulating a bit mask; "and" and "or" operators are provided which return +a LinkFlags object. Be careful with ordinary equality and assignment +operators. */ enum class LinkFlags : uint32_t { kDefault = 0, kStatic = 1 << 0, ///< Implicitly welded to World. @@ -33,16 +31,20 @@ enum class LinkFlags : uint32_t { kShadow = 1 << 3 ///< Link is a shadow (internal use only). }; -/* Joint properties that can affect how the SpanningForest gets built. Or-ing -these also produces a JointFlags object. */ +/* Joint properties that can affect how the SpanningForest gets built. This enum +is emulating a bit mask; "and" and "or" operators are provided which return +a JointFlags object. Be careful with ordinary equality and assignment +operators. */ enum class JointFlags : uint32_t { kDefault = 0, kMustBeModeled = 1 << 0 ///< Model explicitly even if ignorable weld. }; -/* Options for how to build the SpanningForest. Or-ing these also produces a -ForestBuildingOptions object. These can be provided as per-model instance -options to locally override global options. */ +/* Options for how to build the SpanningForest. These can be provided as +per-model instance options to locally override global options. This enum +is emulating a bit mask; "and", "or", and "not" operators are provided which +return a ForestBuildingOptions object. Be careful with ordinary equality and +assignment operators. */ enum class ForestBuildingOptions : uint32_t { kDefault = 0, kStatic = 1 << 0, ///< Weld all links to World. @@ -83,6 +85,9 @@ inline ForestBuildingOptions operator&(ForestBuildingOptions left, return static_cast(static_cast(left) & static_cast(right)); } +inline ForestBuildingOptions operator~(ForestBuildingOptions options) { + return static_cast(~static_cast(options)); +} } // namespace internal } // namespace multibody diff --git a/multibody/topology/spanning_forest.cc b/multibody/topology/spanning_forest.cc index c861457c8a3a..e613ef00edf4 100644 --- a/multibody/topology/spanning_forest.cc +++ b/multibody/topology/spanning_forest.cc @@ -318,13 +318,13 @@ void SpanningForest::AssignCoordinates() { for (auto& mobod : data_.mobods) { mobod.q_start_ = next_q; mobod.v_start_ = next_v; - if (!mobod.joint_ordinal().is_valid()) { + if (!mobod.active_joint_ordinal().is_valid()) { mobod.nq_ = mobod.nv_ = 0; // Treat World as though welded. mobod.nq_inboard_ = mobod.nv_inboard_ = 0; continue; } const JointTraitsIndex joint_traits_index = - joints(mobod.joint_ordinal()).traits_index(); + joints(mobod.active_joint_ordinal()).traits_index(); const LinkJointGraph::JointTraits& joint_traits = graph().joint_traits()[joint_traits_index]; @@ -337,8 +337,9 @@ void SpanningForest::AssignCoordinates() { } /* Keep a running count of inboard coordinates. */ - DRAKE_DEMAND(mobod.inboard().is_valid()); // Non-World must have inboard. - const Mobod& parent = mobods(mobod.inboard()); + DRAKE_DEMAND( + mobod.inboard_mobod().is_valid()); // Non-World must have inboard. + const Mobod& parent = mobods(mobod.inboard_mobod()); /* Parent should have been processed before child. */ DRAKE_DEMAND(parent.nq_inboard_ >= 0 && parent.nv_inboard_ >= 0); mobod.nq_inboard_ = parent.nq_inboard_ + mobod.nq_; @@ -789,7 +790,8 @@ const SpanningForest::Mobod& SpanningForest::AddNewMobod( new_mobod_index); mutable_graph().AddToWeldedLinksAssembly( - inboard_mobod.link_ordinal(), outboard_link_ordinal, joint_ordinal); + inboard_mobod.active_link_ordinal(), outboard_link_ordinal, + joint_ordinal); } return new_mobod; @@ -950,9 +952,9 @@ const SpanningForest::Mobod& SpanningForest::JoinExistingMobod( DRAKE_DEMAND(weld_joint.traits_index() == LinkJointGraph::weld_joint_traits_index()); const WeldedLinksAssemblyIndex assembly_index = - mutable_graph().AddToWeldedLinksAssembly(inboard_mobod->link_ordinal(), - follower_link_ordinal, - weld_joint_ordinal); + mutable_graph().AddToWeldedLinksAssembly( + inboard_mobod->active_link_ordinal(), follower_link_ordinal, + weld_joint_ordinal); mutable_graph().set_primary_mobod_for_link( follower_link_ordinal, inboard_mobod->index(), weld_joint.index()); inboard_mobod->follower_link_ordinals_.push_back(follower_link_ordinal); @@ -978,7 +980,7 @@ void SpanningForest::GrowAssemblyMobod( graph().index_to_ordinal(outboard_link_index); const Link& outboard_link = links(outboard_link_ordinal); if (link_is_already_in_forest(outboard_link_ordinal)) { - const Link& inboard_link = links(mobod->link_ordinal()); + const Link& inboard_link = links(mobod->active_link_ordinal()); DRAKE_DEMAND(outboard_link.welded_links_assembly() == inboard_link.welded_links_assembly()); mutable_graph().AddUnmodeledJointToWeldedLinksAssembly( @@ -1023,9 +1025,9 @@ std::vector SpanningForest::FindPathFromWorld( MobodIndex index) const { const Mobod* mobod = &mobods(index); std::vector path(mobod->level() + 1); - while (mobod->inboard().is_valid()) { + while (mobod->inboard_mobod().is_valid()) { path[mobod->level()] = mobod->index(); - mobod = &mobods(mobod->inboard()); + mobod = &mobods(mobod->inboard_mobod()); } DRAKE_DEMAND(mobod->is_world()); path[0] = MobodIndex(0); @@ -1051,14 +1053,14 @@ MobodIndex SpanningForest::FindFirstCommonAncestor( // Get down to a common level, then go down both branches. while (branch1->level() > branch2->level()) - branch1 = &mobods(branch1->inboard()); + branch1 = &mobods(branch1->inboard_mobod()); while (branch2->level() > branch1->level()) - branch2 = &mobods(branch2->inboard()); + branch2 = &mobods(branch2->inboard_mobod()); // Both branches are at the same level now. while (branch1->index() != branch2->index()) { - branch1 = &mobods(branch1->inboard()); - branch2 = &mobods(branch2->inboard()); + branch1 = &mobods(branch1->inboard_mobod()); + branch2 = &mobods(branch2->inboard_mobod()); } return branch1->index(); // Same as branch2->index(). @@ -1079,19 +1081,19 @@ MobodIndex SpanningForest::FindPathsToFirstCommonAncestor( // Get down to a common level, then go down both branches. while (branch1->level() > branch2->level()) { path1->push_back(branch1->index()); - branch1 = &mobods(branch1->inboard()); + branch1 = &mobods(branch1->inboard_mobod()); } while (branch2->level() > branch1->level()) { path2->push_back(branch2->index()); - branch2 = &mobods(branch2->inboard()); + branch2 = &mobods(branch2->inboard_mobod()); } // Both branches are at the same level now. while (branch1->index() != branch2->index()) { path1->push_back(branch1->index()); path2->push_back(branch2->index()); - branch1 = &mobods(branch1->inboard()); - branch2 = &mobods(branch2->inboard()); + branch1 = &mobods(branch1->inboard_mobod()); + branch2 = &mobods(branch2->inboard_mobod()); } return branch1->index(); // Same as branch2->index(). diff --git a/multibody/topology/spanning_forest.h b/multibody/topology/spanning_forest.h index eafe22bda91c..d247e9703b4d 100644 --- a/multibody/topology/spanning_forest.h +++ b/multibody/topology/spanning_forest.h @@ -354,17 +354,16 @@ class SpanningForest { return graph().get_forest_building_options_in_use(index); } - /* Returns the Link that is represented by the given Mobod. This could be - one of the Links from the original graph or an added shadow Link. If this - Mobod represents a WeldedLinksAssembly, the Link returned here is the - "active" Link, that is, the one whose mobilizer is used to move the whole - Assembly. Cost is O(1) and very fast. + /* Returns the Link that is represented by the given Mobod, or the active + link if given a composite mobod. This could be one of the Links from the + original graph or an added shadow Link. An active link is the one whose + mobilizer is used to move the whole mobod. Cost is O(1) and very fast. @pre mobod_index is in range */ - inline LinkOrdinal mobod_to_link_ordinal(MobodIndex mobod_index) const; + inline LinkOrdinal mobod_to_active_link_ordinal(MobodIndex mobod_index) const; - /* Returns all the Links mobilized by this Mobod. The "active" Link returned - by mobod_to_link() comes first, then any other Links in the same Assembly. - O(1), very fast. + /* Returns all the Links mobilized by this Mobod. The active link returned + by mobod_to_active_link() comes first, then any other links in the same + composite mobod. O(1), very fast. @pre mobod_index is in range */ inline const std::vector& mobod_to_link_ordinals( MobodIndex mobod_index) const; diff --git a/multibody/topology/spanning_forest_debug.cc b/multibody/topology/spanning_forest_debug.cc index 55763c6edeb6..d7b2fabbcbda 100644 --- a/multibody/topology/spanning_forest_debug.cc +++ b/multibody/topology/spanning_forest_debug.cc @@ -53,12 +53,12 @@ void SpanningForest::SanityCheckForest() const { DRAKE_THROW_UNLESS(world_mobod.is_anchored()); DRAKE_THROW_UNLESS(world_mobod.has_massful_follower_link()); DRAKE_THROW_UNLESS(world_mobod.index() == MobodIndex(0)); - DRAKE_THROW_UNLESS(world_mobod.link_ordinal() == LinkOrdinal(0)); - DRAKE_THROW_UNLESS(!world_mobod.joint_ordinal().is_valid()); + DRAKE_THROW_UNLESS(world_mobod.active_link_ordinal() == LinkOrdinal(0)); + DRAKE_THROW_UNLESS(!world_mobod.active_joint_ordinal().is_valid()); DRAKE_THROW_UNLESS(!world_mobod.tree().is_valid()); DRAKE_THROW_UNLESS(world_mobod.level() == 0); - DRAKE_THROW_UNLESS(!world_mobod.inboard().is_valid()); - DRAKE_THROW_UNLESS(ssize(world_mobod.outboards()) == ssize(trees())); + DRAKE_THROW_UNLESS(!world_mobod.inboard_mobod().is_valid()); + DRAKE_THROW_UNLESS(ssize(world_mobod.outboard_mobods()) == ssize(trees())); /* WeldedMobods groups and WeldedLinksAssemblies are different but in either case there is always a World group or assembly, it must come first, and the @@ -78,8 +78,10 @@ void SpanningForest::SanityCheckForest() const { for (MobodIndex index(1); index < ssize(mobods()); ++index) { const Mobod& mobod = mobods(index); DRAKE_THROW_UNLESS(mobod.index() == index); - DRAKE_THROW_UNLESS(links(mobod.link_ordinal()).mobod_index() == index); - DRAKE_THROW_UNLESS(joints(mobod.joint_ordinal()).mobod_index() == index); + DRAKE_THROW_UNLESS(links(mobod.active_link_ordinal()).mobod_index() == + index); + DRAKE_THROW_UNLESS(joints(mobod.active_joint_ordinal()).mobod_index() == + index); // The mobod's Tree must include the mobod. const Tree& tree = trees(mobod.tree()); @@ -174,7 +176,7 @@ void SpanningForest::SanityCheckForest() const { DRAKE_THROW_UNLESS(joint.ordinal() == joint_ordinal); DRAKE_THROW_UNLESS(&joint_by_index(joint.index()) == &joint); if (!joint.mobod_index().is_valid()) continue; // Not modeled - DRAKE_THROW_UNLESS(mobods(joint.mobod_index()).joint_ordinal() == + DRAKE_THROW_UNLESS(mobods(joint.mobod_index()).active_joint_ordinal() == joint_ordinal); } @@ -217,7 +219,7 @@ void SpanningForest::SanityCheckForest() const { DRAKE_THROW_UNLESS(mobod.is_anchored() == should_be_anchored); if (is_active_mobod) { - const LinkOrdinal active_link_ordinal = mobod.link_ordinal(); + const LinkOrdinal active_link_ordinal = mobod.active_link_ordinal(); const Link& active_link = links(active_link_ordinal); const std::optional link_assembly = active_link.welded_links_assembly(); @@ -256,10 +258,10 @@ std::string SpanningForest::GenerateGraphvizString( followers += fmt::format("L({}) ", ordinal); graphviz += fmt::format( "mobod{} [color={}] [label=\"mobod({}){}\n{}\"];\n", mobod.index(), - links(mobod.link_ordinal()).is_shadow() ? "red" : "black", + links(mobod.active_link_ordinal()).is_shadow() ? "red" : "black", mobod.index(), mobod.has_massful_follower_link() ? "" : "*", followers); if (mobod.is_world()) continue; - const Mobod& inboard = mobods(mobod.inboard()); + const Mobod& inboard = mobods(mobod.inboard_mobod()); const std::string color = mobod.is_reversed() ? "purple" : mobod.is_weld() ? "black" @@ -267,7 +269,7 @@ std::string SpanningForest::GenerateGraphvizString( const std::string arrow_type = mobod.is_weld() ? "box" : "normal"; const std::string style = mobod.is_weld() ? "bold" : "solid"; - const LinkJointGraph::Joint& joint = joints(mobod.joint_ordinal()); + const LinkJointGraph::Joint& joint = joints(mobod.active_joint_ordinal()); graphviz += fmt::format( "mobod{} -> mobod{} [arrowhead={}] [fontsize=10] [style={}]" "[label=\"mobilizer({})\nJ({}) {}{}\nq{} v{}\"] [color={}];\n", diff --git a/multibody/topology/spanning_forest_inlines.h b/multibody/topology/spanning_forest_inlines.h index 0e920f0f5f13..00f0eeb9fbd6 100644 --- a/multibody/topology/spanning_forest_inlines.h +++ b/multibody/topology/spanning_forest_inlines.h @@ -23,9 +23,9 @@ inline int SpanningForest::num_mobods() const { return std::ssize(mobods()); } -inline LinkOrdinal SpanningForest::mobod_to_link_ordinal( +inline LinkOrdinal SpanningForest::mobod_to_active_link_ordinal( MobodIndex mobod_index) const { - return mobods(mobod_index).link_ordinal(); + return mobods(mobod_index).active_link_ordinal(); } inline const std::vector& SpanningForest::mobod_to_link_ordinals( diff --git a/multibody/topology/spanning_forest_mobod.h b/multibody/topology/spanning_forest_mobod.h index 10f88d75f14c..a03e72716f14 100644 --- a/multibody/topology/spanning_forest_mobod.h +++ b/multibody/topology/spanning_forest_mobod.h @@ -14,7 +14,16 @@ namespace multibody { // TODO(sherm1) Promote from internal once API has stabilized: issue #11307. namespace internal { -/* Everything you might want to know about a mobilized body. */ +/* Everything you might want to know about a mobilized body. + +A %Mobod models one or more Links and a single Joint. Those Links are said to +"follow" this mobod. If there is more than one follower it is because the links +are welded together (by unmodeled weld joints); in that case this is a +"composite mobod". The modeled joint is the one that connects this mobod to its +inboard mobod. We call that joint the mobod's "active joint", and that joint's +link that follows this mobod the "active link". (The active link may be either +the parent or child link of the active joint, depending on how the forest was +constructed from the graph.) */ class SpanningForest::Mobod { public: /* (Internal use only) Copy/Move constructor & assignment. */ @@ -69,33 +78,40 @@ class SpanningForest::Mobod { /* Returns the index of this %Mobod's unique inboard %Mobod. The index is invalid if this is the World %Mobod. The inboard %Mobod's level() is one less that this %Mobod's level(). */ - MobodIndex inboard() const { return inboard_mobod_; } + MobodIndex inboard_mobod() const { return inboard_mobod_; } /* Returns the indices of all %Mobods for which this %Mobod serves as the inboard body. Each of the outboard %Mobods has a level() one higher than this %Mobod's level(). */ - const std::vector& outboards() const { return outboard_mobods_; } + const std::vector& outboard_mobods() const { + return outboard_mobods_; + } /* Returns the ordinal of the Link mobilized by this %Mobod. If this is a composite %Mobod (representing a collection of welded-together links), this is - the most-inboard link of that composite, the one with the Joint whose - mobilizer connects the composite %Mobod to its inboard %Mobod. + the most-inboard ("active") link of that composite, the one with the modeled + Joint whose mobilizer connects the composite %Mobod to its inboard %Mobod. @see follower_link_ordinals(), joint() */ - LinkOrdinal link_ordinal() const { return follower_link_ordinals()[0]; } + LinkOrdinal active_link_ordinal() const { + return follower_link_ordinals()[0]; + } /* Returns true if _any_ of the follower links is massful, in which case this Mobod is also massful. */ bool has_massful_follower_link() const { return has_massful_follower_link_; } /* Returns all the Links that are mobilized by this %Mobod. If this is a - composite %Mobod, the first link returned is the most-inboard link as - returned by link_ordinal(). There is always at least one link. - @see link_ordinal() */ + composite %Mobod, the first link returned is the most-inboard ("active") link + as returned by active_link_ordinal(). There is always at least one link. + @see active_link_ordinal() */ const std::vector& follower_link_ordinals() const { DRAKE_ASSERT(!follower_link_ordinals_.empty()); return follower_link_ordinals_; } + /* Returns true if there is more than one link following this Mobod. */ + bool is_composite() const { return ssize(follower_link_ordinals_) > 1; } + /* Returns true if the given Link is one of the followers of this %Mobod. */ bool HasFollower(LinkOrdinal link_ordinal) const { DRAKE_DEMAND(link_ordinal.is_valid()); @@ -109,7 +125,7 @@ class SpanningForest::Mobod { returned here is the modeled joint whose mobilizer connects the composite %Mobod to its inboard %Mobod in the forest. The returned ordinal is invalid only if this is the World %Mobod. */ - JointOrdinal joint_ordinal() const { return joint_ordinal_; } + JointOrdinal active_joint_ordinal() const { return joint_ordinal_; } /* Returns the index of the Tree of which this %Mobod is a member. The index is invalid if and only if this is the World %Mobod. */ @@ -204,14 +220,15 @@ class SpanningForest::Mobod { std::vector* to_be_renumbered); // Links represented by this Mobod. The first one is always present and is - // the active Link if we're mobilizing a LinkComposite. + // always the active link. std::vector follower_link_ordinals_; // Set to true if _any_ follower link has mass. bool has_massful_follower_link_{false}; - // Corresponding Joint (user or modeling joint). If this is a composite Mobod, - // this is the Joint whose mobilizer is this Mobod's inboard mobilizer. + // The Joint modeled by this mobod's mobilizer. If this is a composite mobod, + // this is the active joint that connects the composite mobod to its inboard + // mobod. JointOrdinal joint_ordinal_; // For an already-existing Joint, must we use a reverse mobilizer? If true, diff --git a/multibody/topology/test/spanning_forest_test.cc b/multibody/topology/test/spanning_forest_test.cc index 92a7e85689da..dda1b6b1c963 100644 --- a/multibody/topology/test/spanning_forest_test.cc +++ b/multibody/topology/test/spanning_forest_test.cc @@ -85,7 +85,7 @@ GTEST_TEST(SpanningForest, WorldOnlyTest) { EXPECT_EQ(forest.height(), 1); EXPECT_EQ(ssize(forest.welded_mobods()), 1); EXPECT_EQ(forest.welded_mobods()[0][0], world_mobod_index); - EXPECT_EQ(forest.mobod_to_link_ordinal(world_mobod_index), + EXPECT_EQ(forest.mobod_to_active_link_ordinal(world_mobod_index), world_link_ordinal); EXPECT_EQ(forest.mobod_to_link_ordinals(world_mobod_index), std::vector{world_link_ordinal}); @@ -107,11 +107,11 @@ GTEST_TEST(SpanningForest, WorldOnlyTest) { EXPECT_TRUE(world.is_leaf_mobod()); EXPECT_FALSE(world.is_reversed()); // Not meaningful though. EXPECT_TRUE(world.is_weld()); // Defined as having no inboard dofs. - EXPECT_FALSE(world.inboard().is_valid()); - EXPECT_TRUE(world.outboards().empty()); - EXPECT_EQ(world.link_ordinal(), world_link_ordinal); + EXPECT_FALSE(world.inboard_mobod().is_valid()); + EXPECT_TRUE(world.outboard_mobods().empty()); + EXPECT_EQ(world.active_link_ordinal(), world_link_ordinal); EXPECT_EQ(world.follower_link_ordinals(), std::vector{world_link_ordinal}); - EXPECT_FALSE(world.joint_ordinal().is_valid()); + EXPECT_FALSE(world.active_joint_ordinal().is_valid()); EXPECT_FALSE(world.tree().is_valid()); EXPECT_EQ(world.welded_mobods_group(), WeldedMobodsIndex(0)); EXPECT_EQ(world.level(), 0); @@ -360,7 +360,7 @@ GTEST_TEST(SpanningForest, MultipleBranchesDefaultOptions) { for (auto mobod_link : mobod_link_map) { EXPECT_EQ(graph.link_to_mobod(LinkIndex(mobod_link.second)), MobodIndex(mobod_link.first)); - EXPECT_EQ(forest.mobod_to_link_ordinal(MobodIndex(mobod_link.first)), + EXPECT_EQ(forest.mobod_to_active_link_ordinal(MobodIndex(mobod_link.first)), LinkOrdinal(mobod_link.second)); // Each Mobod has only a single Link that follows it. EXPECT_EQ(forest.mobod_to_link_ordinals(MobodIndex(mobod_link.first)), @@ -511,16 +511,17 @@ GTEST_TEST(SpanningForest, MultipleBranchesBaseJointOptions) { EXPECT_TRUE(tree1.front().is_anchored()); EXPECT_FALSE(tree2.front().is_anchored()); - EXPECT_FALSE(graph.links(tree0.front().link_ordinal()).is_anchored()); - EXPECT_TRUE(graph.links(tree1.front().link_ordinal()).is_anchored()); - EXPECT_FALSE(graph.links(tree2.front().link_ordinal()).is_anchored()); + EXPECT_FALSE(graph.links(tree0.front().active_link_ordinal()).is_anchored()); + EXPECT_TRUE(graph.links(tree1.front().active_link_ordinal()).is_anchored()); + EXPECT_FALSE(graph.links(tree2.front().active_link_ordinal()).is_anchored()); // There is only the World assembly, but now tree1's base link and its // ephemeral weld joint are included. EXPECT_EQ(ssize(graph.welded_links_assemblies()), 1); // just World - EXPECT_EQ(graph.welded_links_assemblies(WeldedLinksAssemblyIndex(0)).links(), - (std::vector{graph.world_link().index(), - graph.links(tree1.front().link_ordinal()).index()})); + EXPECT_EQ( + graph.welded_links_assemblies(WeldedLinksAssemblyIndex(0)).links(), + (std::vector{graph.world_link().index(), + graph.links(tree1.front().active_link_ordinal()).index()})); EXPECT_EQ(graph.welded_links_assemblies(WeldedLinksAssemblyIndex(0)).joints(), (std::vector{JointIndex(14)})); // Ephemeral joints start at 13. EXPECT_FALSE( @@ -795,7 +796,8 @@ GTEST_TEST(SpanningForest, SerialChainAndMore) { // Counts for generic middle Mobod. const SpanningForest::Mobod& mobod_for_link3 = forest.mobods(graph.link_by_index(LinkIndex(3)).mobod_index()); - EXPECT_EQ(graph.links(mobod_for_link3.link_ordinal()).index(), LinkIndex(3)); + EXPECT_EQ(graph.links(mobod_for_link3.active_link_ordinal()).index(), + LinkIndex(3)); EXPECT_EQ(mobod_for_link3.q_start(), 2); EXPECT_EQ(mobod_for_link3.v_start(), 2); EXPECT_EQ(mobod_for_link3.nq(), 1); @@ -1308,10 +1310,10 @@ GTEST_TEST(SpanningForest, WeldedSubgraphs) { const std::array mobod2joint{-1, 12, 11, 9, 8, 10, 13, 14, 3, 0, 7, 4, 6, 5, 2, 1, 15}; for (const SpanningForest::Mobod& mobod : forest.mobods()) { - EXPECT_EQ(graph.links(mobod.link_ordinal()).index(), + EXPECT_EQ(graph.links(mobod.active_link_ordinal()).index(), LinkIndex(mobod2link[mobod.index()])); if (mobod.is_world()) continue; // No joint for World mobod. - EXPECT_EQ(graph.joints(mobod.joint_ordinal()).index(), + EXPECT_EQ(graph.joints(mobod.active_joint_ordinal()).index(), JointIndex(mobod2joint[mobod.index()])); } @@ -1510,8 +1512,9 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) { EXPECT_EQ(ssize(forest.trees()), 2); EXPECT_EQ(forest.trees()[0].num_mobods(), 4); EXPECT_EQ(forest.trees()[1].num_mobods(), 3); - EXPECT_EQ(graph.links(forest.mobods(MobodIndex(4)).link_ordinal()).index(), - LinkIndex(7)); + EXPECT_EQ( + graph.links(forest.mobods(MobodIndex(4)).active_link_ordinal()).index(), + LinkIndex(7)); // Changing just 3 to massless results in the same forest. // (Tests Case 2 in ExtendTreesOneLevel()) @@ -1530,8 +1533,9 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) { EXPECT_EQ(ssize(forest.trees()), 2); EXPECT_EQ(forest.trees()[0].num_mobods(), 4); EXPECT_EQ(forest.trees()[1].num_mobods(), 3); - EXPECT_EQ(graph.links(forest.mobods(MobodIndex(4)).link_ordinal()).index(), - LinkIndex(7)); + EXPECT_EQ( + graph.links(forest.mobods(MobodIndex(4)).active_link_ordinal()).index(), + LinkIndex(7)); // Changing both 3 and 4 to massless breaks the loop at 6 instead of 4. // (Tests Case 3 in ExtendTreesOneLevel()) @@ -1547,8 +1551,9 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) { EXPECT_EQ(ssize(forest.trees()), 2); EXPECT_EQ(forest.trees()[0].num_mobods(), 5); EXPECT_EQ(forest.trees()[1].num_mobods(), 2); - EXPECT_EQ(graph.links(forest.mobods(MobodIndex(5)).link_ordinal()).index(), - LinkIndex(7)); + EXPECT_EQ( + graph.links(forest.mobods(MobodIndex(5)).active_link_ordinal()).index(), + LinkIndex(7)); } /* Here is a tricky case that should be handled correctly and without warnings. @@ -1735,10 +1740,10 @@ GTEST_TEST(SpanningForest, DoubleLoop) { const std::array mobod2link{0, 1, 2, 5, 8, 4, 7, 9, 3, 6}; const std::array mobod2joint{-1, 8, 0, 3, 6, 1, 4, 7, 2, 5}; for (const SpanningForest::Mobod& mobod : forest.mobods()) { - EXPECT_EQ(graph.links(mobod.link_ordinal()).index(), + EXPECT_EQ(graph.links(mobod.active_link_ordinal()).index(), LinkIndex(mobod2link[mobod.index()])); if (mobod.is_world()) continue; // No joint for World mobod. - EXPECT_EQ(graph.joints(mobod.joint_ordinal()).index(), + EXPECT_EQ(graph.joints(mobod.active_joint_ordinal()).index(), JointIndex(mobod2joint[mobod.index()])); } diff --git a/multibody/tree/body_node.h b/multibody/tree/body_node.h index f56e0f3cfa58..7325368c576d 100644 --- a/multibody/tree/body_node.h +++ b/multibody/tree/body_node.h @@ -120,7 +120,7 @@ class BodyNode : public MultibodyElement { // MultibodyTree::Finalize() method call. void add_child_node(const BodyNode* child) { children_.push_back(child); } - MobodIndex inboard_mobod_index() const { return mobod().inboard(); } + MobodIndex inboard_mobod_index() const { return mobod().inboard_mobod(); } // Returns a constant reference to the body B associated with this node. const RigidBody& body() const { @@ -135,7 +135,7 @@ class BodyNode : public MultibodyElement { const RigidBody& parent_body() const { DRAKE_ASSERT(get_parent_body_index().is_valid()); DRAKE_ASSERT(this->has_parent_tree()); - return this->get_parent_tree().get_body(get_parent_body_index()); + return this->get_parent_tree().get_link(get_parent_body_index()); } // Returns a const pointer to the parent (inboard) body node or nullptr if @@ -227,8 +227,10 @@ class BodyNode : public MultibodyElement { // An already updated position kinematics cache in sync with positions. // @param[out] H_PB_W_cache // The cache entry being calculated; just this node's H_PB is updated. - - // @note `H_PB_W` is only a function of this node's generalized positions q. + // + // @note Although `H_PB_P` would be a function of only this node's generalized + // positions qₘ, the output value is expressed in World so requires more + // kinematics data. // // @pre The position kinematics cache `pc` was already updated to be in sync // with positions by MultibodyTree::CalcPositionKinematicsCache(). diff --git a/multibody/tree/body_node_impl.cc b/multibody/tree/body_node_impl.cc index 15b2d4b32086..4233b7aee1d3 100644 --- a/multibody/tree/body_node_impl.cc +++ b/multibody/tree/body_node_impl.cc @@ -113,6 +113,10 @@ void BodyNodeImpl::CalcPositionKinematicsCache_BaseToTip( break; } + // TODO(sherm1) Calculate X_WL for composites. Currently we don't make + // composites so X_WL = X_WB if L is the link on B. + pc->SetX_WL(mobilizer_->mobod().active_link_ordinal(), X_WB); + // Compute shift vector p_PoBo_W from the parent origin to the body origin. const Vector3& p_PoBo_P = X_PB.translation(); const math::RotationMatrix& R_WP = X_WP.rotation(); @@ -122,6 +126,12 @@ void BodyNodeImpl::CalcPositionKinematicsCache_BaseToTip( // TODO(sherm1) Consider combining this with VelocityCache computation // so that we don't have to make a separate pass. Or better, get rid of this // computation altogether by working in better frames. + +// Hinge matrix for this node. H_PB_W ∈ ℝ⁶ˣⁿᵐ with nm ∈ [0; 6] the number of +// mobilities for this node. Therefore, the return is a Matrix<6,kNv> since the +// number of columns depends on the mobilizer type. It is returned as an +// Eigen::Map to the memory allocated in the std::vector H_PB_W_cache so that we +// can work with H_PB_W as with any other Eigen matrix object. template void BodyNodeImpl:: CalcAcrossNodeJacobianWrtVExpressedInWorld( @@ -197,13 +207,6 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( DRAKE_ASSERT(mobod_index() != world_mobod_index()); DRAKE_ASSERT(vc != nullptr); - // Hinge matrix for this node. H_PB_W ∈ ℝ⁶ˣⁿᵐ with nm ∈ [0; 6] the - // number of mobilities for this node. Therefore, the return is a - // Matrix<6,kNv> since the number of columns depends on the mobilizer type. - // It is returned as an Eigen::Map to the memory allocated in the - // std::vector H_PB_W_cache so that we can work with H_PB_W as with any - // other Eigen matrix object. - // As a guideline for developers, a summary of the computations performed in // this method is provided: // Notation: @@ -248,15 +251,15 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( // Jain (2010)) where p_MoBo_F = 0 and thus V_PB_W = V_FM_W. // Generalized coordinates local to this node's mobilizer. - const T* q_ptr = get_q(positions); - const T* v_ptr = get_v(velocities); + const T* q_B = get_q(positions); + const T* v_B = get_v(velocities); // ========================================================================= // Computation of V_PB_W in Eq. (1). See summary at the top of this method. // Update V_FM using the operator V_FM = H_FM * vm: SpatialVelocity& V_FM = get_mutable_V_FM(vc); - V_FM = mobilizer_->calc_V_FM(q_ptr, v_ptr); + V_FM = mobilizer_->calc_V_FM(q_B, v_B); // Compute V_PB_W = R_WF * V_FM.Shift(p_MoBo_F), Eq. (4). // Side note to developers: in operator form for rigid bodies this would be @@ -269,7 +272,7 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( // Hinge matrix for this node. H_PB_W ∈ ℝ⁶ˣⁿᵛ with nv ∈ [0; 6] the // 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> v(v_ptr); + const Eigen::Map> v(v_B); V_PB_W.get_coeffs() = H_PB_W * v; } else { V_PB_W.get_coeffs().setZero(); @@ -289,7 +292,12 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( // ========================================================================= // Update velocity V_WB of this node's body B in the world frame. Using the // recursive Eq. (1). See summary at the top of this method. - get_mutable_V_WB(vc) = V_WP.ComposeWithMovingFrameVelocity(p_PB_W, V_PB_W); + SpatialVelocity& V_WB = get_mutable_V_WB(vc); + V_WB = V_WP.ComposeWithMovingFrameVelocity(p_PB_W, V_PB_W); + + // TODO(sherm1) Calculate V_WL for composites. Currently we don't make + // composites so V_WL = V_WB if L is the link on B. + vc->SetV_WL(mobilizer_->mobod().active_link_ordinal(), V_WB); } // As a guideline for developers, a summary of the computations performed in @@ -824,7 +832,7 @@ void BodyNodeImpl:: // Get the spatial acceleration of the parent. const SpatialAcceleration& A_WP = - ac->get_A_WB(mobilizer_->mobod().inboard()); + ac->get_A_WB(mobilizer_->mobod().inboard_mobod()); // Shift vector p_PoBo_W from the parent origin to the body origin. const Vector3& p_PoBo_W = get_p_PoBo_W(pc); diff --git a/multibody/tree/deformable_body.cc b/multibody/tree/deformable_body.cc index 6e5bf119f7a4..fefc69058702 100644 --- a/multibody/tree/deformable_body.cc +++ b/multibody/tree/deformable_body.cc @@ -64,7 +64,7 @@ MultibodyConstraintId DeformableBody::AddFixedConstraint( const RigidBody& body_B, const math::RigidTransform& X_BA, const geometry::Shape& shape_G, const math::RigidTransform& X_BG) { DRAKE_THROW_UNLESS(this->has_parent_tree()); - if (&this->get_parent_tree().get_body(body_B.index()) != &body_B) { + if (&this->get_parent_tree().get_link(body_B.index()) != &body_B) { throw std::logic_error(fmt::format( "AddFixedConstraint(): The rigid body with name {} is not registered " "with the MultibodyPlant owning the deformable model.", diff --git a/multibody/tree/element_collection.cc b/multibody/tree/element_collection.cc index f38250ae0d6e..6cd144644491 100644 --- a/multibody/tree/element_collection.cc +++ b/multibody/tree/element_collection.cc @@ -174,9 +174,12 @@ template class ElementCollection; template class ElementCollection; template class ElementCollection; -template class ElementCollection; -template class ElementCollection; -template class ElementCollection; +// `links_` in MultibodyTree uses `Link` (a template alias for RigidBody). +// GCC substitutes through aliases, but Clang preserves the alias name in +// mangled symbols, so we must instantiate with `Link` to match both compilers. +template class ElementCollection; +template class ElementCollection; +template class ElementCollection; template class ElementCollection; template class ElementCollection class Element, typename Index> @@ -52,6 +52,12 @@ class ElementCollection final { /* Returns the total number of (non-null) elements. */ int num_elements() const { return ssize(indices_packed_); } + /* Returns the total number of indices ever allocated (including the ones + that might be invalid now). This is the size that should be used for any array + that is to be addressed by index. next_index() will return this value as the + next unused index. */ + int num_indices() const { return ssize(elements_by_index_); } + /* Returns a read-only view of the (non-null) elements. The result is only guaranteed to remain valid until the next call to any non-const member function. @@ -96,7 +102,7 @@ class ElementCollection final { /* Returns a reference to the list of currently-valid indices. The result is only guaranteed to remain valid until the next call to any non-const member function. */ - const std::vector& indices() const { return indices_packed_; } + const std::vector& valid_indices() const { return indices_packed_; } /* Returns a reference to the name lookup dictionary. The result is only guaranteed to remain valid until the next call to any non-const member @@ -107,7 +113,7 @@ class ElementCollection final { /* Returns the index that is one beyond the current maximum, i.e., the index that can be used for the next element passed to Add(). */ - Index next_index() const { return Index{ssize(elements_by_index_)}; } + Index next_index() const { return Index{num_indices()}; } /* Adds a new element. If the element->index() equals the next_index(), then the element will be appended to the end (and next_index() will increment). diff --git a/multibody/tree/frame.h b/multibody/tree/frame.h index fb5d6c9b1bf7..8a0c22acd23b 100644 --- a/multibody/tree/frame.h +++ b/multibody/tree/frame.h @@ -22,31 +22,41 @@ std::string DeprecateWhenEmptyName(std::string name, std::string_view type); template class RigidBody; +template +using Link = RigidBody; + /// %Frame is an abstract class representing a _material frame_ (also called a -/// _physical frame_) of its underlying RigidBody. The %Frame's origin is a -/// material point of its RigidBody, and its axes have fixed directions +/// _physical frame_) of its underlying RigidBody (Link). The %Frame's origin +/// is a material point of its RigidBody, and its axes have fixed directions /// in that body. A %Frame's pose (position and orientation) with respect to its -/// RigidBodyFrame may be parameterized, but is fixed (not time or state -/// dependent) once parameters have been set. +/// RigidBodyFrame (LinkFrame) may be parameterized, but is fixed (not time or +/// state dependent) once parameters have been set. /// /// An important characteristic of a %Frame is that forces or torques applied to -/// a %Frame are applied to the %Frame's underlying RigidBody. Force-producing +/// a %Frame are applied to the %Frame's underlying body. Force-producing /// elements like joints, actuators, and constraints usually employ two %Frames, /// with one %Frame connected to one body and the other connected to a different -/// body. Every %Frame F can report the RigidBody B to which it is attached and -/// its pose X_BF with respect to B's RigidBodyFrame. +/// body. Every %Frame F can report the Link (RigidBody) L to which it is +/// attached and its pose X_LF with respect to L's LinkFrame (RigidBodyFrame). /// /// A %Frame's pose in World (or relative to other frames) is always calculated -/// starting with its pose relative to its underlying RigidBodyFrame. +/// starting with its pose relative to its underlying LinkFrame. /// Subclasses derived from %Frame differ in how kinematic calculations are /// performed. For example, the angular velocity of a FixedOffsetFrame or -/// RigidBodyFrame is identical to the angular velocity of its underlying body, +/// LinkFrame (RigidBodyFrame) is identical to the angular velocity of its +/// underlying body, /// whereas the translational velocity of a FixedOffsetFrame differs from that -/// of a RigidBodyFrame. +/// of a LinkFrame. /// /// %Frame provides methods for obtaining its current orientation, position, /// motion, etc. from a Context passed to those methods. /// +/// @note For historical reasons, many of the method names here use "Body" to +/// mean "Link". The distinction matters when we form composite bodies, which +/// consist of multiple links welded together. Those composites form a single +/// _rigid body_ in the physics sense. Frames only know about their Links, not +/// how they may have been combined into a composite body. +/// /// @tparam_default_scalar template class Frame : public MultibodyElement { @@ -58,15 +68,25 @@ class Frame : public MultibodyElement { /// Returns this element's unique index. FrameIndex index() const { return this->template index_impl(); } - /// Returns a const reference to the body associated to this %Frame. - const RigidBody& body() const { return body_; } + /// Returns a const reference to the RigidBody (Link) to which this %Frame is + /// attached (synonym for link()). + const RigidBody& body() const { return link(); } + + /// Returns a const reference to the Link (RigidBody) to which this %Frame is + /// attached (synonym for body()). + const Link& link() const { return link_; } /// Returns true if `this` is the world frame. bool is_world_frame() const { return this->index() == world_frame_index(); } - /// Returns true if `this` is the body frame. - bool is_body_frame() const { - return this->index() == body_.body_frame().index(); + /// Returns true if `this` is the RigidBodyFrame (LinkFrame) of the + /// associated RigidBody (Link). This is a synonym for is_link_frame(). + bool is_body_frame() const { return is_link_frame(); } + + /// Returns true if `this` is the LinkFrame (RigidBodyFrame) of the + /// associated Link (RigidBody). This is a synonym for is_body_frame(). + bool is_link_frame() const { + return this->index() == link().link_frame().index(); } /// Returns the name of this frame. The name will never be empty. @@ -78,33 +98,33 @@ class Frame : public MultibodyElement { /// MultibodyPlant. ScopedName scoped_name() const; - /// Returns a reference to the body-relative pose X_BF giving the pose of this - /// Frame with respect to its body's RigidBodyFrame. This may depend on - /// parameters in the Context but not on time or state. The first time this is - /// called after a parameter change will precalculate offset poses for all - /// %Frames into the Context's cache; subsequent calls on any %Frame are very - /// fast. + /// Returns a reference to the link-relative pose X_LF giving the pose of this + /// %Frame with respect to its link's LinkFrame (RigidBodyFrame). This may + /// depend on parameters in the Context but not on time or state. The first + /// time this is called after a parameter change will precalculate offset + /// poses for all %Frames into the Context's cache; subsequent calls on any + /// %Frame are very fast. const math::RigidTransform& EvalPoseInBodyFrame( const systems::Context& context) const { const internal::FrameBodyPoseCache& frame_body_poses = this->GetParentTreeSystem().EvalFrameBodyPoses(context); - return get_X_BF(frame_body_poses); + return get_X_LF(frame_body_poses); } - /// Returns the pose `X_BF` of `this` frame F in the body frame B associated - /// with this frame. - /// In particular, if `this` **is** the body frame B, this method directly - /// returns the identity transformation. - /// Note that this ONLY depends on the Parameters in the context; it does - /// not depend on time, input, state, etc. + /// Returns the pose `X_LF` of `this` frame F in the LinkFrame + /// (RigidBodyFrame) L of this %Frame's Link (RigidBody). In particular, if + /// `this` **is** the link frame L, this method directly returns the + /// identity transformation. Note that this ONLY depends on the Parameters + /// in the context; it does not depend on time, input, state, etc. math::RigidTransform CalcPoseInBodyFrame( const systems::Context& context) const { return DoCalcPoseInBodyFrame(context.get_parameters()); } - /// Returns the rotation matrix `R_BF` that relates body frame B to `this` - /// frame F (B is the body frame to which `this` frame F is attached). - /// @note If `this` is B, this method returns the identity RotationMatrix. + /// Returns the rotation matrix `R_LF` that relates link frame L to `this` + /// frame F (L is the LinkFrame of the Link (RigidBody) to which `this` + /// frame F is attached). + /// @note If `this` is L, this method returns the identity RotationMatrix. /// Note that this ONLY depends on the Parameters in the context; it does /// not depend on time, input, state, etc. math::RotationMatrix CalcRotationMatrixInBodyFrame( @@ -112,15 +132,12 @@ class Frame : public MultibodyElement { return DoCalcRotationMatrixInBodyFrame(context.get_parameters()); } - /// Variant of CalcPoseInBodyFrame() that returns the fixed pose `X_BF` of - /// `this` frame F in the body frame B associated with this frame. + /// Variant of CalcPoseInBodyFrame() that returns the fixed pose `X_LF` of + /// `this` frame F in the link frame L associated with this frame. /// @throws std::exception if called on a %Frame that does not have a /// fixed offset in the body frame. - // %Frame sub-classes that can represent the fixed pose of `this` frame F in - // a body frame B, must override this method. - // An example of a frame sub-class not implementing this method would be that - // of a frame on a soft body, for which its pose in the body frame depends - // on the state of deformation of the body. + // Frame sub-classes that can represent the fixed pose of `this` frame F in + // a link frame L, must override this method. virtual math::RigidTransform GetFixedPoseInBodyFrame() const { throw std::logic_error( "Attempting to retrieve a fixed pose from a frame of type '" + @@ -128,14 +145,12 @@ class Frame : public MultibodyElement { "', which does not support this operation."); } - /// Returns the rotation matrix `R_BF` that relates body frame B to `this` - /// frame F (B is the body frame to which `this` frame F is attached). + /// Returns the rotation matrix `R_LF` that relates link frame L to `this` + /// frame F (L is the LinkFrame of the Link (RigidBody) to which `this` + /// frame F is attached). /// @throws std::exception if `this` frame F is a %Frame that does not have - /// a fixed offset in the body frame B (i.e., `R_BF` is not constant). - /// %Frame sub-classes that have a constant `R_BF` must override this method. - /// An example of a frame sub-class not implementing this method would be that - /// of a frame on a soft body, for which its pose in the body frame depends - /// on the state of deformation of the body. + /// a fixed offset in the link frame L (i.e., `R_LF` is not constant). + /// %Frame sub-classes that have a constant `R_LF` must override this method. virtual math::RotationMatrix GetFixedRotationMatrixInBodyFrame() const { throw std::logic_error( "Unable to retrieve a fixed rotation matrix from a frame of type '" + @@ -143,21 +158,21 @@ class Frame : public MultibodyElement { "', which does not support this method."); } - // TODO(jwnimmer-tri) These next four functions only exist so that BodyFrame - // can override their NVI body to be a simple copy instead of ComposeXX or - // ComposeRR against an identity. It is not at all clear to me that this - // implementation complexity is buying us any measurable speedup at runtime. + // TODO(jwnimmer-tri) These next four functions only exist so that + // RigidBodyFrame can override their NVI body to be a simple copy instead of + // ComposeXX or ComposeRR against an identity. It is not at all clear to me + // that this implementation complexity is buying us any measurable speedup at + // runtime. /// Given the offset pose `X_FQ` of a frame Q in `this` frame F, this method - /// computes the pose `X_BQ` of frame Q in the body frame B to which this - /// frame is attached. - /// In other words, if the pose of `this` frame F in the body frame B is - /// `X_BF`, this method computes the pose `X_BQ` of frame Q in the body frame - /// B as `X_BQ = X_BF * X_FQ`. - /// In particular, if `this` **is** the body frame B, i.e. `X_BF` is the - /// identity transformation, this method directly returns `X_FQ`. - /// Specific frame subclasses can override this method to provide faster - /// implementations if needed. + /// computes the pose `X_LQ` of frame Q in the link frame L of the Link + /// (RigidBody) to which this frame is attached. In other words, if the + /// pose of `this` frame F in the link frame L is `X_LF`, this method + /// computes the pose `X_LQ` of frame Q in the link frame L as + /// `X_LQ = X_LF * X_FQ`. In particular, if `this` **is** the link frame L, + /// i.e. `X_LF` is identically the identity transform, this method directly + /// returns `X_FQ`. Specific frame subclasses can override this method to + /// provide faster implementations if needed. math::RigidTransform CalcOffsetPoseInBody( const systems::Context& context, const math::RigidTransform& X_FQ) const { @@ -173,9 +188,10 @@ class Frame : public MultibodyElement { } #endif - /// Calculates and returns the rotation matrix `R_BQ` that relates body frame - /// B to frame Q via `this` intermediate frame F, i.e., `R_BQ = R_BF * R_FQ` - /// (B is the body frame to which `this` frame F is attached). + /// Calculates and returns the rotation matrix `R_LQ` that relates link frame + /// L to frame Q via `this` intermediate frame F, i.e., `R_LQ = R_LF * R_FQ` + /// (L is the link frame of the Link (RigidBody) to which `this` frame F is + /// attached). /// @param[in] R_FQ rotation matrix that relates frame F to frame Q. math::RotationMatrix CalcOffsetRotationMatrixInBody( const systems::Context& context, @@ -193,8 +209,8 @@ class Frame : public MultibodyElement { #endif /// Variant of CalcOffsetPoseInBody() that given the offset pose `X_FQ` of a - /// frame Q in `this` frame F, returns the pose `X_BQ` of frame Q in the body - /// frame B to which this frame is attached. + /// frame Q in `this` frame F, returns the pose `X_LQ` of frame Q in the link + /// frame L to which this frame is attached. /// @throws std::exception if called on a %Frame that does not have a /// fixed offset in the body frame. virtual math::RigidTransform GetFixedOffsetPoseInBody( @@ -202,12 +218,13 @@ class Frame : public MultibodyElement { return GetFixedPoseInBodyFrame() * X_FQ; } - /// Calculates and returns the rotation matrix `R_BQ` that relates body frame - /// B to frame Q via `this` intermediate frame F, i.e., `R_BQ = R_BF * R_FQ` - /// (B is the body frame to which `this` frame F is attached). + /// Calculates and returns the rotation matrix `R_LQ` that relates link frame + /// L to frame Q via `this` intermediate frame F, i.e., `R_LQ = R_LF * R_FQ` + /// (L is the link frame of the Link (RigidBody) to which `this` frame F is + /// attached). /// @param[in] R_FQ rotation matrix that relates frame F to frame Q. /// @throws std::exception if `this` frame F is a %Frame that does not have - /// a fixed offset in the body frame B (i.e., `R_BF` is not constant). + /// a fixed offset in the link frame L (i.e., `R_LF` is not constant). virtual math::RotationMatrix GetFixedRotationMatrixInBody( const math::RotationMatrix& R_FQ) const { return GetFixedRotationMatrixInBodyFrame() * R_FQ; @@ -216,7 +233,7 @@ class Frame : public MultibodyElement { /// Computes and returns the pose `X_WF` of `this` frame F in the world /// frame W as a function of the state of the model stored in `context`. /// @note RigidBody::EvalPoseInWorld() provides a more efficient way to obtain - /// the pose for a body frame. + /// the pose for a RigidBodyFrame (LinkFrame). math::RigidTransform CalcPoseInWorld( const systems::Context& context) const { DRAKE_THROW_UNLESS(this->has_parent_tree()); @@ -261,10 +278,8 @@ class Frame : public MultibodyElement { /// velocity ω measured in a frame M and expressed in a frame E). const Vector3& EvalAngularVelocityInWorld( const systems::Context& context) const { - // TODO(Mitiguy) The calculation below assumes "this" frame is attached to a - // rigid body (not a soft body). Modify if soft bodies are possible. - const SpatialVelocity& V_WB = body().EvalSpatialVelocityInWorld(context); - const Vector3& w_WF_W = V_WB.rotational(); + const SpatialVelocity& V_WL = link().EvalSpatialVelocityInWorld(context); + const Vector3& w_WF_W = V_WL.rotational(); return w_WF_W; } @@ -292,7 +307,8 @@ class Frame : public MultibodyElement { /// frame W). The translational part is v_WFo_W (translational velocity v of /// frame F's origin point Fo, measured and expressed in the world frame W). /// @note RigidBody::EvalSpatialVelocityInWorld() provides a more efficient - /// way to obtain a body frame's spatial velocity measured in the world frame. + /// way to obtain a RigidBodyFrame (LinkFrame) spatial velocity measured in + /// the world frame. /// @see CalcSpatialVelocity(), CalcRelativeSpatialVelocityInWorld(), and /// CalcSpatialAccelerationInWorld(). SpatialVelocity CalcSpatialVelocityInWorld( @@ -314,20 +330,20 @@ class Frame : public MultibodyElement { const Frame& frame_M, const Frame& frame_E) const; - /// Calculates `this` frame C's spatial velocity relative to another frame B, + /// Calculates `this` frame F's spatial velocity relative to another frame B, /// measured and expressed in the world frame W. /// @param[in] context contains the state of the multibody system. /// @param[in] other_frame which is frame B. - /// @return V_W_BC_W = V_WC_W - V_WB_W, frame C's spatial velocity relative to + /// @return V_W_BF_W = V_WF_W - V_WB_W, frame F's spatial velocity relative to /// frame B, measured and expressed in the world frame W. The rotational part - /// of the returned quantity is ω_BC_W (C's angular velocity measured in B and - /// expressed in W). The translational part is v_W_BoCo_W (Co's translational + /// of the returned quantity is ω_BF_W (F's angular velocity measured in B and + /// expressed in W). The translational part is v_W_BoFo_W (Fo's translational /// velocity relative to Bo, measured and expressed in world frame W). 
-  ///     ω_BC_W  = ω_WC_W - ω_WB_W
-  ///  v_W_BoCo_W = v_WCo_W - v_WBo_W = DtW(p_BoCo)
+  ///     ω_BF_W  = ω_WF_W - ω_WB_W
+  ///  v_W_BoFo_W = v_WFo_W - v_WBo_W = DtW(p_BoFo)
   ///
- /// where DtW(p_BoCo) is the time-derivative in frame W of p_BoCo (position - /// vector from Bo to Co), and this vector is expressed in frame W. + /// where DtW(p_BoFo) is the time-derivative in frame W of p_BoFo (position + /// vector from Bo to Fo), and this vector is expressed in frame W. /// @note The method CalcSpatialVelocityInWorld() is more efficient and /// coherent if any of `this`, other_frame, or the world frame W are the same. /// @see CalcSpatialVelocityInWorld() and CalcRelativeSpatialVelocity(). @@ -336,32 +352,32 @@ class Frame : public MultibodyElement { const Frame& frame_B = other_frame; const SpatialVelocity V_WB_W = frame_B.CalcSpatialVelocityInWorld(context); - const SpatialVelocity V_WC_W = CalcSpatialVelocityInWorld(context); - return V_WC_W - V_WB_W; + const SpatialVelocity V_WF_W = CalcSpatialVelocityInWorld(context); + return V_WF_W - V_WB_W; } - /// Calculates `this` frame C's spatial velocity relative to another frame B, + /// Calculates `this` frame F's spatial velocity relative to another frame B, /// measured in a frame M, expressed in a frame E. /// @param[in] context contains the state of the multibody system. /// @param[in] other_frame which is frame B. /// @param[in] measured_in_frame which is frame M. /// @param[in] expressed_in_frame which is frame E. - /// @return V_M_BC_E = V_MC_E - V_MB_E, frame C's spatial velocity relative to + /// @return V_M_BF_E = V_MF_E - V_MB_E, frame F's spatial velocity relative to /// frame B, measured in frame M, expressed in frame E. The rotational part - /// of the returned quantity is ω_BC_E (C's angular velocity measured in B and - /// expressed in E). The translational part is v_M_BoCo_E (Co's translational + /// of the returned quantity is ω_BF_E (F's angular velocity measured in B and + /// expressed in E). The translational part is v_M_BoFo_E (Fo's translational /// velocity relative to Bo, measured in M, and expressed in E). 
-  ///  ω_BC_E = ω_MC_E - ω_MB_E
-  ///  v_M_BoCo_E = v_MCo_E - v_MBo_E = DtM(p_BoCo)
+  ///  ω_BF_E = ω_MF_E - ω_MB_E
+  ///  v_M_BoFo_E = v_MFo_E - v_MBo_E = DtM(p_BoFo)
   ///
- /// where DtM(p_BoCo) is the time-derivative in frame M of p_BoCo (position - /// vector from Bo to Co), and this vector is expressed in frame E. + /// where DtM(p_BoFo) is the time-derivative in frame M of p_BoFo (position + /// vector from Bo to Fo), and this vector is expressed in frame E. /// @note The method CalcSpatialVelocity() is more efficient and coherent /// if any of `this`, other_frame, or measured_in_frame are the same. - /// Also, the value of V_M_BoCo does not depend on the measured_in_frame if - /// Bo and Co are coincident (i.e., p_BoCo = 0), in which case consider the + /// Also, the value of V_M_BoFo does not depend on the measured_in_frame if + /// Bo and Fo are coincident (i.e., p_BoFo = 0), in which case consider the /// more efficient method CalcRelativeSpatialVelocityInWorld(). - /// Lastly, the calculation of elongation between Bo and Co can be done with + /// Lastly, the calculation of elongation between Bo and Fo can be done with /// relative translational velocity, but elongation does not depend on the /// measured-in-frame (hence consider CalcRelativeSpatialVelocityInWorld()). /// @see CalcSpatialVelocityInWorld(), CalcSpatialVelocity(), and @@ -375,9 +391,9 @@ class Frame : public MultibodyElement { const Frame& frame_E = expressed_in_frame; const SpatialVelocity V_MB_E = frame_B.CalcSpatialVelocity(context, frame_M, frame_E); - const SpatialVelocity V_MC_E = + const SpatialVelocity V_MF_E = CalcSpatialVelocity(context, frame_M, frame_E); - return V_MC_E - V_MB_E; + return V_MF_E - V_MB_E; } /// Calculates `this` frame F's spatial acceleration measured and expressed in @@ -420,24 +436,24 @@ class Frame : public MultibodyElement { const systems::Context& context, const Frame& measured_in_frame, const Frame& expressed_in_frame) const; - /// Calculates `this` frame C's spatial acceleration relative to another + /// Calculates `this` frame F's spatial acceleration relative to another /// frame B, measured and expressed in the world frame W. /// @param[in] context contains the state of the multibody system. /// @param[in] other_frame which is frame B. - /// @return A_W_BC_W = A_WC_W - A_WB_W, frame C's spatial acceleration + /// @return A_W_BF_W = A_WF_W - A_WB_W, frame F's spatial acceleration /// relative to frame B, measured and expressed in the world frame W. /// - /// In general, A_W_BC = DtW(V_W_BC), the time-derivative in the world frame W - /// of frame C's spatial velocity relative to frame B. The rotational part of - /// the returned quantity is α_WC_W - α_WB_W = DtW(ω_BC)_W. For 3D analysis, - /// DtW(ω_BC) ≠ α_BC. The translational part of the returned quantity is - /// a_W_BoCo_W (Co's translational acceleration relative to Bo, measured and + /// In general, A_W_BF = DtW(V_W_BF), the time-derivative in the world frame W + /// of frame F's spatial velocity relative to frame B. The rotational part of + /// the returned quantity is α_WF_W - α_WB_W = DtW(ω_BF)_W. For 3D analysis, + /// DtW(ω_BF) ≠ α_BF. The translational part of the returned quantity is + /// a_W_BoFo_W (Fo's translational acceleration relative to Bo, measured and /// expressed in world frame W). 
-  ///  α_WC_W - α_WB_W = DtW(ω_WC)_W - DtW(ω_WB)_W = DtW(ω_BC)_W
-  ///  a_W_BoCo_W = a_WCo_W - a_WBo_W = DtW(v_WCo) - DtW(v_WBo) = Dt²W(p_BoCo)_W
+  ///  α_WF_W - α_WB_W = DtW(ω_WF)_W - DtW(ω_WB)_W = DtW(ω_BF)_W
+  ///  a_W_BoFo_W = a_WFo_W - a_WBo_W = DtW(v_WFo) - DtW(v_WBo) = Dt²W(p_BoFo)_W
   ///
- /// where Dt²W(p_BoCo)_W is the 2ⁿᵈ time-derivative in frame W of p_BoCo (the - /// position vector from Bo to Co), and this result is expressed in frame W. + /// where Dt²W(p_BoFo)_W is the 2ⁿᵈ time-derivative in frame W of p_BoFo (the + /// position vector from Bo to Fo), and this result is expressed in frame W. /// @note The method CalcSpatialAccelerationInWorld() is more efficient and /// coherent if any of `this`, other_frame, or the world frame W are the same. /// @see CalcSpatialAccelerationInWorld(), CalcRelativeSpatialAcceleration(). @@ -446,33 +462,33 @@ class Frame : public MultibodyElement { const Frame& frame_B = other_frame; const SpatialAcceleration A_WB_W = frame_B.CalcSpatialAccelerationInWorld(context); - const SpatialAcceleration A_WC_W = + const SpatialAcceleration A_WF_W = CalcSpatialAccelerationInWorld(context); - return A_WC_W - A_WB_W; + return A_WF_W - A_WB_W; } - /// Calculates `this` frame C's spatial acceleration relative to another + /// Calculates `this` frame F's spatial acceleration relative to another /// frame B, measured in a frame M, expressed in a frame E. /// @param[in] context contains the state of the multibody system. /// @param[in] other_frame which is frame B. /// @param[in] measured_in_frame which is frame M. /// @param[in] expressed_in_frame which is frame E. - /// @return A_M_BC_E = A_MC_E - A_MB_E, frame C's spatial acceleration + /// @return A_M_BF_E = A_MF_E - A_MB_E, frame F's spatial acceleration /// relative to frame B, measured in frame M, expressed in frame E. /// - /// In general, A_M_BC = DtW(V_M_BC), the time-derivative in frame M of - /// frame C's spatial velocity relative to frame B. The rotational part of the - /// returned quantity is α_MC_E - α_MB_E = DtM(ω_BC)_E. Note: For 3D analysis, - /// DtM(ω_BC) ≠ α_BC. The translational part of the returned quantity is - /// a_M_BoCo_E (Co's translational acceleration relative to Bo, measured in + /// In general, A_M_BF = DtW(V_M_BF), the time-derivative in frame M of + /// frame F's spatial velocity relative to frame B. The rotational part of the + /// returned quantity is α_MF_E - α_MB_E = DtM(ω_BF)_E. Note: For 3D analysis, + /// DtM(ω_BF) ≠ α_BF. The translational part of the returned quantity is + /// a_M_BoFo_E (Fo's translational acceleration relative to Bo, measured in /// frame M, expressed in frame E). 
-  ///  α_MC_E - α_MB_E = DtM(ω_MC)_E - DtM(ω_MB)_E = DtM(ω_BC)_E
-  ///  a_M_BoCo_E = a_MCo_E - a_MBo_E = DtM(v_MCo) - DtM(v_MBo) = Dt²M(p_BoCo)_E
+  ///  α_MF_E - α_MB_E = DtM(ω_MF)_E - DtM(ω_MB)_E = DtM(ω_BF)_E
+  ///  a_M_BoFo_E = a_MFo_E - a_MBo_E = DtM(v_MFo) - DtM(v_MBo) = Dt²M(p_BoFo)_E
   ///
- /// where Dt²M(p_BoCo)_E is the 2ⁿᵈ time-derivative in frame M of p_BoCo (the - /// position vector from Bo to Co), and this result is expressed in frame E. + /// where Dt²M(p_BoFo)_E is the 2ⁿᵈ time-derivative in frame M of p_BoFo (the + /// position vector from Bo to Fo), and this result is expressed in frame E. /// @note The calculation of the 2ⁿᵈ time-derivative of the distance between - /// Bo and Co can be done with relative translational acceleration, but this + /// Bo and Fo can be done with relative translational acceleration, but this /// calculation does not depend on the measured-in-frame, hence in this case, /// consider CalcRelativeSpatialAccelerationInWorld() since it is faster. /// @see CalcSpatialAccelerationInWorld(), CalcSpatialAcceleration(), and @@ -486,9 +502,9 @@ class Frame : public MultibodyElement { const Frame& frame_E = expressed_in_frame; const SpatialAcceleration A_MB_E = frame_B.CalcSpatialAcceleration(context, frame_M, frame_E); - const SpatialAcceleration A_MC_E = + const SpatialAcceleration A_MF_E = CalcSpatialAcceleration(context, frame_M, frame_E); - return A_MC_E - A_MB_E; + return A_MF_E - A_MB_E; } /// (Advanced) NVI to DoCloneToScalar() templated on the scalar type of the @@ -511,46 +527,47 @@ class Frame : public MultibodyElement { /// @name Internal use only /// These functions work directly with the frame body pose cache entry. //@{ - /// (Internal use only) A %Frame's pose-in-parent X_PF can be parameterized, - /// the parent's pose may also be parameterized, and so on. Thus the - /// calculation of this frame's pose in its body (X_BF) can be expensive. - /// There is a cache entry that holds the calculated X_BF, evaluated - /// whenever parameters change. This allows us to grab X_BF as a const - /// reference rather than having to extract and reformat parameters, and + /// (Internal use only) A %Frame's pose-in-parent-frame X_PF can be + /// parameterized, the parent frame's pose may also be parameterized, and so + /// on. Thus the calculation of this frame's pose in its link frame (X_LF) can + /// be expensive. There is a cache entry that holds the calculated X_LF, + /// evaluated whenever parameters change. This allows us to grab X_LF as a + /// const reference rather than having to extract and reformat parameters, and /// compose with parent and ancestor poses at runtime. /// - /// At the time parameters are allocated we assign a slot in the body pose - /// cache entry to each %Frame and record its index using this function. (The - /// index for a RigidBodyFrame will refer to an identity transform.) Note that - /// the body pose index is not necessarily the same as the %Frame index - /// because all RigidBodyFrames can share an entry. (Of course if you know you - /// are working with a RigidBodyFrame you don't need to ask about its body - /// pose!) - void set_body_pose_index_in_cache(int body_pose_index) { - body_pose_index_in_cache_ = body_pose_index; - } + /// When we are optimizing assemblies using composite Mobods, the pose of + /// a frame on its Mobod (X_BF) can differ from its pose on its Link (X_LF). + /// Most multibody computations need X_BF, but X_LF is available also. - /// (Internal use only) Retrieve this %Frame's body pose index in the cache. - int get_body_pose_index_in_cache() const { return body_pose_index_in_cache_; } + /// (Internal use only) Given an already up-to-date frame body pose cache, + /// extract X_LF for this %Frame from it. + /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() + /// since the last parameter change; we can't check here. + /// @retval X_LF pose of this frame in its Link's frame + const math::RigidTransform& get_X_LF( + const internal::FrameBodyPoseCache& frame_body_poses) const { + return frame_body_poses.get_X_LF(index()); + } /// (Internal use only) Given an already up-to-date frame body pose cache, - /// extract X_BF for this %Frame from it. + /// extract X_BF for this %Frame from it. Note that X_BF is F's pose on its + /// mobilized body B which might not be the same as its link L. /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() /// since the last parameter change; we can't check here. - /// @retval X_BF pose of this frame in its body's frame + /// @retval X_BF pose of this frame in its Mobod's frame const math::RigidTransform& get_X_BF( const internal::FrameBodyPoseCache& frame_body_poses) const { - return frame_body_poses.get_X_BF(body_pose_index_in_cache_); + return frame_body_poses.get_X_BF(index()); } /// (Internal use only) Given an already up-to-date frame body pose cache, /// extract X_FB (=X_BF⁻¹) for this %Frame from it. /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() /// since the last parameter change; we can't check here. - /// @retval X_FB inverse of this frame's pose in its body's frame + /// @retval X_FB inverse of this frame's pose in its Mobod's frame const math::RigidTransform& get_X_FB( const internal::FrameBodyPoseCache& frame_body_poses) const { - return frame_body_poses.get_X_FB(body_pose_index_in_cache_); + return frame_body_poses.get_X_FB(index()); } /// (Internal use only) Given an already up-to-date frame body pose cache, @@ -558,22 +575,22 @@ class Frame : public MultibodyElement { /// precomputed in the cache so is very fast to check. /// @note Be sure you have called MultibodyTreeSystem::EvalFrameBodyPoses() /// since the last parameter change; we can't check here. - /// @see get_X_BF() + /// @see get_X_BF(), get_X_FB() bool is_X_BF_identity( const internal::FrameBodyPoseCache& frame_body_poses) const { - return frame_body_poses.is_X_BF_identity(body_pose_index_in_cache_); + return frame_body_poses.is_X_BF_identity(index()); } //@} protected: /// Only derived classes can use this constructor. It creates a %Frame - /// object attached to `body` and puts the frame in the body's model + /// object attached to `link` and puts the frame in the link's model /// instance. - explicit Frame(const std::string& name, const RigidBody& body, + explicit Frame(const std::string& name, const Link& link, std::optional model_instance = {}) - : MultibodyElement(model_instance.value_or(body.model_instance())), + : MultibodyElement(model_instance.value_or(link.model_instance())), name_(internal::DeprecateWhenEmptyName(name, "Frame")), - body_(body) {} + link_(link) {} /// Called by DoDeclareParameters(). Derived classes may choose to override /// to declare their sub-class specific parameters. @@ -652,10 +669,8 @@ class Frame : public MultibodyElement { const std::string name_; - // The body associated with this frame. - const RigidBody& body_; - - int body_pose_index_in_cache_{-1}; + // The link to which this frame is attached. + const Link& link_; }; } // namespace multibody diff --git a/multibody/tree/frame_body_pose_cache.h b/multibody/tree/frame_body_pose_cache.h index fa5dcecba3b7..f1a51d8f5b07 100644 --- a/multibody/tree/frame_body_pose_cache.h +++ b/multibody/tree/frame_body_pose_cache.h @@ -13,84 +13,147 @@ namespace drake { namespace multibody { namespace internal { -/* This class is one of the cache entries in the Context. It holds the -precalculated body-relative poses X_BF of every Frame F. Since FixedOffsetFrame -body poses are parameterized, and given with respect to a parent frame P which -may itself be a parameterized FixedOffsetFrame, we need to precalculate X_BF -once the parameters have been set so that we don't have to do that calculation -repeatedly at runtime. We also precalculate the inverse X_FB since that -is often needed as well, and record whether X_BF (and of course X_FB) is the -identity transform, for use in runtime optimizations. - -Every Frame is allocated one slot here and the index of that slot (which we -refer to as `body_pose_index` in this class) is stored in the Frame object for -fast retrieval (the indices are assigned in Finalize()). Since RigidBodyFrames -have identity poses by definition, they all share a single entry (the 0th) here -to permit getting the body pose of any Frame efficiently in cases where you -don't know what kind of Frame you have. Of course if you know you are working -with RigidBodyFrames you don't have to use this cache. +/* This class is one of the cache entries in the Context. It can be filled in +once parameters have known values (and must be recalculated when parameters +change). It holds the following items. + +Frame & Link poses +------------------ + - the link-relative pose X_LF of every Frame F on the Link to which it is + fixed. FixedOffsetFrame link poses are parameterized, and given with + respect to a parent frame P which may itself be a parameterized + FixedOffsetFrame. We need to precalculate X_LF so that we don't have to do + that calculation repeatedly at runtime. + - the pose X_BL of each link's frame L on its Mobod frame B. Note that frame B + is always the link frame of the mobod's active (most inboard) link. X_BL is + necessarily identity unless B is a composite mobod and L is not the active + link. + - since a frame is fixed to its link L, and L is fixed to its mobod B, we + can calculate each frame's mobod-relative pose X_BF (= X_BL*X_LF). This + can only differ from X_LF when mobod B is composite and L is not the active + link of B. + - the inverse X_FB since that is often needed as well. + - whether X_BF (and of course X_FB) is the identity transform, for use in + runtime optimizations. + +Mass properties +--------------- + - the SpatialInertia M_LLo_L of every link L about its link origin Lo, + expressed in L. Since mass properties can be parameterized, we need to + precalculate these inertias so that we don't have to do that repeatedly at + runtime. + - the SpatialInertia M_BBo_B of every mobod B about its body origin Bo, + expressed in B. This differs from M_LLo_L when B is a composite mobod. + @tparam_default_scalar */ template class FrameBodyPoseCache { public: DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FrameBodyPoseCache); - explicit FrameBodyPoseCache(int num_mobods, - int num_frame_body_pose_slots_needed) - : X_BF_pool_(num_frame_body_pose_slots_needed), - X_FB_pool_(num_frame_body_pose_slots_needed), - is_X_BF_identity_(num_frame_body_pose_slots_needed), + explicit FrameBodyPoseCache(int num_links, int num_frames, int num_mobods) + : X_LF_pool_(num_frames, math::RigidTransform::Identity()), + X_BF_pool_(num_frames, math::RigidTransform::Identity()), + X_FB_pool_(num_frames, math::RigidTransform::Identity()), + is_X_BF_identity_(num_frames, true), + X_BL_pool_(num_links, math::RigidTransform::Identity()), + is_X_BL_identity_(num_links, true), + M_LLo_L_pool_(num_links, SpatialInertia::NaN()), M_BBo_B_pool_(num_mobods, SpatialInertia::NaN()) { - DRAKE_DEMAND(num_frame_body_pose_slots_needed > 0); + // Initially all transforms are identity, mass props are NaN. + } + + const math::RigidTransform& get_X_LF(FrameIndex index) const { + DRAKE_ASSERT(0 <= index && index < ssize(X_LF_pool_)); + return X_LF_pool_[index]; + } + + const math::RigidTransform& get_X_BF(FrameIndex index) const { + // This method must be very fast in Release. + DRAKE_ASSERT(0 <= index && index < ssize(X_BF_pool_)); + return X_BF_pool_[index]; + } - // All RigidBodyFrames share this body pose. - X_BF_pool_[0] = X_FB_pool_[0] = math::RigidTransform::Identity(); - is_X_BF_identity_[0] = true; + const math::RigidTransform& get_X_FB(FrameIndex index) const { + // This method must be very fast in Release. + DRAKE_ASSERT(0 <= index && index < ssize(X_FB_pool_)); + return X_FB_pool_[index]; } - const math::RigidTransform& get_X_BF(int body_pose_index) const { + // We're given a frame F that is fixed to some link L, and L is fixed to + // some mobod B. Denote B's active link as L₀. By definition, L₀'s + // LinkFrame is also mobilized body B's body frame. + // + // This method returns true if + // (1) F is B's body frame (that is, F is L's LinkFrame and L≡L₀), or + // (2) T is nonsymbolic and F is currently coincident B's body frame. + // + // This should be used only for performance optimization, so that a false + // negative harmlessly leads to treating X_BF as a general transform. + bool is_X_BF_identity(FrameIndex index) const { // This method must be very fast in Release. - DRAKE_ASSERT(0 <= body_pose_index && body_pose_index < ssize(X_BF_pool_)); - return X_BF_pool_[body_pose_index]; + DRAKE_ASSERT(0 <= index && index < ssize(is_X_BF_identity_)); + return static_cast(is_X_BF_identity_[index]); } - const math::RigidTransform& get_X_FB(int body_pose_index) const { + const math::RigidTransform& get_X_BL(LinkOrdinal ordinal) const { // This method must be very fast in Release. - DRAKE_ASSERT(0 <= body_pose_index && body_pose_index < ssize(X_FB_pool_)); - return X_FB_pool_[body_pose_index]; + DRAKE_ASSERT(0 <= ordinal && ordinal < ssize(X_BL_pool_)); + return X_BL_pool_[ordinal]; } - // Returns true if F is a body frame or is coincident with a body frame, - // unless T is symbolic, in which case we always return false. This should be - // used only for performance optimization, so that a false negative - // harmlessly leads to treating X_BF as a general transform. - bool is_X_BF_identity(int body_pose_index) const { + bool is_X_BL_identity(LinkOrdinal ordinal) const { // This method must be very fast in Release. - DRAKE_ASSERT(0 <= body_pose_index && - body_pose_index < ssize(is_X_BF_identity_)); - return static_cast(is_X_BF_identity_[body_pose_index]); + DRAKE_ASSERT(0 <= ordinal && ordinal < ssize(is_X_BL_identity_)); + return static_cast(is_X_BL_identity_[ordinal]); + } + + const SpatialInertia& get_M_LLo_L(LinkOrdinal ordinal) const { + DRAKE_ASSERT(0 <= ordinal && ordinal < ssize(M_LLo_L_pool_)); + return M_LLo_L_pool_[ordinal]; } - const SpatialInertia& get_M_BBo_B(MobodIndex index) const { + const SpatialInertia& get_M_BBo_B(MobodIndex ordinal) const { // This method must be very fast in Release. - DRAKE_ASSERT(0 <= index && index < ssize(M_BBo_B_pool_)); - return M_BBo_B_pool_[index]; + DRAKE_ASSERT(0 <= ordinal && ordinal < ssize(M_BBo_B_pool_)); + return M_BBo_B_pool_[ordinal]; } - void SetX_BF(int body_pose_index, const math::RigidTransform& X_BF) { + void SetX_LF(FrameIndex index, const math::RigidTransform& X_LF) { // This method is only called when parameters change. - DRAKE_DEMAND(0 <= body_pose_index && body_pose_index < ssize(X_BF_pool_)); - // RigidBodyFrames use pose index 0; we already know X_BF is identity. - if (body_pose_index == 0) return; - X_BF_pool_[body_pose_index] = X_BF; - X_FB_pool_[body_pose_index] = X_BF.inverse(); + DRAKE_DEMAND(0 <= index && index < ssize(X_LF_pool_)); + X_LF_pool_[index] = X_LF; + } + + void SetX_BF(FrameIndex index, const math::RigidTransform& X_BF) { + // This method is only called when parameters change. + DRAKE_DEMAND(0 <= index && index < ssize(X_BF_pool_)); + X_BF_pool_[index] = X_BF; + X_FB_pool_[index] = X_BF.inverse(); if constexpr (scalar_predicate::is_bool) { - is_X_BF_identity_[body_pose_index] = X_BF.IsExactlyIdentity(); + is_X_BF_identity_[index] = X_BF.IsExactlyIdentity(); } else { - is_X_BF_identity_[body_pose_index] = static_cast(false); + is_X_BF_identity_[index] = static_cast(false); } } + void SetX_BL(LinkOrdinal ordinal, const math::RigidTransform& X_BL) { + // This method is only called when parameters change. + DRAKE_DEMAND(0 <= ordinal && ordinal < ssize(X_BL_pool_)); + X_BL_pool_[ordinal] = X_BL; + if constexpr (scalar_predicate::is_bool) { + is_X_BL_identity_[ordinal] = X_BL.IsExactlyIdentity(); + } else { + is_X_BL_identity_[ordinal] = static_cast(false); + } + } + + void SetM_LLo_L(LinkOrdinal ordinal, const SpatialInertia& M_LLo_L) { + // This method is only called when parameters change. + DRAKE_DEMAND(0 <= ordinal && ordinal < ssize(M_LLo_L_pool_)); + M_LLo_L_pool_[ordinal] = M_LLo_L; + } + void SetM_BBo_B(MobodIndex index, const SpatialInertia& M_BBo_B) { // This method is only called when parameters change. DRAKE_DEMAND(0 <= index && index < ssize(M_BBo_B_pool_)); @@ -100,13 +163,18 @@ class FrameBodyPoseCache { private: // Sizes are set on construction. - // These are indexed by Frame::get_body_pose_index_in_cache(). + // These are indexed by FrameIndex. + std::vector> X_LF_pool_; std::vector> X_BF_pool_; std::vector> X_FB_pool_; std::vector is_X_BF_identity_; // fast vector equivalent - // Spatial inertia of mobilized body B, about its body origin Bo, expressed - // in B. These are indexed by MobodIndex. + // These are indexed by LinkOrdinal. + std::vector> X_BL_pool_; + std::vector is_X_BL_identity_; + std::vector> M_LLo_L_pool_; + + // This is indexed by MobodIndex. std::vector> M_BBo_B_pool_; }; diff --git a/multibody/tree/joint.h b/multibody/tree/joint.h index 9a0b9331e13a..0e0973b8f7ba 100644 --- a/multibody/tree/joint.h +++ b/multibody/tree/joint.h @@ -1005,8 +1005,9 @@ class Joint : public MultibodyElement { In the case of revolute, prismatic, and screw joints we have an axis â whose components are the same in Jp and Jc. However, for maximum speed, the - available mobilizers are specialized to rotate only about a coordinate axis. - TODO(sherm1) Make that happen. + available mobilizers for revolute and prismatic are specialized to rotate + about or translate along a coordinate axis. + As an example, if the mobilizer rotates around z, we want new frames F and M with Fz=Mz=â, Fo=Jpo, Mo=Jco. We also want F==M when Jp==Jc, i.e. at the joint zero position so that the coordinate q will mean the same thing using F and M diff --git a/multibody/tree/linear_spring_damper.cc b/multibody/tree/linear_spring_damper.cc index 0c8b41568490..9d37c1fcaf8a 100644 --- a/multibody/tree/linear_spring_damper.cc +++ b/multibody/tree/linear_spring_damper.cc @@ -144,12 +144,12 @@ template std::unique_ptr> LinearSpringDamper::TemplatedDoCloneToScalar( const internal::MultibodyTree& tree_clone) const { - const RigidBody& bodyA_clone = tree_clone.get_body(bodyA().index()); - const RigidBody& bodyB_clone = tree_clone.get_body(bodyB().index()); + const Link& linkA_clone = tree_clone.get_link(bodyA().index()); + const Link& linkB_clone = tree_clone.get_link(bodyB().index()); // Make the LinearSpringDamper clone. auto spring_damper_clone = std::make_unique>( - bodyA_clone, p_AP(), bodyB_clone, p_BQ(), free_length(), stiffness(), + linkA_clone, p_AP(), linkB_clone, p_BQ(), free_length(), stiffness(), damping()); return spring_damper_clone; diff --git a/multibody/tree/linear_spring_damper.h b/multibody/tree/linear_spring_damper.h index fbdf7f223780..de7f33fe3b96 100644 --- a/multibody/tree/linear_spring_damper.h +++ b/multibody/tree/linear_spring_damper.h @@ -14,7 +14,7 @@ template class RigidBody; /// This %ForceElement models a spring-damper attached between two points on -/// two different bodies. +/// two different bodies (links). /// Given a point P on a body A and a point Q on a body B with positions /// p_AP and p_BQ, respectively, this spring-damper applies equal and /// opposite forces on bodies A and B according to: 
diff --git a/multibody/tree/multibody_element.h b/multibody/tree/multibody_element.h
index 824f80ca7dc8..0b446cd21815 100644
--- a/multibody/tree/multibody_element.h
+++ b/multibody/tree/multibody_element.h
@@ -220,8 +220,11 @@ class MultibodyElement {
   // Give unit tests access to the tree.
   friend class MultibodyElementTester;
 
+  // Index and ordinal are given the same numerical value here. The ordinal
+  // can be overridden with set_ordinal().
   void set_parent_tree(const internal::MultibodyTree* tree, int64_t index) {
     index_ = index;
+    ordinal_ = index;
     parent_tree_ = tree;
   }
 
diff --git a/multibody/tree/multibody_forces.cc b/multibody/tree/multibody_forces.cc
index adb64f429e0a..42e2641c88e7 100644
--- a/multibody/tree/multibody_forces.cc
+++ b/multibody/tree/multibody_forces.cc
@@ -7,7 +7,7 @@ namespace multibody {
 
 template 
 MultibodyForces::MultibodyForces(const internal::MultibodyTree& model)
-    : MultibodyForces(model.num_bodies(), model.num_velocities()) {
+    : MultibodyForces(model.num_links(), model.num_velocities()) {
   DRAKE_DEMAND(model.is_finalized());
 }
 
@@ -42,7 +42,7 @@ template 
 bool MultibodyForces::CheckHasRightSizeForModel(
     const internal::MultibodyTree& model) const {
   return model.num_velocities() == num_velocities() &&
-         model.num_bodies() == num_bodies();
+         model.num_links() == num_bodies();
 }
 
 template 
diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc
index cccc58fb4dda..94a39f52a0c6 100644
--- a/multibody/tree/multibody_tree.cc
+++ b/multibody/tree/multibody_tree.cc
@@ -56,8 +56,8 @@ MultibodyTree::MultibodyTree() {
   // correct.
   DRAKE_DEMAND(world_instance == world_model_instance());
 
-  world_rigid_body_ = &AddRigidBody("world", world_model_instance(),
-                                    SpatialInertia::NaN());
+  world_link_ =
+      &AddLink("world", world_model_instance(), SpatialInertia::NaN());
 
   // `default_model_instance()` hardcodes the returned index.  Make sure it's
   // correct.
@@ -95,35 +95,36 @@ template 
 MultibodyTree::~MultibodyTree() = default;
 
 template 
-const RigidBody& MultibodyTree::AddRigidBody(
+const Link& MultibodyTree::AddLink(
     const std::string& name, ModelInstanceIndex model_instance,
-    const SpatialInertia& M_BBo_B) {
+    const SpatialInertia& M_LLo_L) {
   if (model_instance >= num_model_instances()) {
     throw std::logic_error("Invalid model instance specified.");
   }
 
-  if (HasBodyNamed(name, model_instance)) {
+  if (HasLinkNamed(name, model_instance)) {
     throw std::logic_error(fmt::format(
         "Model instance '{}' already contains a body named '{}'. Body names "
         "must be unique within a given model.",
         model_instances_.get_element(model_instance).name(), name));
   }
 
-  const RigidBody& body = this->AddRigidBodyImpl(
-      std::make_unique>(name, model_instance, M_BBo_B));
-  return body;
+  const Link& link = this->AddLinkImpl(
+      std::make_unique>(name, model_instance, M_LLo_L));
+  return link;
 }
 
 template 
-const RigidBody& MultibodyTree::AddRigidBody(
-    const std::string& name, const SpatialInertia& M_BBo_B) {
+const RigidBody& MultibodyTree::AddLink(
+    const std::string& name, const SpatialInertia& M_LLo_L) {
   if (num_model_instances() != 2) {
+    // MbP API is AddRigidBody() rather than AddLink().
     throw std::logic_error(
-        "This model has more model instances than the default.  Please "
+        "This model has more model instances than the default. Please "
         "call AddRigidBody() with an explicit model instance.");
   }
 
-  return AddRigidBody(name, default_model_instance(), M_BBo_B);
+  return AddLink(name, default_model_instance(), M_LLo_L);
 }
 
 template 
@@ -150,7 +151,7 @@ void MultibodyTree::RemoveJoint(const Joint& joint) {
 
   // Update the ordinals for all joints with higher indices than the
   // one being removed.
-  for (JointIndex index : joints_.indices()) {
+  for (JointIndex index : joints_.valid_indices()) {
     if (index > joint_index) {
       Joint& mutable_joint = joints_.get_mutable_element(index);
       mutable_joint.set_ordinal(mutable_joint.ordinal() - 1);
@@ -194,8 +195,7 @@ bool MultibodyTree::HasUniqueFloatingBaseBodyImpl(
   std::optional base_body_index =
       MaybeGetUniqueBaseBodyIndex(model_instance);
   return base_body_index.has_value() &&
-         rigid_bodies_.get_element(base_body_index.value())
-             .is_floating_base_body();
+         links_.get_element(base_body_index.value()).is_floating_base_body();
 }
 
 template 
@@ -208,8 +208,7 @@ const RigidBody& MultibodyTree::GetUniqueFloatingBaseBodyOrThrowImpl(
         fmt::format("Model {} does not have a unique base body.",
                     model_instances_.get_element(model_instance).name()));
   }
-  const RigidBody& result =
-      rigid_bodies_.get_element(base_body_index.value());
+  const RigidBody& result = links_.get_element(base_body_index.value());
   if (!result.is_floating_base_body()) {
     throw std::logic_error(fmt::format(
         "Model {} has a unique base body, but it is not a floating base body.",
@@ -244,7 +243,7 @@ template 
 const auto& GetElementByIndex(const MultibodyTree& tree,
                               const ElementIndex index) {
   if constexpr (std::is_same_v) {
-    return tree.get_body(index);
+    return tree.get_link(index);
   }
   if constexpr (std::is_same_v) {
     return tree.get_frame(index);
@@ -421,20 +420,19 @@ const auto& GetElementByName(const MultibodyTree& tree,
 }  // namespace
 
 template 
-int MultibodyTree::NumBodiesWithName(std::string_view name) const {
-  return static_cast(rigid_bodies_.names_map().count(name));
+int MultibodyTree::NumLinksWithName(std::string_view name) const {
+  return static_cast(links_.names_map().count(name));
 }
 
 template 
-bool MultibodyTree::HasBodyNamed(std::string_view name) const {
-  return HasElementNamed(*this, name, std::nullopt, rigid_bodies_.names_map());
+bool MultibodyTree::HasLinkNamed(std::string_view name) const {
+  return HasElementNamed(*this, name, std::nullopt, links_.names_map());
 }
 
 template 
-bool MultibodyTree::HasBodyNamed(std::string_view name,
+bool MultibodyTree::HasLinkNamed(std::string_view name,
                                     ModelInstanceIndex model_instance) const {
-  return HasElementNamed(*this, name, model_instance,
-                         rigid_bodies_.names_map());
+  return HasElementNamed(*this, name, model_instance, links_.names_map());
 }
 
 template 
@@ -476,13 +474,13 @@ bool MultibodyTree::HasModelInstanceNamed(std::string_view name) const {
 }
 
 template 
-std::vector MultibodyTree::GetBodyIndices(
+std::vector MultibodyTree::GetLinkIndices(
     ModelInstanceIndex model_instance) const {
   DRAKE_THROW_UNLESS(model_instances_.has_element(model_instance));
-  std::vector indices;
-  for (const Body* body : rigid_bodies_.elements()) {
-    if (body->model_instance() == model_instance) {
-      indices.emplace_back(body->index());
+  std::vector indices;
+  for (const Link* link : links_.elements()) {
+    if (link->model_instance() == model_instance) {
+      indices.emplace_back(link->index());
     }
   }
   return indices;
@@ -540,59 +538,56 @@ const Frame& MultibodyTree::GetFrameByName(
 }
 
 template 
-const RigidBody& MultibodyTree::GetRigidBodyByName(
-    std::string_view name) const {
-  return GetElementByName(*this, name, std::nullopt, rigid_bodies_.names_map());
+const Link& MultibodyTree::GetLinkByName(std::string_view name) const {
+  return GetElementByName(*this, name, std::nullopt, links_.names_map());
 }
 
 template 
-const RigidBody& MultibodyTree::GetRigidBodyByName(
+const Link& MultibodyTree::GetLinkByName(
     std::string_view name, ModelInstanceIndex model_instance) const {
-  return GetElementByName(*this, name, model_instance,
-                          rigid_bodies_.names_map());
+  return GetElementByName(*this, name, model_instance, links_.names_map());
 }
 
 template 
-const RigidBody& MultibodyTree::AddRigidBodyImpl(
-    std::unique_ptr> body) {
+const Link& MultibodyTree::AddLinkImpl(std::unique_ptr> link) {
   if (is_finalized()) {
     throw std::logic_error(
         "This MultibodyTree is finalized already. "
         "Therefore adding more bodies is not allowed. "
         "See documentation for Finalize() for details.");
   }
-  if (body == nullptr) {
-    throw std::logic_error("Input body is a nullptr.");
+  if (link == nullptr) {
+    throw std::logic_error("Input link is a nullptr.");
   }
 
-  DRAKE_DEMAND(body->model_instance().is_valid());
+  DRAKE_DEMAND(link->model_instance().is_valid());
 
-  const BodyIndex body_index(num_bodies());
+  const LinkIndex link_index(num_links());
 
-  if (body_index == 0) {
+  if (link_index == 0) {
     // We're adding the first RigidBody -- must be World!
-    DRAKE_DEMAND(body->name() == "world");
-    DRAKE_DEMAND(body->model_instance() == world_model_instance());
+    DRAKE_DEMAND(link->name() == "world");
+    DRAKE_DEMAND(link->model_instance() == world_model_instance());
     // The LinkJointGraph should already contain only World.
     DRAKE_DEMAND(ssize(link_joint_graph_.links()) == 1);
-    DRAKE_DEMAND(link_joint_graph_.link_by_index(body_index).name() == "world");
+    DRAKE_DEMAND(link_joint_graph_.link_by_index(link_index).name() == "world");
   } else {
-    // Make note in the graph of the new rigid body.
-    link_joint_graph_.AddLink(body->name(), body->model_instance());
+    // Make note in the graph of the new rigid link.
+    link_joint_graph_.AddLink(link->name(), link->model_instance());
   }
 
-  body->set_parent_tree(this, body_index);
+  link->set_parent_tree(this, link_index);
   // MultibodyTree can access selected private methods in RigidBody through its
   // RigidBodyAttorney.
-  // - Register body frame.
-  Frame* body_frame =
-      &internal::RigidBodyAttorney::get_mutable_body_frame(body.get());
-  const FrameIndex body_frame_index(num_frames());
-  body_frame->set_parent_tree(this, body_frame_index);
-  DRAKE_DEMAND(body_frame->name() == body->name());
-  frames_.AddBorrowed(body_frame);
-  // - Register body.
-  return rigid_bodies_.Add(std::move(body));
+  // - Register link frame.
+  Frame* link_frame =
+      &internal::RigidBodyAttorney::get_mutable_link_frame(link.get());
+  const FrameIndex link_frame_index(num_frames());
+  link_frame->set_parent_tree(this, link_frame_index);
+  DRAKE_DEMAND(link_frame->name() == link->name());
+  frames_.AddBorrowed(link_frame);
+  // - Register link.
+  return links_.Add(std::move(link));
 }
 
 template 
@@ -645,18 +640,18 @@ ModelInstanceIndex MultibodyTree::GetModelInstanceByName(
 }
 
 template 
-std::set MultibodyTree::GetBodiesKinematicallyAffectedBy(
+std::set MultibodyTree::GetLinksKinematicallyAffectedBy(
     const std::vector& joint_indexes) const {
-  // For each Joint, find its implementing Mobod and collect the RigidBodies
+  // For each Joint, find its implementing Mobod and collect the links
   // in the subtree rooted by that Mobod. Duplicates are weeded out
-  // and the returned BodyIndexes are sorted.
-  std::set links;
+  // and the returned LinkIndexes are sorted.
+  std::set links;
   for (const JointIndex& joint_index : joint_indexes) {
     if (get_joint(joint_index).num_velocities() == 0) continue;  // Skip welds.
     const MobodIndex mobod_index =
         graph().joint_by_index(joint_index).mobod_index();
     DRAKE_DEMAND(mobod_index.is_valid());
-    const std::vector subtree_links =
+    const std::vector subtree_links =
         forest().FindSubtreeLinks(mobod_index);
     links.insert(subtree_links.cbegin(), subtree_links.cend());
   }
@@ -664,17 +659,17 @@ std::set MultibodyTree::GetBodiesKinematicallyAffectedBy(
 }
 
 template 
-std::set MultibodyTree::GetBodiesOutboardOfBodies(
-    const std::vector& body_indexes) const {
-  // For each given rigid body, find the Mobod it follows and collect the bodies
+std::set MultibodyTree::GetLinksOutboardOfLinks(
+    const std::vector& link_indexes) const {
+  // For each given link, find the Mobod it follows and collect the links
   // in the subtree rooted by that Mobod. Duplicates are weeded out and the
-  // returned BodyIndexes are sorted.
-  std::set bodies;
-  for (const BodyIndex& body_index : body_indexes) {
+  // returned LinkIndexes are sorted.
+  std::set bodies;
+  for (const LinkIndex& body_index : link_indexes) {
     const MobodIndex mobod_index =
         graph().link_by_index(body_index).mobod_index();
     DRAKE_DEMAND(mobod_index.is_valid());
-    const std::vector subtree_bodies =
+    const std::vector subtree_bodies =
         forest().FindSubtreeLinks(mobod_index);
     bodies.insert(subtree_bodies.cbegin(), subtree_bodies.cend());
   }
@@ -745,13 +740,12 @@ void MultibodyTree::SetVelocitiesInArray(
 
 /* Create Joint implementations from the already-built SpanningForest. Joints
 are implemented with a Mobilizer (either forward or reversed), unless they are
-welds between Links that were merged onto a single Mobod. We visit the forest's
-mobilized bodies (Mobods) in depth-first order and create Mobilizers in the same
-order as Mobods. We make a stub 0th Mobilizer for World so that Mobilizers and
-Mobods are numbered identically. (Think of it as a weld of World to the
-universe.) Joints that are interior to optimized WeldedLinksAssemblies
-(composite bodies) won't get modeled at all since they don't appear in the
-forest. */
+welds between Links that were merged onto a composite Mobod. We visit the
+forest's mobilized bodies (Mobods) in depth-first order and create Mobilizers in
+the same order as Mobods. We make a stub 0th Mobilizer for World so that
+Mobilizers and Mobods are numbered identically. (Think of it as a weld of World
+to the universe.) Joints in WeldedLinksAssemblies that are interior to composite
+Mobods won't get modeled at all since they don't appear in the forest. */
 template 
 void MultibodyTree::CreateJointImplementations() {
   DRAKE_DEMAND(!is_finalized());
@@ -775,8 +769,12 @@ void MultibodyTree::CreateJointImplementations() {
       continue;
     }
 
+    // N.B. If `mobod` is composite, this is the modeled ("active") joint that
+    // connects it to its inboard Mobod. The joint connects a frame on `mobod`s
+    // active link to a frame on _some_ link of the inboard mobod (not
+    // necessarily its active link).
     const JointIndex joint_index =
-        forest().joints(mobod.joint_ordinal()).index();
+        forest().joints(mobod.active_joint_ordinal()).index();
     Joint& joint = joints_.get_mutable_element(joint_index);
 
     // We allow reversed mobilizers only for a subset of joint types.
@@ -827,8 +825,7 @@ const Mobilizer& MultibodyTree::GetFreeBodyMobilizerOrThrow(
 
 template 
 const Frame& MultibodyTree::AddOrGetJointFrame(
-    const RigidBody& body,
-    const std::optional>& X_BF,
+    const RigidBody& body, const std::optional>& X_BF,
     ModelInstanceIndex joint_instance, std::string_view joint_name,
     std::string_view frame_suffix) {
   if (X_BF.has_value()) {
@@ -846,11 +843,11 @@ void MultibodyTree::FinalizeInternals() {
 
   // Give different multibody elements the chance to perform any finalize-time
   // setup.
-  for (const auto& body_index : rigid_bodies_.indices()) {
+  for (const auto& body_index : links_.valid_indices()) {
     // This sets the body's is_floating_base_body() flag appropriately.
-    rigid_bodies_.get_mutable_element(body_index).SetTopology();
+    links_.get_mutable_element(body_index).SetTopology();
   }
-  for (const auto& frame_index : frames_.indices()) {
+  for (const auto& frame_index : frames_.valid_indices()) {
     // We (re)set the topology on all frames. The rigid body frames' topologies
     // will already have been set in the body loop immediately above, but it
     // doesn't hurt to set them again. The important thing is that the non-body
@@ -863,7 +860,7 @@ void MultibodyTree::FinalizeInternals() {
   for (const auto& force_element : force_elements_) {
     force_element->SetTopology();
   }
-  for (const auto& actuator_index : actuators_.indices()) {
+  for (const auto& actuator_index : actuators_.valid_indices()) {
     actuators_.get_mutable_element(actuator_index).SetTopology();
   }
 
@@ -904,6 +901,58 @@ void MultibodyTree::FinalizeInternals() {
   is_finalized_ = true;
 }
 
+template 
+void MultibodyTree::SetBaseBodyJointType(
+    BaseBodyJointType joint_type,
+    std::optional model_instance) {
+  DRAKE_THROW_UNLESS(!is_finalized());
+  LinkJointGraph& graph = mutable_graph();
+
+  // Obtain the current option flags to preserve the ones that don't have
+  // to do with base body joints.
+  ForestBuildingOptions options =
+      model_instance.has_value()
+          ? graph.get_forest_building_options_in_use(*model_instance)
+          : graph.get_global_forest_building_options();
+
+  // Clear the existing relevant settings. This leaves us with the default
+  // which is to use quaternion floating joints for base bodies.
+  options = options & ~ForestBuildingOptions::kUseRpyFloatingJoints;
+  options = options & ~ForestBuildingOptions::kUseFixedBase;
+
+  switch (joint_type) {
+    case BaseBodyJointType::kQuaternionFloatingJoint:
+      // Nothing to do here; this is the default.
+      break;
+    case BaseBodyJointType::kRpyFloatingJoint:
+      options = options | ForestBuildingOptions::kUseRpyFloatingJoints;
+      break;
+    case BaseBodyJointType::kWeldJoint:
+      options = options | ForestBuildingOptions::kUseFixedBase;
+      break;
+  }
+
+  if (model_instance.has_value()) {
+    graph.SetForestBuildingOptions(*model_instance, options);
+  } else {
+    graph.SetGlobalForestBuildingOptions(options);
+  }
+}
+
+template 
+BaseBodyJointType MultibodyTree::GetBaseBodyJointType(
+    std::optional model_instance) const {
+  const ForestBuildingOptions options =
+      model_instance.has_value()
+          ? graph().get_forest_building_options_in_use(*model_instance)
+          : graph().get_global_forest_building_options();
+  if (static_cast(options & ForestBuildingOptions::kUseRpyFloatingJoints))
+    return BaseBodyJointType::kRpyFloatingJoint;
+  if (static_cast(options & ForestBuildingOptions::kUseFixedBase))
+    return BaseBodyJointType::kWeldJoint;
+  return BaseBodyJointType::kQuaternionFloatingJoint;
+}
+
 template 
 void MultibodyTree::Finalize() {
   DRAKE_MBT_THROW_IF_FINALIZED();
@@ -962,21 +1011,20 @@ void MultibodyTree::Finalize() {
       if (added_joint.traits_index() ==
           LinkJointGraph::quaternion_floating_joint_traits_index()) {
         return AddEphemeralJoint(
-            added_joint.name(), world_body(),
-            get_body(added_joint.child_link_index()));
+            added_joint.name(), world_link(),
+            get_link(added_joint.child_link_index()));
       }
       if (added_joint.traits_index() ==
           LinkJointGraph::rpy_floating_joint_traits_index()) {
         return AddEphemeralJoint(
-            added_joint.name(), world_body(),
-            get_body(added_joint.child_link_index()));
+            added_joint.name(), world_link(),
+            get_link(added_joint.child_link_index()));
       }
       if (added_joint.traits_index() ==
           LinkJointGraph::weld_joint_traits_index()) {
         return AddEphemeralJoint(
-            added_joint.name(), world_body(),
-            get_body(added_joint.child_link_index()),
-            math::RigidTransform());
+            added_joint.name(), world_link(),
+            get_link(added_joint.child_link_index()), RigidTransform());
       }
       DRAKE_UNREACHABLE();
     }();
@@ -1001,17 +1049,17 @@ template 
 void MultibodyTree::CreateBodyNode(MobodIndex mobod_index) {
   const SpanningForest::Mobod& mobod = forest().mobods(mobod_index);
   const LinkJointGraph::Link& active_link =
-      forest().links(mobod.link_ordinal());
+      forest().links(mobod.active_link_ordinal());
   const BodyIndex body_index = active_link.index();
 
-  const RigidBody& body = rigid_bodies_.get_element(body_index);
+  const RigidBody& body = links_.get_element(body_index);
 
   std::unique_ptr> body_node;
   const Mobilizer* const mobilizer = mobilizers_[mobod_index].get();
   if (body_index == world_index()) {
-    body_node = std::make_unique>(&world_body(), mobilizer);
+    body_node = std::make_unique>(&world_link(), mobilizer);
   } else {
-    BodyNode* parent_node = body_nodes_[mobod.inboard()].get();
+    BodyNode* parent_node = body_nodes_[mobod.inboard_mobod()].get();
 
     // Only the mobilizer knows how to create a BodyNode with compile-time
     // fixed sizes.
@@ -1277,35 +1325,38 @@ void MultibodyTree::SetFreeBodyRandomAnglesDistributionOrThrow(
   maybe_rpy_mobilizer->set_random_angles_distribution(angles.vector());
 }
 
-// Note that the result is indexed by BodyIndex, not MobodIndex.
+// Note that the result is indexed by LinkIndex (BodyIndex), not MobodIndex.
+// That is, we're returning X_WL for each Link L.
 template 
-void MultibodyTree::CalcAllBodyPosesInWorld(
+void MultibodyTree::CalcAllLinkPosesInWorld(
     const systems::Context& context,
-    std::vector>* X_WB) const {
-  DRAKE_THROW_UNLESS(X_WB != nullptr);
-  if (ssize(*X_WB) != num_bodies()) {
-    X_WB->resize(num_bodies(), RigidTransform::Identity());
-  }
+    std::vector>* X_WL) const {
+  DRAKE_THROW_UNLESS(X_WL != nullptr);
+  const int num_link_indices = links_.num_indices();
+  X_WL->resize(num_link_indices);
+
   const PositionKinematicsCache& pc = EvalPositionKinematics(context);
-  for (BodyIndex body_index(0); body_index < num_bodies(); ++body_index) {
-    const MobodIndex mobod_index = get_body(body_index).mobod_index();
-    X_WB->at(body_index) = pc.get_X_WB(mobod_index);
+  for (LinkIndex link_index(0); link_index < num_link_indices; ++link_index) {
+    (*X_WL)[link_index] = has_link(link_index)
+                              ? pc.get_X_WL(get_link(link_index).ordinal())
+                              : RigidTransform::Identity();  // invalid
   }
 }
 
-// Note that the result is indexed by BodyIndex, not MobodIndex.
+// Note that the result is indexed by LinkIndex (BodyIndex), not MobodIndex.
 template 
-void MultibodyTree::CalcAllBodySpatialVelocitiesInWorld(
+void MultibodyTree::CalcAllLinkSpatialVelocitiesInWorld(
     const systems::Context& context,
-    std::vector>* V_WB) const {
-  DRAKE_THROW_UNLESS(V_WB != nullptr);
-  if (ssize(*V_WB) != num_bodies()) {
-    V_WB->resize(num_bodies(), SpatialVelocity::Zero());
-  }
+    std::vector>* V_WL) const {
+  DRAKE_THROW_UNLESS(V_WL != nullptr);
+  const int num_link_indices = links_.num_indices();
+  V_WL->resize(num_link_indices);
+
   const VelocityKinematicsCache& vc = EvalVelocityKinematics(context);
-  for (BodyIndex body_index(0); body_index < num_bodies(); ++body_index) {
-    const MobodIndex mobod_index = get_body(body_index).mobod_index();
-    V_WB->at(body_index) = vc.get_V_WB(mobod_index);
+  for (LinkIndex link_index(0); link_index < num_link_indices; ++link_index) {
+    (*V_WL)[link_index] = has_link(link_index)
+                              ? vc.get_V_WL(get_link(link_index).ordinal())
+                              : SpatialVelocity::NaN();  // invalid
   }
 }
 
@@ -1370,7 +1421,7 @@ void MultibodyTree::CalcBlockSystemJacobianCache(
     // Note: Tree mobods never include World.
     for (const SpanningForest::Mobod& mobod : tree) {
       const MobodIndex index_B = mobod.index();
-      const MobodIndex index_P = mobod.inboard();
+      const MobodIndex index_P = mobod.inboard_mobod();
       const SpanningForest::Mobod& parent = forest().mobods(index_P);
       const Vector3& p_PoBo_W = pc.get_p_PoBo_W(index_B);
       const int row_B = 6 * (index_B - base_index);
@@ -1392,7 +1443,7 @@ void MultibodyTree::CalcBlockSystemJacobianCache(
           Jvi_V_WB.template head<3>() = w_WP;
           Jvi_V_WB.template tail<3>() = v_WP + w_WP.cross(p_PoBo_W);
         }
-        ancestor = &forest().mobods(ancestor->inboard());
+        ancestor = &forest().mobods(ancestor->inboard_mobod());
       }
       // Parent contributions are done, just need to fill in the local
       // contribution from H_PB_W.
@@ -1438,7 +1489,7 @@ void MultibodyTree::CalcVelocityKinematicsCache(
   }
 }
 
-// Result is indexed by MobodIndex, not BodyIndex.
+// Result is indexed by MobodIndex, not LinkIndex (or BodyIndex).
 template 
 void MultibodyTree::CalcSpatialInertiasInWorld(
     const systems::Context& context,
@@ -1453,18 +1504,18 @@ void MultibodyTree::CalcSpatialInertiasInWorld(
   // Skip the world.
   // TODO(joemasterjohn): Consider an optimization to avoid calculating spatial
   //  inertias for locked floating bodies.
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
-    const RigidTransform& X_WB = pc.get_X_WB(body.mobod_index());
+  for (LinkIndex link_index(1); link_index < num_links(); ++link_index) {
+    const Link& link = get_link(link_index);
+    const RigidTransform& X_WB = pc.get_X_WB(link.mobod_index());
 
-    // Orientation of B in W.
+    // Orientation of Mobod B in W.
     const RotationMatrix& R_WB = X_WB.rotation();
 
-    // Spatial inertia of body B about Bo and expressed in the body frame B.
+    // Spatial inertia of Mobod B about Bo and expressed in the body frame B.
     const SpatialInertia& M_BBo_B =
-        frame_body_pose_cache.get_M_BBo_B(body.mobod_index());
+        frame_body_pose_cache.get_M_BBo_B(link.mobod_index());
     // Re-express body B's spatial inertia in the world frame W.
-    SpatialInertia& M_BBo_W = (*M_B_W_all)[body.mobod_index()];
+    SpatialInertia& M_BBo_W = (*M_B_W_all)[link.mobod_index()];
     M_BBo_W = M_BBo_B;               // Wrong frame.
     M_BBo_W.ReExpressInPlace(R_WB);  // Fixed.
   }
@@ -1505,25 +1556,22 @@ void MultibodyTree::CalcFrameBodyPoses(
     FrameBodyPoseCache* frame_body_poses) const {
   DRAKE_DEMAND(frame_body_poses != nullptr);
 
-  // All RigidBodyFrames share this entry which is set once and forever.
-  DRAKE_ASSERT(frame_body_poses->get_X_BF(0).IsExactlyIdentity());
-  DRAKE_ASSERT(frame_body_poses->get_X_FB(0).IsExactlyIdentity());
+  DRAKE_ASSERT(frame_body_poses->get_X_BF(FrameIndex(0)).IsExactlyIdentity());
+  DRAKE_ASSERT(frame_body_poses->get_X_FB(FrameIndex(0)).IsExactlyIdentity());
 
   // The first pass locates each frame with respect to the body frame B
   // of the body to which it is fixed.
   for (const Frame* frame : frames_.elements()) {
-    const int body_pose_index_in_cache = frame->get_body_pose_index_in_cache();
-    if (frame->is_body_frame()) {
-      DRAKE_DEMAND(body_pose_index_in_cache == 0);
-      continue;
-    }
     // TODO(sherm1) Note that we're unnecessarily recalculating the parent
     //  and ancestor poses. Likely OK since we expect short sequences and
     //  the whole computation is done only when parameters change. But if
     //  there is a performance issue, consider doing this in topological
     //  order (or memoizing) so we don't have to recalculate.
-    frame_body_poses->SetX_BF(body_pose_index_in_cache,
-                              frame->CalcPoseInBodyFrame(context));
+    const RigidTransform X_LF = frame->CalcPoseInBodyFrame(context);
+    frame_body_poses->SetX_LF(frame->index(), X_LF);
+
+    // TODO(sherm1) When we support composites, X_BF ≠ X_LF.
+    frame_body_poses->SetX_BF(frame->index(), X_LF);
   }
 
   // For every mobilized body, precalculate its body-frame spatial inertia
@@ -1535,10 +1583,13 @@ void MultibodyTree::CalcFrameBodyPoses(
     const Mobilizer& mobilizer = get_mobilizer(mobod.index());
 
     // Get the parameterized spatial inertia.
-    const RigidBody& body = mobilizer.outboard_body();
-    const SpatialInertia M_BBo_B =
-        body.CalcSpatialInertiaInBodyFrame(context);
-    frame_body_poses->SetM_BBo_B(mobod.index(), M_BBo_B);
+    const Link& link = mobilizer.outboard_body();
+    const SpatialInertia M_LLo_L =
+        link.CalcSpatialInertiaInBodyFrame(context);
+    frame_body_poses->SetM_LLo_L(link.ordinal(), M_LLo_L);
+
+    // TODO(sherm1) When we support composites, M_BBo_B ≠ M_LLo_L.
+    frame_body_poses->SetM_BBo_B(mobod.index(), M_LLo_L);
   }
 }
 
@@ -1640,8 +1691,8 @@ void MultibodyTree::CalcDynamicBiasForces(
   const VelocityKinematicsCache& vc = this->EvalVelocityKinematics(context);
 
   // Skip the world.
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
 
     const SpatialInertia& M_B_W =
         spatial_inertia_in_world_cache[body.mobod_index()];
@@ -1727,8 +1778,8 @@ VectorX MultibodyTree::CalcInverseDynamics(
     const systems::Context& context, const VectorX& known_vdot,
     const MultibodyForces& external_forces) const {
   // Temporary storage used in the computation of inverse dynamics.
-  std::vector> A_WB(num_bodies());
-  std::vector> F_BMo_W(num_bodies());
+  std::vector> A_WB(num_links());
+  std::vector> F_BMo_W(num_links());
   VectorX tau(num_velocities());
   CalcInverseDynamics(context, known_vdot, external_forces.body_forces(),
                       external_forces.generalized_forces(), &A_WB, &F_BMo_W,
@@ -2085,9 +2136,9 @@ RigidTransform MultibodyTree::CalcRelativeTransform(
   // Find each Frame's pose on its own body (F on A, G on B).
   const FrameBodyPoseCache& frame_body_pose_cache =
       EvalFrameBodyPoses(context);
-  const math::RigidTransform& X_AF =
+  const RigidTransform& X_AF =
       frame_F.get_X_BF(frame_body_pose_cache);  // B==A
-  const math::RigidTransform& X_BG =
+  const RigidTransform& X_BG =
       frame_G.get_X_BF(frame_body_pose_cache);  // F==G
 
   // Find each body's pose in World.
@@ -2163,8 +2214,8 @@ void MultibodyTree::CalcPointsVelocities(
 template 
 T MultibodyTree::CalcTotalMass(const systems::Context& context) const {
   T total_mass = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     const T& body_mass = body.get_mass(context);
     total_mass += body_mass;
   }
@@ -2176,8 +2227,8 @@ T MultibodyTree::CalcTotalMass(
     const systems::Context& context,
     const std::vector& model_instances) const {
   T total_mass = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -2190,7 +2241,7 @@ T MultibodyTree::CalcTotalMass(
 template 
 Vector3 MultibodyTree::CalcCenterOfMassPositionInWorld(
     const systems::Context& context) const {
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -2202,8 +2253,8 @@ Vector3 MultibodyTree::CalcCenterOfMassPositionInWorld(
   Vector3 sum_mi_pi = Vector3::Zero();
 
   // Sum over all the bodies except the 0th body (which is the world body).
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
 
     // total_mass = ∑ mᵢ.
     const T& body_mass = body.get_mass(context);
@@ -2230,7 +2281,7 @@ Vector3 MultibodyTree::CalcCenterOfMassPositionInWorld(
     const std::vector& model_instances) const {
   // Reminder: MultibodyTree always declares a world body and 2 model instances
   // "world" and "default" so num_model_instances() should always be >= 2.
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -2250,8 +2301,8 @@ Vector3 MultibodyTree::CalcCenterOfMassPositionInWorld(
   // code below ensures a body's contribution to the sum occurs only once.
   // Duplicate model_instances in std::vector are ignored.
   int number_of_non_world_bodies_processed = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -2313,14 +2364,14 @@ SpatialInertia MultibodyTree::CalcSpatialInertia(
     if (body_index == 0) continue;  // No contribution from the world body.
 
     // Ensure MultibodyPlant method contains a valid body_index.
-    if (body_index >= num_bodies()) {
+    if (body_index >= num_links()) {
       throw std::logic_error(
           "CalcSpatialInertia(): contains an invalid BodyIndex.");
     }
 
     // Get the current body B's spatial inertia about Bo (body B's origin),
     // expressed in the world frame W.
-    const MobodIndex mobod_index = get_body(body_index).mobod_index();
+    const MobodIndex mobod_index = get_link(body_index).mobod_index();
     const SpatialInertia& M_BBo_W = M_Bi_W[mobod_index];
 
     // Shift M_BBo_W from about-point Bo to about-point Wo and add to the sum.
@@ -2345,7 +2396,7 @@ SpatialInertia MultibodyTree::CalcSpatialInertia(
 template 
 Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld(
     const systems::Context& context) const {
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -2357,8 +2408,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld(
   Vector3 sum_mi_vi = Vector3::Zero();
 
   // Sum over all the bodies except the 0th body (which is the world body).
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
 
     // total_mass = ∑ mᵢ.
     const T& body_mass = body.get_mass(context);
@@ -2389,7 +2440,7 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld(
     const std::vector& model_instances) const {
   // Reminder: MultibodyTree always declares a world body and 2 model instances
   // "world" and "default" so num_model_instances() should always be >= 2.
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -2409,8 +2460,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld(
   // code below ensures a body's contribution to the sum occurs only once.
   // Duplicate model_instances in std::vector are ignored.
   int number_of_non_world_bodies_processed = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -2445,7 +2496,7 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld(
 template 
 Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld(
     const systems::Context& context) const {
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -2459,8 +2510,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld(
   // issue a significantly less helpful exception message.
   // Sum over all the bodies except the 0th body (which is the world body).
   T total_mass = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     const T& body_mass = body.get_mass(context);
     total_mass += body_mass;  // total_mass = ∑ mᵢ.
   }
@@ -2473,8 +2524,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld(
 
   // Sum over all the bodies except the 0th body (which is the world body).
   Vector3 sum_mi_ai = Vector3::Zero();
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     const T& body_mass = body.get_mass(context);
     const Vector3 ai_WBcm_W =
         body.CalcCenterOfMassTranslationalAccelerationInWorld(context);
@@ -2492,7 +2543,7 @@ template 
 Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld(
     const systems::Context& context,
     const std::vector& model_instances) const {
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -2509,8 +2560,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld(
   // is listed multiple times.
   T total_mass = 0;
   int number_of_non_world_bodies_processed = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -2543,8 +2594,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld(
   // The code below ensures a body's contribution to the sum occurs only once.
   // Duplicate model_instances in std::vector are ignored.
   Vector3 sum_mi_ai = Vector3::Zero();
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -2577,8 +2628,8 @@ SpatialMomentum MultibodyTree::CalcSpatialMomentumInWorldAboutPoint(
     const systems::Context& context,
     const std::vector& model_instances,
     const Vector3& p_WoP_W) const {
-  // Assemble a list of BodyIndex.
-  std::vector body_indexes;
+  // Assemble a list of LinkIndexes.
+  std::vector link_indexes;
   for (auto model_instance : model_instances) {
     // If invalid model_instance, throw an exception with a helpful message.
     if (!model_instances_.has_element(model_instance)) {
@@ -2587,15 +2638,15 @@ SpatialMomentum MultibodyTree::CalcSpatialMomentumInWorldAboutPoint(
           " contains an invalid model_instance.");
     }
 
-    const std::vector body_index_in_instance =
-        GetBodyIndices(model_instance);
-    for (BodyIndex body_index : body_index_in_instance)
-      body_indexes.push_back(body_index);
+    const std::vector link_index_in_instance =
+        GetLinkIndices(model_instance);
+    for (LinkIndex link_index : link_index_in_instance)
+      link_indexes.push_back(link_index);
   }
 
   // Form spatial momentum about Wo (origin of world frame W), expressed in W.
   const SpatialMomentum L_WS_W =
-      CalcBodiesSpatialMomentumInWorldAboutWo(context, body_indexes);
+      CalcBodiesSpatialMomentumInWorldAboutWo(context, link_indexes);
 
   // Shift the spatial momentum from Wo to point P.
   return L_WS_W.Shift(p_WoP_W);
@@ -2604,7 +2655,7 @@ SpatialMomentum MultibodyTree::CalcSpatialMomentumInWorldAboutPoint(
 template 
 SpatialMomentum MultibodyTree::CalcBodiesSpatialMomentumInWorldAboutWo(
     const systems::Context& context,
-    const std::vector& body_indexes) const {
+    const std::vector& link_indexes) const {
   // For efficiency, evaluate all bodies' spatial inertia, velocities, and pose.
   const std::vector>& M_Bi_W =
       EvalSpatialInertiaInWorldCache(context);
@@ -2615,15 +2666,15 @@ SpatialMomentum MultibodyTree::CalcBodiesSpatialMomentumInWorldAboutWo(
   // S's spatial momentum in W about Wo (the origin of W), expressed in W.
   SpatialMomentum L_WS_W = SpatialMomentum::Zero();
 
-  // Add contributions from each body Bi.
-  for (BodyIndex body_index : body_indexes) {
-    if (body_index == 0) continue;  // No contribution from the world body.
+  // Add contributions from each link Bi.
+  for (LinkIndex link_index : link_indexes) {
+    if (link_index == 0) continue;  // No contribution from the world link.
 
-    // Ensure MultibodyPlant method contains a valid body_index.
-    DRAKE_DEMAND(body_index < num_bodies());
+    // Ensure MultibodyPlant method contains a valid link_index.
+    DRAKE_DEMAND(link_index < num_links());
 
-    // Form the current body's spatial momentum in W about Bo, expressed in W.
-    const MobodIndex mobod_index = get_body(body_index).mobod_index();
+    // Form the current link's spatial momentum in W about Bo, expressed in W.
+    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;
@@ -2641,28 +2692,28 @@ SpatialMomentum MultibodyTree::CalcBodiesSpatialMomentumInWorldAboutWo(
 }
 
 template 
-const RigidTransform& MultibodyTree::EvalBodyPoseInWorld(
-    const systems::Context& context, const RigidBody& body_B) const {
+const RigidTransform& MultibodyTree::EvalLinkPoseInWorld(
+    const systems::Context& context, const Link& link_L) const {
   DRAKE_MBT_THROW_IF_NOT_FINALIZED();
-  body_B.HasThisParentTreeOrThrow(this);
-  return EvalPositionKinematics(context).get_X_WB(body_B.mobod_index());
+  link_L.HasThisParentTreeOrThrow(this);
+  return EvalPositionKinematics(context).get_X_WL(link_L.ordinal());
 }
 
 template 
-const SpatialVelocity& MultibodyTree::EvalBodySpatialVelocityInWorld(
-    const systems::Context& context, const RigidBody& body_B) const {
+const SpatialVelocity& MultibodyTree::EvalLinkSpatialVelocityInWorld(
+    const systems::Context& context, const Link& link_L) const {
   DRAKE_MBT_THROW_IF_NOT_FINALIZED();
-  body_B.HasThisParentTreeOrThrow(this);
-  return EvalVelocityKinematics(context).get_V_WB(body_B.mobod_index());
+  link_L.HasThisParentTreeOrThrow(this);
+  return EvalVelocityKinematics(context).get_V_WL(link_L.ordinal());
 }
 
 template 
 const SpatialAcceleration&
-MultibodyTree::EvalBodySpatialAccelerationInWorld(
-    const systems::Context& context, const RigidBody& body_B) const {
+MultibodyTree::EvalLinkSpatialAccelerationInWorld(
+    const systems::Context& context, const Link& link_L) const {
   DRAKE_MBT_THROW_IF_NOT_FINALIZED();
-  body_B.HasThisParentTreeOrThrow(this);
-  return EvalAccelerationKinematics(context).get_A_WB(body_B.mobod_index());
+  link_L.HasThisParentTreeOrThrow(this);
+  return EvalAccelerationKinematics(context).get_A_WB(link_L.mobod_index());
 }
 
 template 
@@ -2700,7 +2751,7 @@ void MultibodyTree::CalcAllBodyBiasSpatialAccelerationsInWorld(
 
   // Ensure AsBias_WB_all is a not nullptr and is properly sized.
   DRAKE_THROW_UNLESS(AsBias_WB_all != nullptr);
-  DRAKE_THROW_UNLESS(static_cast(AsBias_WB_all->size()) == num_bodies());
+  DRAKE_THROW_UNLESS(static_cast(AsBias_WB_all->size()) == num_links());
 
   // To calculate a generic body A's spatial acceleration bias in world W,
   // note that body A's spatial velocity in world W is
@@ -2727,7 +2778,7 @@ SpatialAcceleration MultibodyTree::CalcBiasSpatialAcceleration(
 
   // Reserve room to store all the bodies' spatial acceleration bias in world W.
   // TODO(Mitiguy) Inefficient use of heap. Per issue #13560, implement caching.
-  std::vector> AsBias_WB_all(num_bodies());
+  std::vector> AsBias_WB_all(num_links());
   CalcAllBodyBiasSpatialAccelerationsInWorld(context, with_respect_to,
                                              &AsBias_WB_all);
 
@@ -3304,7 +3355,7 @@ void MultibodyTree::CalcJacobianCenterOfMassTranslationalVelocity(
   DRAKE_THROW_UNLESS(Js_v_AScm_E->cols() == num_columns);
 
   // Reminder: MultibodyTree always declares a world body (0ᵗʰ body).
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -3314,8 +3365,8 @@ void MultibodyTree::CalcJacobianCenterOfMassTranslationalVelocity(
 
   Js_v_AScm_E->setZero();
   T composite_mass = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     const Vector3 pi_BoBcm = body.CalcCenterOfMassInBodyFrame(context);
     MatrixX Jsi_v_ABcm_E(3, num_columns);
     CalcJacobianTranslationalVelocity(
@@ -3348,7 +3399,7 @@ void MultibodyTree::CalcJacobianCenterOfMassTranslationalVelocity(
   DRAKE_THROW_UNLESS(Js_v_AScm_E->cols() == num_columns);
 
   // Reminder: MultibodyTree always declares a world body.
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -3368,8 +3419,8 @@ void MultibodyTree::CalcJacobianCenterOfMassTranslationalVelocity(
   // code below ensures a body's contribution to the sum occurs only once.
   // Duplicate model_instances in std::vector are ignored.
   int number_of_non_world_bodies_processed = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -3412,7 +3463,7 @@ Vector3 MultibodyTree::CalcBiasCenterOfMassTranslationalAcceleration(
     const systems::Context& context, JacobianWrtVariable with_respect_to,
     const Frame& frame_A, const Frame& frame_E) const {
   // Reminder: MultibodyTree always declares a world body (0ᵗʰ body).
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -3422,8 +3473,8 @@ Vector3 MultibodyTree::CalcBiasCenterOfMassTranslationalAcceleration(
 
   T composite_mass = 0;
   Vector3 asBias_AScm_E = Vector3::Zero();
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     const Vector3 pi_BoBcm_B = body.CalcCenterOfMassInBodyFrame(context);
     const Frame& frame_B = body.body_frame();
     const SpatialAcceleration AsBiasi_AScm_E = CalcBiasSpatialAcceleration(
@@ -3449,7 +3500,7 @@ Vector3 MultibodyTree::CalcBiasCenterOfMassTranslationalAcceleration(
     JacobianWrtVariable with_respect_to, const Frame& frame_A,
     const Frame& frame_E) const {
   // Reminder: MultibodyTree always declares a world body (0ᵗʰ body).
-  if (num_bodies() <= 1) {
+  if (num_links() <= 1) {
     std::string message = fmt::format(
         "{}(): This MultibodyPlant only contains "
         "the world_body() so its center of mass is undefined.",
@@ -3468,8 +3519,8 @@ Vector3 MultibodyTree::CalcBiasCenterOfMassTranslationalAcceleration(
   // code below ensures a body's contribution to the sum occurs only once.
   // Duplicate model_instances in std::vector are ignored.
   int number_of_non_world_bodies_processed = 0;
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     if (std::find(model_instances.begin(), model_instances.end(),
                   body.model_instance()) != model_instances.end()) {
       const T& body_mass = body.get_mass(context);
@@ -3528,8 +3579,8 @@ T MultibodyTree::CalcKineticEnergy(
   const VectorX& reflected_inertia = EvalReflectedInertiaCache(context);
   T twice_kinetic_energy_W = 0.0;
   // Add contributions from each body (except World).
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const MobodIndex mobod_index = get_body(body_index).mobod_index();
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const MobodIndex mobod_index = get_link(body_index).mobod_index();
     const SpatialInertia& M_B_W = M_Bi_W[mobod_index];
     const SpatialVelocity& V_WB = vc.get_V_WB(mobod_index);
     const SpatialMomentum L_WB = M_B_W * V_WB;
@@ -3615,12 +3666,14 @@ void MultibodyTree::ThrowDefaultMassInertiaError() const {
     // At this point we're looking at a non-World, terminal WeldedMobods group.
     // Find the matching WeldedLinksAssembly that carries the mass properties.
     const std::optional link_assembly_index =
-        graph().links(active_mobod.link_ordinal()).welded_links_assembly();
+        graph()
+            .links(active_mobod.active_link_ordinal())
+            .welded_links_assembly();
     DRAKE_DEMAND(link_assembly_index.has_value());  // Should be an assembly!
     const auto& link_assembly =
         graph().welded_links_assemblies(*link_assembly_index);
     DRAKE_DEMAND(link_assembly.links()[0] ==
-                 graph().links(active_mobod.link_ordinal()).index());
+                 graph().links(active_mobod.active_link_ordinal()).index());
 
     ThrowIfTerminalBodyHasBadDefaultMassProperties(link_assembly.links(),
                                                    active_mobod.index());
@@ -3632,7 +3685,7 @@ void MultibodyTree::ThrowDefaultMassInertiaError() const {
     if (!mobod.is_leaf_mobod()) continue;  // An interior Mobod.
 
     ThrowIfTerminalBodyHasBadDefaultMassProperties(
-        {graph().links(mobod.link_ordinal()).index()}, mobod.index());
+        {graph().links(mobod.active_link_ordinal()).index()}, mobod.index());
   }
 }
 
@@ -3648,7 +3701,7 @@ void MultibodyTree::ThrowIfTerminalBodyHasBadDefaultMassProperties(
   const bool can_rotate = active_mobilizer.can_rotate();
   const bool can_translate = active_mobilizer.can_translate();
 
-  const std::string& active_link_name = get_body(link_assembly[0]).name();
+  const std::string& active_link_name = get_link(link_assembly[0]).name();
   const char* description = is_assembly
                                 ? "the active body for a terminal assembly"
                                 : "a terminal body";
@@ -3681,7 +3734,7 @@ double MultibodyTree::CalcTotalDefaultMass(
     const std::vector& body_indexes) const {
   double total_mass = 0;
   for (BodyIndex body_index : body_indexes) {
-    const RigidBody& body_B = get_body(body_index);
+    const RigidBody& body_B = get_link(body_index);
     const double mass_B = body_B.default_mass();
     if (!std::isnan(mass_B)) total_mass += mass_B;
   }
@@ -3692,7 +3745,7 @@ template 
 bool MultibodyTree::IsAnyDefaultRotationalInertiaNaN(
     const std::vector& body_indexes) const {
   for (BodyIndex body_index : body_indexes) {
-    const RigidBody& body_B = get_body(body_index);
+    const RigidBody& body_B = get_link(body_index);
     const RotationalInertia I_BBo_B =
         body_B.default_rotational_inertia();
     if (I_BBo_B.IsNaN()) return true;
@@ -3704,7 +3757,7 @@ template 
 bool MultibodyTree::AreAllDefaultRotationalInertiaZero(
     const std::vector& body_indexes) const {
   for (BodyIndex body_index : body_indexes) {
-    const RigidBody& body_B = get_body(body_index);
+    const RigidBody& body_B = get_link(body_index);
     const RotationalInertia I_BBo_B =
         body_B.default_rotational_inertia();
     if (!I_BBo_B.IsZero()) return false;
@@ -4106,8 +4159,8 @@ std::unique_ptr> MultibodyTree::CloneToScalar()
   tree_clone->frames_.ResizeToMatch(frames_);
 
   // Skipping the world body at body_index = 0.
-  for (BodyIndex body_index(1); body_index < num_bodies(); ++body_index) {
-    const RigidBody& body = get_body(body_index);
+  for (BodyIndex body_index(1); body_index < num_links(); ++body_index) {
+    const RigidBody& body = get_link(body_index);
     tree_clone->CloneBodyAndAdd(body);
   }
 
@@ -4208,7 +4261,7 @@ RigidBody* MultibodyTree::CloneBodyAndAdd(
   // MultibodyTree can access selected private methods in RigidBody through its
   // RigidBodyAttorney.
   Frame* body_frame_clone =
-      &internal::RigidBodyAttorney::get_mutable_body_frame(body_clone.get());
+      &internal::RigidBodyAttorney::get_mutable_link_frame(body_clone.get());
   body_frame_clone->set_parent_tree(this, body_frame_index);
   body_frame_clone->set_model_instance(body.model_instance());
 
@@ -4219,7 +4272,7 @@ RigidBody* MultibodyTree::CloneBodyAndAdd(
   // The order in which bodies are added into owned_bodies_ is important to keep
   // the topology invariant. Therefore this method is called from
   // MultibodyTree::CloneToScalar() within a loop by original body_index.
-  return &rigid_bodies_.Add(std::move(body_clone));
+  return &links_.Add(std::move(body_clone));
 }
 
 template 
@@ -4282,11 +4335,11 @@ std::optional MultibodyTree::MaybeGetUniqueBaseBodyIndex(
   // just one of those.
   const SpanningForest::Mobod& world = forest().world_mobod();
   std::optional base_body_index{};
-  for (const MobodIndex& base_mobod_index : world.outboards()) {
+  for (const MobodIndex& base_mobod_index : world.outboard_mobods()) {
     const SpanningForest::Mobod& base_mobod = forest().mobods(base_mobod_index);
     DRAKE_DEMAND(base_mobod.level() == 1);
     const LinkJointGraph::Link& active_link =
-        forest().links(base_mobod.link_ordinal());
+        forest().links(base_mobod.active_link_ordinal());
     if (active_link.model_instance() != model_instance) continue;
     if (base_body_index.has_value())  // Not unique if already set.
       return std::nullopt;
diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h
index 2eee2b0cb8b9..7f3c2a396a0d 100644
--- a/multibody/tree/multibody_tree.h
+++ b/multibody/tree/multibody_tree.h
@@ -36,10 +36,14 @@ namespace multibody {
 template 
 class RigidBodyFrame;
 template 
+using LinkFrame = RigidBodyFrame;
+template 
 class Frame;
 template 
 class RigidBody;
 template 
+using Link = RigidBody;
+template 
 class Joint;
 template 
 class JointActuator;
@@ -56,6 +60,16 @@ enum class JacobianWrtVariable {
   kV      ///< J = ∂V/∂v
 };
 
+/// The kind of joint to be used to connect base bodies to world at Finalize().
+/// See @ref mbp_working_with_free_bodies "Working with free bodies"
+/// for definitions and discussion.
+/// @see SetBaseBodyJointType() for details.
+enum class BaseBodyJointType {
+  kQuaternionFloatingJoint,  ///< 6 dofs, unrestricted orientation.
+  kRpyFloatingJoint,         ///< 6 dofs using 3 angles; has singularity.
+  kWeldJoint,                ///< 0 dofs, fixed to World.
+};
+
 /// @cond
 // Helper macro to throw an exception within methods that should not be called
 // post-finalize.
@@ -81,122 +95,135 @@ class Mobilizer;
 template 
 class QuaternionFloatingMobilizer;
 
-// %MultibodyTree provides a representation for a physical system consisting of
-// a collection of interconnected rigid and deformable bodies. As such, it owns
-// and manages each of the elements that belong to this physical system.
-// Multibody dynamics elements include bodies, joints, force elements and
-// constraints.
+// MultibodyTree provides a representation for a physical system consisting of a
+// collection of interconnected rigid bodies. As such, it owns and manages
+// each of the elements that belong to this physical system. Multibody
+// dynamics elements include links, joints, force elements and constraints.
 //
-// @tparam_default_scalar
+// @note for unfortunate historical reasons, the user-facing API provided by
+// MultibodyPlant uses RigidBody for the user-specified object that should
+// have been called Link, because multiple links can be welded together to
+// form a single rigid body. MultibodyPlant methods often have "Body" in their
+// name when they are really referring to links, and use BodyIndex to select
+// individual RigidBody objects. Luckily the link/rigid body distinction
+// doesn't matter much in the user-facing API, but it matters a lot internally
+// where the distinction between a Link and a (possibly multi-link) rigid body
+// is critical. We will be strict internally to use Link and LinkIndex for the
+// user-provided objects (those are aliases for RigidBody and BodyIndex,
+// resp.). We call the true rigid bodies "mobilized bodies" or Mobods (indexed
+// by MobodIndex), but sometimes just "bodies" where the context is clear.
+// The internal terminology is consistent with multibody/topology.
+//
+// Since MultibodyTree is the internal implementation class for MultibodyPlant,
+// error messages that are to be delivered to users must be aware of the
+// terminology differences and use terms the user will understand.
 template 
 class MultibodyTree {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MultibodyTree);
 
-  // Creates a MultibodyTree containing only a **world** body and a
+  // Creates a MultibodyTree containing only a World link and a
   // UniformGravityFieldElement.
   MultibodyTree();
 
   ~MultibodyTree();
 
-  // @name Methods to add new MultibodyTree elements.
-  //
-  // To create a %MultibodyTree users will add multibody elements like bodies,
+  // Methods to add new MultibodyTree elements
+
+  // To create a MultibodyTree users will add multibody elements like links,
   // joints, force elements, constraints, etc, using one of these methods.
   // Once a user is done adding multibody elements, the Finalize() method
-  // **must** be called before invoking any %MultibodyTree method.
+  // **must** be called before invoking any MultibodyTree method.
   // See Finalize() for details.
-  // @{
-  // See this Reviewable comment:
-  // https://reviewable.io/reviews/robotlocomotion/drake/5583#-KgGqGisnX9uMuYDkHpx
 
-  // Creates a rigid body with the provided name, model instance, and spatial
-  // inertia.  This method returns a constant reference to the body just added,
-  // which will remain valid for the lifetime of `this` %MultibodyTree.
+  // Creates a Link (a.k.a. RigidBody) with the provided name, model instance,
+  // and spatial inertia. This method returns a constant reference to the
+  // link just added, which will remain valid for the lifetime of `this`
+  // MultibodyTree.
   //
   // Example of usage:
   // @code
   //   MultibodyTree model;
   //   // ... Code to define spatial_inertia, a SpatialInertia object ...
   //   ModelInstanceIndex model_instance = model.AddModelInstance("instance");
-  //   const RigidBody& body =
-  //     model.AddRigidBody("BodyName", model_instance, spatial_inertia);
+  //   const Link& link =
+  //     model.AddLink("LinkName", model_instance, spatial_inertia);
   // @endcode
   //
   // @param[in] name
-  //   A string that identifies the new body to be added to `this` model. A
-  //   std::runtime_error is thrown if a body named `name` already is part of
-  //   @p model_instance. See HasBodyNamed(), RigidBody::name().
+  //   A string that identifies the new link to be added to `this` model. A
+  //   std::runtime_error is thrown if a link named `name` already is part of
+  //   @p model_instance. See HasLinkNamed(), RigidBody::name().
   // @param[in] model_instance
   //   A model instance index which this body is part of.
-  // @param[in] M_BBo_B
-  //   The SpatialInertia of the new rigid body to be added to `this` model,
-  //   computed about the body frame origin `Bo` and expressed in the body
-  //   frame B.
-  // @returns A constant reference to the new RigidBody just added, which will
-  //          remain valid for the lifetime of `this` %MultibodyTree.
-  // @throws std::exception if a body named `name` already exists in this
+  // @param[in] M_LLo_L
+  //   The SpatialInertia of the new link to be added to `this` model,
+  //   computed about the link frame origin Lo and expressed in the link
+  //   frame L.
+  // @returns A constant reference to the new Link just added, which will
+  //          remain valid for the lifetime of `this` MultibodyTree.
+  // @throws std::exception if a link named `name` already exists in this
   //         model instance.
   // @throws std::exception if the model instance does not exist.
-  const RigidBody& AddRigidBody(const std::string& name,
-                                   ModelInstanceIndex model_instance,
-                                   const SpatialInertia& M_BBo_B);
-
-  // Creates a rigid body with the provided name, model instance, and spatial
-  // inertia.  The newly created body will be placed in the default model
-  // instance.  This method returns a constant reference to the body just
-  // added, which will remain valid for the lifetime of `this` %MultibodyTree.
+  const Link& AddLink(const std::string& name,
+                         ModelInstanceIndex model_instance,
+                         const SpatialInertia& M_LLo_L);
+
+  // Creates a Link with the provided name, and spatial inertia. The newly
+  // created link will be placed in the default model instance. This method
+  // returns a constant reference to the link just added, which will remain
+  // valid for the lifetime of `this` MultibodyTree.
   //
   // Example of usage:
   // @code
   //   MultibodyTree model;
   //   // ... Code to define spatial_inertia, a SpatialInertia object ...
-  //   const RigidBody& body =
-  //     model.AddRigidBody("BodyName", spatial_inertia);
+  //   const Link& link =
+  //     model.AddLink("LinkName", spatial_inertia);
   // @endcode
   //
   // @param[in] name
-  //   A string that identifies the new body to be added to `this` model. A
-  //   std::runtime_error is thrown if a body named `name` already is part of
-  //   the model in the default model instance. See HasBodyNamed(),
+  //   A string that identifies the new link to be added to `this` model. A
+  //   std::runtime_error is thrown if a link named `name` already is part of
+  //   the model in the default model instance. See HasLinkNamed(),
   //   RigidBody::name().
-  // @param[in] M_BBo_B
-  //   The SpatialInertia of the new rigid body to be added to `this` model,
-  //   computed about the body frame origin `Bo` and expressed in the body
-  //   frame B.
-  // @returns A constant reference to the new RigidBody just added, which will
-  //          remain valid for the lifetime of `this` %MultibodyTree.
-  // @throws std::exception if a body named `name` already exists.
+  // @param[in] M_LLo_L
+  //   The SpatialInertia of the new link to be added to `this` model,
+  //   computed about the link frame origin Lo and expressed in the link
+  //   frame L.
+  // @returns A constant reference to the new Link just added, which will
+  //          remain valid for the lifetime of `this` MultibodyTree.
+  // @throws std::exception if a link named `name` already exists.
   // @throws std::exception if additional model instances have been created
   //                        beyond the world and default instances.
-  const RigidBody& AddRigidBody(const std::string& name,
-                                   const SpatialInertia& M_BBo_B);
+  const Link& AddLink(const std::string& name,
+                         const SpatialInertia& M_LLo_L);
 
-  // Takes ownership of `frame` and adds it to `this` %MultibodyTree. Returns
+  // Takes ownership of `frame` and adds it to `this` MultibodyTree. Returns
   // a constant reference to the frame just added, which will remain valid for
-  // the lifetime of `this` %MultibodyTree.
+  // the lifetime of `this` MultibodyTree.
   //
   // Example of usage:
   // @code
   //   MultibodyTree model;
-  //   // ... Define body and X_BF ...
+  //   // ... Define link and X_BF ...
   //   const FixedOffsetFrame& frame =
-  //       model.AddFrame(std::make_unique>(body, X_BF));
+  //       model.AddFrame(std::make_unique>(link, X_BF));
   // @endcode
   //
   // @throws std::exception if `frame` is a nullptr.
   // @throws std::exception if Finalize() was already called on `this` tree.
   //
   // @param[in] frame A unique pointer to a frame to be added to `this`
-  //                  %MultibodyTree. The frame class must be specialized on
-  //                  the same scalar type T as this %MultibodyTree.
+  //                  MultibodyTree. The frame class must be specialized on
+  //                  the same scalar type T as this MultibodyTree.
   // @returns A constant reference of type `FrameType` to the created frame.
   //          This reference which will remain valid for the lifetime of `this`
-  //          %MultibodyTree.
+  //          MultibodyTree.
   //
   // @tparam FrameType The type of the specific sub-class of Frame to add. The
   //                   template needs to be specialized on the same scalar type
-  //                   T of this %MultibodyTree.
+  //                   T of this MultibodyTree.
   template