From efb5409b0ae91ac70ce59599902b6b6ef0c1cdb7 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Thu, 6 Aug 2026 17:38:06 -0700 Subject: [PATCH] Added "enable closed loops" option and shadow bodies. --- .../generated_docstrings/multibody_plant.h | 45 ++ .../generated_docstrings/multibody_tree.h | 27 +- multibody/plant/BUILD.bazel | 11 + multibody/plant/multibody_plant.cc | 29 + multibody/plant/multibody_plant.h | 29 + .../plant/test/auto_closed_topology_test.cc | 579 ++++++++++++++++++ multibody/plant/test/multibody_plant_test.cc | 33 + multibody/tree/multibody_tree.cc | 126 +++- multibody/tree/multibody_tree.h | 38 +- multibody/tree/rigid_body.cc | 36 +- multibody/tree/rigid_body.h | 78 ++- 11 files changed, 998 insertions(+), 33 deletions(-) create mode 100644 multibody/plant/test/auto_closed_topology_test.cc diff --git a/bindings/generated_docstrings/multibody_plant.h b/bindings/generated_docstrings/multibody_plant.h index c925b47eb2bc..bd39e54e858f 100644 --- a/bindings/generated_docstrings/multibody_plant.h +++ b/bindings/generated_docstrings/multibody_plant.h @@ -5305,6 +5305,19 @@ See also: Raises: RuntimeError if called pre-finalize.)"""; } GetEffortUpperLimits; + // Symbol: drake::multibody::MultibodyPlant::GetEnableLoopTopology + struct /* GetEnableLoopTopology */ { + // Source: drake/multibody/plant/multibody_plant.h + const char* doc = +R"""((Internal use only for now) Returns the current setting for whether +Finalize() automatically deals with closed-topology (looped) systems. + +Note: + This function can be called pre-Finalize() or post-Finalize(). + +See also: + SetEnableLoopTopology(), Finalize())"""; + } GetEnableLoopTopology; // Symbol: drake::multibody::MultibodyPlant::GetFloatingBaseBodies struct /* GetFloatingBaseBodies */ { // Source: drake/multibody/plant/multibody_plant.h @@ -6817,6 +6830,38 @@ Parameter ``params``: if params.bodyA() or params.bodyB() do not correspond to rigid bodies in ``this`` MultibodyPlant.)"""; } SetDistanceConstraintParams; + // Symbol: drake::multibody::MultibodyPlant::SetEnableLoopTopology + struct /* SetEnableLoopTopology */ { + // Source: drake/multibody/plant/multibody_plant.h + const char* doc = +R"""((Internal use only for now) For systems whose links and joints form +one or more kinematic loops (a "closed topology"), controls whether +Finalize() should deal with those automatically. The default setting +is *not* to deal with kinematic loops (if one is encountered, an +exception is thrown). + +Note: + This feature is in development and is not yet functional. + +To deal with loops automatically requires modifying the system's +topology so that it is structured as a tree of links and joints, plus +constraints needed to enforce loop closure. Breaking a loop is done by +splitting a link within that loop. Mass properties are divided between +the original ("primary") link and the new ("shadow") link. Then a weld +constraint is added between the primary and shadow links to enforce +loop closure. When the weld constraint is satisfied, the original +physics is restored. + +Parameter ``enable``: + Whether Finalize() should automatically model closed kinematic + loops rather than throwing. + +Raises: + RuntimeError if called after Finalize(). + +See also: + GetEnableLoopTopology(), Finalize())"""; + } SetEnableLoopTopology; // Symbol: drake::multibody::MultibodyPlant::SetFloatingBaseBodyPoseInAnchoredFrame struct /* SetFloatingBaseBodyPoseInAnchoredFrame */ { // Source: drake/multibody/plant/multibody_plant.h diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index d5a020abd2fd..05be43d00aa1 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -7164,6 +7164,11 @@ Parameter ``context``: Precondition: the context makes sense for use by this RigidBody.)"""; } CalcSpatialInertiaInBodyFrame; + // Symbol: drake::multibody::RigidBody::CalcSpatialInertiaInBodyFrameFromParameters + struct /* CalcSpatialInertiaInBodyFrameFromParameters */ { + // Source: drake/multibody/tree/rigid_body.h + const char* doc = R"""()"""; + } CalcSpatialInertiaInBodyFrameFromParameters; // Symbol: drake::multibody::RigidBody::CloneToScalar struct /* CloneToScalar */ { // Source: drake/multibody/tree/rigid_body.h @@ -7299,6 +7304,10 @@ Parameter ``com``: Raises: RuntimeError if context is null. +Raises: + RuntimeError if this is an ephemeral shadow link (its mass + properties are not independently settable). + Warning: Do not use this function unless it is needed (think twice).)"""; } SetCenterOfMassInBodyFrame; @@ -7331,7 +7340,11 @@ Parameter ``center_of_mass_position``: the context makes sense for use by this RigidBody. Raises: - RuntimeError if context is null.)"""; + RuntimeError if context is null. + +Raises: + RuntimeError if this is an ephemeral shadow link (its mass + properties are not independently settable).)"""; } SetCenterOfMassInBodyFrameAndPreserveCentralInertia; // Symbol: drake::multibody::RigidBody::SetMass struct /* SetMass */ { @@ -7354,7 +7367,11 @@ Parameter ``mass``: the context makes sense for use by this RigidBody. Raises: - RuntimeError if context is null.)"""; + RuntimeError if context is null. + +Raises: + RuntimeError if this is an ephemeral shadow link (its mass + properties are not independently settable).)"""; } SetMass; // Symbol: drake::multibody::RigidBody::SetSpatialInertiaInBodyFrame struct /* SetSpatialInertiaInBodyFrame */ { @@ -7376,7 +7393,11 @@ Parameter ``M_Bo_B``: the context makes sense for use by this RigidBody. Raises: - RuntimeError if context is null.)"""; + RuntimeError if context is null. + +Raises: + RuntimeError if this is an ephemeral shadow link (its mass + properties are not independently settable).)"""; } SetSpatialInertiaInBodyFrame; // Symbol: drake::multibody::RigidBody::Unlock struct /* Unlock */ { diff --git a/multibody/plant/BUILD.bazel b/multibody/plant/BUILD.bazel index a1667c4bcec2..6b1d242d7dd5 100644 --- a/multibody/plant/BUILD.bazel +++ b/multibody/plant/BUILD.bazel @@ -459,6 +459,17 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "auto_closed_topology_test", + deps = [ + ":plant", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", + "//geometry:scene_graph", + "//multibody/parsing", + ], +) + drake_cc_googletest( name = "fused_welds_test", deps = [ diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 842ac7cf835e..262d0732fd2f 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1168,6 +1168,10 @@ geometry::GeometryId MultibodyPlant::RegisterVisualGeometry( template const std::vector& MultibodyPlant::GetVisualGeometriesForBody(const RigidBody& body) const { + // Check that visual_geometries_ has been sized correctly and that the + // body index is valid. + DRAKE_ASSERT(ssize(visual_geometries_) == num_bodies()); + DRAKE_ASSERT(body.index() < num_bodies()); return visual_geometries_[body.index()]; } @@ -1209,6 +1213,9 @@ template const std::vector& MultibodyPlant::GetCollisionGeometriesForBody( const RigidBody& body) const { + // Check that collision_geometries_ has been sized correctly and that the + // body index is valid. + DRAKE_ASSERT(ssize(collision_geometries_) == num_bodies()); DRAKE_ASSERT(body.index() < num_bodies()); return collision_geometries_[body.index()]; } @@ -1468,6 +1475,11 @@ void MultibodyPlant::SetFuseWeldedLinks( mutable_tree().SetFuseWeldedLinks(fuse, model_instance); } +template +void MultibodyPlant::SetEnableLoopTopology(bool enable) { + mutable_tree().SetEnableLoopTopology(enable); +} + template BaseBodyJointType MultibodyPlant::GetBaseBodyJointType( std::optional model_instance) const { @@ -1480,6 +1492,11 @@ bool MultibodyPlant::GetFuseWeldedLinks( return internal_tree().GetFuseWeldedLinks(model_instance); } +template +bool MultibodyPlant::GetEnableLoopTopology() const { + return internal_tree().GetEnableLoopTopology(); +} + template void MultibodyPlant::DeclareMiscContinuousStates() { DRAKE_DEMAND(!is_discrete()); @@ -1501,6 +1518,18 @@ void MultibodyPlant::Finalize() { // After finalizing the base class, the tree is read-only. internal::MultibodyTreeSystem::Finalize(); + // At Finalize(), multibody tree may create shadow links (when loop + // topology is allowed), which don't come through AddRigidBody() and so have + // no entries in the per-body geometry arrays yet. A shadow never carries any + // geometry of its own -- it's an internal modeling artifact that coincides + // with its primary link -- but these arrays are indexed by BodyIndex and so + // must stay dense over num_bodies(); see GetVisualGeometriesForBody(). Note + // that shadows deliberately get no SceneGraph frame: body_index_to_frame_id_ + // is map-keyed and is documented to tolerate bodies with no frame. + // TODO(sherm1) Give shadows a SceneGraph frame for visualization purposes. + visual_geometries_.resize(num_bodies()); + collision_geometries_.resize(num_bodies()); + if (geometry_source_is_registered()) { ApplyDefaultCollisionFilters(); } diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 0843f712e3ff..21df3a175a97 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -1823,6 +1823,28 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { void SetFuseWeldedLinks( bool fuse, std::optional model_instance = {}); + /// (Internal use only for now) For systems whose links and joints + /// form one or more kinematic loops (a "closed topology"), controls + /// whether Finalize() should deal with those automatically. The default + /// setting is _not_ to deal with kinematic loops (if one is encountered, + /// an exception is thrown). + /// + /// @note This feature is in development and is not yet functional. + /// + /// To deal with loops automatically requires modifying the system's topology + /// so that it is structured as a tree of links and joints, plus constraints + /// needed to enforce loop closure. Breaking a loop is done by splitting + /// a link within that loop. Mass properties are divided between the original + /// ("primary") link and the new ("shadow") link. Then a weld constraint is + /// added between the primary and shadow links to enforce loop closure. When + /// the weld constraint is satisfied, the original physics is restored. + /// + /// @param[in] enable Whether Finalize() should automatically model closed + /// kinematic loops rather than throwing. + /// @throws std::exception if called after Finalize(). + /// @see GetEnableLoopTopology(), Finalize() + void SetEnableLoopTopology(bool enable); + /// 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 @@ -1847,6 +1869,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { bool GetFuseWeldedLinks( std::optional model_instance = {}) const; + /// (Internal use only for now) Returns the current setting for whether + /// Finalize() automatically deals with closed-topology (looped) systems. + /// + /// @note This function can be called pre-Finalize() or post-Finalize(). + /// @see SetEnableLoopTopology(), Finalize() + bool GetEnableLoopTopology() 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. diff --git a/multibody/plant/test/auto_closed_topology_test.cc b/multibody/plant/test/auto_closed_topology_test.cc new file mode 100644 index 000000000000..2d05d559eef7 --- /dev/null +++ b/multibody/plant/test/auto_closed_topology_test.cc @@ -0,0 +1,579 @@ +/* Tests for automatic handling of closed-topology (looped) systems, enabled via +MultibodyPlant::SetEnableLoopTopology(). When enabled, Finalize() breaks each +kinematic loop using the shadow links and loop constraints produced by the +underlying LinkJointGraph/SpanningForest. This file tests that adding shadow +links splits mass properties properly among the primary link and all of its +shadows. + +Most tests use a planar four-bar linkage (three moving links -- driver, coupler, +rocker -- plus World, connected by four revolute joints) which forms a single +kinematic loop. See examples/multibody/four_bar for runnable examples of similar +mechanisms. The final test uses a double loop that gives one primary link two +shadows. */ + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/autodiff.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/geometry/scene_graph.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/internal_geometry_names.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/revolute_joint.h" +#include "drake/multibody/tree/rigid_body.h" +#include "drake/systems/framework/context.h" + +namespace drake { +namespace multibody { +namespace { + +/* A planar four-bar linkage described with no spanning tree specified, i.e. as +a raw loop. There are three moving links (driver, coupler, rocker) plus World +and four revolute joints; the linkage moves in the World x-z plane (so the +default -z gravity lies in its plane of motion) and all four revolute axes point +in +y, into the page as drawn below. + +The linkage is a parallelogram: the driver and the rocker are the same length, +so the coupler stays parallel to the ground and every pose in the file below is +exact. Note that this model is _assembled_ as written -- it has to be, since an +SDF joint has a single joint frame from which both of Drake's joint frames (on +parent and on child) are derived, making a parsed model loop-consistent at q = 0 +by construction. Watching a loop close from an unassembled start needs the C++ +API; see examples/multibody/four_bar. + +Each link's frame origin is at its inboard pivot; each mass center is at the +link's midpoint, with the rotational inertia of a uniform density thin rod +(mL^2/12). + + Dc,Co *==============================* Cr,Rc + | coupler C | + | 2m, 1kg | + driver D | | rocker R + 1m, 1kg | | 1m, 2kg + | | + Wd,Do *------------- Wo -------------* Wr,Ro Wz + World 2m | Wy + | / + +----- Wx + +In parent-child order the joints connect World-Do, World-Ro, Dc-Co, Cr-Rc. Each +label pair above marks a pivot, where the two named frames coincide in this +assembled configuration; Wd and Wr are the World-fixed pivots, which need no +frames of their own -- because each child link's origin sits at its pivot, +naming world puts the joint's World-side frame at the child +origin. */ +constexpr char kFourBarLoopSdf[] = R"""( + + + + + -1 0 0 0 0 0 + + 0 0 0.5 0 0 0 + 1 + + 0.0833333333333333 + 0.0833333333333333 + 0 + 0 0 0 + + + + + 0 0 1 0 0 0 + + + 1 0 0 0 0 0 + + 0 0 0.5 0 0 0 + 2 + + 0.1666666666666667 + 0.1666666666666667 + 0 + 0 0 0 + + + + + 0 0 1 0 0 0 + + + -1 0 1 0 0 0 + + 1 0 0 0 0 0 + 1 + + 0 + 0.3333333333333333 + 0.3333333333333333 + 0 0 0 + + + + + 2 0 0 0 0 0 + + + world + driver + 0 1 0 + + + world + rocker + 0 1 0 + + + Dc + coupler + 0 1 0 + + + Cr + Rc + 0 1 0 + + + +)"""; + +/* Builds and finalizes the four-bar loop plant, with automatic handling of +kinematic loops enabled. */ +std::unique_ptr> MakeFourBarPlant() { + auto plant = std::make_unique>(0.0 /* continuous */); + plant->SetEnableLoopTopology(true); + Parser(plant.get()).AddModelsFromString(kFourBarLoopSdf, "sdf"); + plant->Finalize(); + return plant; +} + +/* Builds and finalizes the four-bar loop plant, with automatic handling +of kinematic loops enabled. Registers the plant as a geometry source for +`scene_graph` and gives the driver, coupler, and rocker one arbitrary visual and +one arbitrary collision geometry apiece. (This is to test that shadow links +don't interfere with geometry access so the particular geometry doesn't +matter.) */ +std::unique_ptr> MakeFourBarPlantWithGeometry( + geometry::SceneGraph* scene_graph) { + auto plant = std::make_unique>(0.0 /* continuous */); + plant->SetEnableLoopTopology(true); + plant->RegisterAsSourceForSceneGraph(scene_graph); + Parser(plant.get()).AddModelsFromString(kFourBarLoopSdf, "sdf"); + for (const std::string name : {"driver", "coupler", "rocker"}) { + const Link& link = plant->GetBodyByName(name); + plant->RegisterVisualGeometry(link, math::RigidTransformd(), + geometry::Sphere(0.1), name + "_visual"); + plant->RegisterCollisionGeometry(link, math::RigidTransformd(), + geometry::Sphere(0.1), name + "_collision", + CoulombFriction(1.0, 1.0)); + } + plant->Finalize(); + return plant; +} + +/* Ensure there is one ephemeral (shadow) link in `plant`, i.e., the +link created when the coupler is split at Finalize() - and return it. */ +const Link& GetSoleShadowLink(const MultibodyPlant& plant) { + std::vector shadows; + for (LinkIndex i(0); i < plant.num_bodies(); ++i) { + if (plant.get_body(i).is_ephemeral()) shadows.push_back(i); + } + EXPECT_EQ(shadows.size(), 1); + // Shadow links are guaranteed to have higher indexes than any user link. + EXPECT_EQ(shadows[0], LinkIndex(plant.num_bodies() - 1)); + return plant.get_body(shadows[0]); +} + +/* The four-bar's kinematic loop is handled by splitting the coupler link's mass +equally into a primary and shadow link. The coupler link is split because the +SpanningForest minimizes the maximum branch length (hence it splits the middle +link): the coupler's primary link is reached via the driver and the coupler's +shadow link is reached via the rocker, yielding two length-2 branches. The +primary link is the coupler except half of its mass properties are given to +the coupler shadow link (which is named coupler$1). */ +GTEST_TEST(ClosedTopologyTest, CouplerIsSplit) { + std::unique_ptr> plant = MakeFourBarPlant(); + + // World + driver + coupler + rocker + the coupler's shadow. + EXPECT_EQ(plant->num_bodies(), 5); + + // There is exactly one shadow, and it is the coupler's. + const Link& shadow = GetSoleShadowLink(*plant); + EXPECT_EQ(shadow.name(), "coupler$1"); + EXPECT_TRUE(shadow.is_ephemeral()); + + // Every user-defined link remains non-ephemeral. + for (const char* name : {"driver", "coupler", "rocker"}) { + EXPECT_FALSE(plant->GetBodyByName(name).is_ephemeral()); + } +} + +/* Objects auto-created during Finalize() have to be marked as ephemeral, per +the MultibodyElement::is_ephemeral() contract. That includes link frames which +are co-created implicitly whenever a link is auto-created (in contrast to an +explicit user call to "add frame"). */ +GTEST_TEST(ClosedTopologyTest, ShadowLinkFramesAreEphemeral) { + std::unique_ptr> plant = MakeFourBarPlant(); + const Link& shadow = GetSoleShadowLink(*plant); + EXPECT_TRUE(shadow.body_frame().is_ephemeral()); + + // Since shadow links are auto-created (not user-created), every frame + // fixed to a shadow link is auto-created during Finalize(). For now, that + // is just the shadow's own link frame; when loop joints get retargeted + // onto the shadow they will bring their mobilizer frames along too. + int num_shadow_frames = 0; + for (FrameIndex index(0); index < plant->num_frames(); ++index) { + const Frame& frame = plant->get_frame(index); + if (frame.body().index() != shadow.index()) continue; + ++num_shadow_frames; + EXPECT_TRUE(frame.is_ephemeral()) << frame.name(); + } + EXPECT_GE(num_shadow_frames, 1); + + // The user's links keep non-ephemeral link frames. + EXPECT_FALSE(plant->world_body().body_frame().is_ephemeral()); + for (const char* name : {"driver", "coupler", "rocker"}) { + EXPECT_FALSE(plant->GetBodyByName(name).body_frame().is_ephemeral()); + } + + // Scalar conversion must carry the flags over; unlike the pre-finalize path, + // it creates the shadow's link frame by cloning rather than by adding a link. + std::unique_ptr> plant_ad = + systems::System::ToAutoDiffXd(*plant); + const Link& shadow_ad = plant_ad->GetBodyByName("coupler$1"); + EXPECT_TRUE(shadow_ad.is_ephemeral()); + EXPECT_TRUE(shadow_ad.body_frame().is_ephemeral()); + EXPECT_FALSE(plant_ad->GetBodyByName("coupler").body_frame().is_ephemeral()); +} + +/* Ensure the coupler link's mass properties is split evenly with its shadow +link. Note: The algorithm used here splits individual links, not mobods. +(Reminder: mobod mass properties may include multiple fused links). Here, the +coupler primary link and the coupler's shadow link coupler$1 each carry half of +the coupler's inertia (and are identical to each other, since their link frames +coincide). The user-facing per-link (default) mass is unchanged for the coupler +(its full declared 1 kg) while the shadow link's default reflects its half +share; the effective (split) inertia checked here is what drives the dynamics. +Also ensure the unsplit links (driver and rocker) are unaffected. */ +GTEST_TEST(ClosedTopologyTest, ShadowMassIsSplitEvenly) { + constexpr double kTol = 1e-14; + std::unique_ptr> plant = MakeFourBarPlant(); + auto context = plant->CreateDefaultContext(); + + const Link& coupler = plant->GetBodyByName("coupler"); + const Link& shadow = plant->GetBodyByName("coupler$1"); + + // The user-facing (model default) mass of the coupler is untouched (its full + // declared 1 kg). The shadow's default mass properties reflect its share of + // the even split: half the coupler's mass, with the same com and unit inertia + // (hence half the rotational inertia). This default is informational only; + // the effective split inertia checked below is what drives the dynamics. + EXPECT_EQ(coupler.default_mass(), 1.0); + EXPECT_EQ(shadow.default_mass(), 0.5); + EXPECT_TRUE( + CompareMatrices(shadow.default_com(), coupler.default_com(), kTol)); + EXPECT_TRUE(CompareMatrices( + shadow.default_rotational_inertia().CopyToFullMatrix3(), + 0.5 * coupler.default_rotational_inertia().CopyToFullMatrix3(), kTol)); + + // Ask each link for its own effective spatial inertia about its origin Lo, + // expressed in its link frame L. + auto M_LLo_L = [&](const Link& link) { + // This function reads the loop-split value from the FrameBodyPoseCache. + return link.CalcSpatialInertiaInBodyFrame(*context); + }; + const SpatialInertia M_LLo_L_coupler = M_LLo_L(coupler); + const SpatialInertia M_LLo_L_shadow = M_LLo_L(shadow); + + // The coupler's primary link and its shadow link each carry half of + // the coupler's 1 kg, have colocated centers of mass, and have + // equal rotational inertia (their link frames coincide). + EXPECT_NEAR(M_LLo_L_coupler.get_mass(), 0.5, kTol); + EXPECT_NEAR(M_LLo_L_shadow.get_mass(), 0.5, kTol); + EXPECT_TRUE(CompareMatrices(M_LLo_L_shadow.get_com(), + M_LLo_L_coupler.get_com(), kTol)); + EXPECT_TRUE(CompareMatrices( + M_LLo_L_shadow.CalcRotationalInertia().CopyToFullMatrix3(), + M_LLo_L_coupler.CalcRotationalInertia().CopyToFullMatrix3(), kTol)); + + // The coupler's primary link and its shadow link each have half the + // user-declared coupler's rotational inertia about Lo, expressed in L. + const Matrix3 half_I_LLo_L = + 0.5 * coupler.default_rotational_inertia().CopyToFullMatrix3(); + EXPECT_TRUE(CompareMatrices( + M_LLo_L_coupler.CalcRotationalInertia().CopyToFullMatrix3(), half_I_LLo_L, + kTol)); + EXPECT_TRUE(CompareMatrices( + M_LLo_L_shadow.CalcRotationalInertia().CopyToFullMatrix3(), half_I_LLo_L, + kTol)); + + // The unsplit links keep their full mass (driver 1 kg, rocker 2 kg). + EXPECT_NEAR(M_LLo_L(plant->GetBodyByName("driver")).get_mass(), 1.0, kTol); + EXPECT_NEAR(M_LLo_L(plant->GetBodyByName("rocker")).get_mass(), 2.0, kTol); +} + +/* A shadow link has no spatial-inertia parameter of its own; its effective mass +properties are sourced from the FrameBodyPoseCache (mirroring its primary's +share of the split). Consequently the per-body context accessors report the +split value for both the primary and the shadow, and system-wide mass +aggregates (which loop over all links, shadows included) count the physical mass +exactly once rather than double-counting the shadow. */ +GTEST_TEST(ClosedTopologyTest, ShadowHasNoIndependentInertiaParameter) { + constexpr double kTol = 1e-14; + std::unique_ptr> plant = MakeFourBarPlant(); + auto context = plant->CreateDefaultContext(); + + const Link& coupler = plant->GetBodyByName("coupler"); + const Link& shadow = plant->GetBodyByName("coupler$1"); + + // The context accessor reports each copy's split share (0.5 kg), not the + // coupler's full declared 1 kg. (default_mass() still reports the declared + // 1 kg for the primary; see ShadowMassIsSplitEvenly.) + EXPECT_NEAR(coupler.get_mass(*context), 0.5, kTol); + EXPECT_NEAR(shadow.get_mass(*context), 0.5, kTol); + + // Ensure the system's total mass properly accounts for the split-link + // coupler (0.5 * primary_link_mass + 0.5 * shadow_link_mass = 1 kg), + // so total mass is: driver 1 kg + rocker 2 kg + coupler 1 kg = 4 kg. + EXPECT_NEAR(plant->CalcTotalMass(*context), 4.0, kTol); +} + +/* Because the split is recomputed in the FrameBodyPoseCache from the primary's +parameter, changing the primary's mass at runtime re-splits across the shadow +automatically -- the primary is the single source of truth. Setting mass on the +shadow directly is disallowed. */ +GTEST_TEST(ClosedTopologyTest, RuntimeMassChangeReSplitsAndShadowIsReadOnly) { + constexpr double kTol = 1e-14; + std::unique_ptr> plant = MakeFourBarPlant(); + auto context = plant->CreateDefaultContext(); + + const Link& coupler = plant->GetBodyByName("coupler"); + const Link& shadow = plant->GetBodyByName("coupler$1"); + + // Set the coupler's (physical) mass to 3 kg; the split follows to 1.5 kg on + // each of the coupler and its shadow, and the total tracks accordingly + // (driver 1 + rocker 2 + coupler 3 = 6 kg). + coupler.SetMass(context.get(), 3.0); + EXPECT_NEAR(coupler.get_mass(*context), 1.5, kTol); + EXPECT_NEAR(shadow.get_mass(*context), 1.5, kTol); + EXPECT_NEAR(plant->CalcTotalMass(*context), 6.0, kTol); + + // The shadow's mass properties are not independently settable. + DRAKE_EXPECT_THROWS_MESSAGE(shadow.SetMass(context.get(), 1.0), + ".*coupler\\$1.*ephemeral shadow link.*"); +} + +/* A shadow link carries no geometry of its own. It is an internal modeling +artifact coincident with its primary link, but it must still have an (empty) +entry in the plant's per-body geometry arrays, which are indexed by BodyIndex +and so must stay dense over num_bodies(). Shadow links are created inside +MultibodyTree::Finalize() rather than by MultibodyPlant::AddRigidBody() (which +is what normally extends those arrays), so Finalize() has to extend them. */ +GTEST_TEST(ClosedTopologyTest, ShadowLinkHasEmptyGeometryEntries) { + geometry::SceneGraph scene_graph; + std::unique_ptr> plant = + MakeFourBarPlantWithGeometry(&scene_graph); + + const Link& coupler = plant->GetBodyByName("coupler"); + const Link& shadow = plant->GetBodyByName("coupler$1"); + + // The primary link keeps the geometry registered on it. + EXPECT_EQ(plant->GetVisualGeometriesForBody(coupler).size(), 1); + EXPECT_EQ(plant->GetCollisionGeometriesForBody(coupler).size(), 1); + + // The shadow link has an empty entry. Without that empty entry, + // these two lookups would read past the end of the per-body arrays. + EXPECT_TRUE(plant->GetVisualGeometriesForBody(shadow).empty()); + EXPECT_TRUE(plant->GetCollisionGeometriesForBody(shadow).empty()); + + // Breaking the loop doesn't invent geometry: driver, coupler, rocker have + // one visual and one collision geometry each. + EXPECT_EQ(plant->num_visual_geometries(), 3); + EXPECT_EQ(plant->num_collision_geometries(), 3); + + // A shadow gets no SceneGraph frame of its own; reporting a pose for it would + // publish a spurious duplicate of its primary. That's safe precisely because + // the frame id table is map-keyed and documented to tolerate bodies with no + // frame -- unlike the dense per-body arrays checked above. + EXPECT_TRUE(plant->GetBodyFrameIdIfExists(coupler.index()).has_value()); + EXPECT_FALSE(plant->GetBodyFrameIdIfExists(shadow.index()).has_value()); + + // The per-body arrays are copied wholesale during scalar conversion, so the + // converted plant must agree with the tree it carries as well. + std::unique_ptr> plant_ad = + systems::System::ToAutoDiffXd(*plant); + EXPECT_EQ(plant_ad->num_bodies(), plant->num_bodies()); + EXPECT_TRUE( + plant_ad->GetVisualGeometriesForBody(plant_ad->GetBodyByName("coupler$1")) + .empty()); + EXPECT_TRUE( + plant_ad + ->GetCollisionGeometriesForBody(plant_ad->GetBodyByName("coupler$1")) + .empty()); +} + +/* Several post-finalize consumers (visualization helpers in particular) walk +every BodyIndex in [0, num_bodies()) and ask the plant for that body's +geometry. Before shadow links were given per-body geometry entries, those walks +read past the end of the arrays once a loop had been broken. Note that the +out-of-range read is undefined behavior rather than an exception, so this test +earns its keep in debug builds (where the density assertions in the accessors +fire) and under the memory sanitizers. */ +GTEST_TEST(ClosedTopologyTest, WalkingEveryBodyForGeometryStaysInRange) { + geometry::SceneGraph scene_graph; + std::unique_ptr> plant = + MakeFourBarPlantWithGeometry(&scene_graph); + + // Summing per-body counts over all bodies (shadows included) must reproduce + // the plant-wide totals -- i.e. every body has an entry and no geometry is + // counted twice. + int num_visual = 0; + int num_collision = 0; + for (BodyIndex i(0); i < plant->num_bodies(); ++i) { + const Link& link = plant->get_body(i); + num_visual += ssize(plant->GetVisualGeometriesForBody(link)); + num_collision += ssize(plant->GetCollisionGeometriesForBody(link)); + } + EXPECT_EQ(num_visual, plant->num_visual_geometries()); + EXPECT_EQ(num_collision, plant->num_collision_geometries()); + + // GeometryNames performs exactly that walk; it is the path taken by contact + // visualization (see also ContactResultsToLcmSystem). + internal::GeometryNames geometry_names; + EXPECT_NO_THROW(geometry_names.ResetBasic(*plant)); +} + +/* This is the same floating double loop as SpanningForest.DoubleLoop. The +middle branch reaches link6 first; when the other two branches subsequently +reach it, link6 is split twice to close the two loops: + + link2 ------> link5 + ^ | + | v + link1 -> link3 -> link6 + | ^ + v | + link4 ------> link7 + +After Finalize(), link6, link6$1, and link6$2 each carry one third of link6's +mass properties. */ +GTEST_TEST(ClosedTopologyTest, OneLinkCanHaveTwoShadows) { + constexpr double kTol = 1e-14; + constexpr double kLink6Mass = 6.0; + MultibodyPlant plant(0.0 /* continuous */); + plant.SetEnableLoopTopology(true); + + const SpatialInertia unit_inertia = + SpatialInertia::MakeUnitary(); + const SpatialInertia link6_inertia = + SpatialInertia::MakeFromCentralInertia( + kLink6Mass, Vector3(0.1, -0.2, 0.3), + kLink6Mass * UnitInertia::SolidSphere(0.4)); + + // Index bodies from 1 to match the LinkIndex values in DoubleLoop. + for (int i = 1; i <= 7; ++i) { + const Link& link = plant.AddRigidBody( + "link" + std::to_string(i), i == 6 ? link6_inertia : unit_inertia); + EXPECT_EQ(link.index(), LinkIndex(i)); + } + + // Preserve DoubleLoop's joint order; it determines branch and shadow order. + const std::vector> joints{{1, 2}, {1, 4}, {1, 3}, {2, 5}, + {4, 7}, {3, 6}, {5, 6}, {7, 6}}; + for (int i = 0; i < ssize(joints); ++i) { + const auto [parent, child] = joints[i]; + plant.AddJoint( + "joint" + std::to_string(i), plant.get_body(LinkIndex(parent)), {}, + plant.get_body(LinkIndex(child)), {}, Vector3::UnitZ()); + } + + plant.Finalize(); + + // Before testing MultibodyPlant's handling of multiple shadows, confirm our + // assumption about the topology produced for this graph. These are fatal + // checks because the remaining test is meaningful only if link6 was split + // twice as we are expecting, with the expected number of links and joints. + ASSERT_NO_THROW(plant.GetBodyByName("link6$2")); + // World + seven user links + two shadows; the ninth joint is the ephemeral + // floating joint that attaches link1 to World. + ASSERT_EQ(plant.num_bodies(), 10); + ASSERT_EQ(plant.num_joints(), 9); + + // We got the topology we're expecting. + const LinkIndex primary_index(6); + const LinkIndex shadow1_index(8); + const LinkIndex shadow2_index(9); + auto context = plant.CreateDefaultContext(); + + const Link& primary = plant.GetBodyByName("link6"); + const Link& shadow1 = plant.GetBodyByName("link6$1"); + const Link& shadow2 = plant.GetBodyByName("link6$2"); + EXPECT_EQ(primary.index(), primary_index); + EXPECT_EQ(shadow1.index(), shadow1_index); + EXPECT_EQ(shadow2.index(), shadow2_index); + EXPECT_FALSE(primary.is_ephemeral()); + EXPECT_TRUE(shadow1.is_ephemeral()); + EXPECT_TRUE(shadow2.is_ephemeral()); + EXPECT_NE(primary.mobod_index(), shadow1.mobod_index()); + EXPECT_NE(primary.mobod_index(), shadow2.mobod_index()); + EXPECT_NE(shadow1.mobod_index(), shadow2.mobod_index()); + + // The primary retains the user's declared defaults. Each shadow's defaults + // report its one-third share, including the rotational inertia about Lo. + EXPECT_EQ(primary.default_mass(), kLink6Mass); + for (const Link* shadow : {&shadow1, &shadow2}) { + EXPECT_NEAR(shadow->default_mass(), kLink6Mass / 3.0, kTol); + EXPECT_TRUE( + CompareMatrices(shadow->default_com(), primary.default_com(), kTol)); + EXPECT_TRUE(CompareMatrices( + shadow->default_rotational_inertia().CopyToFullMatrix3(), + primary.default_rotational_inertia().CopyToFullMatrix3() / 3.0, kTol)); + } + + // Effective mass properties are split equally among the primary and its two + // shadows. Summing over the plant still counts link6's physical mass once. + const Matrix3 one_third_I_LLo_L = + primary.default_rotational_inertia().CopyToFullMatrix3() / 3.0; + for (const Link* copy : {&primary, &shadow1, &shadow2}) { + const SpatialInertia M_LLo_L = + copy->CalcSpatialInertiaInBodyFrame(*context); + EXPECT_NEAR(M_LLo_L.get_mass(), kLink6Mass / 3.0, kTol); + EXPECT_TRUE( + CompareMatrices(M_LLo_L.get_com(), primary.default_com(), kTol)); + EXPECT_TRUE( + CompareMatrices(M_LLo_L.CalcRotationalInertia().CopyToFullMatrix3(), + one_third_I_LLo_L, kTol)); + } + EXPECT_NEAR(plant.CalcTotalMass(*context), 12.0, kTol); + + // As in RuntimeMassChangeReSplitsAndShadowIsReadOnly, changing the primary's + // mass at runtime re-splits its mass properties. Here the new 12 kg mass and + // correspondingly scaled rotational inertia are divided among three copies. + constexpr double kUpdatedLink6Mass = 12.0; + primary.SetMass(context.get(), kUpdatedLink6Mass); + for (const Link* copy : {&primary, &shadow1, &shadow2}) { + const SpatialInertia M_LLo_L = + copy->CalcSpatialInertiaInBodyFrame(*context); + EXPECT_NEAR(M_LLo_L.get_mass(), kUpdatedLink6Mass / 3.0, kTol); + EXPECT_TRUE( + CompareMatrices(M_LLo_L.get_com(), primary.default_com(), kTol)); + EXPECT_TRUE(CompareMatrices( + M_LLo_L.CalcRotationalInertia().CopyToFullMatrix3(), + (kUpdatedLink6Mass / kLink6Mass) * one_third_I_LLo_L, kTol)); + } + EXPECT_NEAR(plant.CalcTotalMass(*context), 18.0, kTol); +} + +} // namespace +} // namespace multibody +} // namespace drake diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc index 9e01d523cf7d..ce38cf3682e7 100644 --- a/multibody/plant/test/multibody_plant_test.cc +++ b/multibody/plant/test/multibody_plant_test.cc @@ -4851,6 +4851,39 @@ GTEST_TEST(MultibodyPlantTest, BaseBodyJointChoice) { } } +// Verify that SetEnableLoopTopology() properly sets its setting and that it +// cannot be used post-Finalize(). Verify that when enabled, there is proper +// scalar conversion (which clones the underlying MultibodyTree). +GTEST_TEST(MultibodyPlantTest, EnableLoopTopologyOption) { + MultibodyPlant plant(0.0); + + // Verify the default setting is disabled (for now). + EXPECT_FALSE(plant.GetEnableLoopTopology()); + + // Verify that pre-Finalize(), the setting can be enabled and/or disabled. + plant.SetEnableLoopTopology(true); + EXPECT_TRUE(plant.GetEnableLoopTopology()); + plant.SetEnableLoopTopology(false); + EXPECT_FALSE(plant.GetEnableLoopTopology()); + + // Leave it enabled and finalize a simple (loop-free) model. + plant.SetEnableLoopTopology(true); + plant.AddRigidBody("body", SpatialInertia::MakeUnitary()); + plant.Finalize(); + EXPECT_TRUE(plant.GetEnableLoopTopology()); + + // Verify that post-Finalize(), cannot _call_ SetEnableLoopTopology(). + DRAKE_EXPECT_THROWS_MESSAGE(plant.SetEnableLoopTopology(false), + ".*is_finalized.*"); + DRAKE_EXPECT_THROWS_MESSAGE(plant.SetEnableLoopTopology(true), + ".*is_finalized.*"); + + // The setting survives scalar conversion, which clones the tree. + std::unique_ptr> ad_plant = + systems::System::ToAutoDiffXd(plant); + EXPECT_TRUE(ad_plant->GetEnableLoopTopology()); +} + GTEST_TEST(SetRandomTest, QuaternionFloatingBody) { // Create a model that contains a single body B. MultibodyPlant plant(0.0); diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 172984c9bf84..c4064aff87bb 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -549,8 +549,11 @@ const Link& MultibodyTree::GetLinkByName( } template -const Link& MultibodyTree::AddLinkImpl(std::unique_ptr> link) { - if (is_finalized()) { +const Link& MultibodyTree::AddLinkImpl(std::unique_ptr> link, + bool is_ephemeral) { + // Auto-created ("ephemeral") shadow links can be added during Finalize() + // (post-BuildForest). All other links must be added pre-finalize. + if (!is_ephemeral && is_finalized()) { throw std::logic_error( "This MultibodyTree is finalized already. " "Therefore adding more bodies is not allowed. " @@ -571,11 +574,13 @@ const Link& MultibodyTree::AddLinkImpl(std::unique_ptr> link) { // The LinkJointGraph should already contain only World. DRAKE_DEMAND(ssize(link_joint_graph_.links()) == 1); DRAKE_DEMAND(link_joint_graph_.link_by_index(link_index).name() == "world"); - } else { - // Make note in the graph of the new rigid link. + } else if (!is_ephemeral) { + // Make note in the graph of the new rigid link. Ephemeral links (e.g. + // shadow links) were created by BuildForest() and are already in the graph. link_joint_graph_.AddLink(link->name(), link->model_instance()); } + link->set_is_ephemeral(is_ephemeral); link->set_parent_tree(this, link_index); // MultibodyTree can access selected private methods in RigidBody through its // LinkAttorney. @@ -584,12 +589,24 @@ const Link& MultibodyTree::AddLinkImpl(std::unique_ptr> link) { &internal::LinkAttorney::get_mutable_link_frame(link.get()); const FrameIndex link_frame_index(num_frames()); link_frame->set_parent_tree(this, link_frame_index); + // Whether or not a LinkFrame is "ephemeral" depends on its link since the + // link_frame comes into existence with the link. + link_frame->set_is_ephemeral(is_ephemeral); DRAKE_DEMAND(link_frame->name() == link->name()); frames_.AddBorrowed(link_frame); // - Register link. return links_.Add(std::move(link)); } +template +const RigidBody& MultibodyTree::AddEphemeralLink( + const std::string& name, ModelInstanceIndex model_instance, + const SpatialInertia& M_BBo_B) { + DRAKE_DEMAND(model_instance < num_model_instances()); + return AddLinkImpl(std::make_unique>(name, model_instance, M_BBo_B), + true /* ephemeral */); +} + template const Joint& MultibodyTree::GetJointByNameImpl( std::string_view name, @@ -993,6 +1010,12 @@ bool MultibodyTree::GetFuseWeldedLinks( ForestBuildingOptions::kFuseWeldedLinksAssemblies); } +template +void MultibodyTree::SetEnableLoopTopology(bool enable) { + DRAKE_THROW_UNLESS(!is_finalized()); + enable_loop_topology_ = enable; +} + template void MultibodyTree::Finalize() { DRAKE_MBT_THROW_IF_FINALIZED(); @@ -1030,16 +1053,58 @@ void MultibodyTree::Finalize() { process (BuildForest()), which augmented the graph with them. We call those "ephemeral" elements. */ - // TODO(sherm1) Add shadow links and loop constraints. - if (!graph.loop_constraints().empty()) { + // TODO(sherm1) Move joints to the shadow links and add loop constraints. + + if (!enable_loop_topology_ && !graph.loop_constraints().empty()) { link_joint_graph_.InvalidateForest(); throw std::runtime_error(fmt::format( - "The bodies and joints of this system form one or " - "more loops in the system graph. Drake currently does not " - "support automatic modeling of such systems; however, they " - "can be modeled with some input changes. See " - "https://drake.mit.edu/troubleshooting.html" - "#mbp-loops-in-graph for advice on how to model systems with loops.")); + "The bodies and joints of this system form one or more loops in the " + "system graph. Automatic modeling of such systems is disabled by " + "default; you can enable it by calling " + "MultibodyPlant::SetEnableLoopTopology(true) before Finalize() " + "but: the feature is in development and not yet functional even when " + "enabled. For now, loops can be modeled with some input changes. See " + "https://drake.mit.edu/troubleshooting.html#mbp-loops-in-graph " + "for advice on how to model systems with loops.")); + } + + /* Add the shadow Links. BuildForest() creates a shadow Link for each + kinematic loop it breaks and that shadow link is needed for a corresponding + subsequent Joint implementation (which will reference the shadow link). + This is all needed before building BodyNodes. We add them in graph order, + immediately after the user links, so that each new shadow link's index + matches its graph shadow-link index. + + We give each shadow a default spatial inertia equal to the primary link's + share of the even split -- mass/(N+1) with the same com and unit inertia, + where N is the primary's shadow count -- so that the shadow's user-facing + default reflects the split rather than being zero. This default is + informational only; the effective runtime inertias of both the primary and its + shadows are still (re)computed in CalcFrameBodyPoses() by dividing the + primary's parameterized inertia, which remains the single source of truth for + the split (so runtime changes to the primary's mass re-split correctly). */ + // TODO(sherm1) Consider making shadow link inertias NaN and prohibiting + // anyone from asking about shadow default mass properties since they are + // not used at all. + for (LinkOrdinal ordinal(graph.num_user_links()); ordinal < graph.num_links(); + ++ordinal) { + const LinkJointGraph::Link& graph_link = graph.links(ordinal); + DRAKE_DEMAND(!graph_link.is_world()); + DRAKE_DEMAND(graph_link.is_shadow()); + const LinkIndex primary_index = graph_link.primary_link(); + const LinkJointGraph::Link& primary_link = + graph.link_by_index(primary_index); + DRAKE_DEMAND(!primary_link.is_world()); // Shouldn't ever split World. + DRAKE_DEMAND(!primary_link.is_shadow()); + const int num_copies = primary_link.num_shadows() + 1; + const SpatialInertia& M_primary = + links_.get_element(primary_index).default_spatial_inertia(); + const SpatialInertia M_shadow(M_primary.get_mass() / num_copies, + M_primary.get_com(), + M_primary.get_unit_inertia()); + const RigidBody& shadow = AddEphemeralLink( + graph_link.name(), graph_link.model_instance(), M_shadow); + DRAKE_DEMAND(shadow.index() == graph_link.index()); } /* Add the ephemeral Joints. */ @@ -1745,10 +1810,29 @@ void MultibodyTree::CalcFrameBodyPoses( // 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); + // Mass properties are split evenly among a primary link and any shadow + // links that were created to break loops. We source the inertia from the + // primary link (a shadow link's default spatial inertia is ignored) and + // divide by the number of copies (primary + shadows). Because a shadow's + // link frame coincides with its primary's (they are welded at identity), + // the body-frame inertia values are identical, so a shadow and its + // primary end up with the same split M_LLo_L. A link with no shadows is + // its own primary with num_copies == 1, i.e. unchanged. + const LinkJointGraph::Link& graph_link = forest().links(link_ordinal); + const LinkIndex primary_index = graph_link.primary_link(); + const int num_copies = + graph().link_by_index(primary_index).num_shadows() + 1; + const Link& source_link = links_.get_element(primary_index); + // Read the primary's inertia straight from its parameter (the source of + // truth) rather than via CalcSpatialInertiaInBodyFrame(), which now reads + // back from the very cache we are building here. + SpatialInertia M_LLo_L = + source_link.CalcSpatialInertiaInBodyFrameFromParameters(context); + if (num_copies > 1) { + M_LLo_L = + SpatialInertia(M_LLo_L.get_mass() / num_copies, + M_LLo_L.get_com(), M_LLo_L.get_unit_inertia()); + } 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). @@ -4449,6 +4533,12 @@ std::unique_ptr> MultibodyTree::CloneToScalar() // The graph and its forest model are scalar type-independent. tree_clone->link_joint_graph_ = this->link_joint_graph_; + // Copy the enable_loop_topology setting so GetEnableLoopTopology() reports + // the same value on the clone. Since cloning only occurs post-Finalize(), + // this flag no longer affects anything about the cloned system (the forest is + // already built); we carry it along purely for reporting consistency. + tree_clone->enable_loop_topology_ = this->enable_loop_topology_; + // Fill the `frame_` collection with nulls. We'll be cloning the frames out // of order, so we can't just append them to the end like we do with the // other kinds of elements. @@ -4560,6 +4650,10 @@ RigidBody* MultibodyTree::CloneBodyAndAdd( &internal::LinkAttorney::get_mutable_link_frame(body_clone.get()); body_frame_clone->set_parent_tree(this, body_frame_index); body_frame_clone->set_model_instance(body.model_instance()); + // Cloning doesn't go through AddLinkImpl(), so carry the link frame's + // ephemeral flag over here (the link's own flag is copied by its + // CloneToScalar()). + body_frame_clone->set_is_ephemeral(body.body_frame().is_ephemeral()); // The order in which frames are added into frames_ is important to keep the // topology invariant. Therefore we index new clones according to the diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index 53a50183070a..e0e749b412dc 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -998,6 +998,12 @@ class MultibodyTree { bool GetFuseWeldedLinks( std::optional model_instance = {}) const; + // See MultibodyPlant API. + void SetEnableLoopTopology(bool enable); + + // See MultibodyPlant API. + bool GetEnableLoopTopology() const { return enable_loop_topology_; } + // 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 @@ -2573,7 +2579,34 @@ class MultibodyTree { // Takes ownership of `link` and adds it to this MultibodyTree. Returns a // constant reference to the link just added, which will remain valid for the // lifetime of this MultibodyTree. Public members AddLink() end up here. - const Link& AddLinkImpl(std::unique_ptr> link); + // If `is_ephemeral` is true this Link was auto-created (not user-created), + // that is, a shadow link used to break a kinematic loop. If auto-created, the + // Link is already in the LinkJointGraph, so don't register it again there. + const Link& AddLinkImpl(std::unique_ptr> link, + bool is_ephemeral = false); + + // (Internal use only) Add a shadow Link (RigidBody) that is auto-created + // when dealing with a kinematic loop (such links are called "ephemeral"). + // + // @param[in] name is the shadow link's name, which is based on the name + // of its primary link. For example, if the primary link is named Fred + // then the first shadow link would be called Fred$1. + // @param[in] model_instance The index of the model instance. + // @param[in] M_BBo_B is the shadow link's default spatial inertia, which + // (at best) is informational ONLY. For internal computational purposes, + // the actual spatial inertia of the shadow link is computed in + // CalcFrameBodyPoses() by dividing the primary link's parameterized + // spatial inertia and assigning an appropriate amount to the shadow link. + // @returns A constant reference to the new shadow link just added. Its + // LinkIndex will match the graph's shadow link index provided + // ephemeral links are added in graph order immediately after all user + // links. + // @throws std::exception if model_instance is not a valid index. + // TODO(sherm1) Consider making the inertia NaN here and forbidding anyone + // from asking about it, since it is never used. + const RigidBody& AddEphemeralLink(const std::string& name, + ModelInstanceIndex model_instance, + const SpatialInertia& M_BBo_B); const Joint& GetJointByNameImpl(std::string_view, std::optional) const; @@ -2925,6 +2958,9 @@ class MultibodyTree { Vector3>>> default_body_poses_; + // See MultibodyPlant::SetEnableLoopTopology() for the meaning of this flag. + bool enable_loop_topology_{false}; + // Back pointer to the owning MultibodyTreeSystem. const MultibodyTreeSystem* tree_system_{}; diff --git a/multibody/tree/rigid_body.cc b/multibody/tree/rigid_body.cc index 917941382ccc..a7409f2355fc 100644 --- a/multibody/tree/rigid_body.cc +++ b/multibody/tree/rigid_body.cc @@ -2,6 +2,8 @@ #include +#include + #include "drake/multibody/tree/model_instance.h" namespace drake { @@ -119,6 +121,13 @@ void RigidBody::DoSetTopology() { template void RigidBody::DoDeclareParameters( internal::MultibodyTreeSystem* tree_system) { + // Ephemeral shadow links (created to break kinematic loops) deliberately have + // no spatial-inertia parameter: their mass properties are not independently + // settable but instead mirror their primary link's parameterized inertia + // (split evenly in CalcFrameBodyPoses()). Leaving the parameter index invalid + // makes any attempt to read or write their parameter fail loudly. + if (this->is_ephemeral()) return; + // Sets model values to dummy values to indicate that the model values are // not used. This class stores the the default values of the parameters. // 10 numeric values are used to store mass, center of mass, moments and @@ -130,6 +139,10 @@ void RigidBody::DoDeclareParameters( template void RigidBody::DoSetDefaultParameters( systems::Parameters* parameters) const { + // Ephemeral shadow links have no spatial-inertia parameter (see + // DoDeclareParameters()), so there is nothing to default. + if (this->is_ephemeral()) return; + // Set the default spatial inertia. systems::BasicVector& spatial_inertia_parameter = parameters->get_mutable_numeric_parameter( @@ -143,10 +156,22 @@ template void RigidBody::ThrowIfNotFinalized(const char* source_method) const { DRAKE_THROW_UNLESS(this->has_parent_tree()); if (!this->get_parent_tree().is_finalized()) { - throw std::runtime_error( - "From '" + std::string(source_method) + - "'. The model to which this rigid body belongs must be finalized. " - "See MultibodyPlant::Finalize()."); + throw std::runtime_error(fmt::format( + "From '{}'. The model to which this rigid body belongs must be " + "finalized. See MultibodyPlant::Finalize().", + source_method)); + } +} + +template +void RigidBody::ThrowIfEphemeralInertia(const char* source_method) const { + if (this->is_ephemeral()) { + throw std::logic_error(fmt::format( + "{}(): RigidBody '{}' is an ephemeral shadow link created " + "automatically to break a kinematic loop; its mass properties are not " + "independently settable. They mirror its primary link and are managed " + "internally. Set the primary link's mass properties instead.", + source_method, this->name())); } } @@ -155,6 +180,7 @@ void RigidBody::SetCenterOfMassInBodyFrameNoModifyInertia( systems::Context* context, const Vector3& center_of_mass_position) const { DRAKE_THROW_UNLESS(context != nullptr); + ThrowIfEphemeralInertia(__func__); const T& x = center_of_mass_position(0); const T& y = center_of_mass_position(1); const T& z = center_of_mass_position(2); @@ -172,6 +198,7 @@ template void RigidBody::SetUnitInertiaAboutBodyOrigin( systems::Context* context, const UnitInertia& G_BBo_B) const { DRAKE_THROW_UNLESS(context != nullptr); + ThrowIfEphemeralInertia(__func__); const T& Gxx = G_BBo_B(0, 0); const T& Gyy = G_BBo_B(1, 1); const T& Gzz = G_BBo_B(2, 2); @@ -199,6 +226,7 @@ void RigidBody::SetCenterOfMassInBodyFrameAndPreserveCentralInertia( systems::Context* context, const Vector3& center_of_mass_position) const { DRAKE_THROW_UNLESS(context != nullptr); + ThrowIfEphemeralInertia(__func__); // Get B's initial spatial inertia about Bo (before Bcm changes location). // Get pi_BoBcm_B position from Bo to Bcm before Bcm changes location. diff --git a/multibody/tree/rigid_body.h b/multibody/tree/rigid_body.h index 2317ff236b70..c7b253614ce0 100644 --- a/multibody/tree/rigid_body.h +++ b/multibody/tree/rigid_body.h @@ -440,10 +440,18 @@ class RigidBody : public MultibodyElement { /// Gets this %RigidBody's (%Link's) mass from the given context. /// @param[in] context contains the state of the multibody system. /// @pre the context makes sense for use by this %RigidBody. + // TODO(sherm1) Consider disallowing mass property inquiries for shadow links + // and instead attributing all the mass to the primary link. const T& get_mass(const systems::Context& context) const { - const systems::BasicVector& spatial_inertia_parameter = - context.get_numeric_parameter(spatial_inertia_parameter_index_); - return internal::parameter_conversion::GetMass(spatial_inertia_parameter); + // Sourced from the FrameBodyPoseCache rather than directly from the + // parameter so that links split to break a kinematic loop (a primary and + // its shadows) report their share of the even mass split. For an ordinary + // (unsplit) link this equals the parameter value. See + // CalcSpatialInertiaInBodyFrame() for details. + return this->get_parent_tree() + .EvalFrameBodyPoses(context) + .get_M_LLo_L(this->ordinal()) + .get_mass(); } /// Returns the pose `X_WB` of this %RigidBody (%Link) B in the world frame W @@ -535,10 +543,13 @@ class RigidBody : public MultibodyElement { /// @pre the context makes sense for use by this %RigidBody. Vector3 CalcCenterOfMassInBodyFrame( const systems::Context& context) const { - const systems::BasicVector& spatial_inertia_parameter = - context.get_numeric_parameter(spatial_inertia_parameter_index_); - return internal::parameter_conversion::GetCenterOfMass( - spatial_inertia_parameter); + // Sourced from the FrameBodyPoseCache; see get_mass() and + // CalcSpatialInertiaInBodyFrame(). (A mass split changes only the mass, not + // the center of mass, but we go through the same cache for uniformity.) + return this->get_parent_tree() + .EvalFrameBodyPoses(context) + .get_M_LLo_L(this->ordinal()) + .get_com(); } /// Calculates %RigidBody (%Link) B's center of mass Bcm's translational @@ -568,10 +579,36 @@ class RigidBody : public MultibodyElement { /// the position vector from Bo to Bcm (B's center of mass), and G_BBo_B /// (B's unit inertia about Bo expressed in B). /// @pre the context makes sense for use by this %RigidBody. + // TODO(sherm1) Consider disallowing mass property inquiries for shadow links + // and instead attributing all the mass to the primary link. SpatialInertia CalcSpatialInertiaInBodyFrame( const systems::Context& context) const { + // Sourced from the FrameBodyPoseCache (its per-link M_LLo_L), NOT directly + // from the spatial-inertia parameter. This matters for links that were + // split to break a kinematic loop: a primary link and each of its shadows + // carry an even share (mass/(N+1)) of the primary's parameterized inertia, + // and the cache is what holds those shares. For an ordinary (unsplit) link + // the cached value is exactly the parameter value. Ephemeral shadow links + // have no parameter of their own, so this cache-based accessor is the only + // way to query their (split) inertia. The cache itself is built in + // CalcFrameBodyPoses() from each primary's parameter, read via + // CalcSpatialInertiaInBodyFrameFromParameters(). + return this->get_parent_tree().EvalFrameBodyPoses(context).get_M_LLo_L( + this->ordinal()); + } + + // (Internal use only) Returns this link's spatial inertia M_BBo_B taken + // directly from the Context parameter (the user-set value), WITHOUT the + // even mass-split applied to loop-broken links. This is the authoritative + // source from which CalcFrameBodyPoses() builds the FrameBodyPoseCache; every + // other caller should use CalcSpatialInertiaInBodyFrame() (which returns the + // split-adjusted value). Only valid for links that own a spatial-inertia + // parameter, i.e. non-ephemeral links (ephemeral shadow links have none). + SpatialInertia CalcSpatialInertiaInBodyFrameFromParameters( + const systems::Context& context) const { // TODO(joemasterjohn): Speed this up when we can store a reference to a // SpatialInertia as an abstract parameter. + DRAKE_ASSERT(!this->is_ephemeral()); const systems::BasicVector& spatial_inertia_parameter = context.get_numeric_parameter(spatial_inertia_parameter_index_); return internal::parameter_conversion::ToSpatialInertia( @@ -586,8 +623,11 @@ class RigidBody : public MultibodyElement { /// I_BBo_B (B's rotational inertia about Bo, expressed in B). /// @pre the context makes sense for use by this RigidBody. /// @throws std::exception if context is null. + /// @throws std::exception if this is an ephemeral shadow link (its mass + /// properties are not independently settable). void SetMass(systems::Context* context, const T& mass) const { DRAKE_THROW_UNLESS(context != nullptr); + ThrowIfEphemeralInertia(__func__); systems::BasicVector& spatial_inertia_parameter = context->get_mutable_numeric_parameter( spatial_inertia_parameter_index_); @@ -607,6 +647,8 @@ class RigidBody : public MultibodyElement { /// or SetCenterOfMassInBodyFrameAndPreserveCentralInertia(). /// @pre the context makes sense for use by this %RigidBody. /// @throws std::exception if context is null. + /// @throws std::exception if this is an ephemeral shadow link (its mass + /// properties are not independently settable). /// @warning Do not use this function unless it is needed (think twice). // TODO(Mitiguy) Consider deprecating this function. void SetCenterOfMassInBodyFrame(systems::Context* context, @@ -628,6 +670,8 @@ class RigidBody : public MultibodyElement { /// Consider using SetSpatialInertiaInBodyFrame() instead. /// @pre the context makes sense for use by this RigidBody. /// @throws std::exception if context is null. + /// @throws std::exception if this is an ephemeral shadow link (its mass + /// properties are not independently settable). void SetCenterOfMassInBodyFrameAndPreserveCentralInertia( systems::Context* context, const Vector3& center_of_mass_position) const; @@ -641,9 +685,12 @@ class RigidBody : public MultibodyElement { /// (B's unit inertia about Bo expressed in B). /// @pre the context makes sense for use by this %RigidBody. /// @throws std::exception if context is null. + /// @throws std::exception if this is an ephemeral shadow link (its mass + /// properties are not independently settable). void SetSpatialInertiaInBodyFrame(systems::Context* context, const SpatialInertia& M_Bo_B) const { DRAKE_THROW_UNLESS(context != nullptr); + ThrowIfEphemeralInertia(__func__); systems::BasicVector& spatial_inertia_parameter = context->get_mutable_numeric_parameter( spatial_inertia_parameter_index_); @@ -785,6 +832,12 @@ class RigidBody : public MultibodyElement { // that the error message can include that detail. void ThrowIfNotFinalized(const char* source_method) const; + // Helper for the mass-property setters: throws if this is an ephemeral shadow + // link, which has no spatial-inertia parameter of its own (its mass + // properties mirror its primary link and are managed internally). The + // invoking method should pass its name for the error message. + void ThrowIfEphemeralInertia(const char* source_method) const; + // For this RigidBody B, set its center of mass position stored in context // to center_of_mass_position, but does not modify other inertia properties. // @param[in, out] context contains the state of the multibody system. @@ -798,6 +851,8 @@ class RigidBody : public MultibodyElement { // checked via CalcSpatialInertiaInBodyFrame().IsPhysicallyValid(). // @pre the context makes sense for use by this %RigidBody. // @throws std::exception if context is null. + // @throws std::exception if this is an ephemeral shadow link (its mass + // properties are not independently settable). void SetCenterOfMassInBodyFrameNoModifyInertia( systems::Context* context, const Vector3& center_of_mass_position) const; @@ -812,6 +867,8 @@ class RigidBody : public MultibodyElement { // checked via CalcSpatialInertiaInBodyFrame().IsPhysicallyValid(). // @pre the context makes sense for use by this %RigidBody. // @throws std::exception if context is null. + // @throws std::exception if this is an ephemeral shadow link (its mass + // properties are not independently settable). void SetUnitInertiaAboutBodyOrigin(systems::Context* context, const UnitInertia& G_BBo_B) const; @@ -820,8 +877,11 @@ class RigidBody : public MultibodyElement { std::unique_ptr> TemplatedDoCloneToScalar( const internal::MultibodyTree& tree_clone) const { unused(tree_clone); - return std::make_unique>(this->name(), - default_spatial_inertia_); + auto clone = std::make_unique>( + this->name(), default_spatial_inertia_); + // Preserve the ephemeral flag (e.g. for shadow links added to break loops). + clone->set_is_ephemeral(this->is_ephemeral()); + return clone; } // MultibodyTree has access to the mutable LinkFrame through LinkAttorney.