From da8c85712f8cd498a7a1a83b2b2b5c078abc1883 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Thu, 6 Aug 2026 17:38:06 -0700 Subject: [PATCH] Add weld constraints and enable shadow visualization. Co-Authored-By: Claude Opus 5 (1M context) --- .../generated_docstrings/multibody_plant.h | 21 +- .../generated_docstrings/multibody_tree.h | 8 + bindings/pydrake/multibody/plant_py.cc | 6 + bindings/pydrake/multibody/test/plant_test.py | 5 + multibody/plant/constraint_specs.h | 8 + multibody/plant/multibody_plant.cc | 84 ++- multibody/plant/multibody_plant.h | 26 +- .../plant/test/auto_closed_topology_test.cc | 569 ++++++++++++++---- multibody/topology/link_joint_graph.h | 4 + multibody/topology/link_joint_graph_inlines.h | 4 + multibody/tree/multibody_tree.cc | 21 +- multibody/tree/rigid_body.h | 7 + visualization/inertia_visualizer.cc | 12 +- visualization/test/inertia_visualizer_test.cc | 63 ++ 14 files changed, 680 insertions(+), 158 deletions(-) diff --git a/bindings/generated_docstrings/multibody_plant.h b/bindings/generated_docstrings/multibody_plant.h index bd39e54e858f..9acaf965b810 100644 --- a/bindings/generated_docstrings/multibody_plant.h +++ b/bindings/generated_docstrings/multibody_plant.h @@ -8236,7 +8236,10 @@ calls will always return the same value.)"""; struct /* num_constraints */ { // Source: drake/multibody/plant/multibody_plant.h const char* doc = -R"""(Returns the total number of constraints specified by the user.)"""; +R"""(Returns the total number of constraints in this model. Prior to +Finalize() these are just the constraints specified by the user. +Finalize() may add "ephemeral" constraints of its own; see +num_loop_constraints().)"""; } num_constraints; // Symbol: drake::multibody::MultibodyPlant::num_coupler_constraints struct /* num_coupler_constraints */ { @@ -8277,6 +8280,17 @@ R"""(Returns the number of joints in the model. See also: AddJoint().)"""; } num_joints; + // Symbol: drake::multibody::MultibodyPlant::num_loop_constraints + struct /* num_loop_constraints */ { + // Source: drake/multibody/plant/multibody_plant.h + const char* doc = +R"""(Returns the number of ephemeral weld constraints that Finalize() added +in order to close topological loops. Each of these welds a shadow link +to the link it is a copy of; see SetEnableLoopTopology(). These are +included in num_constraints() and num_weld_constraints(), and are +indistinguishable from user-added welds to the constraint solvers. +Returns zero prior to Finalize().)"""; + } num_loop_constraints; // Symbol: drake::multibody::MultibodyPlant::num_misc_continuous_states struct /* num_misc_continuous_states */ { // Source: drake/multibody/plant/multibody_plant.h @@ -8366,7 +8380,10 @@ calls will always return the same value.)"""; struct /* num_weld_constraints */ { // Source: drake/multibody/plant/multibody_plant.h const char* doc = -R"""(Returns the total number of weld constraints specified by the user.)"""; +R"""(Returns the total number of weld constraints. Before Finalize() these +are just the weld constraints specified by the user. Finalize() may +add "ephemeral" weld constraints of its own; see +num_loop_constraints().)"""; } num_weld_constraints; // Symbol: drake::multibody::MultibodyPlant::physical_models struct /* physical_models */ { diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index fc744842398f..5e9c09c9a459 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -7750,6 +7750,14 @@ base body*, meaning it had no explicit joint to a parent body and is mobilized by an automatically-added (ephemeral) floating (6 dof) joint to World. +Note: + A Link can be a floating base body even though a user-defined + joint names it as a child link, if that joint does not connect it + to World: when a closed kinematic loop has no joint to World at + all, one of the loop's links must still serve as a base body. What + this flag reports is that the Link is mobilized by an ephemeral + floating joint, not that the user left it jointless. + Note: A floating base body is not necessarily modeled with a quaternion mobilizer, see has_quaternion_dofs(). Alternative options include diff --git a/bindings/pydrake/multibody/plant_py.cc b/bindings/pydrake/multibody/plant_py.cc index f466582290f5..71643cbaa868 100644 --- a/bindings/pydrake/multibody/plant_py.cc +++ b/bindings/pydrake/multibody/plant_py.cc @@ -220,6 +220,8 @@ void DoScalarDependentDefinitions(py::module_ m, T) { cls_doc.num_force_elements.doc) .def("num_constraints", &Class::num_constraints, cls_doc.num_constraints.doc) + .def("num_loop_constraints", &Class::num_loop_constraints, + cls_doc.num_loop_constraints.doc) .def("num_model_instances", &Class::num_model_instances, cls_doc.num_model_instances.doc) .def("num_positions", @@ -257,6 +259,10 @@ void DoScalarDependentDefinitions(py::module_ m, T) { .def("SetUseSampledOutputPorts", &Class::SetUseSampledOutputPorts, py::arg("use_sampled_output_ports"), cls_doc.SetUseSampledOutputPorts.doc) + .def("SetEnableLoopTopology", &Class::SetEnableLoopTopology, + py::arg("enable"), cls_doc.SetEnableLoopTopology.doc) + .def("GetEnableLoopTopology", &Class::GetEnableLoopTopology, + cls_doc.GetEnableLoopTopology.doc) .def( "AddJoint", [](Class* self, const Joint& joint) { diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py index 050dcc7bed9f..14a11a37da91 100644 --- a/bindings/pydrake/multibody/test/plant_test.py +++ b/bindings/pydrake/multibody/test/plant_test.py @@ -199,6 +199,11 @@ def test_multibody_plant_construction_api(self, T): plant.SetUseSampledOutputPorts(use_sampled_output_ports=False) self.assertEqual(plant.has_sampled_output_ports(), False) self.assertEqual(plant.time_step(), 0.0) + self.assertEqual(plant.GetEnableLoopTopology(), False) + plant.SetEnableLoopTopology(enable=True) + self.assertEqual(plant.GetEnableLoopTopology(), True) + plant.SetEnableLoopTopology(enable=False) + self.assertEqual(plant.num_loop_constraints(), 0) spatial_inertia = SpatialInertia.NaN() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) body.default_mass() diff --git a/multibody/plant/constraint_specs.h b/multibody/plant/constraint_specs.h index 25f22a4ed2d0..4a9862865e15 100644 --- a/multibody/plant/constraint_specs.h +++ b/multibody/plant/constraint_specs.h @@ -82,6 +82,14 @@ struct WeldConstraintSpec { BodyIndex body_B; // Index of body B. math::RigidTransform X_BQ; // Pose of frame Q in B's body frame. MultibodyConstraintId id; // Id of this constraint in the plant. + + // Whether this constraint was added by MultibodyPlant::Finalize() rather than + // by the user; currently that means it is closing a topological loop that was + // broken by splitting a link into a primary and a shadow. Analogous to + // MultibodyElement::is_ephemeral(). + // TODO(sherm1) When the constraint specs acquire a common base class (see + // #21415), move this flag and `id` there. + bool is_ephemeral{false}; }; // Struct to store the specification for a fixed constraint between vertices of diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 262d0732fd2f..e55822f44d35 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -1519,17 +1519,30 @@ void MultibodyPlant::Finalize() { 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. + // topology is allowed). Those don't come through AddRigidBody() so have + // no entries in the per-body geometry arrays yet. Add additional entries + // for them now to keep these arrays properly sized. visual_geometries_.resize(num_bodies()); collision_geometries_.resize(num_bodies()); + // Next, register the shadow links with SceneGraph so that we can visualize + // them. This enables Meshcat to visualize the shadow link inertia ellipsoids, + // and can be used to add visual geometry to the shadow links if desired + // (typically for pedagogical or debugging purposes -- shadows links are + // normally invisible). + for (BodyIndex index(0); index < num_bodies(); ++index) { + const RigidBody& body = get_body(index); + if (body.is_ephemeral()) { + RegisterRigidBodyWithSceneGraph(body); // A no-op if no SceneGraph. + } + } + + // Add the weld constraints that reconnect primary links to their shadow links + // if we had to break topological loops. This must happen before + // FinalizePlantOnly() below, so we have a chance to declare the parameters + // that determine constraint active/deactivated status. + AddEphemeralLoopConstraints(); + if (geometry_source_is_registered()) { ApplyDefaultCollisionFilters(); } @@ -1637,6 +1650,31 @@ void MultibodyPlant::SetUpJointLimitsParameters() { } } +template +void MultibodyPlant::AddEphemeralLoopConstraints() { + // A shadow link's link frame is coincident with its primary's by + // construction, so the constrained frames P and Q are just the two body + // frames and both offsets are the identity. The graph documents that the + // primary link is always the parent, which sets the sign convention for the + // constraint multipliers. + // N.B. We add the spec directly rather than calling AddWeldConstraint(), + // which is a pre-finalize-only API. + for (const internal::LinkJointGraph::LoopConstraint& loop_constraint : + internal_tree().graph().loop_constraints()) { + const MultibodyConstraintId id = MultibodyConstraintId::get_new_id(); + internal::WeldConstraintSpec spec{ + .body_A = loop_constraint.primary_link(), + .X_AP = math::RigidTransform::Identity(), + .body_B = loop_constraint.shadow_link(), + .X_BQ = math::RigidTransform::Identity(), + .id = id, + .is_ephemeral = true}; + // A shadow link is by definition distinct from its primary. + DRAKE_DEMAND(spec.IsValid()); + weld_constraints_specs_[id] = spec; + } +} + template void MultibodyPlant::FinalizeConstraints() { for (auto& [constraint_id, spec] : ball_constraints_specs_) { @@ -3467,12 +3505,32 @@ void MultibodyPlant::ThrowIfUnsupportedContinuousTimeDynamics( // TODO(#23759,#23760,#23762,#23763,#23992): revisit this check and error // message as constraints are implemented for CENIC. if (num_constraints() > 0) { - throw std::logic_error( + // Loop constraints are not the user's doing -- Finalize() added them to + // close the kinematic loops in the model -- so say where they came from + // rather than leaving the user hunting for constraints they never added. + const int num_loops = num_loop_constraints(); + const char* s = num_constraints() == 1 ? "" : "s"; + std::string constraints; + if (num_loops == 0) { + constraints = fmt::format("{} constraint{}", num_constraints(), s); + } else if (num_loops == num_constraints()) { + constraints = fmt::format( + "{} constraint{}, which Finalize() added in order to close the " + "kinematic loops in this model (see SetEnableLoopTopology())", + num_loops, s); + } else { + constraints = fmt::format( + "{} constraint{}, {} of which Finalize() added in order to close the " + "kinematic loops in this model (see SetEnableLoopTopology())", + num_constraints(), s, num_loops); + } + throw std::logic_error(fmt::format( "Currently this MultibodyPlant is set to use continuous time. " - "Continuous time does not support constraints. Use a discrete time " - "model and set_discrete_contact_approximation() to set a model " - "approximation that uses the SAP solver instead (kSap, kSimilar, or " - "kLagged)."); + "Continuous time does not support constraints, but this model has {}. " + "Use a discrete time model and set_discrete_contact_approximation() to " + "set a model approximation that uses the SAP solver instead (kSap, " + "kSimilar, or kLagged).", + constraints)); } // TODO(#24061): consider rejecting models with joint limits here, once CENIC diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index a173ad91ffcc..18a8897ecff6 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -1937,13 +1937,26 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// MultibodyConstraintManager class to consolidate constraint management. --> /// @{ - /// Returns the total number of constraints specified by the user. + /// Returns the total number of constraints in this model. Prior to + /// Finalize() these are just the constraints specified by the user. + /// Finalize() may add "ephemeral" constraints of its own; see + /// num_loop_constraints(). int num_constraints() const { return num_coupler_constraints() + num_distance_constraints() + num_ball_constraints() + num_weld_constraints() + num_tendon_constraints(); } + /// Returns the number of ephemeral weld constraints that Finalize() added in + /// order to close topological loops. Each of these welds a shadow link to the + /// link it is a copy of; see SetEnableLoopTopology(). These are included in + /// num_constraints() and num_weld_constraints(), and are indistinguishable + /// from user-added welds to the constraint solvers. Returns zero prior to + /// Finalize(). + int num_loop_constraints() const { + return internal_tree().graph().num_loop_constraints(); + } + /// Returns a list of all constraint identifiers. The returned vector becomes /// invalid after any calls to Add*Constraint() or RemoveConstraint(). std::vector GetConstraintIds() const; @@ -1961,7 +1974,9 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { /// Returns the total number of ball constraints specified by the user. int num_ball_constraints() const { return ssize(ball_constraints_specs_); } - /// Returns the total number of weld constraints specified by the user. + /// Returns the total number of weld constraints. Before Finalize() these are + /// just the weld constraints specified by the user. Finalize() may add + /// "ephemeral" weld constraints of its own; see num_loop_constraints(). int num_weld_constraints() const { return ssize(weld_constraints_specs_); } /// Returns the total number of tendon constraints specified by the @@ -6291,6 +6306,13 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem { // that still guarantees stability. void SetUpJointLimitsParameters(); + // Adds an ephemeral weld constraint for each loop constraint the modeler + // introduced when it broke a closed kinematic loop by splitting a link into + // a primary link and a shadow link. Called during Finalize(), after the tree + // (and hence the shadow links) has been finalized, but before + // FinalizePlantOnly() declares the constraint parameters. + void AddEphemeralLoopConstraints(); + // Some constraints support std::optional specs, which implies that the // kinematics should be used to compute values such that the constraint is // satisfied by the default context at the moment Finalize() is called. This diff --git a/multibody/plant/test/auto_closed_topology_test.cc b/multibody/plant/test/auto_closed_topology_test.cc index d5d8497c0158..0b5ce61b0a04 100644 --- a/multibody/plant/test/auto_closed_topology_test.cc +++ b/multibody/plant/test/auto_closed_topology_test.cc @@ -1,19 +1,26 @@ /* 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, and that the loop joint is retargeted from the primary link onto its -shadow. +underlying LinkJointGraph/SpanningForest. This file tests that + - adding shadow links splits mass properties properly among the primary + link and all of its shadows, + - splitting a primary link into more than two pieces works, + - loop joints are correctly retargeted from a frame on the primary to + a corresponding frame on the shadow, + - the loop weld constraints are added to the plant and correctly joint + the primary and shadow links together at their origins, and + - the activate/deactivate parameters are properly allocated for the + loop weld constraints and work properly. 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. */ +mechanisms. The multiple-shadow links test uses a double loop that gives one +primary link two shadows. */ #include #include +#include #include #include #include @@ -23,12 +30,14 @@ shadows. */ #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/kinematics_vector.h" #include "drake/geometry/scene_graph.h" #include "drake/geometry/shape_specification.h" #include "drake/math/autodiff.h" #include "drake/math/rigid_transform.h" #include "drake/math/roll_pitch_yaw.h" #include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/constraint_specs.h" #include "drake/multibody/plant/internal_geometry_names.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/tree/fixed_offset_frame.h" @@ -37,6 +46,7 @@ shadows. */ #include "drake/multibody/tree/rigid_body.h" #include "drake/multibody/tree/shadow_frame.h" #include "drake/systems/framework/context.h" +#include "drake/systems/framework/diagram_builder.h" namespace drake { namespace multibody { @@ -153,39 +163,36 @@ constexpr char kFourBarLoopSdf[] = R"""( )"""; -/* 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)); +/* A four-bar loop model with automatic loop modeling enabled: a MultibodyPlant +and a SceneGraph, with the driver and the coupler (the link we know gets split; +see CouplerIsSplit) each given one visual and one collision geometry. The plant +is left _unfinalized_, since Finalize() is what does the loop modeling these +tests are about; each test finalizes when it is ready. The DiagramBuilder is +kept only to own the two systems -- the tests work with the plant and its +Context directly rather than building a diagram. */ +class FourBar { + public: + explicit FourBar(double time_step = 0.0 /* continuous */) { + plant_ = &AddMultibodyPlantSceneGraph(&builder_, time_step).plant; + MultibodyPlant& plant = *plant_; + plant.SetEnableLoopTopology(true); + Parser(&plant).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; -} + + MultibodyPlant& plant() { return *plant_; } + + private: + systems::DiagramBuilder builder_; + MultibodyPlant* 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. */ @@ -208,19 +215,21 @@ 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(); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); // World + driver + coupler + rocker + the coupler's shadow. - EXPECT_EQ(plant->num_bodies(), 5); + EXPECT_EQ(plant.num_bodies(), 5); // There is exactly one shadow, and it is the coupler's. - const Link& shadow = GetSoleShadowLink(*plant); + 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()); + EXPECT_FALSE(plant.GetBodyByName(name).is_ephemeral()); } } @@ -229,18 +238,19 @@ 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); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + 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(). That includes - // the shadow link's own link frame, and a joint frame that was originally - // on the primary link. A mobilizer implementing the joint may also add - // an ephemeral frame of its own, but doesn't in this case. + // fixed to a shadow link is auto-created during Finalize(): the + // shadow's own link frame, plus the frames the retargeted loop joint's + // mobilizer needed on the shadow side. int num_shadow_frames = 0; - for (FrameIndex index(0); index < plant->num_frames(); ++index) { - const Frame& frame = plant->get_frame(index); + 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(); @@ -248,15 +258,15 @@ GTEST_TEST(ClosedTopologyTest, ShadowLinkFramesAreEphemeral) { EXPECT_GE(num_shadow_frames, 2); // The user's links keep non-ephemeral link frames. - EXPECT_FALSE(plant->world_body().body_frame().is_ephemeral()); + 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()); + 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); + 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()); @@ -268,17 +278,20 @@ 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. */ +are intended to 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(); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); - const Link& coupler = plant->GetBodyByName("coupler"); - const Link& shadow = plant->GetBodyByName("coupler$1"); + 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 @@ -304,7 +317,7 @@ GTEST_TEST(ClosedTopologyTest, ShadowMassIsSplitEvenly) { // 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). + // equal rotational inertia (their link frames are intended to 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(), @@ -325,8 +338,8 @@ GTEST_TEST(ClosedTopologyTest, ShadowMassIsSplitEvenly) { 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); + 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 @@ -337,11 +350,13 @@ 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(); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); - const Link& coupler = plant->GetBodyByName("coupler"); - const Link& shadow = plant->GetBodyByName("coupler$1"); + 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 @@ -352,7 +367,7 @@ GTEST_TEST(ClosedTopologyTest, ShadowHasNoIndependentInertiaParameter) { // 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); + EXPECT_NEAR(plant.CalcTotalMass(*context), 4.0, kTol); } /* Because the split is recomputed in the FrameBodyPoseCache from the primary's @@ -361,11 +376,13 @@ 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(); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); - const Link& coupler = plant->GetBodyByName("coupler"); - const Link& shadow = plant->GetBodyByName("coupler$1"); + 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 @@ -373,7 +390,7 @@ GTEST_TEST(ClosedTopologyTest, RuntimeMassChangeReSplitsAndShadowIsReadOnly) { 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); + 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), @@ -381,45 +398,39 @@ GTEST_TEST(ClosedTopologyTest, RuntimeMassChangeReSplitsAndShadowIsReadOnly) { } /* 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. */ +artifact intended to coincide 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); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); - const Link& coupler = plant->GetBodyByName("coupler"); - const Link& shadow = plant->GetBodyByName("coupler$1"); + 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); + 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()); + 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()); + EXPECT_EQ(plant.num_visual_geometries(), 3); + EXPECT_EQ(plant.num_collision_geometries(), 3); // 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()); + systems::System::ToAutoDiffXd(plant); + EXPECT_EQ(plant_ad->num_bodies(), plant.num_bodies()); EXPECT_TRUE( plant_ad->GetVisualGeometriesForBody(plant_ad->GetBodyByName("coupler$1")) .empty()); @@ -429,6 +440,62 @@ GTEST_TEST(ClosedTopologyTest, ShadowLinkHasEmptyGeometryEntries) { .empty()); } +/* A shadow link does, however, get a SceneGraph frame of its own. It is a body +like any other: it has a pose, which agrees with its primary's only to the +extent that the loop-closing weld constraint is satisfied -- an unassembled +model can start far from that. Nothing hangs geometry on that frame (a shadow +carries none, per the test above), but SceneGraph is where a body's pose is +published, and a consumer that walks the bodies looking for something to draw +has nothing to attach to without it. Like the geometry arrays above, this is +bookkeeping that AddRigidBody() would normally do, so Finalize() has to. */ +GTEST_TEST(ClosedTopologyTest, ShadowLinkHasItsOwnSceneGraphFrame) { + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + + const Link& coupler = plant.GetBodyByName("coupler"); + const Link& shadow = plant.GetBodyByName("coupler$1"); + + const std::optional shadow_frame = + plant.GetBodyFrameIdIfExists(shadow.index()); + ASSERT_TRUE(shadow_frame.has_value()); + + // It is the shadow's own frame, not a second reference to its primary's ... + const geometry::FrameId coupler_frame = + plant.GetBodyFrameIdOrThrow(coupler.index()); + EXPECT_NE(*shadow_frame, coupler_frame); + + // ... and it maps back to the shadow link. + EXPECT_EQ(plant.GetBodyFromFrameId(*shadow_frame), &shadow); + + // The pose port reports that frame, with a pose that really is the shadow's + // own. To be sure we aren't just looking at a copy of the primary's pose, we + // move the loop's two branches independently: the driver carries the primary + // coupler, while the rocker carries the shadow. + std::unique_ptr> context = + plant.CreateDefaultContext(); + plant.GetJointByName("world_driver") + .set_angle(context.get(), 0.3); + plant.GetJointByName("world_rocker") + .set_angle(context.get(), -0.4); + + const geometry::FramePoseVector& poses = + plant.get_geometry_pose_output_port() + .Eval>(*context); + ASSERT_TRUE(poses.has_id(*shadow_frame)); + EXPECT_TRUE( + poses.value(*shadow_frame) + .IsExactlyEqualTo(plant.EvalBodyPoseInWorld(*context, shadow))); + EXPECT_FALSE(poses.value(*shadow_frame) + .IsNearlyEqualTo(poses.value(coupler_frame), 1e-10)); + + // Frame ids are copied wholesale during scalar conversion (where Finalize() + // does not re-run), so the shadow keeps the frame it was given here. + std::unique_ptr> plant_ad = + systems::System::ToAutoDiffXd(plant); + EXPECT_EQ(plant_ad->GetBodyFrameIdIfExists(shadow.index()), shadow_frame); +} + /* 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 @@ -437,27 +504,27 @@ 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); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); // 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)); + 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()); + 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)); + EXPECT_NO_THROW(geometry_names.ResetBasic(plant)); } /* This is the same floating double loop as SpanningForest.DoubleLoop. The @@ -607,27 +674,28 @@ RetargetedJoint GetSoleRetargetedJoint(const MultibodyPlant& plant) { } /* Loop breaking cuts the coupler into a primary and a shadow link, which means -one of the two joints attached to the coupler must be re-aimed at the shadow. +one of the two joints attached to the coupler must be re-targeted to the shadow. The forest reaches the primary through the driver, so it is the coupler end of the "coupler_rocker" joint that has to move; its mobilizer must then take its outboard frame from the shadow link rather than from the coupler. We check that structurally rather than by comparing poses. On this model the -substitution is numerically a no-op: a shadow's link frame coincides with its -primary's, and neither the coupler nor its shadow is fused into a welded -composite, so the substituted frame's pose in its body frame is identical to -the user frame's. Only the structure -- which link the mobilizer's frame is -fixed to -- distinguishes a retargeted joint from one that was left pointing at -the primary. (In a debug build BodyNodeImpl also asserts this invariant, -frame_M.body() == body_B, but we want a check that holds in every build.) */ +substitution is numerically a no-op: a shadow's link frame is intended to +coincide with its primary's, and neither the coupler nor its shadow is fused +into a welded composite, so the substituted frame's pose in its link frame is +identical to the pose of the user's frame on the primary link. Only the +structure -- which link the mobilizer's frame is fixed to -- distinguishes a +retargeted joint from one that was left pointing at the primary. */ GTEST_TEST(ClosedTopologyTest, LoopJointIsRetargetedToTheShadowLink) { - std::unique_ptr> plant = MakeFourBarPlant(); - const Link& coupler = plant->GetBodyByName("coupler"); - const Link& rocker = plant->GetBodyByName("rocker"); - const Link& shadow = GetSoleShadowLink(*plant); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + const Link& coupler = plant.GetBodyByName("coupler"); + const Link& rocker = plant.GetBodyByName("rocker"); + const Link& shadow = GetSoleShadowLink(plant); // It is the coupler_rocker joint's parent (coupler) end that got moved. - const RetargetedJoint retargeted = GetSoleRetargetedJoint(*plant); + const RetargetedJoint retargeted = GetSoleRetargetedJoint(plant); ASSERT_NE(retargeted.joint, nullptr); const Joint& loop_joint = *retargeted.joint; EXPECT_EQ(loop_joint.name(), "coupler_rocker"); @@ -673,11 +741,13 @@ the joint would no longer connect what the user asked it to. This is a parameter-level relationship, so we can see it without evaluating kinematics. */ GTEST_TEST(ClosedTopologyTest, MovingThePrimaryFrameMovesTheShadowFrame) { constexpr double kTol = 1e-14; - std::unique_ptr> plant = MakeFourBarPlant(); - auto context = plant->CreateDefaultContext(); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); - const Link& shadow = GetSoleShadowLink(*plant); - const Joint& loop_joint = plant->GetJointByName("coupler_rocker"); + const Link& shadow = GetSoleShadowLink(plant); + const Joint& loop_joint = plant.GetJointByName("coupler_rocker"); // The parser materialized this joint's frame on the coupler as a // FixedOffsetFrame, whose offset is a Context parameter we can change. const auto& Jp = dynamic_cast&>( @@ -694,9 +764,10 @@ GTEST_TEST(ClosedTopologyTest, MovingThePrimaryFrameMovesTheShadowFrame) { loop_joint.GetMobilizerInUse().outboard_frame(); ASSERT_EQ(frame_M.body().index(), shadow.index()); - // The shadow-side frame starts out coincident with the user's frame. Each - // pose is measured in its own link frame, and those two link frames coincide, - // so coincident frames have equal poses. + // The shadow-side frame starts out with the same pose as the user's frame. + // Each pose is measured in its own link frame, and those two link frames are + // intended to coincide (when the weld constraint is satisfied) so the two + // local poses will be the same. const math::RigidTransformd X_CJp = Jp.CalcPoseInBodyFrame(*context); EXPECT_TRUE(CompareMatrices( shadow_frame.CalcPoseInBodyFrame(*context).GetAsMatrix34(), @@ -730,9 +801,11 @@ converted plant must come out modeling the broken loop the same way, with the delegation intact. */ GTEST_TEST(ClosedTopologyTest, RetargetingSurvivesScalarConversion) { constexpr double kTol = 1e-14; - std::unique_ptr> plant = MakeFourBarPlant(); + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); std::unique_ptr> plant_ad = - systems::System::ToAutoDiffXd(*plant); + systems::System::ToAutoDiffXd(plant); const Link& shadow_ad = plant_ad->GetBodyByName("coupler$1"); EXPECT_TRUE(shadow_ad.is_ephemeral()); @@ -844,6 +917,240 @@ GTEST_TEST(ClosedTopologyTest, ShadowFrameKeepsItsModelInstanceWhenCloned) { plant_ad->GetFrameByName(shadow_frame.name(), rocker_instance).index(), shadow_frame.index()); } + +/* Returns the sole ephemeral weld constraint spec in `plant`, i.e. the one +Finalize() added to close the loop. */ +const internal::WeldConstraintSpec& GetSoleLoopConstraintSpec( + const MultibodyPlant& plant) { + std::vector found; + for (const auto& [id, spec] : plant.get_weld_constraint_specs()) { + EXPECT_EQ(spec.id, id); + if (spec.is_ephemeral) found.push_back(&spec); + } + EXPECT_EQ(found.size(), 1); + static const internal::WeldConstraintSpec kEmpty; + return found.empty() ? kEmpty : *found.front(); +} + +/* Breaking the loop leaves the coupler and its shadow as two disconnected +copies of the same link; a weld constraint between them is what actually closes +the loop. The user didn't ask for that constraint, so it is marked ephemeral, +and because both copies share a link frame it welds the two body frames +directly (no offsets). */ +GTEST_TEST(ClosedTopologyTest, LoopIsClosedByAnEphemeralWeldConstraint) { + // Nothing exists before Finalize(): the loop constraints are a product of + // forest building, and the user added no constraints of their own. + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + EXPECT_EQ(plant.num_loop_constraints(), 0); + EXPECT_EQ(plant.num_constraints(), 0); + + plant.Finalize(); + + // The single loop of the four-bar costs a single weld constraint, which is + // reported by the plant-wide count like any other. + EXPECT_EQ(plant.num_loop_constraints(), 1); + EXPECT_EQ(plant.num_weld_constraints(), 1); + EXPECT_EQ(plant.num_constraints(), 1); + EXPECT_EQ(plant.GetConstraintIds().size(), 1); + + const internal::WeldConstraintSpec& spec = GetSoleLoopConstraintSpec(plant); + EXPECT_EQ(plant.GetConstraintIds().at(0), spec.id); + + // The primary link is body A (the parent), which fixes the sign convention + // for the constraint's multipliers. + const Link& coupler = plant.GetBodyByName("coupler"); + const Link& shadow = GetSoleShadowLink(plant); + EXPECT_EQ(spec.body_A, coupler.index()); + EXPECT_EQ(spec.body_B, shadow.index()); + + // The welded frames are the two body frames themselves: a shadow's link + // frame is intended to coincide with its primary link's frame, so both + // offsets are the identity. + EXPECT_TRUE(spec.X_AP.IsExactlyIdentity()); + EXPECT_TRUE(spec.X_BQ.IsExactlyIdentity()); +} + +/* An ephemeral loop constraint is an ordinary weld constraint in every respect +except that the user didn't add it, so it has to coexist with -- and be +distinguishable from -- the user's own weld constraints. */ +GTEST_TEST(ClosedTopologyTest, LoopConstraintCoexistsWithUserConstraints) { + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + const MultibodyConstraintId user_id = plant.AddWeldConstraint( + plant.GetBodyByName("driver"), math::RigidTransformd(), + plant.GetBodyByName("rocker"), math::RigidTransformd()); + EXPECT_EQ(plant.num_weld_constraints(), 1); + plant.Finalize(); + + // The user's weld plus the loop-closing weld. + EXPECT_EQ(plant.num_weld_constraints(), 2); + EXPECT_EQ(plant.num_constraints(), 2); + EXPECT_EQ(plant.num_loop_constraints(), 1); + + // The user's constraint is untouched and is not confused for an ephemeral + // one; GetSoleLoopConstraintSpec() checks the converse, that exactly one of + // the two is ephemeral. + EXPECT_FALSE(plant.get_weld_constraint_specs(user_id).is_ephemeral); + EXPECT_NE(GetSoleLoopConstraintSpec(plant).id, user_id); +} + +/* Loop constraints are created by Finalize(), which a scalar-converted plant +does not re-run: it copies the constraint specs and finalizes only the plant +itself. So the converted plant must come out with the same constraints, under +the same ids (which the Context's active status map is keyed on). */ +GTEST_TEST(ClosedTopologyTest, LoopConstraintSurvivesScalarConversion) { + FourBar four_bar; + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + std::unique_ptr> plant_ad = + systems::System::ToAutoDiffXd(plant); + + // In particular, no second copy of the loop weld was added on the way. + EXPECT_EQ(plant_ad->num_weld_constraints(), 1); + EXPECT_EQ(plant_ad->num_constraints(), 1); + + // The graph (and hence the loop constraint count, which is sourced from it) + // is cloned along with the tree. + EXPECT_EQ(plant_ad->num_loop_constraints(), 1); + + const internal::WeldConstraintSpec& spec = GetSoleLoopConstraintSpec(plant); + const auto& specs_ad = plant_ad->get_weld_constraint_specs(); + ASSERT_TRUE(specs_ad.contains(spec.id)); + const internal::WeldConstraintSpec& spec_ad = specs_ad.at(spec.id); + EXPECT_TRUE(spec_ad.is_ephemeral); + EXPECT_EQ(spec_ad.body_A, spec.body_A); + EXPECT_EQ(spec_ad.body_B, spec.body_B); +} + +/* A loop constraint has to reach the solver like any other constraint. That +requires it to be in place before Finalize() declares the constraint +parameters, since the solver looks up each constraint's active status by id as +it builds its model. It is a live constraint in that model, so its status is +also settable -- deactivating it re-opens the loop, which is useful for +debugging assembly problems, at the cost of leaving the shadow link dangling +(with its share of the split mass) on the loop joint. */ +GTEST_TEST(ClosedTopologyTest, LoopConstraintReachesTheDiscreteSolver) { + FourBar four_bar(0.01 /* discrete, SAP */); + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + EXPECT_EQ(plant.get_discrete_contact_solver(), DiscreteContactSolver::kSap); + auto context = plant.CreateDefaultContext(); + + const MultibodyConstraintId id = GetSoleLoopConstraintSpec(plant).id; + + // Like any constraint it starts out active, and the discrete update -- which + // is where SAP consults that status and assembles the constraint -- runs. + EXPECT_TRUE(plant.GetConstraintActiveStatus(*context, id)); + auto state = plant.AllocateDiscreteVariables(); + EXPECT_NO_THROW( + plant.CalcForcedDiscreteVariableUpdate(*context, state.get())); + + // Deactivating it is allowed, and SAP then skips it. + plant.SetConstraintActiveStatus(context.get(), id, false); + EXPECT_FALSE(plant.GetConstraintActiveStatus(*context, id)); + EXPECT_NO_THROW( + plant.CalcForcedDiscreteVariableUpdate(*context, state.get())); +} + +/* Continuous time (with anything but the CENIC integrator, which computes its +dynamics by a different route) does not support constraints -- and a looped +model has them whether or not the user asked for any. So the complaint has to +say where they came from; otherwise it tells the user to go remove constraints +they never added. */ +GTEST_TEST(ClosedTopologyTest, ContinuousDynamicsSaysWhereLoopConstraintsCame) { + FourBar four_bar; // Continuous. + MultibodyPlant& plant = four_bar.plant(); + plant.Finalize(); + auto context = plant.CreateDefaultContext(); + ASSERT_EQ(plant.num_constraints(), plant.num_loop_constraints()); + + DRAKE_EXPECT_THROWS_MESSAGE( + plant.EvalTimeDerivatives(*context), + ".*does not support constraints, but this model has 1 constraint, which " + "Finalize\\(\\) added in order to close the kinematic loops.*"); + + // With constraints from both sources, the message accounts for both. + FourBar mixed_four_bar; + MultibodyPlant& mixed_plant = mixed_four_bar.plant(); + mixed_plant.AddWeldConstraint( + mixed_plant.GetBodyByName("driver"), math::RigidTransformd(), + mixed_plant.GetBodyByName("rocker"), math::RigidTransformd()); + mixed_plant.Finalize(); + auto mixed_context = mixed_plant.CreateDefaultContext(); + + DRAKE_EXPECT_THROWS_MESSAGE( + mixed_plant.EvalTimeDerivatives(*mixed_context), + ".*this model has 2 constraints, 1 of which Finalize\\(\\) added in " + "order to close the kinematic loops.*"); +} + +/* A closed loop need not be anchored to World at all -- think of a linkage +floating in space. The forest then has to choose one of the loop's own links to +serve as a base body and give it an ephemeral floating joint, even though that +link is already named as a child by one of the loop's joints. This case +previously caused a bug; here we verify it works properly. + +The model is the simplest possible loop: three links in a cycle, nothing welded +to World. */ +GTEST_TEST(ClosedTopologyTest, UnanchoredLoopGetsAFloatingBaseBody) { + MultibodyPlant plant(0.0 /* continuous */); + plant.SetEnableLoopTopology(true); + const Link& link_a = plant.AddRigidBody( + "a", SpatialInertia::SolidCubeWithMass(1.0, 0.1)); + const Link& link_b = plant.AddRigidBody( + "b", SpatialInertia::SolidCubeWithMass(1.0, 0.1)); + const Link& link_c = plant.AddRigidBody( + "c", SpatialInertia::SolidCubeWithMass(1.0, 0.1)); + const Vector3 axis = Vector3::UnitZ(); + plant.AddJoint("a_b", link_a, {}, link_b, {}, axis); + plant.AddJoint("b_c", link_b, {}, link_c, {}, axis); + plant.AddJoint("c_a", link_c, {}, link_a, {}, axis); + + EXPECT_NO_THROW(plant.Finalize()); + + // The loop was broken the usual way, and the assembly floats: 6 dofs for the + // floating base plus one per revolute joint. + EXPECT_EQ(plant.num_loop_constraints(), 1); + // Which of the three links the forest chooses to split is not what this test + // is about, so just check that one of them was. + const std::string shadow_name = GetSoleShadowLink(plant).name(); + EXPECT_TRUE(shadow_name == "a$1" || shadow_name == "b$1" || + shadow_name == "c$1"); + EXPECT_EQ(plant.num_velocities(), 6 + 3); + + // Exactly one of the loop's links is the floating base body, and it is the + // child of a user joint (which is what used to trigger the bug). + std::vector floating; + for (const Link* link : {&link_a, &link_b, &link_c}) { + if (link->is_floating_base_body()) floating.push_back(link->index()); + } + ASSERT_EQ(floating.size(), 1); + const Link& base = plant.get_body(floating.at(0)); + // It is also the child link of one of the loop's user joints -- precisely the + // situation that used to trip the DRAKE_DEMAND. + int user_joints_naming_base_as_child = 0; + for (const char* name : {"a_b", "b_c", "c_a"}) { + if (plant.GetJointByName(name).child_body().index() == base.index()) { + ++user_joints_naming_base_as_child; + } + } + EXPECT_EQ(user_joints_naming_base_as_child, 1); + + // The default pose of that body must route through its ephemeral floating + // joint -- that routing is exactly what the repaired loop sets up. + const math::RigidTransformd X_WB(math::RollPitchYawd(0.1, 0.2, 0.3), + Vector3(1.0, 2.0, 3.0)); + plant.SetDefaultFloatingBaseBodyPose(base, X_WB); + EXPECT_TRUE(CompareMatrices( + plant.GetDefaultFloatingBaseBodyPose(base).GetAsMatrix34(), + X_WB.GetAsMatrix34(), 1e-14)); + auto context = plant.CreateDefaultContext(); + EXPECT_TRUE( + CompareMatrices(plant.EvalBodyPoseInWorld(*context, base).GetAsMatrix34(), + X_WB.GetAsMatrix34(), 1e-14)); +} + } // namespace } // namespace multibody } // namespace drake diff --git a/multibody/topology/link_joint_graph.h b/multibody/topology/link_joint_graph.h index 1d12b650d18b..a7ca9489b93c 100644 --- a/multibody/topology/link_joint_graph.h +++ b/multibody/topology/link_joint_graph.h @@ -447,6 +447,10 @@ class LinkJointGraph { [[nodiscard]] inline const LoopConstraint& loop_constraints( LoopConstraintIndex constraint_index) const; + /* Returns the number of LoopConstraints, which is zero unless there is a + valid forest that had to break one or more loops. */ + [[nodiscard]] inline int num_loop_constraints() const; + /* Links with this ordinal or higher are ephemeral (added during forest-building). See the class comment for more information. */ [[nodiscard]] int num_user_links() const { return data_.num_user_links; } diff --git a/multibody/topology/link_joint_graph_inlines.h b/multibody/topology/link_joint_graph_inlines.h index b258f8c93d8f..b096a3630dda 100644 --- a/multibody/topology/link_joint_graph_inlines.h +++ b/multibody/topology/link_joint_graph_inlines.h @@ -103,6 +103,10 @@ inline const LinkJointGraph::LoopConstraint& LinkJointGraph::loop_constraints( return loop_constraints().at(loop_constraint_index); } +inline int LinkJointGraph::num_loop_constraints() const { + return std::ssize(loop_constraints()); +} + } // namespace internal } // namespace multibody } // namespace drake diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index 9816d4e8bea6..0fd7c0eff4a1 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -907,13 +907,18 @@ void MultibodyTree::FinalizeInternals() { for (JointIndex i : GetJointIndices()) { auto& joint = joints_.get_mutable_element(i); const RigidBody& body = joint.child_body(); - if (LinkAttorney::is_floating_base_body_pre_finalize(body)) { - DRAKE_DEMAND(joint.is_ephemeral()); - const auto [quaternion, translation] = - GetDefaultFloatingBaseBodyPoseAsQuaternionVec3Pair(body); - joint.SetDefaultPosePair(quaternion, translation); - default_body_poses_[body.index()] = joint.index(); - } + if (!LinkAttorney::is_floating_base_body_pre_finalize(body)) continue; + // A floating base body is mobilized by the ephemeral floating joint the + // forest added for it, but it can be a _user_ joint's child link as well: + // when a closed kinematic loop has no joint to World, the forest still has + // to choose one of the loop's links to serve as a base body, and that link + // is already named as a child by one of the loop's own joints. Only the + // ephemeral joint actually mobilizes the body, so skip any other. + if (!joint.is_ephemeral()) continue; + const auto [quaternion, translation] = + GetDefaultFloatingBaseBodyPoseAsQuaternionVec3Pair(body); + joint.SetDefaultPosePair(quaternion, translation); + default_body_poses_[body.index()] = joint.index(); } is_finalized_ = true; @@ -1054,8 +1059,6 @@ void MultibodyTree::Finalize() { process (BuildForest()), which augmented the graph with them. We call those "ephemeral" elements. */ - // 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( diff --git a/multibody/tree/rigid_body.h b/multibody/tree/rigid_body.h index c7b253614ce0..f4b1a0ba0544 100644 --- a/multibody/tree/rigid_body.h +++ b/multibody/tree/rigid_body.h @@ -307,6 +307,13 @@ class RigidBody : public MultibodyElement { /// mobilized by an automatically-added (ephemeral) floating (6 dof) joint /// to World. /// + /// @note A %Link can be a floating base body even though a user-defined joint + /// names it as a child link, if that joint does not connect it to World: when + /// a closed kinematic loop has no joint to World at all, one of the loop's + /// links must still serve as a base body. What this flag reports is that the + /// %Link is mobilized by an ephemeral floating joint, not that the user left + /// it jointless. + /// /// @note A floating base body is not necessarily modeled with a quaternion /// mobilizer, see has_quaternion_dofs(). Alternative options include a /// roll-pitch-yaw (rpy) parametrization of rotations, see diff --git a/visualization/inertia_visualizer.cc b/visualization/inertia_visualizer.cc index 2ce486bd8c0a..ecdbed801e01 100644 --- a/visualization/inertia_visualizer.cc +++ b/visualization/inertia_visualizer.cc @@ -43,10 +43,20 @@ InertiaVisualizer::InertiaVisualizer( } const multibody::RigidBody& body = plant.get_body(i); + // Not every body necessarily has a SceneGraph frame -- none of them do if + // `plant` was never registered as a geometry source. We would have nothing + // to hang an inertia ellipsoid on for such a body, so skip it rather than + // dereferencing an empty optional. + const std::optional Bo_frame = + plant.GetBodyFrameIdIfExists(i); + if (!Bo_frame.has_value()) { + continue; + } + // Add a Bcm geometry frame. Item item; item.body = i; - item.Bo_frame = plant.GetBodyFrameIdIfExists(i).value(); + item.Bo_frame = *Bo_frame; item.Bcm_frame = scene_graph->RegisterFrame( source_id_, SceneGraph::world_frame_id(), GeometryFrame{fmt::format( diff --git a/visualization/test/inertia_visualizer_test.cc b/visualization/test/inertia_visualizer_test.cc index 4d63d90bd11e..ffe75e7f8dba 100644 --- a/visualization/test/inertia_visualizer_test.cc +++ b/visualization/test/inertia_visualizer_test.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -108,6 +109,68 @@ TEST_F(InertiaVisualizerConfigTest, InertiaButNoIllustrationConfig) { EXPECT_TRUE(meshcat_->HasPath("/drake/inertia/InertiaVisualizer/acrobot")); } +// When MultibodyPlant models a closed kinematic loop it splits a link into a +// primary link and an ephemeral "shadow" copy, which carries half of the +// primary's mass and has a pose of its own. It is a body like any other here, +// so it gets an inertia ellipsoid of its own -- showing the shadow's share of +// the mass, where the shadow actually is. +GTEST_TEST(InertiaVisualizerTest, ShadowLinkGetsAnEllipsoid) { + // A minimal closed loop: two links joined to World and to each other. + constexpr char kLoopSdf[] = R"""( + + + + + + + world + upper + 0 0 1 + + + world + lower + 0 0 1 + + + upper + lower + 0 0 1 + + + + )"""; + systems::DiagramBuilder builder; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0.01); + plant.SetEnableLoopTopology(true); + Parser(&plant).AddModelsFromString(kLoopSdf, "sdf"); + plant.Finalize(); + + // Breaking the loop added an ephemeral shadow body, with a frame of its own. + ASSERT_EQ(plant.num_loop_constraints(), 1); + const multibody::BodyIndex shadow_index(plant.num_bodies() - 1); + const multibody::RigidBody& shadow = plant.get_body(shadow_index); + ASSERT_TRUE(shadow.is_ephemeral()); + ASSERT_TRUE(plant.GetBodyFrameIdIfExists(shadow_index).has_value()); + + InertiaVisualizer::AddToBuilder(&builder, plant, &scene_graph); + + // The visualizer registers one frame per body it draws an ellipsoid for, so + // the shadow is drawn iff a frame named for it turns up in SceneGraph. + const std::string expected_name = fmt::format( + "InertiaVisualizer::{}::{}", + plant.GetModelInstanceName(shadow.model_instance()), shadow.name()); + const geometry::SceneGraphInspector& inspector = + scene_graph.model_inspector(); + std::vector frame_names; + for (const geometry::FrameId frame_id : inspector.GetAllFrameIds()) { + frame_names.push_back(inspector.GetName(frame_id)); + } + EXPECT_TRUE(std::find(frame_names.begin(), frame_names.end(), + expected_name) != frame_names.end()) + << "No SceneGraph frame named '" << expected_name << "'."; +} + // This class allows testing the results of inertia geometry calculation. // // The CalculateInertiaGeometryFor() method returns the inertia ellipsoid