From a4a79e0d6704de3880a53f9621af61ed6c22ed1a Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Tue, 9 Jun 2026 16:06:18 -0700 Subject: [PATCH] Compute composite mass properties --- .../generated_docstrings/multibody_plant.h | 62 ++- .../generated_docstrings/multibody_tree.h | 55 +- multibody/plant/BUILD.bazel | 9 + multibody/plant/multibody_plant.cc | 12 + multibody/plant/multibody_plant.h | 57 ++- multibody/plant/test/composite_test.cc | 417 +++++++++++++++ multibody/tree/body_node_impl.cc | 22 +- multibody/tree/force_element.h | 49 +- multibody/tree/frame.h | 5 +- multibody/tree/frame_body_pose_cache.h | 8 + .../tree/linear_bushing_roll_pitch_yaw.h | 2 +- multibody/tree/multibody_forces.cc | 4 +- multibody/tree/multibody_forces.h | 30 +- multibody/tree/multibody_tree.cc | 474 +++++++++++++----- multibody/tree/multibody_tree.h | 10 + multibody/tree/position_kinematics_cache.cc | 54 ++ multibody/tree/position_kinematics_cache.h | 64 +-- 17 files changed, 1096 insertions(+), 238 deletions(-) create mode 100644 multibody/plant/test/composite_test.cc diff --git a/bindings/generated_docstrings/multibody_plant.h b/bindings/generated_docstrings/multibody_plant.h index 0b223a81b209..7f3dea2015be 100644 --- a/bindings/generated_docstrings/multibody_plant.h +++ b/bindings/generated_docstrings/multibody_plant.h @@ -3752,7 +3752,7 @@ method computes: where ``M(q)`` is the model's mass matrix (including rigid body mass properties and reflected_inertia "reflected inertias"), ``C(q, v)v`` is the bias term for Coriolis and gyroscopic effects and ``tau_app`` -consists of a vector applied generalized forces. The last term is a +consists of a vector of applied generalized forces. The last term is a summation over all bodies in the model where ``Fapp_Bo_W`` is an applied spatial force on body B at ``Bo`` which gets projected into the space of generalized forces with the transpose of ``Jv_V_WB(q)`` @@ -5011,7 +5011,7 @@ be used by Finalize(); post-finalize it returns the joint type that *was* used if there were any base bodies in need of a joint. See also: - SetBaseBodyJointType(), Finalize())"""; + SetBaseBodyJointType(), GetCombineWeldedBodies(), Finalize())"""; } GetBaseBodyJointType; // Symbol: drake::multibody::MultibodyPlant::GetBodiesKinematicallyAffectedBy struct /* GetBodiesKinematicallyAffectedBy */ { @@ -5168,6 +5168,25 @@ geometries for ``body`` previously registered with a SceneGraph. See also: RegisterCollisionGeometry(), Finalize())"""; } GetCollisionGeometriesForBody; + // Symbol: drake::multibody::MultibodyPlant::GetCombineWeldedBodies + struct /* GetCombineWeldedBodies */ { + // Source: drake/multibody/plant/multibody_plant.h + const char* doc = +R"""((Internal use only for now) Returns the global or a model_instance +setting for whether or not to combine welded RigidBody (Link) +elements. + +Note: + This function can be called pre-Finalize() or post-Finalize(). + +Parameter ``model_instance``: + (optional). If this argument is missing or not recognized, returns + the global setting. Otherwise returns the setting for this + specific model_instance. + +See also: + SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize())"""; + } GetCombineWeldedBodies; // Symbol: drake::multibody::MultibodyPlant::GetConstraintActiveStatus struct /* GetConstraintActiveStatus */ { // Source: drake/multibody/plant/multibody_plant.h @@ -6641,8 +6660,45 @@ Parameter ``model_instance``: RuntimeError if called after Finalize(). See also: - GetBaseBodyJointType(), Finalize())"""; + GetBaseBodyJointType(), SetCombineWeldedBodies(), Finalize())"""; } SetBaseBodyJointType; + // Symbol: drake::multibody::MultibodyPlant::SetCombineWeldedBodies + struct /* SetCombineWeldedBodies */ { + // Source: drake/multibody/plant/multibody_plant.h + const char* doc = +R"""((Internal use only for now) Controls whether welded-together RigidBody +(Link) elements are to be combined into a single composite mobilized +body in the generated model. If so, those Weld joints will not be +modeled (i.e. will have no corresponding mobilizer) in the +post-Finalize() model and there will be fewer mobilized bodies and +modeled joints in the generated model than in the user's specification +of links and joints. Results for the original RigidBody (Link) +elements can still be obtained by name or BodyIndex, but no results +(in particular, no reaction forces) are available for the unmodeled +Weld joints. + +You can set this flag globally or on a per-model instance basis. If +there is a setting for a joint's model instance then that setting is +used; otherwise, the global setting is used. + +The default global setting for Drake is *not* to combine welded +RigidBody elements. + +Parameter ``combine``: + Whether to combine welded-together bodies. This only affects a + particular model instance if the ``model_instance`` argument is + also provided, otherwise it sets the global value. + +Parameter ``model_instance``: + (optional) if present, specifies a particular model instance to + which the ``combine`` argument applies. + +Raises: + RuntimeError if called after Finalize(). + +See also: + GetCombineWeldedBodies(), SetBaseBodyJointType(), Finalize())"""; + } SetCombineWeldedBodies; // Symbol: drake::multibody::MultibodyPlant::SetConstraintActiveStatus struct /* SetConstraintActiveStatus */ { // Source: drake/multibody/plant/multibody_plant.h diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index 77e9871e3144..a25290b18dd8 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -1956,14 +1956,14 @@ R"""(Returns the owning MultibodyPlant LeafSystem. // Source: drake/multibody/tree/force_element.h const char* doc = R"""(A ForceElement allows modeling state and time dependent forces in a -MultibodyTree model. Examples of such forces are springs, dampers, +MultibodyPlant model. Examples of such forces are springs, dampers, drag and gravity. Forces that depend on accelerations such as virtual mass cannot be modeled with a ForceElement. This abstract class provides an API that all force elements subclasses must implement in order to be fully defined. These are: - CalcAndAddForceContribution(): computes the force contribution of a force - element in a MultibodyTree model. + element in a MultibodyPlant model. - CalcPotentialEnergy(): computes a force element potential energy contribution. - CalcConservativePower(): computes the power generated by conservative @@ -1980,7 +1980,7 @@ model, different force elements may write into the array of spatial forces ``F_B_W`` or the array of generalized forces ``tau``. Parameter ``context``: - The context containing the state of the MultibodyTree model. + The context containing the state of the MultibodyPlant model. Parameter ``pc``: A position kinematics cache object already updated to be in sync @@ -1995,7 +1995,7 @@ Parameter ``forces``: output ``this`` force element adds its contribution into ``forces``. This method will abort if the ``forces`` pointer is null or if the forces object is not compatible with ``this`` - MultibodyTree, see MultibodyForces∷CheckInvariants(). + MultibodyPlant, see MultibodyForces∷CheckInvariants(). Precondition: The position kinematics ``pc`` must have been previously updated @@ -2076,6 +2076,13 @@ parameters of this method. It assumes ``forces`` to be a valid pointer to a MultibodyForces object compatible with the MultibodyTree model owning ``this`` force element. +Note: + Although spatial forces are applied to *links*, it is the + responsibility of the implementation to shift those forces to the + mobilized bodies that the links follow. When we are combining + welded-together links into composites, there can be multiple links + on the same mobilized body. + Precondition: The position kinematics ``pc`` must have been previously updated with a call to CalcPositionKinematicsCache(). @@ -4596,16 +4603,16 @@ class documentation states that a Space-fixed (extrinsic) X-Y-Z rotation with roll-pitch-yaw angles [q₀ q₁ q₂] is equivalent to a Body-fixed (intrinsic) Z-Y-X rotation by yaw-pitch-roll angles [q₂ q₁ q₀]. In the context of "gimbal torques", the Body-fixed Z-Y-X rotation -sequence with angles [q₂ q₁ q₀] is physical meaningful as it produces -torques associated with successive frames in a gimbal as τ₂ 𝐀𝐳, τ₁ 𝐏𝐲, -τ₀ 𝐂𝐱, where each of 𝐀𝐳, 𝐏𝐲, 𝐂𝐱 are unit vectors associated with a -frame in the yaw-pitch-roll rotation sequence and 𝐏𝐲 is a unit vector -of the "pitch" intermediate frame. As described earlier, torque 𝐭 is -the moment of the bushing forces on frame C about Cp. Scalars tx, ty, -tz are defined so 𝐭 can be expressed ``𝐭 = [tx ty tz]ᴀ = tx 𝐀𝐱 + ty 𝐀𝐲 -+ tz 𝐀𝐳``. As shown in code documentation, the relationship of [tx ty -tz] to [τ₀ τ₁ τ₂] was found by equating 𝐭's power to τ's power as 𝐭 ⋅ -w_AC = τ ⋅ q̇. +sequence with angles [q₂ q₁ q₀] is physically meaningful as it +produces torques associated with successive frames in a gimbal as τ₂ +𝐀𝐳, τ₁ 𝐏𝐲, τ₀ 𝐂𝐱, where each of 𝐀𝐳, 𝐏𝐲, 𝐂𝐱 are unit vectors associated +with a frame in the yaw-pitch-roll rotation sequence and 𝐏𝐲 is a unit +vector of the "pitch" intermediate frame. As described earlier, torque +𝐭 is the moment of the bushing forces on frame C about Cp. Scalars tx, +ty, tz are defined so 𝐭 can be expressed ``𝐭 = [tx ty tz]ᴀ = tx 𝐀𝐱 + +ty 𝐀𝐲 + tz 𝐀𝐳``. As shown in code documentation, the relationship of +[tx ty tz] to [τ₀ τ₁ τ₂] was found by equating 𝐭's power to τ's power +as 𝐭 ⋅ w_AC = τ ⋅ q̇. .. raw:: html @@ -5438,11 +5445,11 @@ that is added during Finalize() should set this flag to ``True``.)"""; struct /* MultibodyForces */ { // Source: drake/multibody/tree/multibody_forces.h const char* doc = -R"""(A class to hold a set of forces applied to a MultibodyTree system. -Forces can include generalized forces as well as body spatial forces. -MultibodyPlant∷CalcGeneralizedForces() can be used to compute the -*total* generalized force, combining generalized_forces() and -body_forces().)"""; +R"""(Holds a set of forces applied to a MultibodyTree system. Forces can +include generalized forces as well as mobilized body (mobod) spatial +forces. MultibodyPlant∷CalcGeneralizedForces() can be used to compute +the *total* generalized force, combining generalized_forces() with the +generalized force equivalent of the body_forces().)"""; // Symbol: drake::multibody::MultibodyForces::AddInForces struct /* AddInForces */ { // Source: drake/multibody/tree/multibody_forces.h @@ -5475,9 +5482,13 @@ abort.)"""; const char* doc_1args_model = R"""((Advanced) Tree overload.)"""; // Source: drake/multibody/tree/multibody_forces.h const char* doc_2args_nb_nv = -R"""(Number of bodies and number of generalized velocities overload. This -constructor is useful for constructing the MultibodyForces structure -before a MultibodyPlant has been constructed.)"""; +R"""((Advanced) Constructs a force object with a given number of mobilized +bodies and corresponding mobilities (number of generalized +velocities). This constructor may be useful for constructing the +MultibodyForces structure before a MultibodyPlant has been +constructed. However, this must be used cautiously since the number of +mobilized bodies and their mobilities are not known until after +Finalize().)"""; } ctor; // Symbol: drake::multibody::MultibodyForces::SetZero struct /* SetZero */ { diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index 722439343c47..64e8f68807fa 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -458,6 +458,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "composite_test", + deps = [ + ":plant", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", + ], +) + drake_cc_googletest( name = "compliant_contact_manager_scalar_conversion_test", deps = [ diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 2f5fb17bc9b8..0dfda4b20940 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1422,12 +1422,24 @@ void MultibodyPlant::SetBaseBodyJointType( mutable_tree().SetBaseBodyJointType(joint_type, model_instance); } +template +void MultibodyPlant::SetCombineWeldedBodies( + bool combine, std::optional model_instance) { + mutable_tree().SetCombineWeldedBodies(combine, model_instance); +} + template BaseBodyJointType MultibodyPlant::GetBaseBodyJointType( std::optional model_instance) const { return internal_tree().GetBaseBodyJointType(model_instance); } +template +bool MultibodyPlant::GetCombineWeldedBodies( + std::optional model_instance) const { + return internal_tree().GetCombineWeldedBodies(model_instance); +} + template void MultibodyPlant::Finalize() { // After finalizing the base class, tree is read-only. diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index d40cb90fb476..4ad6e82101c4 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -1759,11 +1759,39 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// @param[in] model_instance (optional) the index of the model instance to /// which `joint_type` is to be applied. /// @throws std::exception if called after Finalize(). - /// @see GetBaseBodyJointType(), Finalize() + /// @see GetBaseBodyJointType(), SetCombineWeldedBodies(), Finalize() void SetBaseBodyJointType( BaseBodyJointType joint_type, std::optional model_instance = {}); + /// (Internal use only for now) Controls whether welded-together RigidBody + /// (Link) elements are to be combined into a single composite mobilized + /// body in the generated model. If so, those Weld joints will not be modeled + /// (i.e. will have no corresponding mobilizer) in the post-Finalize() + /// model and there will be fewer mobilized bodies and modeled joints in the + /// generated model than in the user's specification of links and joints. + /// Results for the original RigidBody (Link) elements can still be obtained + /// by name or BodyIndex, but no results (in particular, no reaction forces) + /// are available for the unmodeled Weld joints. + /// + /// You can set this flag globally or on a per-model instance basis. If + /// there is a setting for a joint's model instance then that setting is + /// used; otherwise, the global setting is used. + /// + /// The default global setting for Drake is _not_ to combine welded RigidBody + /// elements. + /// + /// @param[in] combine Whether to combine welded-together bodies. This only + /// affects a particular model instance if the `model_instance` argument is + /// also provided, otherwise it sets the global value. + /// @param[in] model_instance (optional) if present, specifies a particular + /// model instance to which the `combine` argument applies. + /// + /// @throws std::exception if called after Finalize(). + /// @see GetCombineWeldedBodies(), SetBaseBodyJointType(), Finalize() + void SetCombineWeldedBodies( + bool combine, std::optional model_instance = {}); + /// Returns the currently-set choice for base body joint type, either for /// the global setting or for a specific model instance if provided. /// If a model instance is provided for which no explicit choice has been @@ -1772,10 +1800,22 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// This can be called any time -- pre-finalize it returns the joint type /// that will be used by Finalize(); post-finalize it returns the joint type /// that _was_ used if there were any base bodies in need of a joint. - /// @see SetBaseBodyJointType(), Finalize() + /// @see SetBaseBodyJointType(), GetCombineWeldedBodies(), Finalize() BaseBodyJointType GetBaseBodyJointType( std::optional model_instance = {}) const; + /// (Internal use only for now) Returns the global or a model_instance setting + /// for whether or not to combine welded RigidBody (Link) elements. + /// + /// @note This function can be called pre-Finalize() or post-Finalize(). + /// + /// @param[in] model_instance (optional). If this argument is missing or + /// not recognized, returns the global setting. Otherwise returns the + /// setting for this specific model_instance. + /// @see SetCombineWeldedBodies(), GetBaseBodyJointType(), Finalize() + bool GetCombineWeldedBodies( + std::optional model_instance = {}) const; + /// This method must be called after all elements in the model (joints, /// bodies, force elements, constraints, etc.) are added and before any /// computations are performed. @@ -4142,12 +4182,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// where `M(q)` is the model's mass matrix (including rigid body mass /// properties and @ref reflected_inertia "reflected inertias"), `C(q, v)v` is /// the bias term for Coriolis and gyroscopic effects and `tau_app` consists - /// of a vector applied generalized forces. The last term is a summation over - /// all bodies in the model where `Fapp_Bo_W` is an applied spatial force on - /// body B at `Bo` which gets projected into the space of generalized forces - /// with the transpose of `Jv_V_WB(q)` (where `Jv_V_WB` is B's spatial - /// velocity Jacobian in W with respect to generalized velocities v). - /// Note: B's spatial velocity in W can be written as `V_WB = Jv_V_WB * v`. + /// of a vector of applied generalized forces. The last term is a summation + /// over all bodies in the model where `Fapp_Bo_W` is an applied spatial + /// force on body B at `Bo` which gets projected into the space of + /// generalized forces with the transpose of `Jv_V_WB(q)` (where `Jv_V_WB` + /// is B's spatial velocity Jacobian in W with respect to generalized + /// velocities v). Note: B's spatial velocity in W can be written as `V_WB + /// = Jv_V_WB * v`. /// /// This method does not compute explicit expressions for the mass matrix nor /// for the bias term, which would be of at least `O(n²)` complexity, but it diff --git a/multibody/plant/test/composite_test.cc b/multibody/plant/test/composite_test.cc new file mode 100644 index 000000000000..c6ebcbad614c --- /dev/null +++ b/multibody/plant/test/composite_test.cc @@ -0,0 +1,417 @@ +/* Tests that mass properties are identical whether welded-together links are +modeled with explicit weld joints or combined into a single composite +mobilized body. The test builds two identical models that differ only in whether +SetCombineWeldedBodies() is enabled. */ + +#include +#include +#include +#include + +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/rigid_body.h" +#include "drake/multibody/tree/rotational_inertia.h" +#include "drake/multibody/tree/spatial_inertia.h" +#include "drake/multibody/tree/weld_joint.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace multibody { +namespace { + +using math::RigidTransformd; +using math::RotationMatrixd; +using systems::Context; + +// Tolerance for numerical comparisons. +constexpr double kTolerance = 32 * std::numeric_limits::epsilon(); + +// Holds one version of the test model (either explicit welds or composites) +// along with its context, ready for kinematics queries. +struct TestModel { + std::unique_ptr> plant; + std::unique_ptr> context; + const RevoluteJoint* revolute{}; + const RigidBody* link1{}; + const RigidBody* link2{}; + const RigidBody* link3{}; + const RigidBody* link4{}; +}; + +/* Builds a test model with the topology: + + Link3 + ^ + | y + [weld] ^ + | | + Link1 --[weld]--> Link2 +----> x + / / + [revolute z] (angle θ) z + / + World --[weld]--> Link4 + +Link2 is offset +1 m in x from Link1's frame. +Link3 is offset +1 m in y from Link2's frame. +Link4 is offset +4 m in x from World frame. + +The positions of the link origins from World origin Wo, expressed in World are: + Link1: (0, 0, 0) + Link2: (cos θ, sin θ, 0) + Link3: (cos θ − sin θ, sin θ + cos θ, 0) + Link4: (4, 0, 0) + +With combine_welded_bodies = false, five mobilized bodies are created, +(World, Link1 via revolute, Link2 via weld, Link3 via weld, Link 4 via weld). +With combine_welded_bodies = true two mobilized bodies are created, +(World with link4 and one composite mobilized body with Link1, Link2, Link3). */ +TestModel MakeModel(bool combine_welded_bodies) { + TestModel m; + m.plant = std::make_unique>(0.0 /* continuous */); + m.plant->SetCombineWeldedBodies(combine_welded_bodies); + + // To facilitate an analytical solution, each link has a trivial inertia, + // namely a 1 kg solid cube, 0.1 m per side. + const SpatialInertia M = + SpatialInertia::SolidCubeWithMass(1.0, 0.1); + + // Add the four links. + m.link1 = &m.plant->AddRigidBody("Link1", M); + m.link2 = &m.plant->AddRigidBody("Link2", M); + m.link3 = &m.plant->AddRigidBody("Link3", M); + m.link4 = &m.plant->AddRigidBody("Link4", M); + + // Revolute joint (z-axis): World to Link1, with Link1 frame at world frame. + m.revolute = &m.plant->AddJoint( + "revolute", m.plant->world_body(), RigidTransformd{}, *m.link1, + RigidTransformd{}, Vector3::UnitZ()); + + // Weld Link2 to Link1, with Link2's joint frame offset +1 m in x from Link1. + const RigidTransformd X_1to2(Vector3(1.0, 0.0, 0.0)); + m.plant->AddJoint("weld12", *m.link1, X_1to2, *m.link2, + RigidTransformd{}, RigidTransformd{}); + + // Weld Link3 to Link2, with Link3's joint frame offset +1 m in y from Link2. + // Using y (not x) avoids a linear layout that could mask transform bugs. + const RigidTransformd X_2to3(Vector3(0.0, 1.0, 0.0)); + m.plant->AddJoint("weld23", *m.link2, X_2to3, *m.link3, + RigidTransformd{}, RigidTransformd{}); + + // Weld Link4 to World, with Link4's joint frame offset +4 m in x from World. + const RigidTransformd X_Wto4(Vector3(4.0, 0.0, 0.0)); + m.plant->AddJoint("weldW4", m.plant->world_body(), X_Wto4, + *m.link4, RigidTransformd{}, RigidTransformd{}); + m.plant->Finalize(); + m.context = m.plant->CreateDefaultContext(); + + // Sanity check: Both models should have the same number of bodies (links), + // joints, and number of states (albeit different number of mobilized bodies). + EXPECT_EQ(m.plant->num_bodies(), 5); // World + 4 links. + EXPECT_EQ(m.plant->num_joints(), 4); // 1 revolute + 3 welds. + 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(), combine_welded_bodies ? 2 : 5); + EXPECT_EQ(tree.num_mobilizers(), combine_welded_bodies ? 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. + + // Sanity check: Some information in the SpanningForest should be the same, + // whether or not the welded links are combined. + const internal::SpanningForest& forest = tree.forest(); + const internal::SpanningForest::Mobod& mobod_0 = + forest.mobods(internal::MobodIndex(0)); + const internal::SpanningForest::Mobod& mobod_1 = + forest.mobods(internal::MobodIndex(1)); + EXPECT_TRUE(mobod_0.is_world()); // World is mobod(0) and LinkOrdinal(0). + EXPECT_TRUE(mobod_1.is_base_body()); // Connects to World via revolute joint. + EXPECT_EQ(mobod_0.active_link_ordinal(), LinkOrdinal(0)); + EXPECT_EQ(mobod_1.active_link_ordinal(), LinkOrdinal(1)); + + // Ensure Mobods (mobilized bodies) have the proper follower link ordinals. + // Each Mobod should have an active link ordinal. If welded links have + // been combined, then there are additional follower link ordinals. + if (combine_welded_bodies) { + EXPECT_EQ(mobod_0.follower_link_ordinals(), + (std::vector{LinkOrdinal(0), LinkOrdinal(4)})); + EXPECT_EQ(mobod_1.follower_link_ordinals(), + (std::vector{LinkOrdinal(1), LinkOrdinal(2), LinkOrdinal(3)})); + } else { + EXPECT_EQ(mobod_0.follower_link_ordinals(), (std::vector{LinkOrdinal(0)})); + EXPECT_EQ(mobod_1.follower_link_ordinals(), (std::vector{LinkOrdinal(1)})); + } + return m; +} + +// Sets the revolute joint angle (q) and angular velocity (v) in the model. +void SetState(const TestModel& m, double angle_rad, double angular_vel) { + m.revolute->set_angle(m.context.get(), angle_rad); + m.revolute->set_angular_rate(m.context.get(), angular_vel); +} + +/* Ensure the composite mobilized body's combined spatial inertia for Link123 +(links 1, 2, 3) is computed correctly. 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 explicit-weld and composite + models at several configurations. + (b) The mass matrix matches the analytically computed value. + +Analytical derivation +--------------------- +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. + +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 +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 composite is 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² */ +GTEST_TEST(CompositeTest, CompositeSpatialInertia) { + const TestModel explicit_model = MakeModel(false /* no combining */); + const TestModel composite_model = MakeModel(true /* combine welds */); + + const double m = 1.0, a = 0.1; // mass m and side-length a of solid cubes. + const Frame& world_frame = explicit_model.plant->world_frame(); + const RigidBody* explicit_links[] = { + explicit_model.link1, explicit_model.link2, explicit_model.link3, + explicit_model.link4}; + const RigidBody* composite_links[] = { + composite_model.link1, composite_model.link2, composite_model.link3, + composite_model.link4}; + + // The mass matrix is configuration-independent for this model (see above), + // but we check at several angles to guard against future changes. + const std::vector angles = {0.0, M_PI / 6, M_PI / 4, -M_PI / 3}; + for (double angle : angles) { + SetState(explicit_model, angle, 0.0); + SetState(composite_model, angle, 0.0); + + // Verify Link123's summed spatial inertia does not depend on whether they + // are a composite body. + SpatialInertia M_EWo_W = explicit_model.plant->CalcSpatialInertia( + *explicit_model.context, world_frame, + {explicit_model.link1->index(), explicit_model.link2->index(), + explicit_model.link3->index()}); + SpatialInertia M_CWo_W = composite_model.plant->CalcSpatialInertia( + *composite_model.context, world_frame, + {composite_model.link1->index(), composite_model.link2->index(), + composite_model.link3->index()}); + EXPECT_TRUE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(), + M_CWo_W.CopyToFullMatrix6(), kTolerance, + MatrixCompareType::relative)) + << "Link123 spatial inertia mismatch at angle = " << angle; + + // Ensure that individual link spatial inertias are reported correctly + // regardless of whether they were fused. + for (int i = 0; i < 4; ++i) { + const RigidBody* explicit_linki = explicit_links[i]; + const RigidBody* composite_linki = composite_links[i]; + M_EWo_W = explicit_model.plant->CalcSpatialInertia( + *explicit_model.context, world_frame, {explicit_linki->index()}); + M_CWo_W = composite_model.plant->CalcSpatialInertia( + *composite_model.context, world_frame, {composite_linki->index()}); + EXPECT_TRUE(CompareMatrices(M_EWo_W.CopyToFullMatrix6(), + M_CWo_W.CopyToFullMatrix6(), kTolerance, + MatrixCompareType::relative)) + << "Spatial inertia mismatch: link" << i + 1 + << " at angle = " << angle; + + // Since link4 is welded to world, special-case calculations are used. For + // this special case, also compare link4 results to an analytical value. + if (i == 3) { + const Vector3 p_WoL4o_W(4.0, 0.0, 0.0); + SpatialInertia M_L4Wo_W_expected = + SpatialInertia::SolidCubeWithMass(m, a).Shift(-p_WoL4o_W); + EXPECT_TRUE(CompareMatrices(M_L4Wo_W_expected.CopyToFullMatrix6(), + M_CWo_W.CopyToFullMatrix6(), kTolerance, + MatrixCompareType::relative)) + << "Inaccurate link4 spatial inertia at angle = " << angle; + } + } + + // Ensure the mass matrix does not depend on welded links being combined. + MatrixX M_explicit(1, 1), M_composite(1, 1); + explicit_model.plant->CalcMassMatrix(*explicit_model.context, &M_explicit); + composite_model.plant->CalcMassMatrix(*composite_model.context, + &M_composite); + EXPECT_TRUE(CompareMatrices(M_explicit, M_composite, kTolerance, + MatrixCompareType::relative)) + << "Mass matrix mismatch at angle = " << angle; + + // Ensure the mass matrix matches the analytical value. + const double Izz = m * a * a / 6.0; // Izz of one cube about its COM. + const double M_expected = (Izz + m * 0.0) + // Link1: d² = 0 + (Izz + m * 1.0) + // Link2: d² = 1² = 1 + (Izz + m * 2.0); // Link3: d² = √2² = 2 + EXPECT_NEAR(M_composite(0, 0), M_expected, kTolerance) + << "Mass matrix analytical mismatch at angle = " << angle; + } +} + +/* Tests that CalcFrameBodyPoses() computes the correct composite mass +properties by exercising every code path (including reverse welds): +- Pass 1: non-identity frame poses X_LF, where link frame L and frame F are + both rigidly attached to the same body (so X_LF is constant). When a + weld joint is NOT reversed, F is the inboard frame on the parent body, whereas + for a reversed weld, F is the inboard frame on the child body. +- Pass 2: poses X_BL, where mobod (mobilized body) frame B and link frame L + are both rigidly attached to the same body (so X_BL is constant). If L is the + composite body's "active" link, frame B is frame L and X_BL is identity. In + general, X_BL is non-identity for each "follower" link L welded into the + composite body B. +- Pass 3: mass property accumulation with both shifting and re-expressing. + +The strategy is to build two versions of the same physical system -- one with +composites enabled and one without -- then verify that the mass matrix and +gravity generalized forces agree. The mass matrix depends on M_BBo_B (the +composite spatial inertia computed in Pass 3), while gravity forces also +depend on p_BoLcm_B (each follower link's center of mass offset from Bo). + +Topology: + LinkC + ^ + | (reversed weld: child=LinkB, parent=LinkC) + | + LinkA --[normal weld]--> LinkB + / + [revolute y] + / + World +*/ +GTEST_TEST(CompositeTest, CalcFrameBodyPosesAllPaths) { + // Normal (not reversed) weld between parent=LinkA → child=LinkB. + // Weld joint parent frame Jp is offset from LinkA's frame A by + // (+0.5, 0, +0.3) with a 30-degree rotation about x. + // Weld joint child frame Jc is offset from LinkB's frame B by (0, +0.2, 0). + // X_JpJc = identity (coincident joint parent Jp and joint child Jp frames). + const RigidTransformd X_AJp(RotationMatrixd::MakeXRotation(M_PI / 6), + Vector3(0.5, 0.0, 0.3)); + const RigidTransformd X_BJc(Vector3(0.0, 0.2, 0.0)); + const RigidTransformd X_JpJc_AB; // Identity transform. + + // Reversed weld between parent=LinkC → child=LinkB (but LinkB is inboard). + // "Reversed" means linkB is the inboard link and LinkC is the outboard + // link. Weld joint parent frame Jp is offset from LinkC's frame C by (0, 0, + // +0.4). Weld joint child frame Jc is offset from LinkB's frame B by (0.6, + // 0, 0). X_JpJc = translation(0.1, 0.2, 0.3) -- frames Jp and Jc are not + // coincident. + const RigidTransformd X_CJp(Vector3(0.0, 0.0, 0.4)); + const RigidTransformd X_BJc2(Vector3(0.6, 0.0, 0.0)); + const RigidTransformd X_JpJc_CB(Vector3(0.1, 0.2, 0.3)); + + // For robust testing, each link has a distinct spatial inertia with + // non-zero products of inertia and non-zero center of mass offsets. + const SpatialInertia M_AAo_A = + SpatialInertia::MakeFromCentralInertia( + 2.0, Vector3(0.1, -0.05, 0.02), + RotationalInertia(0.03, 0.05, 0.04, 0.001, -0.002, 0.0015)); + const SpatialInertia M_BBo_B = + SpatialInertia::MakeFromCentralInertia( + 1.5, Vector3(-0.08, 0.04, 0.06), + RotationalInertia(0.05, 0.06, 0.04, -0.001, 0.002, 0.001)); + const SpatialInertia M_CCo_C = + SpatialInertia::MakeFromCentralInertia( + 3.0, Vector3(0.0, 0.1, -0.07), + RotationalInertia(0.08, 0.04, 0.07, 0.002, 0.001, -0.003)); + + // --- Helper lambda to build a model. --- + auto make_model = [&](bool combine) { + auto plant = std::make_unique>(0.0); + plant->SetCombineWeldedBodies(combine); + + const auto& linkA = plant->AddRigidBody("LinkA", M_AAo_A); + const auto& linkB = plant->AddRigidBody("LinkB", M_BBo_B); + const auto& linkC = plant->AddRigidBody("LinkC", M_CCo_C); + + // Revolute: World → LinkA about y. + plant->AddJoint("revolute", plant->world_body(), + RigidTransformd{}, linkA, RigidTransformd{}, + Vector3::UnitY()); + + // Normal weld: LinkA(parent) → LinkB(child). + plant->AddJoint("weld_AB", linkA, X_AJp, linkB, X_BJc, + X_JpJc_AB); + + // Reversed weld: LinkC(parent) → LinkB(child). Because LinkB is already + // connected to the tree via LinkA, the spanning forest will traverse this + // joint from LinkB (inboard) to LinkC (outboard), making it "reversed". + plant->AddJoint("weld_CB", linkC, X_CJp, linkB, X_BJc2, + X_JpJc_CB); + + plant->Finalize(); + auto context = plant->CreateDefaultContext(); + + return std::pair{std::move(plant), std::move(context)}; + }; + + // --- Build both models. --- + auto [plant_nc, context_nc] = make_model(false); // no composites + auto [plant_c, context_c] = make_model(true); // composites + + const auto& tree_nc = GetInternalTree(*plant_nc); + const auto& tree_c = GetInternalTree(*plant_c); + + // Sanity check: Both models should have the same number of bodies (World + + // 3 links), number of joints (1 revolute + 2 welds), and number of states + // (1 revolute angle and 1 revolute angular rate), but they should differ in + // the number of mobilized bodies. + EXPECT_EQ(plant_nc->num_bodies(), plant_c->num_bodies()); + EXPECT_EQ(plant_nc->num_joints(), plant_c->num_joints()); + EXPECT_EQ(plant_nc->num_positions(), plant_c->num_positions()); + EXPECT_EQ(plant_nc->num_velocities(), plant_c->num_velocities()); + ASSERT_EQ(tree_nc.num_mobods(), 4); // World + LinkA + LinkB + LinkC. + ASSERT_EQ(tree_c.num_mobods(), + 2); // World + composite body (links A, B, C). + + const auto& rev_nc = plant_nc->GetJointByName("revolute"); + const auto& rev_c = plant_c->GetJointByName("revolute"); + + for (const double angle : {M_PI / 5, -M_PI / 3}) { + rev_nc.set_angle(context_nc.get(), angle); + rev_c.set_angle(context_c.get(), angle); + + // The mass matrix (1×1) should agree between the two models. This + // directly validates that CalcFrameBodyPoses produced the correct + // composite body inertia (M_BBo_B), since the mass matrix is computed + // from it. + MatrixX mass_nc(1, 1), mass_c(1, 1); + plant_nc->CalcMassMatrix(*context_nc, &mass_nc); + plant_c->CalcMassMatrix(*context_c, &mass_c); + EXPECT_TRUE(CompareMatrices(mass_nc, mass_c, kTolerance, + MatrixCompareType::relative)) + << "Mass matrix mismatch at angle = " << angle; + + // Gravity generalized forces should agree (exercises AccumulateGravity + // which depends on p_BoLcm_B computed in CalcFrameBodyPoses). + const VectorX tau_g_nc = + plant_nc->CalcGravityGeneralizedForces(*context_nc); + const VectorX tau_g_c = + plant_c->CalcGravityGeneralizedForces(*context_c); + EXPECT_TRUE(CompareMatrices(tau_g_nc, tau_g_c, kTolerance, + MatrixCompareType::relative)) + << "Gravity generalized forces mismatch at angle = " << angle; + } +} + +} // namespace +} // namespace multibody +} // namespace drake diff --git a/multibody/tree/body_node_impl.cc b/multibody/tree/body_node_impl.cc index 7b2902edf474..1efa2e938808 100644 --- a/multibody/tree/body_node_impl.cc +++ b/multibody/tree/body_node_impl.cc @@ -113,9 +113,25 @@ void BodyNodeImpl::CalcPositionKinematicsCache_BaseToTip( break; } - // TODO(sherm1) Calculate X_WL for composites. Currently we don't make - // composites so X_WL = X_WB if L is the link on B. - pc->SetX_WL(mobilizer_->mobod().active_link_ordinal(), X_WB); + // Set X_WL for the active (most-inboard) link of this mobod. Its link frame + // 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); + + // For composite 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ₒ + if (mobod.is_composite()) { + const auto& followers = mobod.follower_link_ordinals(); + // 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); + } + } // Compute shift vector p_PoBo_W from the parent origin to the body origin. const Vector3& p_PoBo_P = X_PB.translation(); diff --git a/multibody/tree/force_element.h b/multibody/tree/force_element.h index f9dba46e4e01..14cb9c060452 100644 --- a/multibody/tree/force_element.h +++ b/multibody/tree/force_element.h @@ -17,14 +17,14 @@ namespace drake { namespace multibody { /// A %ForceElement allows modeling state and time dependent forces in a -/// MultibodyTree model. Examples of such forces are springs, dampers, drag and +/// MultibodyPlant model. Examples of such forces are springs, dampers, drag and /// gravity. Forces that depend on accelerations such as virtual mass cannot be -/// modeled with a %ForceElement. -/// This abstract class provides an API that all force elements subclasses must -/// implement in order to be fully defined. These are: +/// modeled with a %ForceElement. This abstract class provides an API that +/// all force elements subclasses must implement in order to be fully defined. +/// These are: /// /// - CalcAndAddForceContribution(): computes the force contribution of a force -/// element in a %MultibodyTree model. +/// element in a MultibodyPlant model. /// - CalcPotentialEnergy(): computes a force element potential energy /// contribution. /// - CalcConservativePower(): computes the power generated by conservative @@ -55,7 +55,7 @@ class ForceElement : public MultibodyElement { /// `F_B_W` or the array of generalized forces `tau`. /// /// @param[in] context - /// The context containing the state of the %MultibodyTree model. + /// The context containing the state of the %MultibodyPlant model. /// @param[in] pc /// A position kinematics cache object already updated to be in sync with /// `context`. @@ -66,7 +66,7 @@ class ForceElement : public MultibodyElement { /// A pointer to a valid, non nullptr, multibody forces object. /// On output `this` force element adds its contribution into `forces`. /// This method will abort if the `forces` pointer is null or if the - /// forces object is not compatible with `this` %MultibodyTree, see + /// forces object is not compatible with `this` %MultibodyPlant, see /// MultibodyForces::CheckInvariants(). /// /// @pre The position kinematics `pc` must have been previously updated with a @@ -101,7 +101,7 @@ class ForceElement : public MultibodyElement { /// arguments of the methods in this group. /// // TODO(amcastro-tri): make this methods DoCalcFoo() when caching gets in and - // make the public API's to only take a Context. + // make the public API's to only take a Context. //@{ /// (Advanced) Calculates the potential energy currently stored given the @@ -125,10 +125,9 @@ class ForceElement : public MultibodyElement { const internal::PositionKinematicsCache& pc) const = 0; /// (Advanced) Calculates and returns the power generated by conservative - /// force elements - /// or zero if `this` force element is non-conservative. This quantity is - /// defined to be positive when the potential energy is decreasing. In other - /// words, if `PE` is the potential energy as defined by + /// force elements or zero if `this` force element is non-conservative. + /// This quantity is defined to be positive when the potential energy is + /// decreasing. In other words, if `PE` is the potential energy as defined by /// CalcPotentialEnergy(), then the conservative power, `Pc`, is /// `Pc = -d(PE)/dt`. /// @@ -139,12 +138,10 @@ class ForceElement : public MultibodyElement { const internal::VelocityKinematicsCache& vc) const = 0; /// (Advanced) Calculates the rate at which mechanical energy is being - /// generated - /// (positive) or dissipated (negative) *other than* by conversion between - /// potential and kinetic energy. Integrating this quantity yields work W, - /// and the total energy `E = PE + KE - W` should be - /// conserved by any physically-correct model, to within integration accuracy - /// of W. + /// generated (positive) or dissipated (negative) *other than* by conversion + /// between potential and kinetic energy. Integrating this quantity yields + /// work W, and the total energy `E = PE + KE - W` should be conserved by + /// any physically-correct model, to within integration accuracy of W. /// @see CalcConservativePower() virtual T CalcNonConservativePower( const systems::Context& context, @@ -178,9 +175,15 @@ class ForceElement : public MultibodyElement { /// CalcAndAddForceContributions() which will already have error-checked /// the parameters so you don't have to. Refer to the documentation for /// CalcAndAddForceContribution() for details describing the purpose and - /// parameters of this method. - /// It assumes `forces` to be a valid pointer to a MultibodyForces object - /// compatible with the MultibodyTree model owning `this` force element. + /// parameters of this method. It assumes `forces` to be a valid pointer to + /// a MultibodyForces object compatible with the MultibodyTree model owning + /// `this` force element. + /// + /// @note Although spatial forces are applied to _links_, it is the + /// responsibility of the implementation to shift those forces to + /// the mobilized bodies that the links follow. When we are combining + /// welded-together links into composites, there can be multiple links + /// on the same mobilized body. /// /// @pre The position kinematics `pc` must have been previously updated with a /// call to CalcPositionKinematicsCache(). @@ -271,9 +274,7 @@ class ForceElement : public MultibodyElement { virtual std::unique_ptr> DoShallowClone() const; private: - // Implementation for MultibodyElement::DoSetTopology(). - // At MultibodyTree::Finalize() time, each force element retrieves its - // topology from the parent MultibodyTree. + // No-op implementation for MultibodyElement::DoSetTopology(). void DoSetTopology() final {} // Implementation for MultibodyElement::DoDeclareParameters(). diff --git a/multibody/tree/frame.h b/multibody/tree/frame.h index 8a0c22acd23b..c2c28a6c8412 100644 --- a/multibody/tree/frame.h +++ b/multibody/tree/frame.h @@ -44,9 +44,8 @@ using Link = RigidBody; /// Subclasses derived from %Frame differ in how kinematic calculations are /// performed. For example, the angular velocity of a FixedOffsetFrame or /// LinkFrame (RigidBodyFrame) is identical to the angular velocity of its -/// underlying body, -/// whereas the translational velocity of a FixedOffsetFrame differs from that -/// of a LinkFrame. +/// underlying body, whereas the translational velocity of a FixedOffsetFrame +/// differs from that of a LinkFrame. /// /// %Frame provides methods for obtaining its current orientation, position, /// motion, etc. from a Context passed to those methods. diff --git a/multibody/tree/frame_body_pose_cache.h b/multibody/tree/frame_body_pose_cache.h index 4850f2e11155..71c6e921f771 100644 --- a/multibody/tree/frame_body_pose_cache.h +++ b/multibody/tree/frame_body_pose_cache.h @@ -179,6 +179,14 @@ class FrameBodyPoseCache { p_BoLcm_B_pool_[ordinal] = p_BoLcm_B; } + // Add in the inertia contribution from link L, after shifting to Bo and + // re-expressing in B. + void AddToM_BBo_B(MobodIndex index, const SpatialInertia& M_LBo_B) { + // This method is only called when parameters change. + DRAKE_DEMAND(0 <= index && index < ssize(M_BBo_B_pool_)); + M_BBo_B_pool_[index] += M_LBo_B; + } + private: // Sizes are set on construction. diff --git a/multibody/tree/linear_bushing_roll_pitch_yaw.h b/multibody/tree/linear_bushing_roll_pitch_yaw.h index de02e42b2c00..552ab48e8640 100644 --- a/multibody/tree/linear_bushing_roll_pitch_yaw.h +++ b/multibody/tree/linear_bushing_roll_pitch_yaw.h @@ -189,7 +189,7 @@ class RigidBody; /// X-Y-Z rotation with roll-pitch-yaw angles [q₀ q₁ q₂] is equivalent to a /// Body-fixed (intrinsic) Z-Y-X rotation by yaw-pitch-roll angles [q₂ q₁ q₀]. /// In the context of "gimbal torques", the Body-fixed Z-Y-X rotation sequence -/// with angles [q₂ q₁ q₀] is physical meaningful as it produces torques +/// with angles [q₂ q₁ q₀] is physically meaningful as it produces torques /// associated with successive frames in a gimbal as τ₂ 𝐀𝐳, τ₁ 𝐏𝐲, τ₀ 𝐂𝐱, /// where each of 𝐀𝐳, 𝐏𝐲, 𝐂𝐱 are unit vectors associated with a frame in the /// yaw-pitch-roll rotation sequence and 𝐏𝐲 is a unit vector of the "pitch" diff --git a/multibody/tree/multibody_forces.cc b/multibody/tree/multibody_forces.cc index 42e2641c88e7..89dc548eddd4 100644 --- a/multibody/tree/multibody_forces.cc +++ b/multibody/tree/multibody_forces.cc @@ -7,7 +7,7 @@ namespace multibody { template MultibodyForces::MultibodyForces(const internal::MultibodyTree& model) - : MultibodyForces(model.num_links(), model.num_velocities()) { + : MultibodyForces(model.num_mobods(), model.num_velocities()) { DRAKE_DEMAND(model.is_finalized()); } @@ -42,7 +42,7 @@ template bool MultibodyForces::CheckHasRightSizeForModel( const internal::MultibodyTree& model) const { return model.num_velocities() == num_velocities() && - model.num_links() == num_bodies(); + model.num_mobods() == num_bodies(); } template diff --git a/multibody/tree/multibody_forces.h b/multibody/tree/multibody_forces.h index bd5f5ab3c6a8..1e6e5eaeb8eb 100644 --- a/multibody/tree/multibody_forces.h +++ b/multibody/tree/multibody_forces.h @@ -17,10 +17,11 @@ template class MultibodyTreeSystem; } // namespace internal -/// A class to hold a set of forces applied to a MultibodyTree system. Forces -/// can include generalized forces as well as body spatial forces. -/// MultibodyPlant::CalcGeneralizedForces() can be used to compute the _total_ -/// generalized force, combining generalized_forces() and body_forces(). +/// Holds a set of forces applied to a MultibodyTree system. Forces +/// can include generalized forces as well as mobilized body (mobod) spatial +/// forces. MultibodyPlant::CalcGeneralizedForces() can be used to compute +/// the _total_ generalized force, combining generalized_forces() with the +/// generalized force equivalent of the body_forces(). /// /// @tparam_default_scalar template @@ -30,17 +31,19 @@ class MultibodyForces { /// Constructs a force object to store a set of forces to be applied to /// the multibody model for `plant`. Forces are initialized to zero, meaning - /// no forces are applied. - /// `plant` must have been already finalized with + /// no forces are applied. `plant` must have been already finalized with /// MultibodyPlant::Finalize() or this constructor will abort. explicit MultibodyForces(const internal::MultibodyTreeSystem& plant); /// (Advanced) Tree overload. explicit MultibodyForces(const internal::MultibodyTree& model); - /// Number of bodies and number of generalized velocities overload. This - /// constructor is useful for constructing the MultibodyForces structure - /// before a MultibodyPlant has been constructed. + /// (Advanced) Constructs a force object with a given number of mobilized + /// bodies and corresponding mobilities (number of generalized velocities). + /// This constructor may be useful for constructing the MultibodyForces + /// structure before a MultibodyPlant has been constructed. However, this + /// must be used cautiously since the number of mobilized bodies and their + /// mobilities are not known until after Finalize(). MultibodyForces(int nb, int nv); ~MultibodyForces(); @@ -88,13 +91,14 @@ class MultibodyForces { bool CheckHasRightSizeForModel(const internal::MultibodyTree& model) const; private: - // Vector holding, for each body in the MultibodyTree, the externally applied - // force F_Bi_W on the i-th body Bi, expressed in the world frame W. - // Store by MobodIndex order. + // Vector holding, for each mobilized body (mobod) in the MultibodyTree, the + // externally applied force F_Bi_W on the i-th mobod Bi, expressed in the + // world frame W, and assumed to act at Bi's origin Bio. Indexed by + // MobodIndex. std::vector> F_B_W_; // Vector of generalized forces applied on each mobilizer in the - // MultibodyTree. + // MultibodyTree. Ordered identically to generalized velocities v. VectorX tau_; }; diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 701deb1f53f5..69f78b95b13f 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -939,6 +939,34 @@ void MultibodyTree::SetBaseBodyJointType( } } +template +void MultibodyTree::SetCombineWeldedBodies( + bool combine, std::optional model_instance) { + DRAKE_THROW_UNLESS(!is_finalized()); + LinkJointGraph& graph = mutable_graph(); + + // Obtain the current option flag to preserve the ones that don't have + // to do with combining rigid bodies. + ForestBuildingOptions options = + model_instance.has_value() + ? graph.get_forest_building_options_in_use(*model_instance) + : graph.get_global_forest_building_options(); + + // Clear the existing setting. This leaves us with the default which is _not_ + // to combine welded bodies. + options = options & ~ForestBuildingOptions::kOptimizeWeldedLinksAssemblies; + + if (combine) { + options = options | ForestBuildingOptions::kOptimizeWeldedLinksAssemblies; + } + + if (model_instance.has_value()) { + graph.SetForestBuildingOptions(*model_instance, options); + } else { + graph.SetGlobalForestBuildingOptions(options); + } +} + template BaseBodyJointType MultibodyTree::GetBaseBodyJointType( std::optional model_instance) const { @@ -953,6 +981,18 @@ BaseBodyJointType MultibodyTree::GetBaseBodyJointType( return BaseBodyJointType::kQuaternionFloatingJoint; } +template +bool MultibodyTree::GetCombineWeldedBodies( + std::optional model_instance) const { + const ForestBuildingOptions options = + model_instance.has_value() + ? graph().get_forest_building_options_in_use(*model_instance) + : graph().get_global_forest_building_options(); + + return static_cast( + options & ForestBuildingOptions::kOptimizeWeldedLinksAssemblies); +} + template void MultibodyTree::Finalize() { DRAKE_MBT_THROW_IF_FINALIZED(); @@ -1365,12 +1405,28 @@ void MultibodyTree::CalcPositionKinematicsCache( const systems::Context& context, PositionKinematicsCache* pc) const { DRAKE_DEMAND(pc != nullptr); + // Ensure parameter-dependent quantities are up to date. const FrameBodyPoseCache& frame_body_pose_cache = EvalFrameBodyPoses(context); + // Update parameter-dependent quantities in the position cache if they are + // not up to date with respect to the FrameBodyPoseCache in the context + // (which we just updated). PositionKinematicsCache needs the + // FrameBodyPoseCache serial number to make that determination. + const systems::CacheEntryValue& fbpc_value = + tree_system_->frame_body_poses_cache_entry().get_cache_entry_value( + context); + const int64_t fbpc_serial_number = fbpc_value.serial_number(); + pc->PrecomputeWorldCompositeIfNeeded(forest(), frame_body_pose_cache, + fbpc_serial_number); + const Eigen::VectorBlock> q_block = get_positions(context); const T* q = q_block.data(); + // The world composite link poses X_WL are independent of q and are set once + // the link pose parameters are known (i.e. when the FrameBodyPoseCache is + // filled in.) So we can skip the World mobod here. + // With the kinematics information across mobilizers and the kinematics // information for each body, we are now in position to perform a base-to-tip // recursion to update world positions and parent to child body transforms. @@ -1504,18 +1560,17 @@ void MultibodyTree::CalcSpatialInertiasInWorld( // Skip the world. // TODO(joemasterjohn): Consider an optimization to avoid calculating spatial // inertias for locked floating bodies. - for (LinkIndex link_index(1); link_index < num_links(); ++link_index) { - const Link& link = get_link(link_index); - const RigidTransform& X_WB = pc.get_X_WB(link.mobod_index()); + for (MobodIndex mobod_index(1); mobod_index < num_mobods(); ++mobod_index) { + const RigidTransform& X_WB = pc.get_X_WB(mobod_index); // Orientation of Mobod B in W. const RotationMatrix& R_WB = X_WB.rotation(); // Spatial inertia of Mobod B about Bo and expressed in the body frame B. const SpatialInertia& M_BBo_B = - frame_body_pose_cache.get_M_BBo_B(link.mobod_index()); + frame_body_pose_cache.get_M_BBo_B(mobod_index); // Re-express body B's spatial inertia in the world frame W. - SpatialInertia& M_BBo_W = (*M_B_W_all)[link.mobod_index()]; + SpatialInertia& M_BBo_W = (*M_B_W_all)[mobod_index]; M_BBo_W = M_BBo_B; // Wrong frame. M_BBo_W.ReExpressInPlace(R_WB); // Fixed. } @@ -1559,8 +1614,12 @@ void MultibodyTree::CalcFrameBodyPoses( DRAKE_ASSERT(frame_body_poses->get_X_BF(FrameIndex(0)).IsExactlyIdentity()); DRAKE_ASSERT(frame_body_poses->get_X_FB(FrameIndex(0)).IsExactlyIdentity()); - // The first pass locates each frame with respect to the body frame B - // of the body to which it is fixed. + // Pass 1 (over all frames): locate each frame F by its pose X_LF with respect + // to the link frame L of the link (RigidBody) to which F is rigidly attached. + // Note: Frame F is fixed to L, but its pose X_LF may be parameter-dependent + // and may be updated at runtime. + // Note: Link frame L is directly or indirectly rigidly connected to a + // mobilized body (Mobod) B with known pose X_BL. Hence X_BF = X_BL * X_LF. for (const Frame* frame : frames_.elements()) { // TODO(sherm1) Note that we're unnecessarily recalculating the parent // and ancestor poses. Likely OK since we expect short sequences and @@ -1569,35 +1628,151 @@ void MultibodyTree::CalcFrameBodyPoses( // order (or memoizing) so we don't have to recalculate. const RigidTransform X_LF = frame->CalcPoseInBodyFrame(context); frame_body_poses->SetX_LF(frame->index(), X_LF); + } + + // Pass 2 (over all mobods): find the pose X_BL of each Link frame L on its + // Mobod B. This is normally identity except if B is a composite mobod and + // Link L is not its "active" (most inboard) link. + std::vector got_X_BL(num_links(), false); + for (const SpanningForest::Mobod& mobod : forest().mobods()) { + // Always set the pose X_BL of the active link L on its mobod B to identity. + const LinkOrdinal active_link_ordinal = mobod.active_link_ordinal(); + frame_body_poses->SetX_BL(active_link_ordinal, + RigidTransform::Identity()); + got_X_BL[active_link_ordinal] = true; + if (!mobod.is_composite()) continue; + + // Continue with the "follower" links in mobod B, which should be in + // inboard ⇒ outboard order (starting after the "active" link that we just + // did above). For each follower link Lₒ we should be able to find an + // attached weld joint where the other (inboard) link Lᵢ already has + // calculated its X_BLᵢ. We use the weld's transform X_JᵢJₒ and connected + // frame poses X_LᵢJᵢ and X_LₒJₒ (just calculated above) to compute + // X_BLₒ = X_BLᵢ * X_LᵢJᵢ * X_JᵢJₒ * (X_LₒJₒ)⁻¹ + // This is complicated by the fact that the weld joint is expressed as + // connecting a parent link P and child link C. Usually Lᵢ=P and Lₒ=C but + // the joint may be reversed such that Lᵢ=C and Lₒ=P. + for (const LinkOrdinal& link_ordinal : mobod.follower_link_ordinals()) { + if (got_X_BL[link_ordinal]) continue; // Already calculated. + const LinkJointGraph::Link& link_Lo = graph().links(link_ordinal); + + // Search for the weld joint connecting Lₒ to an inboard link Lᵢ. + for (JointIndex joint_index : link_Lo.joints()) { + const LinkJointGraph::Joint& graph_joint = + graph().joint_by_index(joint_index); + if (!graph_joint.is_weld()) continue; // Wrong joint type (not a weld). + + // We have a weld joint, but it may connect Lₒ to an outboard link which + // has not yet been processed (its X_BL has not yet been computed). If + // so skip this weld joint and look for an inboard link Lᵢ whose X_BLᵢ + // is already computed (X_BLᵢ is needed to compute X_BLₒ). + const LinkIndex link_Li_index = + graph_joint.other_link_index(link_Lo.index()); + const LinkJointGraph::Link& link_Li = + graph().link_by_index(link_Li_index); + if (!got_X_BL[link_Li.ordinal()]) continue; // Lᵢ not yet processed. + + // Found the weld joint connecting inboard Lᵢ (X_BLᵢ known) to + // outboard Lₒ. Retrieve X_BLᵢ for use below in calculating X_BLₒ. + const RigidTransform& X_BLi = + frame_body_poses->get_X_BL(link_Li.ordinal()); + + // The weld joint stores pose X_JpJc (joint parent frame Jp to joint + // child frame Jc). If the joint is reversed (Lₒ is the parent link + // rather than child link) Jᵢ=Jc and Jₒ=Jp; otherwise Jᵢ=Jp and Jₒ=Jc. + const WeldJoint& weld_joint = + dynamic_cast&>(get_joint(joint_index)); + const bool is_reversed = + graph_joint.child_link_index() != link_Lo.index(); + const Frame& frame_Ji = is_reversed ? weld_joint.frame_on_child() + : weld_joint.frame_on_parent(); + const Frame& frame_Jo = is_reversed ? weld_joint.frame_on_parent() + : weld_joint.frame_on_child(); + + // Get the relevant X_LF frame poses that were calculated in Pass 1. + const RigidTransform& X_LiJi = + frame_body_poses->get_X_LF(frame_Ji.index()); + const RigidTransform& X_LoJo = + frame_body_poses->get_X_LF(frame_Jo.index()); + + // Get X_JᵢJₒ to form X_BLₒ = X_BLᵢ * X_LᵢJᵢ * X_JᵢJₒ * (X_LₒJₒ)⁻¹ + const math::RigidTransform& X_JpJc = weld_joint.X_FM(); + // Since a weld joint's X_FM() is constant, it returns a rigid transform + // of type double (not type T). Hence it needs to be typecast to . + const RigidTransform X_JiJo = + (is_reversed ? X_JpJc.inverse() : X_JpJc).cast(); + const RigidTransform X_BLo = + X_BLi * X_LiJi * X_JiJo * X_LoJo.inverse(); + frame_body_poses->SetX_BL(link_ordinal, X_BLo); + got_X_BL[link_ordinal] = true; + break; // Done with this follower link. + } + } + } - // TODO(sherm1) When we support composites, X_BF ≠ X_LF. - frame_body_poses->SetX_BF(frame->index(), X_LF); + // TODO(sherm1) Pass 2b could be removed or Debug-only. + // Pass 2b (over all links): sanity check that Pass 2 got X_BL for every link. + for (const Link* link : links_.elements()) { + DRAKE_DEMAND(got_X_BL[link->ordinal()]); } - // TODO(sherm1) Currently X_BL is always identity so we don't need to set - // it here. When we support composites, we'll need to set it here as well. + // Pass 3 (over all frames again): Calculate the pose X_BF for each frame F + // in the Mobod (mobilized body) frame B as X_BF = X_BL * X_LF. + // Note: Frame F is fixed to link L and link L is fixed to B, but pose X_LF + // may be parameter-dependent. Hence X_BF may need to be updated at runtime. + for (const Frame* frame : frames_.elements()) { + const RigidTransform& X_LF = frame_body_poses->get_X_LF(frame->index()); + const Link& link = frame->body(); + const RigidTransform& X_BL = frame_body_poses->get_X_BL(link.ordinal()); + frame_body_poses->SetX_BF(frame->index(), X_BL * X_LF); + } - // For every mobilized body, precalculate its body-frame spatial inertia - // M_BBo_B from the parameterization of that inertia. + // Pass 4 (over all mobods): For every mobod, precalculate its body-frame + // spatial inertia M_BBo_B by shifting and accumulating the spatial inertia + // M_LLo_L of each of its "follower" links. For a non-composite mobod there is + // only one follower link. For a composite, multiple follower links are welded + // together as a rigid unit. + // We also calculate and save p_BoLcm_B, the offset from mobod origin Bo to + // each of its follower links' centers of mass, expressed in B. for (const SpanningForest::Mobod& mobod : forest().mobods()) { - if (mobod.is_world()) continue; - // TODO(sherm1) Can't handle optimized WeldedLinksAssemblies yet. - DRAKE_DEMAND(ssize(mobod.follower_link_ordinals()) == 1); - const Mobilizer& mobilizer = get_mobilizer(mobod.index()); - - // Get the parameterized spatial inertia. - const Link& link = mobilizer.outboard_body(); - const SpatialInertia M_LLo_L = - link.CalcSpatialInertiaInBodyFrame(context); - frame_body_poses->SetM_LLo_L(link.ordinal(), M_LLo_L); - - const Vector3& p_LoLcm_L = M_LLo_L.get_com(); - const RigidTransform& X_BL = frame_body_poses->get_X_BL(link.ordinal()); - const Vector3 p_BoLcm_B = X_BL * p_LoLcm_L; - frame_body_poses->Set_p_BoLcm_B(link.ordinal(), p_BoLcm_B); + // Initialize each non-world Mobod's mass properties to zero and then + // accumulate mass contributions from all of its follower links. + // Note: The world mobod's spatial inertia is intentionally left as NaN. + const bool is_world_mobod = mobod.is_world(); + if (!is_world_mobod) { + frame_body_poses->SetM_BBo_B(mobod.index(), SpatialInertia::Zero()); + } - // TODO(sherm1) When we support composites, M_BBo_B ≠ M_LLo_L. - frame_body_poses->SetM_BBo_B(mobod.index(), M_LLo_L); + // Accumulate the spatial inertia from each link following this mobod. + for (const LinkOrdinal& link_ordinal : mobod.follower_link_ordinals()) { + const LinkIndex link_index = forest().links(link_ordinal).index(); + const Link& link = links_.get_element(link_index); + const SpatialInertia M_LLo_L = + link.CalcSpatialInertiaInBodyFrame(context); + frame_body_poses->SetM_LLo_L(link_ordinal, M_LLo_L); + + // Set p_BoLcm_B and accumulate M_LLo_L into M_BBo_B (unless B is world). + const Vector3& p_LoLcm_L = M_LLo_L.get_com(); + if (!frame_body_poses->is_X_BL_identity(link_ordinal)) { + 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 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); + } + } else { + // X_BL is identity, so p_BoLcm_B = p_LoLcm_L and M_LBo_B = M_LLo_L. + frame_body_poses->Set_p_BoLcm_B(link_ordinal, p_LoLcm_L); + if (!is_world_mobod) { + frame_body_poses->AddToM_BBo_B(mobod.index(), M_LLo_L); + } + } + } } } @@ -1786,8 +1961,8 @@ VectorX MultibodyTree::CalcInverseDynamics( const systems::Context& context, const VectorX& known_vdot, const MultibodyForces& external_forces) const { // Temporary storage used in the computation of inverse dynamics. - std::vector> A_WB(num_links()); - std::vector> F_BMo_W(num_links()); + std::vector> A_WB(num_mobods()); + std::vector> F_BMo_W(num_mobods()); VectorX tau(num_velocities()); CalcInverseDynamics(context, known_vdot, external_forces.body_forces(), external_forces.generalized_forces(), &A_WB, &F_BMo_W, @@ -2389,16 +2564,15 @@ SpatialInertia MultibodyTree::CalcSpatialInertia( // For efficiency, evaluate all bodies' spatial inertia and pose. const std::vector>& M_Bi_W = EvalSpatialInertiaInWorldCache(context); + const FrameBodyPoseCache& frame_body_poses = EvalFrameBodyPoses(context); const PositionKinematicsCache& pc = EvalPositionKinematics(context); // Add each body's spatial inertia in the world frame W to this system // S's spatial inertia in W about Wo (the origin of W), expressed in W. - // TODO(Mitiguy) Create SpatialInertia::Zero() and use it below. - SpatialInertia M_SWo_W(0., Vector3::Zero(), - UnitInertia::TriaxiallySymmetric(0)); + SpatialInertia M_SWo_W = SpatialInertia::Zero(); for (BodyIndex body_index : body_indexes) { - if (body_index == 0) continue; // No contribution from the world body. + if (body_index == world_index()) continue; // World inertia does not add. // Ensure MultibodyPlant method contains a valid body_index. if (body_index >= num_links()) { @@ -2407,14 +2581,35 @@ SpatialInertia MultibodyTree::CalcSpatialInertia( } // Get the current body B's spatial inertia about Bo (body B's origin), - // expressed in the world frame W. + // expressed in the world frame W. Start the calculation with a cached value + // for M_BBo_W if B is not a composite body (i.e., it is a one-link Mobod). const MobodIndex mobod_index = get_link(body_index).mobod_index(); - const SpatialInertia& M_BBo_W = M_Bi_W[mobod_index]; + if (!get_mobod(mobod_index).is_composite()) { + const SpatialInertia& M_BBo_W = M_Bi_W[mobod_index]; - // Shift M_BBo_W from about-point Bo to about-point Wo and add to the sum. - const RigidTransform& X_WB = pc.get_X_WB(mobod_index); - const Vector3& p_WoBo_W = X_WB.translation(); - M_SWo_W += M_BBo_W.Shift(-p_WoBo_W); // Shift from Bo to Wo by p_BoWo_W. + // Shift M_BBo_W from about-point Bo to about-point Wo and add to the sum. + const RigidTransform& X_WB = pc.get_X_WB(mobod_index); + const Vector3& p_WoBo_W = X_WB.translation(); + M_SWo_W += M_BBo_W.Shift(-p_WoBo_W); // Shift from Bo to Wo by p_BoWo_W. + } else { + // For a composite body (having more than one link), need to calculate + // the individual link's spatial inertia about Wo expressed in W since + // there is no cached value (and perhaps no need for one). + const LinkOrdinal link_ordinal = + graph().link_by_index(body_index).ordinal(); + + // M_LLo_L: inertia of link L about its origin Lo, expressed in L. + const SpatialInertia& M_LLo_L = + frame_body_poses.get_M_LLo_L(link_ordinal); + + // X_WL: pose of L's frame relative to world frame W. + const RigidTransform& X_WL = pc.get_X_WL(link_ordinal); + + // Re-express M_LLo to world W, shift that to world origin, add to sum. + const SpatialInertia M_LLo_W = M_LLo_L.ReExpress(X_WL.rotation()); + const Vector3& p_WoLo_W = X_WL.translation(); + M_SWo_W += M_LLo_W.Shift(-p_WoLo_W); // Shift from Lo to Wo by p_LoWo_W. + } } // If frame_F is the world frame W, return now. @@ -2465,7 +2660,7 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld( } // For a system S with center of mass Scm, Scm's translational velocity in the - // world frame W is calculated as v_WScm_W = ∑ (mᵢ vᵢ) / mₛ, where mₛ = ∑ mᵢ, + // world frame W is calculated as v_WScm_W = ∑ (mᵢ vᵢ) / mₛ, where mₛ = ∑ mᵢ, // mᵢ is the mass of the iᵗʰ body, and vᵢ is Bᵢcm's velocity in world frame W // (Bᵢcm is the center of mass of the iᵗʰ body). return sum_mi_vi / total_mass; @@ -2488,14 +2683,14 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalVelocityInWorld( T total_mass = 0; Vector3 sum_mi_vi = Vector3::Zero(); - // Sum over all the bodies that are in model_instances except for the 0th body + // Sum over all bodies that are in model_instances except for the 0ᵗʰ body // (which is the world body), and count each body's contribution only once. - // Reminder: Although it is not possible for a body to belong to multiple - // model instances [as RigidBody::model_instance() returns a body's unique - // model instance], it is possible for the same model instance to be added - // multiple times to std::vector& model_instances). The - // code below ensures a body's contribution to the sum occurs only once. - // Duplicate model_instances in std::vector are ignored. + // Reminder: Although it is not possible for a body to belong to + // multiple model instances [as RigidBody::model_instance() returns a body's + // unique model instance], it is possible for the same model instance to be + // added multiple times to std::vector& + // model_instances). The code below ensures a body's contribution to the sum + // occurs only once. Duplicate model_instances in std::vector are ignored. int number_of_non_world_bodies_processed = 0; for (BodyIndex body_index(1); body_index < num_links(); ++body_index) { const RigidBody& body = get_link(body_index); @@ -2569,10 +2764,10 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld( sum_mi_ai += body_mass * ai_WBcm_W; // sum_mi_ai = ∑ mᵢ * ai_WBcm_W. } - // For a system S with center of mass Scm, Scm's translational acceleration in - // the world W is calculated as a_WScm_W = ∑ (mᵢ aᵢ) / mₛ, where mₛ = ∑ mᵢ, - // mᵢ is the mass of the iᵗʰ body, and aᵢ is Bᵢcm's acceleration in world W - // (Bᵢcm is the center of mass of the iᵗʰ body). + // For a system S with center of mass Scm, Scm's translational acceleration + // in the world W is calculated as a_WScm_W = ∑ (mᵢ aᵢ) / mₛ, where mₛ = ∑ + // mᵢ, mᵢ is the mass of the iᵗʰ body, and aᵢ is Bᵢcm's acceleration in + // world W (Bᵢcm is the center of mass of the iᵗʰ body). return sum_mi_ai / total_mass; } @@ -2593,8 +2788,8 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld( // Why? Acceleration calculations may require a dynamic analysis that will // issue a significantly less helpful exception message. // Sum over all the bodies in model_instances except the 0th body (which is - // the world body). Each body is counted only once even if its model instance - // is listed multiple times. + // the world body). Each body is counted only once even if its model + // instance is listed multiple times. T total_mass = 0; int number_of_non_world_bodies_processed = 0; for (BodyIndex body_index(1); body_index < num_links(); ++body_index) { @@ -2622,14 +2817,14 @@ Vector3 MultibodyTree::CalcCenterOfMassTranslationalAccelerationInWorld( throw std::logic_error(message); } - // Sum over all the bodies that are in model_instances except for the 0th body - // (which is the world body), and count each body's contribution only once. - // Reminder: Although it is not possible for a body to belong to multiple - // model instances [as RigidBody::model_instance() returns a body's unique - // model instance], it is possible for the same model instance to be added - // multiple times to std::vector& model_instances). - // The code below ensures a body's contribution to the sum occurs only once. - // Duplicate model_instances in std::vector are ignored. + // Sum over all the bodies that are in model_instances except for the 0th + // body (which is the world body), and count each body's contribution only + // once. Reminder: Although it is not possible for a body to belong to + // multiple model instances [as RigidBody::model_instance() returns a body's + // unique model instance], it is possible for the same model instance to be + // added multiple times to std::vector& + // model_instances). The code below ensures a body's contribution to the sum + // occurs only once. Duplicate model_instances in std::vector are ignored. Vector3 sum_mi_ai = Vector3::Zero(); for (BodyIndex body_index(1); body_index < num_links(); ++body_index) { const RigidBody& body = get_link(body_index); @@ -2649,7 +2844,7 @@ template SpatialMomentum MultibodyTree::CalcSpatialMomentumInWorldAboutPoint( const systems::Context& context, const Vector3& p_WoP_W) const { // Assemble a list of ModelInstanceIndex. - // Skip model_instance_index(0) which always contains the "world" body -- the + // Skip model_instance_index(0) which always contains the "world" body. The // spatial momentum of the world body measured in the world is always zero. std::vector model_instances; for (ModelInstanceIndex model_instance_index(1); @@ -2693,14 +2888,16 @@ template SpatialMomentum MultibodyTree::CalcBodiesSpatialMomentumInWorldAboutWo( const systems::Context& context, const std::vector& link_indexes) const { - // For efficiency, evaluate all bodies' spatial inertia, velocities, and pose. + // For efficiency, evaluate all bodies' spatial inertia, velocities, and + // pose. const std::vector>& M_Bi_W = EvalSpatialInertiaInWorldCache(context); const PositionKinematicsCache& pc = EvalPositionKinematics(context); const VelocityKinematicsCache& vc = EvalVelocityKinematics(context); - // Accumulate each body's spatial momentum in the world frame W to this system - // S's spatial momentum in W about Wo (the origin of W), expressed in W. + // Accumulate each body's spatial momentum in the world frame W to this + // system S's spatial momentum in W about Wo (the origin of W), expressed in + // W. SpatialMomentum L_WS_W = SpatialMomentum::Zero(); // Add contributions from each link Bi. @@ -2793,12 +2990,15 @@ void MultibodyTree::CalcAllBodyBiasSpatialAccelerationsInWorld( // To calculate a generic body A's spatial acceleration bias in world W, // note that body A's spatial velocity in world W is // V_WA = J𝑠_V_WA ⋅ 𝑠 - // which upon vector differentiation in W gives A's spatial acceleration in W + // which upon vector differentiation in W gives A's spatial acceleration in + // W // A_WA = J𝑠_V_WA ⋅ 𝑠̇ + J̇𝑠_V_WA ⋅ 𝑠 - // Since A𝑠Bias_WA can be defined as the term in A_WA that does not include 𝑠̇, + // Since A𝑠Bias_WA can be defined as the term in A_WA that does not include + // 𝑠̇, // A𝑠Bias_WA = J̇𝑠_V_WA ⋅ 𝑠 = A_WA − J𝑠_V_WA ⋅ 𝑠̇ - // One way to calculate A𝑠Bias_WA is to evaluate A_WA with 𝑠̇ = 0. Hence, set - // 𝑠̇ = 0 to calculate all bodies' spatial acceleration biases in world W. + // One way to calculate A𝑠Bias_WA is to evaluate A_WA with 𝑠̇ = 0. Hence, + // set 𝑠̇ = 0 to calculate all bodies' spatial acceleration biases in world + // W. const PositionKinematicsCache& pc = EvalPositionKinematics(context); const VelocityKinematicsCache& vc = EvalVelocityKinematics(context); const VectorX vdot = VectorX::Zero(num_velocities()); @@ -2813,8 +3013,10 @@ SpatialAcceleration MultibodyTree::CalcBiasSpatialAcceleration( // TODO(mitiguy) Allow with_respect_to be JacobianWrtVariable::kQDot. DRAKE_THROW_UNLESS(with_respect_to == JacobianWrtVariable::kV); - // Reserve room to store all the bodies' spatial acceleration bias in world W. - // TODO(Mitiguy) Inefficient use of heap. Per issue #13560, implement caching. + // Reserve room to store all the bodies' spatial acceleration bias in world + // W. + // TODO(Mitiguy) Inefficient use of heap. Per issue #13560, implement + // caching. std::vector> AsBias_WB_all(num_links()); CalcAllBodyBiasSpatialAccelerationsInWorld(context, with_respect_to, &AsBias_WB_all); @@ -2852,8 +3054,9 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( // Fp's angular acceleration in body_A is equal to body_B's angular // acceleration in body_A, and hence can be denoted α_AB and can be // calculated by rearranging the "angular acceleration addition theorem" - // (from eqn (12) in SpatialAcceleration::ComposeWithMovingFrameAcceleration() - // or Chap 8, Angular velocity/acceleration [Mitiguy 2019], reference below). + // (from eqn (12) in + // SpatialAcceleration::ComposeWithMovingFrameAcceleration() or Chap 8, + // Angular velocity/acceleration [Mitiguy 2019], reference below). // (1) α_WB = α_WA + α_AB + w_WA x w_AB is rearranged to // (2) α_AB = α_WB - α_WA - w_WA x w_AB, where // α_AB is body B's angular acceleration in body A, @@ -2864,9 +3067,10 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( // // The translational acceleration part of A_AFp is denoted a_AFp and can be // calculated by rearranging the "one point moving on a rigid frame formula" - // (from eqn (13) in SpatialAcceleration::ComposeWithMovingFrameAcceleration() - // or from Chapter 10, Points: Velocity and acceleration [Mitiguy 2019] - // or from section 2.8, page 39 [Kane & Levinson 1985], references below) + // (from eqn (13) in + // SpatialAcceleration::ComposeWithMovingFrameAcceleration() or from Chapter + // 10, Points: Velocity and acceleration [Mitiguy 2019] or from section 2.8, + // page 39 [Kane & Levinson 1985], references below) // (3) a_WFp = a_WAp + a_AFp + 2 w_WA x v_AFp is rearranged to // (4) a_AFp = a_WFp - a_WAp - 2 w_WA x v_AFp, where // point Ap is the point fixed to body_A that is coincident with Fp, @@ -2876,15 +3080,17 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( // w_WA is body A's angular velocity in frame W, // v_AFp is Fp's translational velocity in body_A. // - // The previous equations also apply to bias acceleration, so eqns (2) and (4) - // apply to bias angular acceleration and bias translational acceleration as + // The previous equations also apply to bias acceleration, so eqns (2) and + // (4) apply to bias angular acceleration and bias translational + // acceleration as // (5) αBias_AB = αBias_WB - αBias_WA - w_WA x w_AB // (6) aBias_AFp = aBias_WFp - aBias_WAp - 2 w_WA x v_AFp // // - [Mitiguy, 2019]: "Advanced Dynamics and Motion Simulation, - // For professional engineers and scientists," Prodigy Press, Sunnyvale CA, - // Available at www.MotionGenesis.com - // - [Kane & Levinson 1985] "Dynamics, Theory and Applications," McGraw-Hill. + // For professional engineers and scientists," Prodigy Press, Sunnyvale + // CA, Available at www.MotionGenesis.com + // - [Kane & Levinson 1985] "Dynamics, Theory and Applications," + // McGraw-Hill. // Available for free .pdf download: https://hdl.handle.net/1813/638 // Shift spatial acceleration from body_B's origin to point Fp of frame_F. const PositionKinematicsCache& pc = EvalPositionKinematics(context); @@ -2901,7 +3107,8 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( A_AFp_W = A_WFp_W; } else { // Point Ap is the point of (fixed to) body_A that is coincident with - // point Fp. Calculate the position vector from Ao (body_A's origin) to Ap. + // point Fp. Calculate the position vector from Ao (body_A's origin) to + // Ap. const RigidTransform X_AF = frame_F.CalcPose(context, frame_A); const Vector3 p_AoAp_A = X_AF * p_FoFp_F; // Note: p_AoAp = p_AoFp @@ -2909,8 +3116,9 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( // Note: Since Ap is regarded as fixed to body_A, Ap's translational // acceleration in the world frame W is calculated as // a_WAp = a_WAo + α_WA x p_AoAp + w_WA x (w_WA x p_AoAp) - // Reminder: p_AoAp is an "instantaneous" position vector, so differentation - // of p_AoAp or a_WAp may produce a result different than you might expect. + // Reminder: p_AoAp is an "instantaneous" position vector, so + // differentation of p_AoAp or a_WAp may produce a result different than + // you might expect. const SpatialAcceleration A_WAp_W = ShiftSpatialAccelerationInWorld(frame_A, p_AoAp_A, A_WA_W, pc, vc); @@ -2922,12 +3130,13 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( // * There may be simulations in which using a least common ancestor is // important for speed or avoiding loss of precision from cancellations. // * Code for operating in the ancestor frame requires conversions for - // quantities that were already available in World; there is some cost to - // that both in execution time and programming effort. + // quantities that were already available in World; there is some cost + // to that both in execution time and programming effort. // * In Simbody, Sherm used the least common ancestor for all constraint // equations and grew to regret it. It was surprisingly complicated and - // the extra transformations made the code (including caching of results) - // complicated, ultimately with questionable saving of computation time. + // the extra transformations made the code (including caching of + // results) complicated, ultimately with questionable saving of + // computation time. // * For Jacobians (one of Drake's fastest recursive calculations), it is // unclear whether typical non-World frame relative accelerations would // involve near-ancestors rather than far-ancestors. If the latter, @@ -2935,7 +3144,8 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( A_AFp_W = A_WFp_W - A_WAp_W; // Calculation of A_AFp_W is unfinished here. // Equation (5) is α_AB = α_WB - α_WA - w_WA x w_AB, - // hence calculate A's angular velocity in W and B's angular velocity in A. + // hence calculate A's angular velocity in W and B's angular velocity in + // A. const Vector3 w_WA_W = body_A.EvalSpatialVelocityInWorld(context).rotational(); SpatialVelocity V_AF_W = @@ -2943,8 +3153,9 @@ SpatialAcceleration MultibodyTree::CalcSpatialAccelerationHelper( const Vector3 w_AF_W = V_AF_W.rotational(); // Frame F is welded to B. A_AFp_W.rotational() -= w_WA_W.cross(w_AF_W); - // Equation (6) is a_AFp = a_WFp - a_WAp - 2 w_WA x v_AFp, hence calculate - // Fp's velocity in A to form the "Coriolis acceleration" 2 w_WA x v_AFp. + // Equation (6) is a_AFp = a_WFp - a_WAp - 2 w_WA x v_AFp, hence + // calculate Fp's velocity in A to form the "Coriolis acceleration" 2 w_WA + // x v_AFp. const RotationMatrix R_WF = frame_F.CalcRotationMatrixInWorld(context); const Vector3 p_FoFp_W = R_WF * p_FoFp_F; const Vector3 v_AFp_W = V_AF_W.Shift(p_FoFp_W).translational(); @@ -2997,7 +3208,8 @@ Matrix3X MultibodyTree::CalcBiasTranslationalAcceleration( // TODO(mitiguy) Allow with_respect_to be JacobianWrtVariable::kQDot. DRAKE_THROW_UNLESS(with_respect_to == JacobianWrtVariable::kV); - // Form frame_B's bias spatial acceleration in frame_A, expressed in frame_E. + // Form frame_B's bias spatial acceleration in frame_A, expressed in + // frame_E. const SpatialAcceleration AsBias_ABo_E = CalcBiasSpatialAcceleration( context, with_respect_to, frame_B, Vector3::Zero(), frame_A, frame_E); @@ -3023,7 +3235,8 @@ Matrix3X MultibodyTree::CalcBiasTranslationalAcceleration( const SpatialAcceleration AsBias_ABp_E = AsBias_ABo_E.Shift(p_BoBp_E, w_AB_E); - // Store only the translational bias acceleration component in the results. + // Store only the translational bias acceleration component in the + // results. asBias_ABi_E_array.col(ipoint) = AsBias_ABp_E.translational(); } return asBias_ABi_E_array; @@ -3044,9 +3257,9 @@ void MultibodyTree::CalcJacobianSpatialVelocity( DRAKE_THROW_UNLESS(Js_V_ABp_E->cols() == num_columns); // The spatial velocity V_WBp can be obtained by composing the spatial - // velocities V_WAp and V_ABp. Expressed in the world frame W this composition - // is V_WBp_W = V_WAp_W + V_ABp_W - // Therefore, V_ABp_W = (Js_V_WBp - Js_V_WAp) ⋅ s. + // velocities V_WAp and V_ABp. Expressed in the world frame W this + // composition is V_WBp_W = V_WAp_W + V_ABp_W Therefore, V_ABp_W = (Js_V_WBp + // - Js_V_WAp) ⋅ s. // // If with_respect_to = JacobianWrtVariable::kQDot, s = q̇ and // Js_V_W{Ap,Bp} = Jq_V_W{Ap,Bp}, @@ -3232,7 +3445,8 @@ void MultibodyTree::CalcJacobianTranslationalVelocity( if (&frame_E != &frame_W) { const RotationMatrix R_EW = CalcRelativeRotationMatrix(context, frame_E, frame_W); - // Extract the 3 x num_columns block that starts at row = 3 * i, column = 0. + // Extract the 3 x num_columns block that starts at row = 3 * i, column = + // 0. for (int i = 0; i < num_points; ++i) { Js_v_ABi_E->template block<3, Eigen::Dynamic>(3 * i, 0, 3, num_columns) = R_EW * Js_v_ABi_E->template block<3, Eigen::Dynamic>(3 * i, 0, 3, @@ -3269,8 +3483,8 @@ void MultibodyTree::CalcJacobianAngularAndOrTranslationalVelocityInWorld( // RigidBody to which frame_F is welded/attached. const RigidBody& body_F = frame_F.body(); - // Return zero Jacobians for bodies anchored to the world, since for anchored - // bodies, w_wF = Js_w_WF * v = 0 and v_WFpi = Js_v_WFpi * v = 0. + // Return zero Jacobians for bodies anchored to the world, since for + // anchored bodies, w_wF = Js_w_WF * v = 0 and v_WFpi = Js_v_WFpi * v = 0. if (body_F.index() == world_index()) return; // Form kinematic path from World to body_F. @@ -3339,12 +3553,12 @@ void MultibodyTree::CalcJacobianAngularAndOrTranslationalVelocityInWorld( } if (Js_v_WFpi_W) { - // Get memory address in the output block Jacobian translational velocity - // Js_v_PFpi_W corresponding to the contribution of the mobilities in - // level ilevel. This address corresponds to point Fpi's Jacobian - // translational velocity in the inboard (parent) body frame P, expressed - // in world frame W. That is, v_PFpi_W = Js_v_PFpi_W * v(B), where v(B) - // are the mobilities that correspond to the current node. + // Get memory address in the output block Jacobian translational + // velocity Js_v_PFpi_W corresponding to the contribution of the + // mobilities in level ilevel. This address corresponds to point Fpi's + // Jacobian translational velocity in the inboard (parent) body frame P, + // expressed in world frame W. That is, v_PFpi_W = Js_v_PFpi_W * v(B), + // where v(B) are the mobilities that correspond to the current node. auto Js_v_PFpi_W = Js_v_WFpi_W->block(0, start_index, 3 * num_points, mobilizer_jacobian_ncols); @@ -3360,9 +3574,10 @@ void MultibodyTree::CalcJacobianAngularAndOrTranslationalVelocityInWorld( const Vector3 p_BoFp_W = p_WoFp - p_WoBo; // Point Fp's Jacobian translational velocity is placed in the output - // memory block in the same order input points Fpi are listed on input. - // Get a mutable alias into Js_v_PFpi_W for the Jacobian translational - // velocity term for the currently indexed (ipoint) point. + // memory block in the same order input points Fpi are listed on + // input. Get a mutable alias into Js_v_PFpi_W for the Jacobian + // translational velocity term for the currently indexed (ipoint) + // point. const int ipoint_row = 3 * ipoint; auto Hv_PFpi_W = Js_v_PFpi_W.block(ipoint_row, 0, 3, mobilizer_jacobian_ncols); @@ -3564,9 +3779,9 @@ Vector3 MultibodyTree::CalcBiasCenterOfMassTranslationalAcceleration( total_mass += body_mass; // total_mass = ∑ mᵢ. ++number_of_non_world_bodies_processed; - // sum_mi_aBiasi = ∑ (mᵢ aBiasᵢ), where mᵢ is the mass of the iᵗʰ body and - // aBiasᵢ is Bᵢcm's bias translational acceleration in frame A, expressed - // in frame E (Bᵢcm is the center of mass of the iᵗʰ body). + // sum_mi_aBiasi = ∑ (mᵢ aBiasᵢ), where mᵢ is the mass of the iᵗʰ body + // and aBiasᵢ is Bᵢcm's bias translational acceleration in frame A, + // expressed in frame E (Bᵢcm is the center of mass of the iᵗʰ body). const Frame& frame_B = body.body_frame(); const Vector3 pi_BoBcm_B = body.CalcCenterOfMassInBodyFrame(context); const Vector3 aBiasi_ABcm_E = CalcBiasTranslationalAcceleration( @@ -3700,8 +3915,9 @@ void MultibodyTree::ThrowDefaultMassInertiaError() const { DRAKE_DEMAND(!active_mobod.is_weld()); // That wouldn't be active! if (active_mobod.nq_outboard() > 0) continue; // Not a terminal group. - // At this point we're looking at a non-World, terminal WeldedMobods group. - // Find the matching WeldedLinksAssembly that carries the mass properties. + // At this point we're looking at a non-World, terminal WeldedMobods + // group. Find the matching WeldedLinksAssembly that carries the mass + // properties. const std::optional link_assembly_index = graph() .links(active_mobod.active_link_ordinal()) @@ -3751,10 +3967,11 @@ void MultibodyTree::ThrowIfTerminalBodyHasBadDefaultMassProperties( } if (can_rotate && IsAnyDefaultRotationalInertiaNaN(link_assembly)) { - throw std::logic_error(fmt::format( - "Body {} is {} that has a NaN rotational inertia, but its joint has a " - "rotational degree of freedom.", - active_link_name, description)); + throw std::logic_error( + fmt::format("Body {} is {} that has a NaN rotational inertia, but " + "its joint has a " + "rotational degree of freedom.", + active_link_name, description)); } if (can_rotate && AreAllDefaultRotationalInertiaZero(link_assembly)) { @@ -3862,8 +4079,8 @@ void MultibodyTree::CalcArticulatedBodyForceCache( const std::vector>& H_PB_W_cache = EvalAcrossNodeJacobianWrtVExpressedInWorld(context); - // Eval spatial inertia M_B_W(q) and force bias Fb_B_W(q, v) as they appear on - // the Newton-Euler equation: M_B_W * A_WB + Fb_B_W = Fapp_B_W. + // Eval spatial inertia M_B_W(q) and force bias Fb_B_W(q, v) as they appear + // on the Newton-Euler equation: M_B_W * A_WB + Fb_B_W = Fapp_B_W. const std::vector>& dynamic_bias_cache = EvalDynamicBiasCache(context); @@ -3898,8 +4115,8 @@ void MultibodyTree::CalcArticulatedBodyForceCache( // Get configuration dependent articulated body inertia cache. const ArticulatedBodyInertiaCache& abic = EvalArticulatedBodyInertiaCache(context); - // We evaluate the kinematics dependent articulated body force bias Zb_Bo_W = - // Pplus_PB_W * Ab_WB. When cached, this corresponds to a significant + // We evaluate the kinematics dependent articulated body force bias Zb_Bo_W + // = Pplus_PB_W * Ab_WB. When cached, this corresponds to a significant // computational gain when performing ABA with the same context (storing the // same q and v) but different applied `forces`. const std::vector>& Zb_Bo_W_cache = @@ -4295,8 +4512,8 @@ 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 RigidBodyAttorney. Frame* body_frame_clone = &internal::RigidBodyAttorney::get_mutable_link_frame(body_clone.get()); body_frame_clone->set_parent_tree(this, body_frame_index); @@ -4306,8 +4523,8 @@ RigidBody* MultibodyTree::CloneBodyAndAdd( // topology invariant. Therefore we index new clones according to the // original body_frame_index. frames_.AddBorrowed(body_frame_clone); - // The order in which bodies are added into owned_bodies_ is important to keep - // the topology invariant. Therefore this method is called from + // The order in which bodies are added into owned_bodies_ is important to + // keep the topology invariant. Therefore this method is called from // MultibodyTree::CloneToScalar() within a loop by original body_index. return &links_.Add(std::move(body_clone)); } @@ -4390,7 +4607,6 @@ DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(( &MultibodyTree::template CloneToScalar )); // clang-format on - } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index 15063620fa5b..e1630bb65bcc 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -965,10 +965,18 @@ class MultibodyTree { BaseBodyJointType joint_type, std::optional model_instance = {}); + // See MultibodyPlant API. + void SetCombineWeldedBodies( + bool combine, std::optional model_instance = {}); + // See MultibodyPlant API. BaseBodyJointType GetBaseBodyJointType( std::optional model_instance = {}) const; + // See MultibodyPlant API. + bool GetCombineWeldedBodies( + std::optional model_instance = {}) const; + // Finalize() must be called after all user-defined elements in the plant // (joints, bodies, force elements, constraints, etc.) have been added and // before any computations are performed. It compiles all the necessary @@ -1403,6 +1411,8 @@ class MultibodyTree { void CalcJointDamping(const systems::Context& context, VectorX* joint_damping) const; + // After all frame pose and mass property parameters have values, calculates + // and caches the state-independent results in the FrameBodyPoseCache. void CalcFrameBodyPoses(const systems::Context& context, FrameBodyPoseCache* frame_body_poses) const; diff --git a/multibody/tree/position_kinematics_cache.cc b/multibody/tree/position_kinematics_cache.cc index cdfdc816244f..dfc37638a3cb 100644 --- a/multibody/tree/position_kinematics_cache.cc +++ b/multibody/tree/position_kinematics_cache.cc @@ -1,4 +1,58 @@ #include "drake/multibody/tree/position_kinematics_cache.h" +#include + +#include "drake/multibody/tree/frame_body_pose_cache.h" + +namespace drake { +namespace multibody { +namespace internal { + +template +PositionKinematicsCache::PositionKinematicsCache( + const SpanningForest& forest) + : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) { + Allocate(); + + // Set known values. + X_WB_pool_[world_mobod_index()] = RigidTransform::Identity(); + X_WL_pool_[world_link_ordinal()] = RigidTransform::Identity(); +} + +template +void PositionKinematicsCache::ComputeWorldComposite( + const SpanningForest& forest, + const FrameBodyPoseCache& frame_body_pose_cache) { + const SpanningForest::Mobod& world_mobod = forest.mobods(MobodIndex(0)); + const std::vector& world_followers = + world_mobod.follower_link_ordinals(); + for (size_t i = 1; i < world_followers.size(); ++i) { + const LinkOrdinal link_ordinal = world_followers[i]; + 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); + } +} + +// Initialize most things to NaN to catch bugs. +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()); + // 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); +} + +} // namespace internal +} // namespace multibody +} // namespace drake + DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class ::drake::multibody::internal::PositionKinematicsCache); diff --git a/multibody/tree/position_kinematics_cache.h b/multibody/tree/position_kinematics_cache.h index f8f5befc8ba8..60eb8f0c49ac 100644 --- a/multibody/tree/position_kinematics_cache.h +++ b/multibody/tree/position_kinematics_cache.h @@ -16,6 +16,9 @@ namespace drake { namespace multibody { namespace internal { +template +class FrameBodyPoseCache; + /* This class is one of the cache entries in the Context. It holds the kinematics results of computations that only depend on the generalized positions q of the system. @@ -39,9 +42,6 @@ Results are indexed by MobodIndex unless otherwise specified: P, expressed in world frame W. - X_FM: Pose of mobilizer's outboard frame M measured and expressed in its inboard frame F. - - H_FM: Mobilizer's hinge matrix, the Jacobian ∂V_FM/∂v that maps the - mobilizer's generalized velocities v to cross-mobilizer spatial - velocities V_FM = H_FM * v. @tparam_default_scalar */ @@ -63,10 +63,7 @@ class PositionKinematicsCache { template using RigidTransform = drake::math::RigidTransform; - explicit PositionKinematicsCache(const SpanningForest& forest) - : num_mobods_(forest.num_mobods()), num_links_(forest.num_links()) { - Allocate(); - } + explicit PositionKinematicsCache(const SpanningForest& forest); // Returns a const reference to pose `X_WB` of the body B (associated with // mobilized body mobod_index) as measured and expressed in the world frame W. @@ -155,39 +152,40 @@ class PositionKinematicsCache { return p_PoBo_W_pool_[mobod_index]; } + // Once we know where the links are placed on the World composite, we can fill + // in X_WL for those links once and for all. X_WL₀ (≜ X_WL[link₀]) and + // X_WB₀ (≜ X_WB[mobod₀]) are identity transforms (set during allocation). + // Consequently, X_WLᵢ = X_WB₀ * X_B₀Lᵢ = X_B₀Lᵢ for each of the links Lᵢ that + // are fixed to World. Do nothing if the serial number hasn't changed. + void PrecomputeWorldCompositeIfNeeded( + const SpanningForest& forest, + const FrameBodyPoseCache& frame_body_pose_cache, + int64_t frame_body_pose_cache_serial_number) { + if (world_composite_serial_number_ == frame_body_pose_cache_serial_number) + return; + ComputeWorldComposite(forest, frame_body_pose_cache); + world_composite_serial_number_ = frame_body_pose_cache_serial_number; + } + private: // Allocates resources for this position kinematics cache. - void Allocate() { - X_WB_pool_.resize(num_mobods_); - // Even though RigidTransform defaults to identity, we make it explicit. - // This pose will never change after this initialization. - X_WB_pool_[world_mobod_index()] = RigidTransform::Identity(); - - X_WL_pool_.resize(num_links_); - X_WL_pool_[world_link_ordinal()] = RigidTransform::Identity(); - - X_PB_pool_.resize(num_mobods_); - X_PB_pool_[world_mobod_index()] = NaNPose(); // It should never be used. - - X_FM_pool_.resize(num_mobods_); - X_FM_pool_[world_mobod_index()] = NaNPose(); // It should never be used. - - p_PoBo_W_pool_.resize(num_mobods_); - // p_PoBo_W for the world body should never be used. - p_PoBo_W_pool_[world_mobod_index()].setConstant( - std::numeric_limits< - typename Eigen::NumTraits::Literal>::quiet_NaN()); - } + void Allocate(); + + // Called when we know we have to recompute the parameter-dependent + // world composite kinematics. + void ComputeWorldComposite( + 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 the translation has NaN values. + // our purposes, it is enough that the translation has NaN values. return RigidTransform( math::RotationMatrix::Identity(), - Vector3::Constant(Eigen::NumTraits::quiet_NaN())); + Vector3::Constant(std::numeric_limits::quiet_NaN())); } // Number of Mobods in the multibody forest, including the World mobod. @@ -197,6 +195,12 @@ class PositionKinematicsCache { // (including the World link) and possibly some ephemeral links. int num_links_{0}; + // The serial number of the last FrameBodyPoseCache that was used to update + // the state-independent quantities in this cache (such as the X_WL for links + // fixed to World). This is used to determine whether we need to recompute + // those quantities when PrecomputeWorldComposite() is called. + int64_t world_composite_serial_number_{-1}; + // These are indexed by MobodIndex so are in depth-first order. std::vector> X_WB_pool_; std::vector> X_PB_pool_;