diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index a25290b18dd8..d5a020abd2fd 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -7051,14 +7051,20 @@ its parent body in the multibody tree by its Mobilizer (also called a permissible motion can be added using Constraint objects to remove more degrees of freedom. +Note: + This object corresponds to a "link" in urdf/sdformat terminology. + We may combine welded-together links into a composite rigid body + internally. We provide aliases Link/LinkIndex for + RigidBody/BodyIndex to allow either terminology to be used. + - [Goldstein 2001] H Goldstein, CP Poole, JL Safko, Classical Mechanics (3rd Edition), Addison-Wesley, 2001.)"""; // Symbol: drake::multibody::RigidBody::AddInForce struct /* AddInForce */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Adds the SpatialForce on this RigidBody B, applied at point P and -expressed in a frame E into ``forces``. +R"""(Adds the SpatialForce on this RigidBody (%Link) B, applied at point P +and expressed in a frame E into ``forces``. Parameter ``context``: The context containing the current state of the model. @@ -7085,21 +7091,22 @@ Parameter ``forces``: struct /* AddInForceInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Adds the SpatialForce on this RigidBody B, applied at body B's origin -Bo and expressed in the world frame W into ``forces``.)"""; +R"""(Adds the SpatialForce on this RigidBody (%Link) B, applied at body B's +origin Bo and expressed in the world frame W into ``forces``.)"""; } AddInForceInWorld; // Symbol: drake::multibody::RigidBody::CalcCenterOfMassInBodyFrame struct /* CalcCenterOfMassInBodyFrame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets this body's center of mass position from the given context. +R"""(Gets this RigidBody's (%Link's) center of mass position from the given +context. Parameter ``context``: contains the state of the multibody system. Returns: - p_BoBcm_B position vector from Bo (this rigid body B's origin) to - Bcm (B's center of mass), expressed in B. + p_BoBcm_B position vector from Bo (this body B's origin) to Bcm + (B's center of mass), expressed in B. Precondition: the context makes sense for use by this RigidBody.)"""; @@ -7108,14 +7115,15 @@ Parameter ``context``: struct /* CalcCenterOfMassTranslationalAccelerationInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Calculates Bcm's translational acceleration in the world frame W. +R"""(Calculates RigidBody (%Link) B's center of mass Bcm's translational +acceleration in the world frame W. Parameter ``context``: The context contains the state of the model. Returns ``a_WBcm_W``: - The translational acceleration of Bcm (this rigid body's center of - mass) in the world frame W, expressed in W. + The translational acceleration of Bcm (this body's center of mass) + in the world frame W, expressed in W. Note: When cached values are out of sync with the state stored in @@ -7127,21 +7135,22 @@ Returns ``a_WBcm_W``: struct /* CalcCenterOfMassTranslationalVelocityInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Calculates Bcm's translational velocity in the world frame W. +R"""(Calculates RigidBody (%Link) B's center of mass Bcm's translational +velocity in the world frame W. Parameter ``context``: The context contains the state of the model. Returns ``v_WBcm_W``: - The translational velocity of Bcm (this rigid body's center of - mass) in the world frame W, expressed in W.)"""; + The translational velocity of Bcm (this body's center of mass) in + the world frame W, expressed in W.)"""; } CalcCenterOfMassTranslationalVelocityInWorld; // Symbol: drake::multibody::RigidBody::CalcSpatialInertiaInBodyFrame struct /* CalcSpatialInertiaInBodyFrame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets this body's spatial inertia about its origin from the given -context. +R"""(Gets this RigidBody's (%Link's) spatial inertia about its origin from +the given context. Parameter ``context``: contains the state of the multibody system. @@ -7159,10 +7168,9 @@ Parameter ``context``: struct /* CloneToScalar */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) This method is mostly intended to be called by -MultibodyTree∷CloneToScalar(). Most users should not call this clone -method directly but rather clone the entire parent MultibodyTree if -needed. +R"""((Internal use only) This method is intended to be called by +MultibodyTree∷CloneToScalar(). Users should not call this clone method +directly but rather clone the entire parent MultibodyTree if needed. See also: MultibodyTree∷CloneToScalar())"""; @@ -7171,15 +7179,15 @@ See also: struct /* EvalPoseInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Returns the pose ``X_WB`` of this RigidBody B in the world frame W as -a function of the state of the model stored in ``context``.)"""; +R"""(Returns the pose ``X_WB`` of this RigidBody (%Link) B in the world +frame W as a function of the state of the model stored in ``context``.)"""; } EvalPoseInWorld; // Symbol: drake::multibody::RigidBody::EvalSpatialAccelerationInWorld struct /* EvalSpatialAccelerationInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Evaluates A_WB, this body B's SpatialAcceleration in the world frame -W. +R"""(Evaluates A_WB, this RigidBody (%Link) B's SpatialAcceleration in the +world frame W. Parameter ``context``: Contains the state of the model. @@ -7198,7 +7206,8 @@ Returns ``A_WB_W``: struct /* EvalSpatialVelocityInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Evaluates V_WB, this body B's SpatialVelocity in the world frame W. +R"""(Evaluates V_WB, this RigidBody (%Link) B's SpatialVelocity in the +world frame W. Parameter ``context``: Contains the state of the model. @@ -7211,15 +7220,15 @@ Returns ``V_WB_W``: struct /* GetForceInWorld */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets the SpatialForce on this RigidBody B from ``forces`` as F_BBo_W: -applied at body B's origin Bo and expressed in world frame W.)"""; +R"""(Gets the SpatialForce on this RigidBody (%Link) B from ``forces`` as +F_BBo_W: applied at body B's origin Bo and expressed in world frame W.)"""; } GetForceInWorld; // Symbol: drake::multibody::RigidBody::Lock struct /* Lock */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(For a floating base RigidBody, lock its inboard joint. Its generalized -velocities will be 0 until it is unlocked. +R"""(For a floating base RigidBody (%Link), lock its inboard joint. Its +generalized velocities will be 0 until it is unlocked. Raises: RuntimeError if this body is not a floating base body.)"""; @@ -7228,8 +7237,8 @@ velocities will be 0 until it is unlocked. struct /* ctor */ { // Source: drake/multibody/tree/rigid_body.h const char* doc_2args = -R"""(Constructs a RigidBody named ``body_name`` with the given default -SpatialInertia. +R"""(Constructs a RigidBody (%Link) named ``body_name`` with the given +default SpatialInertia. Parameter ``body_name``: A name associated with this body. @@ -7244,8 +7253,8 @@ Parameter ``M_BBo_B``: used for spatial inertia quantities.)"""; // Source: drake/multibody/tree/rigid_body.h const char* doc_3args = -R"""(Constructs a RigidBody named ``body_name`` with the given default -SpatialInertia. +R"""(Constructs a RigidBody (%Link) named ``body_name`` with the given +default SpatialInertia. Parameter ``body_name``: A name associated with this body. @@ -7266,8 +7275,8 @@ Parameter ``M_BBo_B``: struct /* SetCenterOfMassInBodyFrame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Sets this body's center of mass position while preserving -its inertia about its body origin. +R"""((Advanced) Sets this RigidBody (%Link) B's center of mass position +while preserving its inertia about its *body origin*. Parameter ``out``: ] context contains the state of the multibody system. It is @@ -7297,8 +7306,8 @@ Parameter ``com``: struct /* SetCenterOfMassInBodyFrameAndPreserveCentralInertia */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Sets this body's center of mass position while preserving its inertia -about its center of mass. +R"""(Sets this RigidBody (%Link) B's center of mass position while +preserving its inertia about its *center of mass*. Parameter ``out``: ] context contains the state of the multibody system. It is @@ -7328,13 +7337,14 @@ Parameter ``center_of_mass_position``: struct /* SetMass */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(For this RigidBody B, sets its mass stored in ``context`` to ``mass``. +R"""(For this RigidBody (%Link) B, sets its mass stored in ``context`` to +``mass``. Parameter ``context``: contains the state of the multibody system. Parameter ``mass``: - mass of this rigid body B. + mass of this body B. Note: This function changes this body B's mass and appropriately scales @@ -7350,8 +7360,8 @@ Parameter ``mass``: struct /* SetSpatialInertiaInBodyFrame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(For this RigidBody B, sets its SpatialInertia that is stored in -``context`` to ``M_Bo_B``. +R"""(For this RigidBody (%Link) B, sets its SpatialInertia that is stored +in ``context`` to ``M_Bo_B``. Parameter ``context``: contains the state of the multibody system. @@ -7372,7 +7382,7 @@ Parameter ``M_Bo_B``: struct /* Unlock */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(For a floating base RigidBody, unlock its inboard joint. +R"""(For a floating base RigidBody (%Link), unlock its inboard joint. Raises: RuntimeError if this body is not a floating base body.)"""; @@ -7381,42 +7391,45 @@ R"""(For a floating base RigidBody, unlock its inboard joint. struct /* body_frame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Compatibility) A synonym for link_frame().)"""; +R"""(Returns a const reference to the associated RigidBodyFrame +(LinkFrame). body_frame() is synonymous with link_frame(). + +Note: + "link" is the terminology used in urdf/sdformat.)"""; } body_frame; // Symbol: drake::multibody::RigidBody::default_com struct /* default_com */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Returns the default value of this RigidBody's center of mass as -measured and expressed in its body frame. This value is initially +R"""(Returns the default value of this RigidBody's (%Link's) center of mass +as measured and expressed in its body frame. This value is initially supplied at construction when specifying this body's SpatialInertia. Returns ``p_BoBcm_B``: - The position of this rigid body B's center of mass ``Bcm`` - measured from Bo (B's frame origin) and expressed in B (body B's - frame).)"""; + The position of this body B's center of mass ``Bcm`` measured from + Bo (B's frame origin) and expressed in B (body B's frame).)"""; } default_com; // Symbol: drake::multibody::RigidBody::default_mass struct /* default_mass */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Returns this RigidBody's default mass, which is initially supplied at -construction when specifying this body's SpatialInertia. +R"""(Returns this RigidBody's (%Link's) default mass, which is initially +supplied at construction when specifying this body's SpatialInertia. Note: - In general, a rigid body's mass can be a constant property stored - in this rigid body's SpatialInertia or a parameter that is stored - in a Context. The default constant mass value is used to - initialize the mass parameter in the Context.)"""; + In general, a body's mass can be a constant property stored in + this body's SpatialInertia or a parameter that is stored in a + Context. The default constant mass value is used to initialize the + mass parameter in the Context.)"""; } default_mass; // Symbol: drake::multibody::RigidBody::default_rotational_inertia struct /* default_rotational_inertia */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets the default value of this body B's rotational inertia about Bo -(B's origin), expressed in B (this body's body frame). This value is -calculated from the SpatialInertia supplied at construction of this -body. +R"""(Gets the default value of this RigidBody (%Link) B's rotational +inertia about Bo (B's origin), expressed in B (this body's body +frame). This value is calculated from the SpatialInertia supplied at +construction of this body. Returns ``I_BBo_B``: body B's rotational inertia about Bo, expressed in B.)"""; @@ -7425,8 +7438,8 @@ Returns ``I_BBo_B``: struct /* default_spatial_inertia */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets the default value of this body B's SpatialInertia about Bo (B's -origin) and expressed in B (this body's frame). +R"""(Gets the default value of this RigidBody (%Link) B's SpatialInertia +about Bo (B's origin) and expressed in B (this body's frame). Returns ``M_BBo_B``: body B's spatial inertia about Bo, expressed in B.)"""; @@ -7435,10 +7448,10 @@ Returns ``M_BBo_B``: struct /* default_unit_inertia */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Returns the default value of this body B's unit inertia about Bo (body -B's origin), expressed in B (this body's body frame). This value is -initially supplied at construction when specifying this body's -SpatialInertia. +R"""(Returns the default value of this RigidBody (%Link) B's unit inertia +about Bo (body B's origin), expressed in B (this body's body frame). +This value is initially supplied at construction when specifying this +body's SpatialInertia. Returns ``G_BBo_B``: rigid body B's unit inertia about Bo, expressed in B.)"""; @@ -7469,13 +7482,14 @@ See also: // Source: drake/multibody/tree/rigid_body.h const char* doc = R"""((Advanced) For floating base bodies (see is_floating_base_body()), -returns the index of this RigidBody's first generalized position in -the vector q of generalized position coordinates for a MultibodyPlant -model. Positions q for this RigidBody are then contiguous starting at -this index. When a floating RigidBody is modeled with quaternion -coordinates (see has_quaternion_dofs()), the four consecutive entries -in the state starting at this index correspond to the quaternion that -parametrizes this RigidBody's orientation. +returns the index of this RigidBody's (%Link's) first generalized +position in the vector q of generalized position coordinates for a +MultibodyPlant model. Positions q for this RigidBody are then +contiguous starting at this index. When a floating RigidBody is +modeled with quaternion coordinates (see has_quaternion_dofs()), the +four consecutive entries in the state starting at this index +correspond to the quaternion that parametrizes this RigidBody's +orientation. Raises: RuntimeError if called pre-finalize @@ -7494,10 +7508,10 @@ See also: // Source: drake/multibody/tree/rigid_body.h const char* doc = R"""((Advanced) For floating base bodies (see is_floating_base_body()), -returns the index of this RigidBody's first generalized velocity in -the vector v of generalized velocities for a MultibodyPlant model. -Velocities v for this RigidBody are then contiguous starting at this -index. +returns the index of this RigidBody's (%Link's) first generalized +velocity in the vector v of generalized velocities for a +MultibodyPlant model. Velocities v for this RigidBody are then +contiguous starting at this index. Raises: RuntimeError if called pre-finalize @@ -7529,33 +7543,33 @@ See also: struct /* get_angular_acceleration_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract this body's angular acceleration in world, -expressed in world. +R"""((Internal use only) Extract this link L's angular acceleration in +world, expressed in world. Parameter ``ac``: velocity kinematics cache. -Returns ``alpha_WB_W``: - B's angular acceleration in world W, expressed in W.)"""; +Returns ``alpha_WL_W``: + L's angular acceleration in world W, expressed in W.)"""; } get_angular_acceleration_in_world; // Symbol: drake::multibody::RigidBody::get_angular_velocity_in_world struct /* get_angular_velocity_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract this body's angular velocity in world, expressed in -world. +R"""((Internal use only) Extract this link's angular velocity in world, +expressed in world. Parameter ``vc``: velocity kinematics cache. -Returns ``w_WB_W``: - rigid body B's angular velocity in world W, expressed in W.)"""; +Returns ``w_WL_W``: + link L's angular velocity in world W, expressed in W.)"""; } get_angular_velocity_in_world; // Symbol: drake::multibody::RigidBody::get_mass struct /* get_mass */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets this body's mass from the given context. +R"""(Gets this RigidBody's (%Link's) mass from the given context. Parameter ``context``: contains the state of the multibody system. @@ -7567,105 +7581,105 @@ Parameter ``context``: struct /* get_origin_acceleration_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract acceleration of this body's origin in world, -expressed in world. +R"""((Internal use only) Extract acceleration of this link L's origin in +world, expressed in world. Parameter ``ac``: acceleration kinematics cache. -Returns ``a_WBo_W``: - acceleration of body origin Bo in world W, expressed in W.)"""; +Returns ``a_WLo_W``: + acceleration of link origin Lo in world W, expressed in W.)"""; } get_origin_acceleration_in_world; // Symbol: drake::multibody::RigidBody::get_origin_position_in_world struct /* get_origin_position_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract the position vector from world origin to this -body's origin, expressed in world. +R"""((Internal use only) Extract the position vector from world origin to +this link's origin, expressed in world. Parameter ``pc``: position kinematics cache. -Returns ``p_WoBo_W``: - position vector from Wo (world origin) to Bo (this body's origin) +Returns ``p_WoLo_W``: + position vector from Wo (world origin) to Lo (this link's origin) expressed in W (world).)"""; } get_origin_position_in_world; // Symbol: drake::multibody::RigidBody::get_origin_velocity_in_world struct /* get_origin_velocity_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract the velocity of this body's origin in world, -expressed in world. +R"""((Internal use only) Extract the velocity of this link's origin in +world, expressed in world. Parameter ``vc``: velocity kinematics cache. -Returns ``v_WBo_W``: - velocity of Bo (body origin) in world W, expressed in W.)"""; +Returns ``v_WLo_W``: + velocity of Lo (link origin) in world W, expressed in W.)"""; } get_origin_velocity_in_world; // Symbol: drake::multibody::RigidBody::get_pose_in_world struct /* get_pose_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract this body's pose in world (from the position -kinematics). +R"""((Internal use only) Extract this link's pose in world (from the +position kinematics). Parameter ``pc``: position kinematics cache. -Returns ``X_WB``: - pose of rigid body B in world frame W.)"""; +Returns ``X_WL``: + pose of this Link L in world frame W.)"""; } get_pose_in_world; // Symbol: drake::multibody::RigidBody::get_rotation_matrix_in_world struct /* get_rotation_matrix_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Extract the RotationMatrix relating the world frame to this -body's frame. +R"""((Internal use only) Extract the RotationMatrix relating the world +frame to this link's frame. Parameter ``pc``: position kinematics cache. -Returns ``R_WB``: - rotation matrix relating rigid body B in world frame W.)"""; +Returns ``R_WL``: + rotation matrix relating world frame W and Link L.)"""; } get_rotation_matrix_in_world; // Symbol: drake::multibody::RigidBody::get_spatial_acceleration_in_world struct /* get_spatial_acceleration_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Returns A_WB, this RigidBody B's SpatialAcceleration in the -world frame W. +R"""((Internal use only) Returns A_WL, this link L's SpatialAcceleration in +the world frame W. Parameter ``ac``: acceleration kinematics cache. -Returns ``A_WB_W``: - this rigid body B's spatial acceleration in the world frame W, - expressed in W (for point Bo, the body frame's origin).)"""; +Returns ``A_WL_W``: + this link L's spatial acceleration in the world frame W, expressed + in W (for point Lo, the link frame's origin).)"""; } get_spatial_acceleration_in_world; // Symbol: drake::multibody::RigidBody::get_spatial_velocity_in_world struct /* get_spatial_velocity_in_world */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Returns V_WB, this RigidBody B's SpatialVelocity in the +R"""((Internal use only) Returns V_WL, this link L's SpatialVelocity in the world frame W. Parameter ``vc``: velocity kinematics cache. -Returns ``V_WB_W``: - this rigid body B's spatial velocity in the world frame W, - expressed in W (for point Bo, the body frame's origin).)"""; +Returns ``V_WL_W``: + this link L's spatial velocity in the world frame W, expressed in + W (for point Lo, the link frame's origin).)"""; } get_spatial_velocity_in_world; // Symbol: drake::multibody::RigidBody::has_quaternion_dofs struct /* has_quaternion_dofs */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) If ``True``, this body's generalized position coordinates q -include a quaternion, which occupies the first four elements of q. -Note that this does not imply that the body is floating base body -since it may have fewer than 6 dofs or its inboard body could be -something other than World. +R"""((Advanced) If ``True``, this RigidBody's (%Link's) generalized +position coordinates q include a quaternion, which occupies the first +four elements of q. Note that this does not imply that the body is a +floating base body since it may have fewer than 6 dofs or its inboard +body could be something other than World. Raises: RuntimeError if called pre-finalize @@ -7682,9 +7696,10 @@ See also: struct /* is_floating_base_body */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Returns ``True`` if this body is a *floating base body*, -meaning it had no explicit joint to a parent body so is mobilized by -an automatically-added (ephemeral) floating (6 dof) joint to World. +R"""((Advanced) Returns ``True`` if this RigidBody (%Link) is a *floating +base body*, meaning it had no explicit joint to a parent body and is +mobilized by an automatically-added (ephemeral) floating (6 dof) joint +to World. Note: A floating base body is not necessarily modeled with a quaternion @@ -7702,9 +7717,9 @@ See also: struct /* is_locked */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Determines whether this RigidBody is currently locked to its inboard -(parent) RigidBody. This is not limited to floating base bodies but -generally Joint∷is_locked() is preferable otherwise. +R"""(Determines whether this RigidBody (%Link) is currently locked to its +inboard (parent) RigidBody. This is not limited to floating base +bodies but generally Joint∷is_locked() is preferable otherwise. Returns: true if the body is locked, false otherwise.)"""; @@ -7714,38 +7729,42 @@ generally Joint∷is_locked() is preferable otherwise. // Source: drake/multibody/tree/rigid_body.h const char* doc = R"""(Returns a const reference to the associated LinkFrame -(RigidBodyFrame).)"""; +(RigidBodyFrame). link_frame() is synonymous with body_frame(). + +Note: + "link" is the terminology used in urdf/sdformat.)"""; } link_frame; // Symbol: drake::multibody::RigidBody::mobod_index struct /* mobod_index */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""((Advanced) Returns the index of the mobilized body ("mobod") in the -computational directed forest structure of the owning MultibodyTree to -which this RigidBody belongs. This serves as the BodyNode index and -the index into all associated quantities.)"""; +R"""((Internal use only) Returns the index of the mobilized body ("mobod") +that this Link follows. This index serves as the BodyNode index and +the index into all associated quantities. More than one link may +follow the same mobod.)"""; } mobod_index; // Symbol: drake::multibody::RigidBody::name struct /* name */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(Gets the ``name`` associated with this rigid body. The name will never -be empty.)"""; +R"""(Gets the ``name`` associated with this RigidBody (%Link). 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.)"""; +R"""((Internal use only) Returns this Link'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 const char* doc = -R"""(Returns scoped name of this body. Neither of the two pieces of the -name will be empty (the scope name and the element name). +R"""(Returns scoped name of this RigidBody (%Link). Neither of the two +pieces of the name will be empty (the scope name and the element +name). Raises: RuntimeError if this element is not associated with a @@ -7756,8 +7775,8 @@ name will be empty (the scope name and the element name). struct /* RigidBodyFrame */ { // Source: drake/multibody/tree/rigid_body.h const char* doc = -R"""(A RigidBodyFrame is a material Frame that serves as the unique -reference frame for a RigidBody. +R"""(A RigidBodyFrame (aka LinkFrame) is a material Frame that serves as +the unique reference frame for a RigidBody (aka Link). Each RigidBody B has a unique body frame for which we use the same symbol B (with meaning clear from context). We represent a body frame diff --git a/multibody/benchmarking/BUILD.bazel b/multibody/benchmarking/BUILD.bazel index 028a62f58a7d..faf67e21c9e6 100644 --- a/multibody/benchmarking/BUILD.bazel +++ b/multibody/benchmarking/BUILD.bazel @@ -40,6 +40,7 @@ drake_cc_googlebench_binary( "//multibody/parsing:parser", "//tools/performance:fixture_common", "//tools/performance:gflags_main", + "@gflags", ], add_test_rule = True, ) diff --git a/multibody/benchmarking/cassie.cc b/multibody/benchmarking/cassie.cc index 5deed244b822..96c8e4454109 100644 --- a/multibody/benchmarking/cassie.cc +++ b/multibody/benchmarking/cassie.cc @@ -1,6 +1,7 @@ #include #include +#include #include "drake/common/drake_assert.h" #include "drake/common/find_resource.h" @@ -15,6 +16,8 @@ namespace drake { namespace multibody { namespace { +DEFINE_bool(fuse, false, "Enable fusing for welded-together links."); + using math::RigidTransform; using math::RollPitchYaw; using symbolic::Expression; @@ -119,16 +122,21 @@ class Cassie : public benchmark::Fixture { void DoSlowSystemJacobian(benchmark::State& state) { DRAKE_DEMAND(want_grad_vdot(state) == false); DRAKE_DEMAND(want_grad_u(state) == false); - const int num_mobods = ssize(plant_->graph().forest().mobods()); + const internal::MultibodyTree& mbtree = GetInternalTree(*plant_); + const internal::SpanningForest& forest = mbtree.forest(); + const int num_mobods = forest.num_mobods(); MatrixX Jv_V_WB_W(6 * num_mobods, plant_->num_velocities()); for (auto _ : state) { InvalidateState(); Jv_V_WB_W.setZero(); - for (BodyIndex index{1}; index < plant_->num_bodies(); ++index) { - const Frame& body_frame = plant_->get_body(index).body_frame(); - auto J = Jv_V_WB_W.block(6 * index, 0, 6, plant_->num_velocities()); + for (internal::MobodIndex mobod_index{1}; mobod_index < num_mobods; + ++mobod_index) { + const Frame& active_link_frame = + mbtree.get_active_link(mobod_index).link_frame(); + auto J = + Jv_V_WB_W.block(6 * mobod_index, 0, 6, plant_->num_velocities()); plant_->CalcJacobianSpatialVelocity( - *context_, JacobianWrtVariable::kV, body_frame, + *context_, JacobianWrtVariable::kV, active_link_frame, Eigen::Vector3::Zero(), plant_->world_frame(), plant_->world_frame(), &J); } @@ -241,6 +249,7 @@ std::unique_ptr> Cassie::MakePlant() { Parser parser(plant.get()); const auto& model = "drake/multibody/benchmarking/cassie_v2.urdf"; parser.AddModels(FindResourceOrThrow(model)); + plant->SetFuseWeldedLinks(FLAGS_fuse); plant->Finalize(); if constexpr (std::is_same_v) { return plant; diff --git a/multibody/optimization/test/toppra_test.cc b/multibody/optimization/test/toppra_test.cc index e713141695e3..8b675f1b64fd 100644 --- a/multibody/optimization/test/toppra_test.cc +++ b/multibody/optimization/test/toppra_test.cc @@ -210,7 +210,7 @@ TEST_F(IiwaToppraTest, JointTorqueLimit) { auto s_path = result.value(); // This tolerance was tuned to work with the given gridpoints. - const double tol = 1e-14; + const double tol = 1e-13; Eigen::MatrixXd M(7, 7); Eigen::VectorXd Cv(7); Eigen::VectorXd G(7); diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 3bd34546738a..842ac7cf835e 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1498,7 +1498,7 @@ void MultibodyPlant::Finalize() { DeclareMiscContinuousStates(); } - // After finalizing the base class, tree is read-only. + // After finalizing the base class, the tree is read-only. internal::MultibodyTreeSystem::Finalize(); if (geometry_source_is_registered()) { diff --git a/multibody/plant/test/fused_welds_test.cc b/multibody/plant/test/fused_welds_test.cc index 0f7a931292a9..452e7486f90a 100644 --- a/multibody/plant/test/fused_welds_test.cc +++ b/multibody/plant/test/fused_welds_test.cc @@ -1,7 +1,8 @@ -/* Tests that mass properties are identical whether welded-together links are -modeled with unfused weld joints or whether links that are welded together -are fused onto a mobilized body (fused Mobod). The test builds two identical -models that differ only in whether SetFuseWeldedLinks() is enabled. */ +/* Tests that mass properties and link kinematics (poses and spatial +velocities) are identical whether links that are welded together are modeled +with explicit weld joints (unfused) or are fused into a single mobilized body +(fused Mobod). The tests build two identical models that differ only in whether +SetFuseWeldedLinks() is enabled. */ #include #include @@ -69,14 +70,14 @@ The positions of the link origins from World origin Wo, expressed in World are: Link3: (cos θ − sin θ, sin θ + cos θ, 0) Link4: (4, 0, 0) -With fuse_welded_bodies = false, five mobilized bodies are created, +With fuse_welded_links = false, five mobilized bodies are created, (World, Link1 via revolute, Link2 via weld, Link3 via weld, Link 4 via weld). -With fuse_welded_bodies = true two mobilized bodies are created, +With fuse_welded_links = true two mobilized bodies are created, (World with link4 and one fused mobilized body with Link1, Link2, Link3). */ -TestModel MakeModel(bool fuse_welded_bodies) { +TestModel MakeModel(bool fuse_welded_links) { TestModel m; m.plant = std::make_unique>(0.0 /* continuous */); - m.plant->SetFuseWeldedLinks(fuse_welded_bodies); + m.plant->SetFuseWeldedLinks(fuse_welded_links); // To facilitate an analytical solution, each link has a trivial inertia, // namely a 1 kg solid cube, 0.1 m per side. @@ -119,8 +120,8 @@ TestModel MakeModel(bool fuse_welded_bodies) { EXPECT_EQ(m.plant->num_positions(), 1); // 1 revolute angle. EXPECT_EQ(m.plant->num_velocities(), 1); // 1 revolute angular rate. const internal::MultibodyTree& tree = GetInternalTree(*m.plant); - EXPECT_EQ(tree.num_mobods(), fuse_welded_bodies ? 2 : 5); - EXPECT_EQ(tree.num_mobilizers(), fuse_welded_bodies ? 2 : 5); + EXPECT_EQ(tree.num_mobods(), fuse_welded_links ? 2 : 5); + EXPECT_EQ(tree.num_mobilizers(), fuse_welded_links ? 2 : 5); // Note: num_mobilizers() == num_mobods() because the World body gets a dummy // weld mobilizer at index 0 to keep mobilizer/body-node indexing identical. @@ -139,7 +140,7 @@ TestModel MakeModel(bool fuse_welded_bodies) { // Ensure Mobods (mobilized bodies) have the proper follower link ordinals. // Each Mobod should have an active link ordinal. If welded links have // been fused, then there are additional follower link ordinals. - if (fuse_welded_bodies) { + if (fuse_welded_links) { EXPECT_EQ(mobod_0.follower_link_ordinals(), (std::vector{LinkOrdinal(0), LinkOrdinal(4)})); EXPECT_EQ(mobod_1.follower_link_ordinals(), @@ -157,12 +158,186 @@ void SetState(const TestModel& m, double angle_rad, double angular_vel) { m.revolute->set_angular_rate(m.context.get(), angular_vel); } +/* Tests that link poses and spatial velocities are identical whether welds are +modeled explicitly (unfused) or combined into a fused mobilized body. This +doesn't check that the values are right, just that they match. Other tests +below will check some numerical results. */ +GTEST_TEST(CompositeTest, LinkKinematicsMatchExplicitWelds) { + const TestModel unfused_model = MakeModel(false); + const TestModel fused_model = MakeModel(true); + + // Test at several non-trivial (q, v) configurations. + const std::vector> configs = { + {0.0, 0.0}, {M_PI / 6, 0.5}, {M_PI / 4, -1.2}, + {M_PI / 2, 2.0}, {-M_PI / 3, 0.7}, + }; + + for (const auto& [angle, omega] : configs) { + SetState(unfused_model, angle, omega); + SetState(fused_model, angle, omega); + + for (const auto& [link_name, unfused_link, fused_link] : + std::vector*, + const RigidBody*>>{ + {"Link1", unfused_model.link1, fused_model.link1}, + {"Link2", unfused_model.link2, fused_model.link2}, + {"Link3", unfused_model.link3, fused_model.link3}, + {"Link4", unfused_model.link4, fused_model.link4}, + }) { + // Compare world poses. + const RigidTransformd& X_WL_unfused = + unfused_link->EvalPoseInWorld(*unfused_model.context); + const RigidTransformd& X_WL_fused = + fused_link->EvalPoseInWorld(*fused_model.context); + + EXPECT_TRUE(X_WL_unfused.IsNearlyEqualTo(X_WL_fused, kTolerance)); + + // Compare world spatial velocities. + const SpatialVelocity& V_WL_unfused = + unfused_link->EvalSpatialVelocityInWorld(*unfused_model.context); + const SpatialVelocity& V_WL_fused = + fused_link->EvalSpatialVelocityInWorld(*fused_model.context); + + EXPECT_TRUE(CompareMatrices(V_WL_unfused.get_coeffs(), + V_WL_fused.get_coeffs(), kTolerance, + MatrixCompareType::relative)); + } + } +} + +/* Test the pose of each link at the zero configuration. */ +GTEST_TEST(CompositeTest, ZeroConfigurationPoses) { + for (bool fuse : {false, true}) { + SCOPED_TRACE(fuse ? "fused" : "unfused"); + const TestModel m = MakeModel(fuse); + SetState(m, 0.0, 0.0); + + // Verify Link1 is at World origin, no rotation. + const RigidTransformd& X_WL1 = m.link1->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL1.IsNearlyEqualTo(RigidTransformd::Identity(), kTolerance)); + + // Verify Link2's position in World is (1, 0, 0), no rotation. + const RigidTransformd& X_WL2 = m.link2->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL2.IsNearlyEqualTo( + RigidTransformd(Vector3(1.0, 0.0, 0.0)), kTolerance)); + + // Verify Link3's position in World is (1, 1, 0), no rotation. + const RigidTransformd& X_WL3 = m.link3->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL3.IsNearlyEqualTo( + RigidTransformd(Vector3(1.0, 1.0, 0.0)), kTolerance)); + + // Verify Link4's position in World is (4, 0, 0), no rotation. + const RigidTransformd& X_WL4 = m.link4->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL4.IsNearlyEqualTo( + RigidTransformd(Vector3(4.0, 0.0, 0.0)), kTolerance)); + } +} + +/* Test the pose of each link when Link1 is rotated 90°. */ +GTEST_TEST(CompositeTest, NinetyDegreePoses) { + for (bool fuse : {false, true}) { + SCOPED_TRACE(fuse ? "fused" : "unfused"); + const TestModel m = MakeModel(fuse); + SetState(m, M_PI / 2, 0.0); + + // Link2 and Link3 share Link1's 90° z-rotation. + const math::RotationMatrixd R_W90z = + math::RotationMatrixd::MakeZRotation(M_PI / 2); + + // Verify Link1 has a 90° z-rotation relative to World + // and Link1's origin is at World origin. + const RigidTransformd& X_WL1 = m.link1->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL1.IsNearlyEqualTo( + RigidTransformd(R_W90z, Vector3::Zero()), kTolerance)); + + // Link2's origin is (1, 0, 0) along Link1's +x direction. Link1's +x + // direction points in World's +y direction due to the 90° z-rotation, so + // verify Link2's origin in World is (0, 1, 0). + const RigidTransformd& X_WL2 = m.link2->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL2.IsNearlyEqualTo( + RigidTransformd(R_W90z, Vector3(0.0, 1.0, 0.0)), kTolerance)); + + // Link3's origin is 1 m along Link2's y-axis. After 90° rotation, Link2's + // y-axis points in World -x, so Link3 is at (0,1,0) + (-1,0,0) = (-1,1,0). + const RigidTransformd& X_WL3 = m.link3->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL3.IsNearlyEqualTo( + RigidTransformd(R_W90z, Vector3(-1.0, 1.0, 0.0)), kTolerance)); + + // Verify Link4's position in World is (4, 0, 0), no rotation. + const RigidTransformd& X_WL4 = m.link4->EvalPoseInWorld(*m.context); + EXPECT_TRUE(X_WL4.IsNearlyEqualTo( + RigidTransformd(Vector3(4.0, 0.0, 0.0)), kTolerance)); + } +} + +/* Verify spatial velocities for individual links, regardless of whether +welded links are fused. */ +GTEST_TEST(CompositeTest, SpatialVelocities) { + const double angle = M_PI / 4; + const double omega = 2.0; + + for (bool fuse : {false, true}) { + SCOPED_TRACE(fuse ? "fused" : "unfused"); + const TestModel m = MakeModel(fuse); + SetState(m, angle, omega); + + // Link2 and Link3 share Link1's angular velocity in world of ω * ẑ. + const Vector3 w_expected = omega * Vector3::UnitZ(); + for (const RigidBody* link : {m.link1, m.link2, m.link3}) { + const SpatialVelocity& V_WL = + link->EvalSpatialVelocityInWorld(*m.context); + EXPECT_TRUE(CompareMatrices(V_WL.rotational(), w_expected, kTolerance, + MatrixCompareType::relative)); + } + + // Verify that the translational velocity of L1o (Link1's origin) in World + // is zero (L1o is at World origin). + const SpatialVelocity& V_WL1o = + m.link1->EvalSpatialVelocityInWorld(*m.context); + EXPECT_TRUE(CompareMatrices(V_WL1o.translational(), Vector3::Zero(), + kTolerance, MatrixCompareType::relative)); + + // Verify that the translational velocity of L2o (Link2's origin) in World + // is ω × p_WL2o. + // p_WL2o = R_W_L1 * [1, 0, 0] = [cos θ, sin θ, 0]. + // v_WL2o = ω ẑ × [cos θ, sin θ, 0] = ω[-sin θ, cos θ, 0]. + const Vector3 v_WL2o_expected = + omega * Vector3(-std::sin(angle), std::cos(angle), 0.0); + const SpatialVelocity& V_WL2o = + m.link2->EvalSpatialVelocityInWorld(*m.context); + EXPECT_TRUE(CompareMatrices(V_WL2o.translational(), v_WL2o_expected, + kTolerance, MatrixCompareType::relative)); + + // Verify that the translational velocity of L3o (Link3's origin) in World + // is ω × p_WL3o. + // p_WL3o = R_WL1 * [1,0,0] + R_WL1 * [0,1,0] + // = [cos θ - sin θ, sin θ + cos θ, 0]. + // v_WL3o = ω ẑ × p_WL3o + // = ω ẑ × [cos θ - sin θ, sin θ + cos θ, 0] + // = ω[-(sin θ + cos θ), cos θ - sin θ, 0]. + const Vector3 v_WL3o_expected = + omega * Vector3(-(std::sin(angle) + std::cos(angle)), + std::cos(angle) - std::sin(angle), 0.0); + const SpatialVelocity& V_WL3o = + m.link3->EvalSpatialVelocityInWorld(*m.context); + EXPECT_TRUE(CompareMatrices(V_WL3o.translational(), v_WL3o_expected, + kTolerance, MatrixCompareType::relative)); + + // Verify Link4's is welded to World (zero spatial velocity). + const SpatialVelocity& V_WL4o = + m.link4->EvalSpatialVelocityInWorld(*m.context); + EXPECT_TRUE(CompareMatrices(V_WL4o.translational(), Vector3::Zero(), + kTolerance, MatrixCompareType::relative)); + } +} + /* Verify that the spatial inertia for fused mobod Link123 (with links 1, 2, 3) -matches the sum of spatial inertias for unfused links 1, 2, 3 at several poses. -Next, ensure spatial inertia for each of the follower links in a fused mobod are -computed correctly. Lastly, the 1x1 mass matrix for this 1-DOF model directly -depends on the spatial inertia of Link123, so we also verify: - (a) The mass matrix is identical between the fused and unfused weld models. +matches the sum of spatial inertias for unfused links 1, 2, 3. Next, ensure +spatial inertia for each of the follower links in a fused mobod are computed +correctly. Lastly, the 1x1 mass matrix for this 1-DOF model directly depends on +the spatial inertia of Link123, so we also verify: + (a) The mass matrix is identical between the unfused and fused + models at several configurations. (b) The mass matrix matches the analytically computed value. Similarly, verify spatial momentum calculations. @@ -173,10 +348,10 @@ The model has 4 links, Linki (i=1,2,3,4), each a 1 kg solid cube of side 0.1 m. Links 1,2,3 are welded together and are connected to World via a revolute joint (z-axis at World origin). Link4 is welded directly to World. The link body-frame origins are coincident with their associated centers of mass, located with: - Link1 origin from World origin: p₁ = (0, 0, 0) expressed in World. - Link2 origin from Link1 origin: p₂ = (1, 0, 0) expressed in Link1. - Link3 origin from Link1 origin: p₃ = (1, 1, 0) expressed in Link1. - Link4 origin from World origin: p₄ = (4, 0, 0) expressed in World. + Link1 origin from World origin: p₁ = (0, 0, 0) expressed in World. + Link2 origin from Link1 origin: p₂ = (1, 0, 0) expressed in Link1. + Link3 origin from Link1 origin: p₃ = (1, 1, 0) expressed in Link1. + Link4 origin from World origin: p₄ = (4, 0, 0) expressed in World. For a solid cube of mass m and side a, its moment of inertia about any axis through its COM is m*a²/6. The parallel axis theorem calculates each cube's @@ -184,13 +359,13 @@ moment of inertia about the revolute's z-axis via: Iᵢ = m*a²/6 + m*(dᵢ)², where dᵢ (i=1,2,3) is the distance between each cube's COM and the revolute's z-axis. Iᵢ is independent of joint angle because the links are welded together. - Link1: I₁ = 1*(0.1)²/6 + 1*0² = 1/600 + 0 (d² = 0) - Link2: I₂ = 1*(0.1)²/6 + 1*1² = 1/600 + 1 (d² = 1) - Link3: I₃ = 1*(0.1)²/6 + 1*√2² = 1/600 + 2 (d² = 2) - Total: Iₜ = 3/600 + 3 = 1/200 + 3 = 3.005 kg·m² */ + Link1: I₁ = 1*(0.1)²/6 + 1*0² = 1/600 + 0 (d² = 0) + Link2: I₂ = 1*(0.1)²/6 + 1*1² = 1/600 + 1 (d² = 1) + Link3: I₃ = 1*(0.1)²/6 + 1*√2² = 1/600 + 2 (d² = 2) + Total: Iₜ = 3/600 + 3 = 1/200 + 3 = 3.005 kg·m² */ GTEST_TEST(FusedTest, CompositeSpatialInertia) { - const TestModel unfused_model = MakeModel(false /* no combining */); - const TestModel fused_model = MakeModel(true /* fuse welds */); + const TestModel unfused_model = MakeModel(false); + const TestModel fused_model = MakeModel(true); const double m = 1.0, a = 0.1; // mass m and side-length a of solid cubes. const Frame& world_frame = unfused_model.plant->world_frame(); diff --git a/multibody/tree/body_node_impl.cc b/multibody/tree/body_node_impl.cc index b5b12acf1d6a..31514ca2da56 100644 --- a/multibody/tree/body_node_impl.cc +++ b/multibody/tree/body_node_impl.cc @@ -62,10 +62,12 @@ void BodyNodeImpl::CalcPositionKinematicsCache_BaseToTip( const math::RigidTransform& X_WP = pc->get_X_WB(inboard_mobod_index()); // Output (updating a cache entry): - // - X_FM(q_B) - // - X_PB(q_B) - // - X_WB(q(W:P), q_B) - // - p_PoBo_W(q_B) + // - X_FM(qm) (qm are the coordinates of body B's mobilizer) + // - X_PB(qm) + // - X_WB(q(W:P), qm) + // - X_WL(q(W:P), qm) (for all links L following mobod B) + // - p_BoLo_W(q(W:P), qm) (for all links L following mobod B) + // - p_PoBo_W(qm) math::RigidTransform& X_FM = pc->get_mutable_X_FM(mobod_index()); math::RigidTransform& X_PB = pc->get_mutable_X_PB(mobod_index()); math::RigidTransform& X_WB = pc->get_mutable_X_WB(mobod_index()); @@ -117,19 +119,25 @@ void BodyNodeImpl::CalcPositionKinematicsCache_BaseToTip( // L coincides with the mobod frame B (X_BL = Identity), so X_WL = X_WB. const SpanningForest::Mobod& mobod = mobilizer_->mobod(); pc->SetX_WL(mobod.active_link_ordinal(), X_WB); + // p_BoLo_W for the active link is always zero (set during allocation). - // For fused mobods, also set X_WL for each follower (non-active) - // link Lₒ. Because Lₒ is rigidly offset from B by X_BLₒ (pre-computed in - // frame_body_pose_cache during CalcFrameBodyPoses), its world pose is simply: - // X_WLₒ = X_WB * X_BLₒ + // For fused mobods, also set X_WLᵢ and p_BoLᵢo_W for each follower + // (non-active) link Lᵢ. Because Lᵢ is rigidly offset from B by X_BLᵢ + // (pre-computed in frame_body_pose_cache during CalcFrameBodyPoses()), its + // world pose is simply: X_WLᵢ = X_WB * X_BLᵢ. if (mobod.is_fused()) { const auto& followers = mobod.follower_link_ordinals(); + const math::RotationMatrix& R_WB = X_WB.rotation(); // followers[0] is the active link (already handled above); start at 1. for (int i = 1; i < ssize(followers); ++i) { const LinkOrdinal follower_ordinal = followers[i]; const math::RigidTransform& X_BL = frame_body_pose_cache.get_X_BL(follower_ordinal); - pc->SetX_WL(follower_ordinal, X_WB * X_BL); + const Vector3 p_BoLo_W = R_WB * X_BL.translation(); + pc->Set_p_BoLo_W(follower_ordinal, p_BoLo_W); + pc->SetX_WL(follower_ordinal, + math::RigidTransform(R_WB * X_BL.rotation(), + X_WB.translation() + p_BoLo_W)); } } @@ -214,6 +222,57 @@ void BodyNodeImpl:: } } +// Notation: +// - B body frame associated with this node. +// - P ("parent") body frame associated with this node's parent. +// - F mobilizer inboard frame attached to body P. +// - M mobilizer outboard frame attached to body B. +// +// Outputs (updating cache entry vc): +// - V_FM(qm, vm) across-mobilizer spatial velocity of M in F. +// - V_PB_W(qm, vm) B's spatial velocity in P, expressed in W. +// - V_WB(q(W:P), qm, vm) B's spatial velocity in W, expressed in W. +// - V_WL(q(W:P), qm, vm) For each link L that follows body B, L's +// spatial velocity in W, expressed in W (for point Lo, the link frame's +// origin). +// +// Inputs +// - the already-calculated position kinematics in pc. +// - V_WP (parent's spatial velocity in World, already calculated since we +// are in a base-to-tip recursion). +// - this mobilizer's generalized velocities vm. +// +// V_WB is calculated by the recursive relation: +// V_WB = V_WPb + V_PB_W (Eq. 5.6 in Jain (2010), p. 77) (1) +// where Pb is a frame aligned with P but with its origin shifted from Po to B's +// origin Bo. Then V_WPb is the spatial velocity of frame Pb, measured and +// expressed in the world frame W. Since V_PB's translational component is also +// for the point Bo, we can add these spatial velocities. Therefore we need to +// develop expressions for the two terms (V_WPb and V_PB_W) in Eq. (1). +// +// Computation of V_PB_W: +// Let Mb be a frame aligned rigidly with M but with its origin at Bo. +// For rigid bodies (which we always have here) +// V_PB_W = V_FMb_W (2) +// which can be computed from the spatial velocity measured in frame F (as +// provided by mobilizer's methods) +// V_FMb_W = R_WF * V_FMb = R_WF * V_FM.Shift(p_MoBo_F) (3) +// arriving at the desired result: +// V_PB_W = R_WF * V_FM.Shift(p_MoBo_F) (4) +// +// V_FM = H_FM * vm is immediately available via an efficient operator provided +// by this node's mobilizer. H_FM(q) is the mobilizer's hinge matrix, and vm +// this mobilizer's generalized velocities. +// +// Computation of V_WPb: +// This can be computed by a simple shift operation from V_WP: +// V_WPb = V_WP.Shift(p_PoBo_W) (5) +// +// Note: +// It is very common to find treatments in which the body frame B is coincident +// with the outboard frame M, that is B ≡ M, leading to slightly simpler +// recursive relations (for instance, see Section 3.3.2 in Jain (2010)) where +// p_MoBo_F = 0 and thus V_PB_W = V_FM_W. template void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( const T* positions, const PositionKinematicsCache& pc, @@ -223,59 +282,16 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( DRAKE_ASSERT(mobod_index() != world_mobod_index()); DRAKE_ASSERT(vc != nullptr); - // As a guideline for developers, a summary of the computations performed in - // this method is provided: - // Notation: - // - B body frame associated with this node. - // - P ("parent") body frame associated with this node's parent. - // - F mobilizer inboard frame attached to body P. - // - M mobilizer outboard frame attached to body B. - // The goal is computing the spatial velocity V_WB of body B measured in the - // world frame W. The calculation is recursive and assumes the spatial - // velocity V_WP of the inboard body P is already computed. These spatial - // velocities are related by the recursive relation: - // V_WB = V_WPb + V_PB_W (Eq. 5.6 in Jain (2010), p. 77) (1) - // where Pb is a frame aligned with P but with its origin shifted from Po - // to B's origin Bo. Then V_WPb is the spatial velocity of frame Pb, - // measured and expressed in the world frame W. Since V_PB's - // translational component is also for the point Bo, we can add these - // spatial velocities. Therefore we need to develop expressions for the two - // terms (V_WPb and V_PB_W) in Eq. (1). - // - // Computation of V_PB_W: - // Let Mb be a frame aligned rigidly with M but with its origin at Bo. - // For rigid bodies (which we always have here) - // V_PB_W = V_FMb_W (2) - // which can be computed from the spatial velocity measured in frame F (as - // provided by mobilizer's methods) - // V_FMb_W = R_WF * V_FMb = R_WF * V_FM.Shift(p_MoBo_F) (3) - // arriving at the desired result: - // V_PB_W = R_WF * V_FM.Shift(p_MoBo_F) (4) - // - // V_FM = H_FM * vm is immediately available from this node's mobilizer where - // H_FM is the mobilizer's hinge matrix, and vm this mobilizer's generalized - // velocities. - // - // Computation of V_WPb: - // This can be computed by a simple shift operation from V_WP: - // V_WPb = V_WP.Shift(p_PoBo_W) (5) - // - // Note: - // It is very common to find treatments in which the body frame B is - // coincident with the outboard frame M, that is B ≡ M, leading to slightly - // simpler recursive relations (for instance, see Section 3.3.2 in - // 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_B = get_q(positions); - const T* v_B = get_v(velocities); + // This node's mobilizer's generalized coordinates and velocities. + const T* qm = get_q(positions); + const T* vm = 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_B, v_B); + V_FM = mobilizer_->calc_V_FM(qm, vm); // 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 @@ -288,7 +304,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_B); + const Eigen::Map> v(vm); V_PB_W.get_coeffs() = H_PB_W * v; } else { V_PB_W.get_coeffs().setZero(); @@ -297,23 +313,30 @@ void BodyNodeImpl::CalcVelocityKinematicsCache_BaseToTip( // ========================================================================= // Computation of V_WPb in Eq. (1). See summary at the top of this method. - // Shift vector between the parent body P and this node's body B, - // expressed in the world frame W. - const Vector3& p_PB_W = get_p_PoBo_W(pc); + // Shift vector between the parent body's origin Po and the origin Bo of this + // node's body B, expressed in the world frame W. + const Vector3& p_PoBo_W = get_p_PoBo_W(pc); // Since we are in a base-to-tip recursion the parent body P's spatial // velocity is already available in the cache. const SpatialVelocity& V_WP = get_V_WP(*vc); // ========================================================================= - // Update velocity V_WB of this node's body B in the world frame. Using the + // 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. 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); + V_WB = V_WP.ComposeWithMovingFrameVelocity(p_PoBo_W, V_PB_W); + + // We have V_WB, now fill in V_WLᵢ for all links Lᵢ following body B. + const SpanningForest::Mobod& mobod_B = this->mobod(); + const std::vector& followers = mobod_B.follower_link_ordinals(); + // The active link L₀ is always the first follower and L₀=B. + vc->SetV_WL(followers[0], V_WB); + for (size_t i = 1; i < followers.size(); ++i) { + const LinkOrdinal link_ordinal = followers[i]; + const Vector3& p_BoLo_W = pc.get_p_BoLo_W(link_ordinal); + vc->SetV_WL(link_ordinal, V_WB.Shift(p_BoLo_W)); + } } // As a guideline for developers, a summary of the computations performed in diff --git a/multibody/tree/frame_body_pose_cache.h b/multibody/tree/frame_body_pose_cache.h index 71c6e921f771..a30b5da8e894 100644 --- a/multibody/tree/frame_body_pose_cache.h +++ b/multibody/tree/frame_body_pose_cache.h @@ -50,6 +50,10 @@ Mass properties - p_BoLcm_B: The position vector from mobilized body origin Bo to the center of mass of L, expressed in B. Indexed by LinkOrdinal. +@note Position kinematics for links fixed to World can be precalculated once the +frame parameters are set. Those entries of the PositionKinematicsCache should +be filled in whenever the FrameBodyPoseCache is recomputed. + @tparam_default_scalar */ template class FrameBodyPoseCache { diff --git a/multibody/tree/joint.h b/multibody/tree/joint.h index 0e0973b8f7ba..10c44302d956 100644 --- a/multibody/tree/joint.h +++ b/multibody/tree/joint.h @@ -774,7 +774,8 @@ class Joint : public MultibodyElement { std::unique_ptr> CloneToScalar( internal::MultibodyTree* tree_clone) const { std::unique_ptr> joint_clone = DoCloneToScalar(*tree_clone); - joint_clone->mobilizer_ = FindMobilizerToScalarClone(tree_clone); + DRAKE_DEMAND(mobilizer_ != nullptr); + joint_clone->mobilizer_ = &tree_clone->get_mutable_variant(*mobilizer_); return joint_clone; } @@ -1031,16 +1032,6 @@ class Joint : public MultibodyElement { const internal::SpanningForest::Mobod& mobod, internal::MultibodyTree* tree) const = 0; - // Helper method to be called within Joint::CloneToScalar() to locate the - // cloned Mobilizer corresponding to this Joint's Mobilizer. - template - internal::Mobilizer* FindMobilizerToScalarClone( - internal::MultibodyTree* tree_clone) const { - internal::Mobilizer* mobilizer_clone = - &tree_clone->get_mutable_variant(*mobilizer_); - return mobilizer_clone; - } - // Implementation for MultibodyElement::DoDeclareParameters(). void DoDeclareParameters( internal::MultibodyTreeSystem* tree_system) final { diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 6a101a15b023..e4e8742bb1e1 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -578,10 +578,10 @@ const Link& MultibodyTree::AddLinkImpl(std::unique_ptr> link) { link->set_parent_tree(this, link_index); // MultibodyTree can access selected private methods in RigidBody through its - // RigidBodyAttorney. + // LinkAttorney. // - Register link frame. Frame* link_frame = - &internal::RigidBodyAttorney::get_mutable_link_frame(link.get()); + &internal::LinkAttorney::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()); @@ -889,7 +889,7 @@ void MultibodyTree::FinalizeInternals() { for (JointIndex i : GetJointIndices()) { auto& joint = joints_.get_mutable_element(i); const RigidBody& body = joint.child_body(); - if (RigidBodyAttorney::is_floating_base_body_pre_finalize(body)) { + if (LinkAttorney::is_floating_base_body_pre_finalize(body)) { DRAKE_DEMAND(joint.is_ephemeral()); const auto [quaternion, translation] = GetDefaultFloatingBaseBodyPoseAsQuaternionVec3Pair(body); @@ -1523,7 +1523,7 @@ void MultibodyTree::CalcVelocityKinematicsCache( // If the model has zero dofs we simply set all spatial velocities to zero and // return since there is no work to be done. if (num_velocities() == 0) { - vc->InitializeToZero(); + vc->SetToZero(); return; } @@ -1757,10 +1757,10 @@ void MultibodyTree::CalcFrameBodyPoses( const RigidTransform& X_BL = frame_body_poses->get_X_BL(link_ordinal); const math::RotationMatrix& R_BL = X_BL.rotation(); - const Vector3& p_BoLo_B = X_BL.translation(); const Vector3 p_BoLcm_B = X_BL * p_LoLcm_L; frame_body_poses->Set_p_BoLcm_B(link_ordinal, p_BoLcm_B); if (!is_world_mobod) { + const Vector3& p_BoLo_B = X_BL.translation(); const SpatialInertia M_LLo_B = M_LLo_L.ReExpress(R_BL); const SpatialInertia M_LBo_B = M_LLo_B.Shift(-p_BoLo_B); frame_body_poses->AddToM_BBo_B(mobod.index(), M_LBo_B); @@ -4525,10 +4525,10 @@ RigidBody* MultibodyTree::CloneBodyAndAdd( auto body_clone = body.CloneToScalar(*this); body_clone->set_parent_tree(this, body_index); body_clone->set_model_instance(body.model_instance()); - // MultibodyTree can access selected private methods in RigidBody through - // its RigidBodyAttorney. + // MultibodyTree can access selected private methods in RigidBody through its + // LinkAttorney. Frame* body_frame_clone = - &internal::RigidBodyAttorney::get_mutable_link_frame(body_clone.get()); + &internal::LinkAttorney::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()); diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index e759d4c94566..10a7cb87fa0e 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -945,21 +945,42 @@ class MultibodyTree { return link_joint_graph_.forest(); } + // Returns a const reference to the LinkJointGraph that is owned by this + // MultibodyTree. [[nodiscard]] const LinkJointGraph& graph() const { return link_joint_graph_; } + // Returns a mutable reference to the LinkJointGraph that is owned by this + // MultibodyTree. [[nodiscard]] LinkJointGraph& mutable_graph() { return link_joint_graph_; } + // Returns a reference to this MultibodyTree's SpanningForest. + // @pre The forest is valid, i.e. we have called Finalize(). [[nodiscard]] const SpanningForest& forest() const { DRAKE_ASSERT(graph().forest_is_valid()); return graph().forest(); } + // Returns a Mobod from this MultibodyTree's SpanningForest. + // @pre The forest is valid, i.e. we have called Finalize(). [[nodiscard]] const SpanningForest::Mobod& get_mobod(MobodIndex index) const { return forest().mobods(index); } + // Every mobod has a unique "active link", the link that connects it to + // its parent mobod (or the world link in the case of the world mobod). + // This returns a reference to the Drake Link corresponding to the active + // link of the given mobod. + // @pre Finalize() has been called. + [[nodiscard]] const Link& get_active_link(MobodIndex index) const { + const SpanningForest::Mobod& mobod = get_mobod(index); + const LinkOrdinal active_link_ordinal = mobod.active_link_ordinal(); + const LinkIndex active_link_index = + forest().links(active_link_ordinal).index(); + return get_link(active_link_index); + } + // See MultibodyPlant API. void SetBaseBodyJointType( BaseBodyJointType joint_type, diff --git a/multibody/tree/position_kinematics_cache.cc b/multibody/tree/position_kinematics_cache.cc index dfc37638a3cb..c2f85a59a2c3 100644 --- a/multibody/tree/position_kinematics_cache.cc +++ b/multibody/tree/position_kinematics_cache.cc @@ -14,9 +14,17 @@ PositionKinematicsCache::PositionKinematicsCache( : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) { Allocate(); - // Set known values. + // Set known values for world-related quantities (here B=W). X_WB_pool_[world_mobod_index()] = RigidTransform::Identity(); X_WL_pool_[world_link_ordinal()] = RigidTransform::Identity(); + p_BoLo_W_pool_[world_link_ordinal()].setZero(); + // Set known values for active links. + // p_BoLo_W is always zero for each mobod's active link L. + for (MobodIndex mobod_index(1); mobod_index < num_mobods_; ++mobod_index) { + const LinkOrdinal active_link = + forest.mobods(mobod_index).active_link_ordinal(); + p_BoLo_W_pool_[active_link].setZero(); + } } template @@ -31,6 +39,9 @@ void PositionKinematicsCache::ComputeWorldComposite( const math::RigidTransform& X_WL = frame_body_pose_cache.get_X_BL(link_ordinal); // B(=W) to link L SetX_WL(link_ordinal, X_WL); + // When B is world, R_WB = R_BB is identity, thus + // p_BoLo_W = R_WB * p_BoLo_B = p_BoLo_B = X_BL.translation(). + Set_p_BoLo_W(link_ordinal, X_WL.translation()); } } @@ -39,15 +50,23 @@ template void PositionKinematicsCache::Allocate() { const Vector3 nan_vec = Vector3::Constant(std::numeric_limits::quiet_NaN()); - X_WB_pool_.resize(num_mobods_, NaNPose()); - X_WL_pool_.resize(num_links_, NaNPose()); - X_PB_pool_.resize(num_mobods_, NaNPose()); + RigidTransform nan_pose = + RigidTransform(math::RotationMatrix::Identity(), nan_vec); + + // These four are indexed by MobodIndex. + X_WB_pool_.resize(num_mobods_, nan_pose); + X_PB_pool_.resize(num_mobods_, nan_pose); + // Mobilizers expect to be able to count on X_FM having been initialized // to the identity matrix. For example, Weld mobilizers just leave it that // way and never write to X_FM. Other mobilizers make use of the known // structure of the identity transform. X_FM_pool_.resize(num_mobods_, RigidTransform::Identity()); p_PoBo_W_pool_.resize(num_mobods_, nan_vec); + + // These are indexed by LinkOrdinal. + X_WL_pool_.resize(num_links_, nan_pose); + p_BoLo_W_pool_.resize(num_links_, nan_vec); } } // namespace internal diff --git a/multibody/tree/position_kinematics_cache.h b/multibody/tree/position_kinematics_cache.h index 60eb8f0c49ac..5bd098e904a0 100644 --- a/multibody/tree/position_kinematics_cache.h +++ b/multibody/tree/position_kinematics_cache.h @@ -40,6 +40,10 @@ Results are indexed by MobodIndex unless otherwise specified: - p_PoBo_W: Position of mobod B's origin Bo measured in its parent (inboard) mobod P, expressed in world frame W. + - p_BoLo_W: + Position of link L's origin Lo measured from its mobod B's origin Bo, + expressed in the world frame W. Indexed by LinkOrdinal. Zero for the + active link L₀ (since B=L₀). - X_FM: Pose of mobilizer's outboard frame M measured and expressed in its inboard frame F. @@ -93,6 +97,20 @@ class PositionKinematicsCache { X_WL_pool_[ordinal] = X_WL; } + // Returns X_BL.translation() re-expressed in the world frame W. + // @param[in] ordinal The LinkOrdinal for the link L. + // @returns p_BoLo_W, the position of link L's origin Lo measured from its + // mobod B's origin Bo, expressed in the world frame W. + const Vector3& get_p_BoLo_W(LinkOrdinal ordinal) const { + DRAKE_ASSERT(0 <= ordinal && ordinal < num_links_); + return p_BoLo_W_pool_[ordinal]; + } + + void Set_p_BoLo_W(LinkOrdinal ordinal, const Vector3& p_BoLo_W) { + DRAKE_DEMAND(0 <= ordinal && ordinal < num_links_); + p_BoLo_W_pool_[ordinal] = p_BoLo_W; + } + // Returns a const reference to the rotation matrix `R_WB` that relates the // orientation of the world frame W with the body frame B. // @param[in] mobod_index The unique index for the computational @@ -177,17 +195,6 @@ class PositionKinematicsCache { const SpanningForest& forest, const FrameBodyPoseCache& frame_body_pose_cache); - // Helper method to initialize poses to garbage values including NaNs. - // This allow us to quickly verify some of the values stored in the pools are - // never used (however we store them anyway to simplify the indexing). - static RigidTransform NaNPose() { - // Note: RotationMatrix will throw in Debug builds if values are NaN. For - // our purposes, it is enough that the translation has NaN values. - return RigidTransform( - math::RotationMatrix::Identity(), - Vector3::Constant(std::numeric_limits::quiet_NaN())); - } - // Number of Mobods in the multibody forest, including the World mobod. int num_mobods_{0}; @@ -207,8 +214,9 @@ class PositionKinematicsCache { std::vector> X_FM_pool_; std::vector> p_PoBo_W_pool_; - // This pool is indexed by LinkOrdinal. + // These pools are indexed by LinkOrdinal. std::vector> X_WL_pool_; + std::vector> p_BoLo_W_pool_; }; } // namespace internal diff --git a/multibody/tree/rigid_body.cc b/multibody/tree/rigid_body.cc index 185e8adb5f29..917941382ccc 100644 --- a/multibody/tree/rigid_body.cc +++ b/multibody/tree/rigid_body.cc @@ -63,6 +63,27 @@ ScopedName RigidBody::scoped_name() const { name_); } +template +void RigidBody::Lock(systems::Context* context) const { + ThrowIfNotFinalized(__func__); + if (!is_floating_base_body()) { + throw std::logic_error(fmt::format( + "Attempted to call Lock() on non-floating-base rigid body {}", name())); + } + mobilizer().Lock(context); +} + +template +void RigidBody::Unlock(systems::Context* context) const { + ThrowIfNotFinalized(__func__); + if (!is_floating_base_body()) { + throw std::logic_error(fmt::format( + "Attempted to call Unlock() on non-floating-base rigid body {}", + name())); + } + mobilizer().Unlock(context); +} + template RigidBody::RigidBody(const std::string& body_name, const SpatialInertia& M) @@ -80,6 +101,55 @@ RigidBody::RigidBody(const std::string& body_name, link_frame_(*this), default_spatial_inertia_(M) {} +template +void RigidBody::DoSetTopology() { + DRAKE_DEMAND(mobilizer_ == nullptr); + const internal::MultibodyTree& tree = this->get_parent_tree(); + const internal::SpanningForest& forest = tree.forest(); + const internal::LinkJointGraph::Link& link = forest.link_by_index(index()); + mobilizer_ = &tree.get_mobilizer(link.mobod_index()); + + // Is this RigidBody the active link on its Mobod? + const bool is_active_link = + link.ordinal() == forest.mobods(link.mobod_index()).active_link_ordinal(); + is_floating_base_body_ = + is_active_link && mobilizer_->is_floating_base_mobilizer(); +} + +template +void RigidBody::DoDeclareParameters( + internal::MultibodyTreeSystem* tree_system) { + // Sets model values to dummy values to indicate that the model values are + // not used. This class stores the the default values of the parameters. + // 10 numeric values are used to store mass, center of mass, moments and + // products of inertia packed into one basic vector. + spatial_inertia_parameter_index_ = + this->DeclareNumericParameter(tree_system, systems::BasicVector(10)); +} + +template +void RigidBody::DoSetDefaultParameters( + systems::Parameters* parameters) const { + // Set the default spatial inertia. + systems::BasicVector& spatial_inertia_parameter = + parameters->get_mutable_numeric_parameter( + spatial_inertia_parameter_index_); + spatial_inertia_parameter.SetFrom( + internal::parameter_conversion::ToBasicVector( + default_spatial_inertia_.template cast())); +} + +template +void RigidBody::ThrowIfNotFinalized(const char* source_method) const { + DRAKE_THROW_UNLESS(this->has_parent_tree()); + if (!this->get_parent_tree().is_finalized()) { + throw std::runtime_error( + "From '" + std::string(source_method) + + "'. The model to which this rigid body belongs must be finalized. " + "See MultibodyPlant::Finalize()."); + } +} + template void RigidBody::SetCenterOfMassInBodyFrameNoModifyInertia( systems::Context* context, diff --git a/multibody/tree/rigid_body.h b/multibody/tree/rigid_body.h index 478d08d4a214..2317ff236b70 100644 --- a/multibody/tree/rigid_body.h +++ b/multibody/tree/rigid_body.h @@ -24,12 +24,20 @@ namespace drake { namespace multibody { -// Forward declaration for RigidBodyFrame. +// Forward declaration for RigidBodyFrame to use. template class RigidBody; -/// A %RigidBodyFrame is a material Frame that serves as the unique reference -/// frame for a RigidBody. +/// %Link is a synonym for the misnamed RigidBody class; prefer %Link in +/// internal code. When we combine welded-together links there are multiple +/// links fixed to a single rigid body (in the physics sense; what we call +/// a "mobilized body" or mobod). Saying that there is more than one +/// RigidBody on a rigid body is too confusing! +template +using Link = RigidBody; + +/// A %RigidBodyFrame (aka LinkFrame) is a material Frame that serves as the +/// unique reference frame for a RigidBody (aka %Link). /// /// Each %RigidBody B has a unique body frame for which we use the same symbol /// B (with meaning clear from context). We represent a body frame by a @@ -138,7 +146,8 @@ class RigidBodyFrame final : public Frame { const internal::MultibodyTree& tree_clone) const; }; -/// LinkFrame is a synonym for RigidBodyFrame and should be preferred. +/// LinkFrame is a synonym for RigidBodyFrame. Prefer %Link and %LinkFrame in +/// internal code. template using LinkFrame = RigidBodyFrame; @@ -148,17 +157,14 @@ using LinkFrame = RigidBodyFrame; namespace internal { template // Attorney-Client idiom to grant MultibodyTree access to a selected set of -// private methods in RigidBody. RigidBodyAttorney serves as a "proxy" to the -// RigidBody class but only providing an interface to a selected subset of +// private methods in RigidBody (Link). LinkAttorney serves as a "proxy" to the +// RigidBody class but only provides an interface to a selected subset of // methods that should be accessible to MultibodyTree. These methods are related // to the construction and finalize stage of the multibody system. -class RigidBodyAttorney { +class LinkAttorney { private: // MultibodyTree keeps a list of mutable pointers to each of the link frames // in the system and therefore it needs mutable access. - // Notice this method is private and therefore users do not have access to it - // even in the rare event they'd attempt to peek into the "internal::" - // namespace. static LinkFrame& get_mutable_link_frame(Link* link) { return link->get_mutable_link_frame(); } @@ -188,10 +194,14 @@ class RigidBodyAttorney { /// freedom obey the Newton-Euler equations of motion. However, within a /// MultibodyTree, a %RigidBody is *not* free in space; instead, it is assigned /// a limited number of degrees of freedom (0-6) with respect to its parent -/// body in the multibody tree by its Mobilizer (also called a -/// "tree joint" or "inboard joint"). Additional constraints on permissible -/// motion can be added using Constraint objects to remove more degrees of -/// freedom. +/// body in the multibody tree by its Mobilizer (also called a "tree joint" +/// or "inboard joint"). Additional constraints on permissible motion can be +/// added using Constraint objects to remove more degrees of freedom. +/// +/// @note This object corresponds to a "link" in urdf/sdformat terminology. We +/// may combine welded-together links into a composite rigid body internally. +/// We provide aliases Link/LinkIndex for RigidBody/BodyIndex to allow either +/// terminology to be used. /// /// - [Goldstein 2001] H Goldstein, CP Poole, JL Safko, Classical Mechanics /// (3rd Edition), Addison-Wesley, 2001. @@ -202,7 +212,7 @@ class RigidBody : public MultibodyElement { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RigidBody); - /// Constructs a %RigidBody named `body_name` with the given default + /// Constructs a %RigidBody (%Link) named `body_name` with the given default /// SpatialInertia. /// /// @param[in] body_name @@ -216,7 +226,7 @@ class RigidBody : public MultibodyElement { const std::string& body_name, const SpatialInertia& M_BBo_B = SpatialInertia::Zero()); - /// Constructs a %RigidBody named `body_name` with the given default + /// Constructs a %RigidBody (%Link) named `body_name` with the given default /// SpatialInertia. /// /// @param[in] body_name @@ -237,63 +247,48 @@ class RigidBody : public MultibodyElement { /// Returns this element's unique index. LinkIndex index() const { return this->template index_impl(); } - /// (Internal use only) Returns this Link's (RigidBody's) unique ordinal. - /// Currently identical to the index but will differ when we permit removal of + /// (Internal use only) Returns this %Link's unique ordinal. Currently + /// identical to the index but will differ when we permit removal of /// Links as we do for Joints. LinkOrdinal ordinal() const { return this->template ordinal_impl(); } - /// Gets the `name` associated with this rigid body. The name will never be - /// empty. + /// Gets the `name` associated with this %RigidBody (%Link). The name will + /// never be empty. const std::string& name() const { return name_; } - /// Returns scoped name of this body. Neither of the two pieces of the name - /// will be empty (the scope name and the element name). + /// Returns scoped name of this %RigidBody (%Link). Neither of the two + /// pieces of the name will be empty (the scope name and the element name). /// @throws std::exception if this element is not associated with a /// MultibodyPlant. ScopedName scoped_name() const; /// Returns a const reference to the associated LinkFrame (RigidBodyFrame). + /// link_frame() is synonymous with body_frame(). + /// @note "link" is the terminology used in urdf/sdformat. const LinkFrame& link_frame() const { return link_frame_; } - /// (Compatibility) A synonym for link_frame(). - const LinkFrame& body_frame() const { return link_frame(); } + /// Returns a const reference to the associated RigidBodyFrame (LinkFrame). + /// body_frame() is synonymous with link_frame(). + /// @note "link" is the terminology used in urdf/sdformat. + const RigidBodyFrame& body_frame() const { return link_frame(); } - /// For a floating base %RigidBody, lock its inboard joint. Its generalized - /// velocities will be 0 until it is unlocked. + // TODO(rpoyner-tri): consider extending the design to allow locking on + // non-floating bodies. + + /// For a floating base %RigidBody (%Link), lock its inboard joint. Its + /// generalized velocities will be 0 until it is unlocked. /// @throws std::exception if this body is not a floating base body. - void Lock(systems::Context* context) const { - ThrowIfNotFinalized(__func__); - // TODO(rpoyner-tri): consider extending the design to allow locking on - // non-floating bodies. - if (!is_floating_base_body()) { - // TODO(jwnimmer-tri) This code is not supposed to be inlined (GSG). - throw std::logic_error(fmt::format( - "Attempted to call Lock() on non-floating-base rigid body {}", - name())); - } - mobilizer().Lock(context); - } + void Lock(systems::Context* context) const; - /// For a floating base %RigidBody, unlock its inboard joint. + /// For a floating base %RigidBody (%Link), unlock its inboard joint. /// @throws std::exception if this body is not a floating base body. - void Unlock(systems::Context* context) const { - ThrowIfNotFinalized(__func__); - // TODO(rpoyner-tri): consider extending the design to allow locking on - // non-floating bodies. - if (!is_floating_base_body()) { - // TODO(jwnimmer-tri) This code is not supposed to be inlined (GSG). - throw std::logic_error(fmt::format( - "Attempted to call Unlock() on non-floating-base rigid body {}", - name())); - } - mobilizer().Unlock(context); - } + void Unlock(systems::Context* context) const; - /// Determines whether this %RigidBody is currently locked to its inboard - /// (parent) %RigidBody. This is not limited to floating base bodies but - /// generally Joint::is_locked() is preferable otherwise. + /// Determines whether this %RigidBody (%Link) is currently locked to its + /// inboard (parent) %RigidBody. This is not limited to floating base + /// bodies but generally Joint::is_locked() is preferable otherwise. /// @returns true if the body is locked, false otherwise. bool is_locked(const systems::Context& context) const { ThrowIfNotFinalized(__func__); @@ -301,15 +296,16 @@ class RigidBody : public MultibodyElement { return mobilizer().is_locked(context); } - /// (Advanced) Returns the index of the mobilized body ("mobod") in the - /// computational directed forest structure of the owning MultibodyTree to - /// which this %RigidBody belongs. This serves as the BodyNode index and the - /// index into all associated quantities. + /// (Internal use only) Returns the index of the mobilized body ("mobod") that + /// this %Link follows. This index serves as the BodyNode index and the + /// index into all associated quantities. More than one link may follow the + /// same mobod. internal::MobodIndex mobod_index() const { return mobilizer().index(); } - /// (Advanced) Returns `true` if this body is a _floating base body_, meaning - /// it had no explicit joint to a parent body so is mobilized by an - /// automatically-added (ephemeral) floating (6 dof) joint to World. + /// (Advanced) Returns `true` if this %RigidBody (%Link) is a _floating base + /// body_, meaning it had no explicit joint to a parent body and is + /// mobilized by an automatically-added (ephemeral) floating (6 dof) joint + /// to World. /// /// @note A floating base body is not necessarily modeled with a quaternion /// mobilizer, see has_quaternion_dofs(). Alternative options include a @@ -323,11 +319,11 @@ class RigidBody : public MultibodyElement { return is_floating_base_body_; } - /// (Advanced) If `true`, this body's generalized position coordinates q - /// include a quaternion, which occupies the first four elements of q. Note - /// that this does not imply that the body is floating base body since it may - /// have fewer than 6 dofs or its inboard body could be something other than - /// World. + /// (Advanced) If `true`, this %RigidBody's (%Link's) generalized position + /// coordinates q include a quaternion, which occupies the first four + /// elements of q. Note that this does not imply that the body is a floating + /// base body since it may have fewer than 6 dofs or its inboard body could + /// be something other than World. /// @throws std::exception if called pre-finalize /// @see is_floating_base_body(), MultibodyPlant::Finalize() bool has_quaternion_dofs() const { @@ -336,13 +332,13 @@ class RigidBody : public MultibodyElement { } /// (Advanced) For floating base bodies (see is_floating_base_body()), - /// returns the index of this %RigidBody's first generalized position in the - /// vector q of generalized position coordinates for a MultibodyPlant model. - /// Positions q for this %RigidBody are then contiguous starting at this - /// index. When a floating %RigidBody is modeled with quaternion coordinates - /// (see has_quaternion_dofs()), the four consecutive entries in the state - /// starting at this index correspond to the quaternion that parametrizes this - /// %RigidBody's orientation. + /// returns the index of this %RigidBody's (%Link's) first generalized + /// position in the vector q of generalized position coordinates for a + /// MultibodyPlant model. Positions q for this %RigidBody are then + /// contiguous starting at this index. When a floating %RigidBody is + /// modeled with quaternion coordinates (see has_quaternion_dofs()), the + /// four consecutive entries in the state starting at this index correspond + /// to the quaternion that parametrizes this %RigidBody's orientation. /// @throws std::exception if called pre-finalize /// @pre this is a floating base body /// @see is_floating_base_body(), has_quaternion_dofs() @@ -354,9 +350,10 @@ class RigidBody : public MultibodyElement { } /// (Advanced) For floating base bodies (see is_floating_base_body()), - /// returns the index of this %RigidBody's first generalized velocity in the - /// vector v of generalized velocities for a MultibodyPlant model. Velocities - /// v for this %RigidBody are then contiguous starting at this index. + /// returns the index of this %RigidBody's (%Link's) first generalized + /// velocity in the vector v of generalized velocities for a MultibodyPlant + /// model. Velocities v for this %RigidBody are then contiguous starting at + /// this index. /// @throws std::exception if called pre-finalize /// @pre this is a floating base body /// @see is_floating_base_body(), MultibodyPlant::Finalize() @@ -398,57 +395,59 @@ class RigidBody : public MultibodyElement { return mobilizer().velocity_suffix(velocity_index_in_body); } - /// Returns this %RigidBody's default mass, which is initially supplied at - /// construction when specifying this body's SpatialInertia. - /// @note In general, a rigid body's mass can be a constant property stored in - /// this rigid body's %SpatialInertia or a parameter that is stored in a + /// Returns this %RigidBody's (%Link's) default mass, which is initially + /// supplied at construction when specifying this body's SpatialInertia. + /// @note In general, a body's mass can be a constant property stored in + /// this body's %SpatialInertia or a parameter that is stored in a /// Context. The default constant mass value is used to initialize the mass /// parameter in the Context. double default_mass() const { return default_spatial_inertia_.get_mass(); } - /// Returns the default value of this %RigidBody's center of mass as measured - /// and expressed in its body frame. This value is initially supplied at - /// construction when specifying this body's SpatialInertia. - /// @retval p_BoBcm_B The position of this rigid body B's center of mass `Bcm` + /// Returns the default value of this %RigidBody's (%Link's) center of mass as + /// measured and expressed in its body frame. This value is initially + /// supplied at construction when specifying this body's SpatialInertia. + /// @retval p_BoBcm_B The position of this body B's center of mass `Bcm` /// measured from Bo (B's frame origin) and expressed in B (body B's frame). const Vector3& default_com() const { return default_spatial_inertia_.get_com(); } - /// Returns the default value of this body B's unit inertia about Bo (body B's - /// origin), expressed in B (this body's body frame). This value is initially - /// supplied at construction when specifying this body's SpatialInertia. + /// Returns the default value of this %RigidBody (%Link) B's unit inertia + /// about Bo (body B's origin), expressed in B (this body's body frame). This + /// value is initially supplied at construction when specifying this body's + /// SpatialInertia. /// @retval G_BBo_B rigid body B's unit inertia about Bo, expressed in B. const UnitInertia& default_unit_inertia() const { return default_spatial_inertia_.get_unit_inertia(); } - /// Gets the default value of this body B's rotational inertia about Bo - /// (B's origin), expressed in B (this body's body frame). This value is - /// calculated from the SpatialInertia supplied at construction of this body. + /// Gets the default value of this %RigidBody (%Link) B's rotational inertia + /// about Bo (B's origin), expressed in B (this body's body frame). This + /// value is calculated from the SpatialInertia supplied at construction of + /// this body. /// @retval I_BBo_B body B's rotational inertia about Bo, expressed in B. RotationalInertia default_rotational_inertia() const { return default_spatial_inertia_.CalcRotationalInertia(); } - /// Gets the default value of this body B's SpatialInertia about Bo - /// (B's origin) and expressed in B (this body's frame). + /// Gets the default value of this %RigidBody (%Link) B's SpatialInertia about + /// Bo (B's origin) and expressed in B (this body's frame). /// @retval M_BBo_B body B's spatial inertia about Bo, expressed in B. const SpatialInertia& default_spatial_inertia() const { return default_spatial_inertia_; } - /// Gets this body's mass from the given context. + /// Gets this %RigidBody's (%Link's) mass from the given context. /// @param[in] context contains the state of the multibody system. - /// @pre the context makes sense for use by this RigidBody. + /// @pre the context makes sense for use by this %RigidBody. const T& get_mass(const systems::Context& context) const { const systems::BasicVector& spatial_inertia_parameter = context.get_numeric_parameter(spatial_inertia_parameter_index_); return internal::parameter_conversion::GetMass(spatial_inertia_parameter); } - /// Returns the pose `X_WB` of this %RigidBody B in the world frame W as a - /// function of the state of the model stored in `context`. + /// Returns the pose `X_WB` of this %RigidBody (%Link) B in the world frame W + /// as a function of the state of the model stored in `context`. const math::RigidTransform& EvalPoseInWorld( const systems::Context& context) const { ThrowIfNotFinalized(__func__); @@ -456,7 +455,8 @@ class RigidBody : public MultibodyElement { return this->get_parent_tree().EvalLinkPoseInWorld(context, *this); } - /// Evaluates V_WB, this body B's SpatialVelocity in the world frame W. + /// Evaluates V_WB, this %RigidBody (%Link) B's SpatialVelocity in the world + /// frame W. /// @param[in] context Contains the state of the model. /// @retval V_WB_W this body B's spatial velocity in the world frame W, /// expressed in W (for point Bo, the body frame's origin). @@ -468,7 +468,8 @@ class RigidBody : public MultibodyElement { *this); } - /// Evaluates A_WB, this body B's SpatialAcceleration in the world frame W. + /// Evaluates A_WB, this %RigidBody (%Link) B's SpatialAcceleration in the + /// world frame W. /// @param[in] context Contains the state of the model. /// @retval A_WB_W this body B's spatial acceleration in the world frame W, /// expressed in W (for point Bo, the body's origin). @@ -483,8 +484,8 @@ class RigidBody : public MultibodyElement { *this); } - /// Gets the SpatialForce on this %RigidBody B from `forces` as F_BBo_W: - /// applied at body B's origin Bo and expressed in world frame W. + /// Gets the SpatialForce on this %RigidBody (%Link) B from `forces` as + /// F_BBo_W: applied at body B's origin Bo and expressed in world frame W. const SpatialForce& GetForceInWorld( const systems::Context&, const MultibodyForces& forces) const { ThrowIfNotFinalized(__func__); @@ -494,8 +495,8 @@ class RigidBody : public MultibodyElement { return forces.body_forces()[mobod_index()]; } - /// Adds the SpatialForce on this %RigidBody B, applied at body B's origin Bo - /// and expressed in the world frame W into `forces`. + /// Adds the SpatialForce on this %RigidBody (%Link) B, applied at body B's + /// origin Bo and expressed in the world frame W into `forces`. void AddInForceInWorld(const systems::Context&, const SpatialForce& F_Bo_W, MultibodyForces* forces) const { @@ -507,7 +508,7 @@ class RigidBody : public MultibodyElement { forces->mutable_body_forces()[mobod_index()] += F_Bo_W; } - /// Adds the SpatialForce on this %RigidBody B, applied at point P and + /// Adds the SpatialForce on this %RigidBody (%Link) B, applied at point P and /// expressed in a frame E into `forces`. /// @param[in] context /// The context containing the current state of the model. @@ -526,9 +527,10 @@ class RigidBody : public MultibodyElement { const SpatialForce& F_Bp_E, const Frame& frame_E, MultibodyForces* forces) const; - /// Gets this body's center of mass position from the given context. + /// Gets this %RigidBody's (%Link's) center of mass position from the given + /// context. /// @param[in] context contains the state of the multibody system. - /// @returns p_BoBcm_B position vector from Bo (this rigid body B's origin) + /// @returns p_BoBcm_B position vector from Bo (this body B's origin) /// to Bcm (B's center of mass), expressed in B. /// @pre the context makes sense for use by this %RigidBody. Vector3 CalcCenterOfMassInBodyFrame( @@ -539,16 +541,18 @@ class RigidBody : public MultibodyElement { spatial_inertia_parameter); } - /// Calculates Bcm's translational velocity in the world frame W. + /// Calculates %RigidBody (%Link) B's center of mass Bcm's translational + /// velocity in the world frame W. /// @param[in] context The context contains the state of the model. - /// @retval v_WBcm_W The translational velocity of Bcm (this rigid body's + /// @retval v_WBcm_W The translational velocity of Bcm (this body's /// center of mass) in the world frame W, expressed in W. Vector3 CalcCenterOfMassTranslationalVelocityInWorld( const systems::Context& context) const; - /// Calculates Bcm's translational acceleration in the world frame W. + /// Calculates %RigidBody (%Link) B's center of mass Bcm's translational + /// acceleration in the world frame W. /// @param[in] context The context contains the state of the model. - /// @retval a_WBcm_W The translational acceleration of Bcm (this rigid body's + /// @retval a_WBcm_W The translational acceleration of Bcm (this body's /// center of mass) in the world frame W, expressed in W. /// @note When cached values are out of sync with the state stored in context, /// this method performs an expensive forward dynamics computation, whereas @@ -556,7 +560,8 @@ class RigidBody : public MultibodyElement { Vector3 CalcCenterOfMassTranslationalAccelerationInWorld( const systems::Context& context) const; - /// Gets this body's spatial inertia about its origin from the given context. + /// Gets this %RigidBody's (%Link's) spatial inertia about its origin from the + /// given context. /// @param[in] context contains the state of the multibody system. /// @returns M_BBo_B spatial inertia of this rigid body B about Bo (B's /// origin), expressed in B. M_BBo_B contains properties related to B's mass, @@ -573,9 +578,10 @@ class RigidBody : public MultibodyElement { spatial_inertia_parameter); } - /// For this %RigidBody B, sets its mass stored in @p context to @p mass. + /// For this %RigidBody (%Link) B, sets its mass stored in `context` to + /// `mass`. /// @param[out] context contains the state of the multibody system. - /// @param[in] mass mass of this rigid body B. + /// @param[in] mass mass of this body B. /// @note This function changes this body B's mass and appropriately scales /// I_BBo_B (B's rotational inertia about Bo, expressed in B). /// @pre the context makes sense for use by this RigidBody. @@ -589,8 +595,8 @@ class RigidBody : public MultibodyElement { internal::parameter_conversion::SpatialInertiaIndex::k_mass, mass); } - /// (Advanced) Sets this body's center of mass position while preserving its - /// inertia about its body origin. + /// (Advanced) Sets this %RigidBody (%Link) B's center of mass position while + /// preserving its inertia about its _body origin_. /// @param[in, out] context contains the state of the multibody system. It is /// modified to store the updated com (center of mass position). /// @param[in] com position vector from Bo (this body B's origin) to Bcm @@ -608,8 +614,8 @@ class RigidBody : public MultibodyElement { SetCenterOfMassInBodyFrameNoModifyInertia(context, com); } - /// Sets this body's center of mass position while preserving its inertia - /// about its center of mass. + /// Sets this %RigidBody (%Link) B's center of mass position while preserving + /// its inertia about its _center of mass_. /// @param[in, out] context contains the state of the multibody system. It is /// modified to store the updated center_of_mass_position and the updated /// G_BBo_B (this body B's unit inertia about B's origin Bo, expressed in B). @@ -626,8 +632,8 @@ class RigidBody : public MultibodyElement { systems::Context* context, const Vector3& center_of_mass_position) const; - /// For this %RigidBody B, sets its SpatialInertia that is stored in - /// @p context to @p M_Bo_B. + /// For this %RigidBody (%Link) B, sets its SpatialInertia that is stored in + /// `context` to `M_Bo_B`. /// @param[out] context contains the state of the multibody system. /// @param[in] M_Bo_B spatial inertia of this rigid body B about Bo (B's /// origin), expressed in B. M_Bo_B contains properties related to B's mass, @@ -645,110 +651,112 @@ class RigidBody : public MultibodyElement { internal::parameter_conversion::ToBasicVector(M_Bo_B)); } - /// @name Methods to access position kinematics quantities. - /// The input PositionKinematicsCache to these methods must be in sync with - /// context. These method's APIs will be deprecated when caching arrives. + /// @name Internal methods to access position kinematics quantities. + /// (Internal use only) The input PositionKinematicsCache to these methods + /// must be in sync with context. ///@{ - /// (Advanced) Extract this body's pose in world (from the position + /// (Internal use only) Extract this link's pose in world (from the position /// kinematics). /// @param[in] pc position kinematics cache. - /// @retval X_WB pose of rigid body B in world frame W. + /// @retval X_WL pose of this %Link L in world frame W. const math::RigidTransform& get_pose_in_world( const internal::PositionKinematicsCache& pc) const { - return pc.get_X_WB(this->mobod_index()); + return pc.get_X_WL(this->ordinal()); } - /// (Advanced) Extract the RotationMatrix relating the world frame to this - /// body's frame. + /// (Internal use only) Extract the RotationMatrix relating the world frame to + /// this link's frame. /// @param[in] pc position kinematics cache. - /// @retval R_WB rotation matrix relating rigid body B in world frame W. + /// @retval R_WL rotation matrix relating world frame W and %Link L. const math::RotationMatrix get_rotation_matrix_in_world( const internal::PositionKinematicsCache& pc) const { return get_pose_in_world(pc).rotation(); } - /// (Advanced) Extract the position vector from world origin to this body's - /// origin, expressed in world. + /// (Internal use only) Extract the position vector from world origin to this + /// link's origin, expressed in world. /// @param[in] pc position kinematics cache. - /// @retval p_WoBo_W position vector from Wo (world origin) to - /// Bo (this body's origin) expressed in W (world). + /// @retval p_WoLo_W position vector from Wo (world origin) to + /// Lo (this link's origin) expressed in W (world). const Vector3 get_origin_position_in_world( const internal::PositionKinematicsCache& pc) const { return get_pose_in_world(pc).translation(); } ///@} - /// @name Methods to access velocity kinematics quantities. - /// The input VelocityKinematicsCache to these methods must be in sync with - /// context. These method's APIs will be deprecated when caching arrives. + /// @name Internal methods to access velocity kinematics quantities. + /// (Internal use only) The input VelocityKinematicsCache to these methods + /// must be in sync with context. ///@{ - /// (Advanced) Returns V_WB, this %RigidBody B's SpatialVelocity in + /// (Internal use only) Returns V_WL, this link L's SpatialVelocity in /// the world frame W. /// @param[in] vc velocity kinematics cache. - /// @retval V_WB_W this rigid body B's spatial velocity in the world - /// frame W, expressed in W (for point Bo, the body frame's origin). + /// @retval V_WL_W this link L's spatial velocity in the world + /// frame W, expressed in W (for point Lo, the link frame's origin). const SpatialVelocity& get_spatial_velocity_in_world( const internal::VelocityKinematicsCache& vc) const { - return vc.get_V_WB(this->mobod_index()); + return vc.get_V_WL(this->ordinal()); } - /// (Advanced) Extract this body's angular velocity in world, expressed in - /// world. + /// (Internal use only) Extract this link's angular velocity in world, + /// expressed in world. /// @param[in] vc velocity kinematics cache. - /// @retval w_WB_W rigid body B's angular velocity in world W, expressed in W. + /// @retval w_WL_W link L's angular velocity in world W, expressed in W. const Vector3& get_angular_velocity_in_world( const internal::VelocityKinematicsCache& vc) const { return get_spatial_velocity_in_world(vc).rotational(); } - /// (Advanced) Extract the velocity of this body's origin in world, expressed - /// in world. + /// (Internal use only) Extract the velocity of this link's origin in world, + /// expressed in world. /// @param[in] vc velocity kinematics cache. - /// @retval v_WBo_W velocity of Bo (body origin) in world W, expressed in W. + /// @retval v_WLo_W velocity of Lo (link origin) in world W, expressed in W. const Vector3& get_origin_velocity_in_world( const internal::VelocityKinematicsCache& vc) const { return get_spatial_velocity_in_world(vc).translational(); } ///@} - /// @name Methods to access acceleration kinematics quantities. - /// The input AccelerationKinematicsCache to these methods must be in sync - /// with context. These method APIs will be deprecated when caching arrives. + /// @name Internal methods to access acceleration kinematics quantities. + /// (Internal use only) The input AccelerationKinematicsCache to these + /// methods must be in sync with context. ///@{ - /// (Advanced) Returns A_WB, this %RigidBody B's SpatialAcceleration in - /// the world frame W. + // TODO(sherm1) Convert these to Link acceleration from Mobod. + + /// (Internal use only) Returns A_WL, this link L's SpatialAcceleration + /// in the world frame W. /// @param[in] ac acceleration kinematics cache. - /// @retval A_WB_W this rigid body B's spatial acceleration in the world - /// frame W, expressed in W (for point Bo, the body frame's origin). + /// @retval A_WL_W this link L's spatial acceleration in the world + /// frame W, expressed in W (for point Lo, the link frame's origin). const SpatialAcceleration& get_spatial_acceleration_in_world( const internal::AccelerationKinematicsCache& ac) const { return ac.get_A_WB(this->mobod_index()); } - /// (Advanced) Extract this body's angular acceleration in world, expressed - /// in world. + /// (Internal use only) Extract this link L's angular acceleration in world, + /// expressed in world. /// @param[in] ac velocity kinematics cache. - /// @retval alpha_WB_W B's angular acceleration in world W, expressed in W. + /// @retval alpha_WL_W L's angular acceleration in world W, expressed in W. const Vector3& get_angular_acceleration_in_world( const internal::AccelerationKinematicsCache& ac) const { return get_spatial_acceleration_in_world(ac).rotational(); } - /// (Advanced) Extract acceleration of this body's origin in world, expressed - /// in world. + /// (Internal use only) Extract acceleration of this link L's origin in + /// world, expressed in world. /// @param[in] ac acceleration kinematics cache. - /// @retval a_WBo_W acceleration of body origin Bo in world W, expressed in W. + /// @retval a_WLo_W acceleration of link origin Lo in world W, expressed in W. const Vector3& get_origin_acceleration_in_world( const internal::AccelerationKinematicsCache& ac) const { return get_spatial_acceleration_in_world(ac).translational(); } ///@} - /// (Advanced) This method is mostly intended to be called by - /// MultibodyTree::CloneToScalar(). Most users should not call this clone + /// (Internal use only) This method is intended to be called by + /// MultibodyTree::CloneToScalar(). Users should not call this clone /// method directly but rather clone the entire parent MultibodyTree if /// needed. /// @sa MultibodyTree::CloneToScalar() @@ -759,61 +767,23 @@ class RigidBody : public MultibodyElement { } private: - // Only friends of RigidBodyAttorney (i.e. MultibodyTree) have access to a + // Only friends of LinkAttorney (i.e. MultibodyTree) have access to a // selected set of private RigidBody methods. - friend class internal::RigidBodyAttorney; + friend class internal::LinkAttorney; // Called near the end of Finalize(). - void DoSetTopology() final { - DRAKE_DEMAND(mobilizer_ == nullptr); - const internal::MultibodyTree& tree = this->get_parent_tree(); - const internal::SpanningForest& forest = tree.forest(); - const internal::LinkJointGraph::Link& link = forest.link_by_index(index()); - mobilizer_ = &tree.get_mobilizer(link.mobod_index()); - - // Is this RigidBody the active link on its Mobod? - const bool is_active_link = - link.ordinal() == - forest.mobods(link.mobod_index()).active_link_ordinal(); - is_floating_base_body_ = - is_active_link && mobilizer_->is_floating_base_mobilizer(); - } + void DoSetTopology() final; // Implementation for MultibodyElement::DoDeclareParameters(). - void DoDeclareParameters( - internal::MultibodyTreeSystem* tree_system) final { - // Sets model values to dummy values to indicate that the model values are - // not used. This class stores the the default values of the parameters. - // 10 numeric values are used to store mass, center of mass, moments and - // products of inertia packed into one basic vector. - spatial_inertia_parameter_index_ = - this->DeclareNumericParameter(tree_system, systems::BasicVector(10)); - } + void DoDeclareParameters(internal::MultibodyTreeSystem* tree_system) final; // Implementation for MultibodyElement::DoSetDefaultParameters(). - void DoSetDefaultParameters(systems::Parameters* parameters) const final { - // Set the default spatial inertia. - systems::BasicVector& spatial_inertia_parameter = - parameters->get_mutable_numeric_parameter( - spatial_inertia_parameter_index_); - spatial_inertia_parameter.SetFrom( - internal::parameter_conversion::ToBasicVector( - default_spatial_inertia_.template cast())); - } + void DoSetDefaultParameters(systems::Parameters* parameters) const final; // Helper method for throwing an exception within public methods that should // not be called pre-finalize. The invoking method should pass its name so // that the error message can include that detail. - void ThrowIfNotFinalized(const char* source_method) const { - DRAKE_THROW_UNLESS(this->has_parent_tree()); - if (!this->get_parent_tree().is_finalized()) { - // TODO(jwnimmer-tri) This code is not supposed to be inlined (GSG). - throw std::runtime_error( - "From '" + std::string(source_method) + - "'. The model to which this rigid body belongs must be finalized. " - "See MultibodyPlant::Finalize()."); - } - } + void ThrowIfNotFinalized(const char* source_method) const; // For this RigidBody B, set its center of mass position stored in context // to center_of_mass_position, but does not modify other inertia properties. @@ -854,8 +824,7 @@ class RigidBody : public MultibodyElement { default_spatial_inertia_); } - // MultibodyTree has access to the mutable LinkFrame through - // RigidBodyAttorney. + // MultibodyTree has access to the mutable LinkFrame through LinkAttorney. LinkFrame& get_mutable_link_frame() { return link_frame_; } const internal::Mobilizer& mobilizer() const { @@ -863,23 +832,22 @@ class RigidBody : public MultibodyElement { return *mobilizer_; } - // A string identifying this link in its model. - // Within a MultibodyPlant model instance this string is guaranteed to be - // unique by MultibodyPlant's API. + // A string identifying this link in its model. Within a MultibodyPlant model + // instance this string is guaranteed to be unique by MultibodyPlant's API. const std::string name_; - // This link's LinkFrame (a.k.a. RigidBodyFrame). + // This link's LinkFrame (aka RigidBodyFrame). LinkFrame link_frame_; - // Spatial inertia about the body frame origin Bo, expressed in B. + // Spatial inertia about the link frame origin Lo, expressed in L. SpatialInertia default_spatial_inertia_; - // System parameter index for this body's SpatialInertia stored in a context. + // System parameter index for this link's SpatialInertia stored in a context. systems::NumericParameterIndex spatial_inertia_parameter_index_; // Below here, members are set at Finalize() via SetTopology(). - // The mobilizer of the Mobod that this body follows. + // The mobilizer of the Mobod that this link follows. const internal::Mobilizer* mobilizer_{}; // True if the mobilizer is a floating base mobilizer and this link is the @@ -887,15 +855,8 @@ class RigidBody : public MultibodyElement { bool is_floating_base_body_{false}; }; -/// Link is a synonym for the mis-named RigidBody class and should be -/// preferred. When we combine welded-together links there will be multiple -/// links fixed to a single rigid body (in the physics sense). Saying that -/// there is more than one RigidBody on a rigid body is too confusing! -template -using Link = RigidBody; - -/// (Compatibility) Prefer Link to Body, however this dispreferred alias -/// is available to permit older code to continue working. +/// (Compatibility) Use %Link or RigidBody instead of Body. An alias is +/// provided here to permit older code to continue working. template using Body = RigidBody; diff --git a/multibody/tree/test/tree_from_mobilizers_test.cc b/multibody/tree/test/tree_from_mobilizers_test.cc index 47f7883b5a37..491fdb413a56 100644 --- a/multibody/tree/test/tree_from_mobilizers_test.cc +++ b/multibody/tree/test/tree_from_mobilizers_test.cc @@ -631,7 +631,7 @@ class PendulumKinematicTests : public PendulumTests { // set the velocity kinematics cache entries to zero so that only tau_g(q) // gets computed (at least for this pendulum model that only includes // gravity and damping). - vc.InitializeToZero(); + vc.SetToZero(); // ====================================================================== // Compute position kinematics. diff --git a/multibody/tree/velocity_kinematics_cache.cc b/multibody/tree/velocity_kinematics_cache.cc index ecb1bdd20ec3..152af2a01bb8 100644 --- a/multibody/tree/velocity_kinematics_cache.cc +++ b/multibody/tree/velocity_kinematics_cache.cc @@ -1,4 +1,56 @@ #include "drake/multibody/tree/velocity_kinematics_cache.h" +#include "multibody_tree_indexes.h" + +namespace drake { +namespace multibody { +namespace internal { + +template +VelocityKinematicsCache::VelocityKinematicsCache( + const SpanningForest& forest) + : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) { + V_WB_pool_.resize(num_mobods_, SpatialVelocity::NaN()); + // Mobilizers are entitled to assume V_FM has been initialized to zero. + // For example, a Weld mobilizer can simply not write to V_FM at all, and + // others can take advantage of known zeroes. + V_FM_pool_.resize(num_mobods_, SpatialVelocity::Zero()); + V_PB_W_pool_.resize(num_mobods_, SpatialVelocity::NaN()); + V_WL_pool_.resize(num_links_, SpatialVelocity::NaN()); + + // For the World mobod, set values that won't ever change. + V_WB_pool_[world_mobod_index()].SetZero(); // World's velocity is zero. + V_WL_pool_[world_link_ordinal()].SetZero(); // " + V_FM_pool_[world_mobod_index()].SetNaN(); // It must never be used. + // V_PB_W_pool_[world] was set to NaN above. It must never be used. + + // Any links that are part of the World composite also have known zero + // velocities in World. + const SpanningForest::Mobod& world_mobod = forest.mobods(MobodIndex(0)); + const std::vector& world_followers = + world_mobod.follower_link_ordinals(); + DRAKE_DEMAND(world_followers[0] == world_link_ordinal()); // Done above. + for (size_t i = 1; i < world_followers.size(); ++i) { + const LinkOrdinal link_ordinal = world_followers[i]; + V_WL_pool_[link_ordinal].SetZero(); + } +} + +template +void VelocityKinematicsCache::SetToZero() { + for (MobodIndex mobod_index(0); mobod_index < num_mobods_; ++mobod_index) { + V_WB_pool_[mobod_index].SetZero(); + V_FM_pool_[mobod_index].SetZero(); + V_PB_W_pool_[mobod_index].SetZero(); + } + for (LinkOrdinal link_ordinal(0); link_ordinal < num_links_; ++link_ordinal) { + V_WL_pool_[link_ordinal].SetZero(); + } +} + +} // namespace internal +} // namespace multibody +} // namespace drake + DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class ::drake::multibody::internal::VelocityKinematicsCache); diff --git a/multibody/tree/velocity_kinematics_cache.h b/multibody/tree/velocity_kinematics_cache.h index 0c8fadcd701e..50398f7a3b50 100644 --- a/multibody/tree/velocity_kinematics_cache.h +++ b/multibody/tree/velocity_kinematics_cache.h @@ -49,35 +49,13 @@ class VelocityKinematicsCache { DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(VelocityKinematicsCache); // Constructs a velocity kinematics cache entry for the given - // SpanningForest. - // In Release builds specific entries are left uninitialized resulting in a - // zero cost operation. However, in Debug builds those entries are set to NaN - // so that operations using this uninitialized cache entry fail fast, easing - // bug detection. - explicit VelocityKinematicsCache(const SpanningForest& forest) - : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) { - Allocate(); - DRAKE_ASSERT_VOID(InitializeToNaN()); - // Sets defaults. - V_WB_pool_[world_mobod_index()].SetZero(); // World's velocity is zero. - V_WL_pool_[world_link_ordinal()].SetZero(); // " - V_FM_pool_[world_mobod_index()].SetNaN(); // It must never be used. - V_PB_W_pool_[world_mobod_index()].SetNaN(); // It must never be used. - } + // SpanningForest and initializes everything to NaN except for the velocities + // of World and World-fixed Links which are known to be zero forever. + explicit VelocityKinematicsCache(const SpanningForest& forest); - // Initializes `this` %VelocityKinematicsCache as if all generalized + // Sets `this` VelocityKinematicsCache as if all generalized // velocities of the corresponding MultibodyTree model were zero. - void InitializeToZero() { - for (MobodIndex mobod_index(0); mobod_index < num_mobods_; ++mobod_index) { - V_WB_pool_[mobod_index].SetZero(); - V_FM_pool_[mobod_index].SetZero(); - V_PB_W_pool_[mobod_index].SetZero(); - } - for (LinkOrdinal link_ordinal(0); link_ordinal < num_links_; - ++link_ordinal) { - V_WL_pool_[link_ordinal].SetZero(); - } - } + void SetToZero(); // Returns V_WB, body B's spatial velocity in the world frame W. // @param[in] mobod_index The unique index for the computational @@ -134,28 +112,6 @@ class VelocityKinematicsCache { } private: - // Allocates resources for this velocity kinematics cache. - void Allocate() { - V_WB_pool_.resize(num_mobods_); - V_FM_pool_.resize(num_mobods_); - V_PB_W_pool_.resize(num_mobods_); - V_WL_pool_.resize(num_links_); - } - - // Initializes all pools to have NaN values to ease bug detection when entries - // are accidentally left uninitialized. - void InitializeToNaN() { - for (MobodIndex mobod_index(0); mobod_index < num_mobods_; ++mobod_index) { - V_WB_pool_[mobod_index].SetNaN(); - V_FM_pool_[mobod_index].SetNaN(); - V_PB_W_pool_[mobod_index].SetNaN(); - } - for (LinkOrdinal link_ordinal(0); link_ordinal < num_links_; - ++link_ordinal) { - V_WL_pool_[link_ordinal].SetNaN(); - } - } - // Number of Mobods in the multibody forest, including the World mobod. int num_mobods_{0};