diff --git a/bindings/generated_docstrings/multibody_tree.h b/bindings/generated_docstrings/multibody_tree.h index 05be43d00aa1..fc744842398f 100644 --- a/bindings/generated_docstrings/multibody_tree.h +++ b/bindings/generated_docstrings/multibody_tree.h @@ -65,6 +65,7 @@ // #include "drake/multibody/tree/scoped_name.h" // #include "drake/multibody/tree/screw_joint.h" // #include "drake/multibody/tree/screw_mobilizer.h" +// #include "drake/multibody/tree/shadow_frame.h" // #include "drake/multibody/tree/spatial_inertia.h" // #include "drake/multibody/tree/uniform_gravity_field_element.h" // #include "drake/multibody/tree/unit_inertia.h" @@ -3815,6 +3816,16 @@ vector. Implementations must meet the styleguide requirements for snake_case accessor methods.)"""; } do_set_default_positions; + // Symbol: drake::multibody::Joint::effective_frame_on_child + struct /* effective_frame_on_child */ { + // Source: drake/multibody/tree/joint.h + const char* doc = R"""()"""; + } effective_frame_on_child; + // Symbol: drake::multibody::Joint::effective_frame_on_parent + struct /* effective_frame_on_parent */ { + // Source: drake/multibody/tree/joint.h + const char* doc = R"""()"""; + } effective_frame_on_parent; // Symbol: drake::multibody::Joint::frame_on_child struct /* frame_on_child */ { // Source: drake/multibody/tree/joint.h @@ -3988,6 +3999,16 @@ R"""(Sets the default generalized position coordinates q₀ to RuntimeError if the dimension of ``default_positions`` does not match num_positions().)"""; } set_default_positions; + // Symbol: drake::multibody::Joint::set_effective_frame_on_child + struct /* set_effective_frame_on_child */ { + // Source: drake/multibody/tree/joint.h + const char* doc = R"""()"""; + } set_effective_frame_on_child; + // Symbol: drake::multibody::Joint::set_effective_frame_on_parent + struct /* set_effective_frame_on_parent */ { + // Source: drake/multibody/tree/joint.h + const char* doc = R"""()"""; + } set_effective_frame_on_parent; // Symbol: drake::multibody::Joint::set_position_limits struct /* set_position_limits */ { // Source: drake/multibody/tree/joint.h @@ -4028,7 +4049,14 @@ R"""(Sets the velocity limits to ``lower_limits`` and ``upper_limits``. R"""(Utility for concrete joint implementations to use to select the inboard/outboard frames for a tree in the spanning forest, given whether they should be reversed from the parent/child frames that are -members of this Joint object.)"""; +members of this Joint object. + +These are the joint's *effective* frames: if loop breaking moved one +end of this joint onto an ephemeral shadow link, the frame for that +end is the substitute frame on the shadow rather than the user's frame +on the primary link. Concrete joints should always build their +mobilizer from these frames, so that they need not know that shadow +links exist.)"""; } tree_frames; // Symbol: drake::multibody::Joint::type_name struct /* type_name */ { diff --git a/multibody/plant/test/auto_closed_topology_test.cc b/multibody/plant/test/auto_closed_topology_test.cc index 2d05d559eef7..d5d8497c0158 100644 --- a/multibody/plant/test/auto_closed_topology_test.cc +++ b/multibody/plant/test/auto_closed_topology_test.cc @@ -3,7 +3,8 @@ 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. +shadows, and that the loop joint is retargeted from the primary link onto its +shadow. 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 @@ -24,12 +25,17 @@ shadows. */ #include "drake/common/test_utilities/expect_throws_message.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/internal_geometry_names.h" #include "drake/multibody/plant/multibody_plant.h" +#include "drake/multibody/tree/fixed_offset_frame.h" +#include "drake/multibody/tree/mobilizer.h" #include "drake/multibody/tree/revolute_joint.h" #include "drake/multibody/tree/rigid_body.h" +#include "drake/multibody/tree/shadow_frame.h" #include "drake/systems/framework/context.h" namespace drake { @@ -228,9 +234,10 @@ GTEST_TEST(ClosedTopologyTest, ShadowLinkFramesAreEphemeral) { 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. + // 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. int num_shadow_frames = 0; for (FrameIndex index(0); index < plant->num_frames(); ++index) { const Frame& frame = plant->get_frame(index); @@ -238,7 +245,7 @@ GTEST_TEST(ClosedTopologyTest, ShadowLinkFramesAreEphemeral) { ++num_shadow_frames; EXPECT_TRUE(frame.is_ephemeral()) << frame.name(); } - EXPECT_GE(num_shadow_frames, 1); + EXPECT_GE(num_shadow_frames, 2); // The user's links keep non-ephemeral link frames. EXPECT_FALSE(plant->world_body().body_frame().is_ephemeral()); @@ -574,6 +581,269 @@ GTEST_TEST(ClosedTopologyTest, OneLinkCanHaveTwoShadows) { EXPECT_NEAR(plant.CalcTotalMass(*context), 18.0, kTol); } +/* Loop breaking retargets exactly one end of exactly one joint onto a shadow +link (in general, one per shadow link). Identifies that joint by looking for a +substituted frame, and reports which end was moved. */ +struct RetargetedJoint { + const Joint* joint{}; + bool moved_parent{}; // Which end of the joint got moved to the shadow. +}; + +RetargetedJoint GetSoleRetargetedJoint(const MultibodyPlant& plant) { + std::vector found; + for (JointIndex index : plant.GetJointIndices()) { + const Joint& joint = plant.get_joint(index); + const bool moved_parent = + &joint.effective_frame_on_parent() != &joint.frame_on_parent(); + const bool moved_child = + &joint.effective_frame_on_child() != &joint.frame_on_child(); + // A joint has at most one end on any one link, so at most one of its ends + // can have been moved to a given shadow of that link. + EXPECT_FALSE(moved_parent && moved_child) << joint.name(); + if (moved_parent || moved_child) found.push_back({&joint, moved_parent}); + } + EXPECT_EQ(found.size(), 1); + return found.empty() ? RetargetedJoint{} : found.front(); +} + +/* 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. +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.) */ +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); + + // It is the coupler_rocker joint's parent (coupler) end that got moved. + const RetargetedJoint retargeted = GetSoleRetargetedJoint(*plant); + ASSERT_NE(retargeted.joint, nullptr); + const Joint& loop_joint = *retargeted.joint; + EXPECT_EQ(loop_joint.name(), "coupler_rocker"); + EXPECT_TRUE(retargeted.moved_parent); + + // The user's view of the joint is unaffected: it still connects the coupler + // to the rocker, through the frames the parser created on those links. + EXPECT_EQ(loop_joint.parent_body().index(), coupler.index()); + EXPECT_EQ(loop_joint.child_body().index(), rocker.index()); + EXPECT_EQ(loop_joint.frame_on_parent().body().index(), coupler.index()); + EXPECT_EQ(loop_joint.frame_on_child().body().index(), rocker.index()); + + // The substitute frame is an ephemeral ShadowFrame fixed to the shadow link, + // with the user's frame on the primary as its source (so it has no + // independent pose of its own; see the next test). + const auto* shadow_frame = dynamic_cast*>( + &loop_joint.effective_frame_on_parent()); + ASSERT_NE(shadow_frame, nullptr); + EXPECT_EQ(shadow_frame->body().index(), shadow.index()); + EXPECT_EQ(shadow_frame->source_frame().index(), + loop_joint.frame_on_parent().index()); + EXPECT_TRUE(shadow_frame->is_ephemeral()); + + // The mobilizer modeling this joint moves the shadow link, not the coupler. + // The shadow is reached through the rocker, so this mobilizer is reversed + // with respect to its joint: the rocker is inboard and the shadow outboard. + // Note that the mobilizer's own frames need not be the joint's frames -- a + // revolute joint inserts an offset frame when it has to align its axis with a + // mobilizer axis, as the reversal here forces it to -- so what matters is + // that the outboard frame ends up fixed to the shadow link, which it does + // because it chains off the ShadowFrame. + const internal::Mobilizer& mobilizer = loop_joint.GetMobilizerInUse(); + EXPECT_EQ(mobilizer.inboard_body().index(), rocker.index()); + EXPECT_EQ(mobilizer.outboard_body().index(), shadow.index()); + EXPECT_EQ(mobilizer.outboard_frame().body().index(), shadow.index()); +} + +/* The substitute frame on the shadow link has no pose parameter of its own; it +delegates to the user's frame on the primary link. So moving the user's frame at +runtime must carry the shadow-side frame along with it -- had we instead +snapshotted the offset during Finalize(), the two would silently disagree and +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(); + + 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&>( + loop_joint.frame_on_parent()); + + // Anchor the checks below to frames that really are on the shadow link; + // otherwise a missing substitution would leave us comparing the user's frame + // to itself and everything would agree vacuously. + const Frame& shadow_frame = loop_joint.effective_frame_on_parent(); + ASSERT_EQ(shadow_frame.body().index(), shadow.index()); + // The mobilizer's outboard frame chains off the shadow frame, so it must + // track the change too. + const Frame& frame_M = + 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. + const math::RigidTransformd X_CJp = Jp.CalcPoseInBodyFrame(*context); + EXPECT_TRUE(CompareMatrices( + shadow_frame.CalcPoseInBodyFrame(*context).GetAsMatrix34(), + X_CJp.GetAsMatrix34(), kTol)); + EXPECT_TRUE( + CompareMatrices(frame_M.CalcPoseInBodyFrame(*context).translation(), + X_CJp.translation(), kTol)); + + // Move the user's frame on the primary coupler to a new offset. + const math::RigidTransformd X_PJp_new(math::RollPitchYawd(0.1, -0.2, 0.3), + Vector3(3.5, 0.25, -0.75)); + Jp.SetPoseInParentFrame(context.get(), X_PJp_new); + const math::RigidTransformd X_CJp_new = Jp.CalcPoseInBodyFrame(*context); + ASSERT_FALSE( + CompareMatrices(X_CJp_new.GetAsMatrix34(), X_CJp.GetAsMatrix34(), kTol)) + << "the test moved the frame nowhere"; + + // With no re-finalization, both the shadow frame and the mobilizer frame + // chained off it report the new offset. + EXPECT_TRUE(CompareMatrices( + shadow_frame.CalcPoseInBodyFrame(*context).GetAsMatrix34(), + X_CJp_new.GetAsMatrix34(), kTol)); + EXPECT_TRUE( + CompareMatrices(frame_M.CalcPoseInBodyFrame(*context).translation(), + X_CJp_new.translation(), kTol)); +} + +/* Scalar conversion clones the mobilizers and the shadow-side frames rather +than rebuilding them (it does not re-run the joint-modeling step), so the +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(); + std::unique_ptr> plant_ad = + systems::System::ToAutoDiffXd(*plant); + + const Link& shadow_ad = plant_ad->GetBodyByName("coupler$1"); + EXPECT_TRUE(shadow_ad.is_ephemeral()); + + const Joint& loop_joint_ad = + plant_ad->GetJointByName("coupler_rocker"); + const auto* shadow_frame_ad = + dynamic_cast*>( + &loop_joint_ad.effective_frame_on_parent()); + ASSERT_NE(shadow_frame_ad, nullptr); + EXPECT_EQ(shadow_frame_ad->body().index(), shadow_ad.index()); + // The clone's source frame is the clone's own frame_on_parent(), not an + // alias back into the double plant. + EXPECT_EQ(&shadow_frame_ad->source_frame(), &loop_joint_ad.frame_on_parent()); + EXPECT_EQ(&loop_joint_ad.effective_frame_on_child(), + &loop_joint_ad.frame_on_child()); + + const internal::Mobilizer& mobilizer_ad = + loop_joint_ad.GetMobilizerInUse(); + EXPECT_EQ(mobilizer_ad.outboard_body().index(), shadow_ad.index()); + EXPECT_EQ(mobilizer_ad.outboard_frame().body().index(), shadow_ad.index()); + + // The cloned frame still delegates rather than holding a copy of the offset. + auto context_ad = plant_ad->CreateDefaultContext(); + const auto& Jp_ad = dynamic_cast&>( + loop_joint_ad.frame_on_parent()); + Jp_ad.SetPoseInParentFrame( + context_ad.get(), + math::RigidTransform(Vector3(3.5, 0.25, -0.75))); + EXPECT_TRUE(CompareMatrices( + math::ExtractValue( + shadow_frame_ad->CalcPoseInBodyFrame(*context_ad).GetAsMatrix34()), + math::ExtractValue( + Jp_ad.CalcPoseInBodyFrame(*context_ad).GetAsMatrix34()), + kTol)); +} + +/* A shadow frame belongs to the model instance of the loop joint whose frame it +substitutes for, which is not necessarily the model instance of the shadow link +it is fixed to: a joint takes its model instance from its child link, while a +shadow link inherits its primary's. Those two disagree whenever the loop is +closed by a joint that spans model instances and it is that joint's parent end +that gets retargeted. A Frame has two clone paths and both are checked here. + +The four-bar built here is the one described at the top of this file, minus the +poses and inertias that don't affect which link gets split, but with the rocker +in a model instance of its own. That puts the retargeted joint (coupler_rocker) +in the rocker's model instance while the coupler's shadow is in the coupler's +model instance. */ +GTEST_TEST(ClosedTopologyTest, ShadowFrameKeepsItsModelInstanceWhenCloned) { + MultibodyPlant plant(0.0 /* continuous */); + plant.SetEnableLoopTopology(true); + const ModelInstanceIndex linkage_instance = plant.AddModelInstance("linkage"); + const ModelInstanceIndex rocker_instance = plant.AddModelInstance("rocker"); + + const SpatialInertia M = SpatialInertia::SolidCubeWithMass( + 1.0 /* mass */, 0.1 /* length */); + const Link& driver = + plant.AddRigidBody("driver", linkage_instance, M); + const Link& coupler = + plant.AddRigidBody("coupler", linkage_instance, M); + const Link& rocker = plant.AddRigidBody("rocker", rocker_instance, M); + + const Vector3 axis = Vector3::UnitY(); + plant.AddJoint("world_driver", plant.world_body(), {}, driver, + {}, axis); + plant.AddJoint("world_rocker", plant.world_body(), {}, rocker, + {}, axis); + plant.AddJoint("driver_coupler", driver, {}, coupler, {}, + axis); + plant.AddJoint("coupler_rocker", coupler, {}, rocker, {}, + axis); + plant.Finalize(); + + const Link& shadow = GetSoleShadowLink(plant); + const RetargetedJoint retargeted = GetSoleRetargetedJoint(plant); + ASSERT_NE(retargeted.joint, nullptr); + const Joint& loop_joint = *retargeted.joint; + ASSERT_EQ(loop_joint.name(), "coupler_rocker"); + // It is the coupler (parent) end that moved onto the coupler's shadow, so + // the joint and the shadow link really do live in different model instances + // -- otherwise the checks below would pass vacuously. + ASSERT_TRUE(retargeted.moved_parent); + ASSERT_EQ(shadow.model_instance(), linkage_instance); + ASSERT_EQ(loop_joint.model_instance(), rocker_instance); + + const Frame& shadow_frame = loop_joint.effective_frame_on_parent(); + ASSERT_EQ(shadow_frame.body().index(), shadow.index()); + EXPECT_EQ(shadow_frame.model_instance(), rocker_instance); + + // A shallow clone keeps the frame's properties, including the model instance. + const std::unique_ptr> shallow_clone = + shadow_frame.ShallowClone(); + EXPECT_EQ(shallow_clone->name(), shadow_frame.name()); + EXPECT_EQ(shallow_clone->model_instance(), rocker_instance); + EXPECT_TRUE(shallow_clone->is_ephemeral()); + + // Scalar conversion also clones the shadow frame, and the clone must be + // reachable by the same instance-scoped name lookup as the original. + std::unique_ptr> plant_ad = + systems::System::ToAutoDiffXd(plant); + const Joint& loop_joint_ad = + plant_ad->GetJointByName("coupler_rocker"); + const Frame& shadow_frame_ad = + loop_joint_ad.effective_frame_on_parent(); + ASSERT_NE(&shadow_frame_ad, &loop_joint_ad.frame_on_parent()); + EXPECT_EQ(shadow_frame_ad.model_instance(), rocker_instance); + EXPECT_EQ( + plant_ad->GetFrameByName(shadow_frame.name(), rocker_instance).index(), + shadow_frame.index()); +} } // namespace } // namespace multibody } // namespace drake diff --git a/multibody/topology/link_joint_graph_link.h b/multibody/topology/link_joint_graph_link.h index 6ae08b63410d..115941e90eb3 100644 --- a/multibody/topology/link_joint_graph_link.h +++ b/multibody/topology/link_joint_graph_link.h @@ -107,6 +107,14 @@ class LinkJointGraph::Link { shadow links, if any. */ const std::vector& shadow_links() const { return shadow_links_; } + /* After the forest has been built, returns the joints that were originally + connected to this %Link but were retargeted to one of its shadow links to + break a loop. This is ordered to match shadow_links(): the i'th joint here is + the one that moved to the i'th shadow link. */ + const std::vector& joints_moved_to_shadow_links() const { + return joints_moved_to_shadow_links_; + } + /* Returns the index of the mobilized body (Mobod) that mobilizes this %Link. If this %Link is part of a WeldedLinksAssembly, the returned Mobod may be a fused mobod (containing some or all of the links in the assembly -- including diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index 2a9bd03e77fc..aa9d52ec2836 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -121,6 +121,7 @@ drake_cc_library( "rpy_floating_mobilizer.cc", "screw_joint.cc", "screw_mobilizer.cc", + "shadow_frame.cc", "uniform_gravity_field_element.cc", "universal_joint.cc", "universal_mobilizer.cc", @@ -170,6 +171,7 @@ drake_cc_library( "rpy_floating_mobilizer.h", "screw_joint.h", "screw_mobilizer.h", + "shadow_frame.h", "uniform_gravity_field_element.h", "universal_joint.h", "universal_mobilizer.h", diff --git a/multibody/tree/joint.h b/multibody/tree/joint.h index 10c44302d956..c9a52b7f39c3 100644 --- a/multibody/tree/joint.h +++ b/multibody/tree/joint.h @@ -767,6 +767,32 @@ class Joint : public MultibodyElement { const internal::SpanningForest::Mobod& mobod, internal::MultibodyTree* tree); + // (Internal use only) When loop breaking moves one end of this joint from a + // user link onto one of that link's ephemeral shadow links, MultibodyTree + // calls one of these (before Build()) with a substitute frame fixed to the + // shadow and with the same pose on the shadow as the user's frame has on the + // primary link. The user-visible frame_on_parent()/frame_on_child() and + // parent_body()/child_body() continue to report the user's own frames and + // links; only the mobilizer sees the substitution, via tree_frames(). + void set_effective_frame_on_parent(const Frame& frame) { + effective_frame_on_parent_ = &frame; + } + void set_effective_frame_on_child(const Frame& frame) { + effective_frame_on_child_ = &frame; + } + + // (Internal use only) Returns the frame this joint's mobilizer should use on + // the parent (child) side: the substitute frame on a shadow link if loop + // breaking installed one, otherwise the user's own frame. + const Frame& effective_frame_on_parent() const { + return effective_frame_on_parent_ != nullptr ? *effective_frame_on_parent_ + : frame_on_parent_; + } + const Frame& effective_frame_on_child() const { + return effective_frame_on_child_ != nullptr ? *effective_frame_on_child_ + : frame_on_child_; + } + // NVI to DoCloneToScalar() templated on the scalar type of the new clone to // be created. This method is intended to be called by // MultibodyTree::CloneToScalar(). @@ -776,6 +802,16 @@ class Joint : public MultibodyElement { std::unique_ptr> joint_clone = DoCloneToScalar(*tree_clone); DRAKE_DEMAND(mobilizer_ != nullptr); joint_clone->mobilizer_ = &tree_clone->get_mutable_variant(*mobilizer_); + // Cloning doesn't re-run Build(), so carry over any shadow-link frame + // substitutions rather than leaving the clone reporting the user frames. + if (effective_frame_on_parent_ != nullptr) { + joint_clone->effective_frame_on_parent_ = + &tree_clone->get_variant(*effective_frame_on_parent_); + } + if (effective_frame_on_child_ != nullptr) { + joint_clone->effective_frame_on_child_ = + &tree_clone->get_variant(*effective_frame_on_child_); + } return joint_clone; } @@ -952,11 +988,18 @@ class Joint : public MultibodyElement { /// inboard/outboard frames for a tree in the spanning forest, given /// whether they should be reversed from the parent/child frames that are /// members of this Joint object. + /// + /// These are the joint's _effective_ frames: if loop breaking moved one end + /// of this joint onto an ephemeral shadow link, the frame for that end is + /// the substitute frame on the shadow rather than the user's frame on the + /// primary link. Concrete joints should always build their mobilizer from + /// these frames, so that they need not know that shadow links exist. std::pair*, const Frame*> tree_frames( bool use_reversed_mobilizer) const { - return use_reversed_mobilizer - ? std::make_pair(&frame_on_child(), &frame_on_parent()) - : std::make_pair(&frame_on_parent(), &frame_on_child()); + return use_reversed_mobilizer ? std::make_pair(&effective_frame_on_child(), + &effective_frame_on_parent()) + : std::make_pair(&effective_frame_on_parent(), + &effective_frame_on_child()); } /// (Internal use only) Returns the mobilizer implementing this joint, @@ -1059,6 +1102,12 @@ class Joint : public MultibodyElement { const Frame& frame_on_parent_; // Frame Jp. const Frame& frame_on_child_; // Frame Jc. + // Substitute frames on an ephemeral shadow link, installed by MultibodyTree + // during Finalize() when loop breaking retargets an end of this joint. Null + // unless substituted; see set_effective_frame_on_parent(). + const Frame* effective_frame_on_parent_{nullptr}; + const Frame* effective_frame_on_child_{nullptr}; + VectorX damping_; // Joint position limits. These vectors have zero size for joints with no diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index c4064aff87bb..9816d4e8bea6 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -25,6 +25,7 @@ #include "drake/multibody/tree/rigid_body.h" #include "drake/multibody/tree/rpy_floating_joint.h" #include "drake/multibody/tree/rpy_floating_mobilizer.h" +#include "drake/multibody/tree/shadow_frame.h" #include "drake/multibody/tree/spatial_inertia.h" #include "drake/multibody/tree/uniform_gravity_field_element.h" #include "drake/multibody/tree/weld_joint.h" @@ -1086,25 +1087,60 @@ void MultibodyTree::Finalize() { // 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 = + for (LinkOrdinal shadow_ordinal(graph.num_user_links()); + shadow_ordinal < graph.num_links(); ++shadow_ordinal) { + const LinkJointGraph::Link& shadow_graph_link = graph.links(shadow_ordinal); + DRAKE_DEMAND(!shadow_graph_link.is_world()); + DRAKE_DEMAND(shadow_graph_link.is_shadow()); + const LinkIndex primary_index = shadow_graph_link.primary_link(); + const LinkJointGraph::Link& primary_graph_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; + DRAKE_DEMAND( + !primary_graph_link.is_world()); // Shouldn't ever split World. + DRAKE_DEMAND(!primary_graph_link.is_shadow()); + const int num_copies = primary_graph_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()); + const Link& shadow_link = AddEphemeralLink( + shadow_graph_link.name(), shadow_graph_link.model_instance(), M_shadow); + DRAKE_DEMAND(shadow_link.index() == shadow_graph_link.index()); + + /* BuildForest() broke the loop by retargeting one of a joint's ends from + the primary link onto this shadow. That joint's frame for that end was + authored on the primary, so we give the joint a substitute frame that is + fixed to the shadow link with the same pose that the user's frame had on the + primary link. We can use the same pose because a shadow link's link frame + coincides with its primary's. A ShadowFrame has no pose parameter of its + own; it delegates to the user's frame, which remains the single source of + truth. + + Joints and mobilizers thus stay shadow-ignorant: + CreateJointImplementations() runs after this loop and Joint::Build() picks + up the substitution through Joint::tree_frames(). The joint's user-visible + frame_on_parent()/ frame_on_child() and parent_body()/child_body() are + unchanged. */ + const JointIndex joint_index = shadow_graph_link.inboard_joint_index(); + Joint& joint = joints_.get_mutable_element(joint_index); + + // Exactly one end of the joint should have been moved to this shadow. + const bool moved_child = !shadow_graph_link.joints_as_child().empty(); + DRAKE_DEMAND(moved_child || !shadow_graph_link.joints_as_parent().empty()); + const Frame& source_frame = + moved_child ? joint.frame_on_child() : joint.frame_on_parent(); + + const Frame& shadow_frame = + AddEphemeralFrame(std::make_unique>( + joint.MakeUniqueOffsetFrameName(source_frame, "shadow"), + shadow_link, source_frame, joint.model_instance())); + + if (moved_child) { + joint.set_effective_frame_on_child(shadow_frame); + } else { + joint.set_effective_frame_on_parent(shadow_frame); + } } /* Add the ephemeral Joints. */ diff --git a/multibody/tree/prismatic_joint.cc b/multibody/tree/prismatic_joint.cc index 90cc6afc876b..4f58b533b0d5 100644 --- a/multibody/tree/prismatic_joint.cc +++ b/multibody/tree/prismatic_joint.cc @@ -127,10 +127,7 @@ PrismaticJoint::MakeMobilizerForJoint( // These are the joint's parent and child frames, but adjusted for // reversal to locate them on the inboard and outboard bodies. We may also // need to reverse the axis so that q will retain its expected sign. - const Frame& Jin = - reverse ? this->frame_on_child() : this->frame_on_parent(); - const Frame& Jout = - reverse ? this->frame_on_parent() : this->frame_on_child(); + const auto [Jin, Jout] = this->tree_frames(reverse); const Eigen::Vector3d axis = reverse ? -axis_ : axis_; // a unit vector // Determine whether the axis is one of +x, +y, +z, or something else. @@ -149,14 +146,14 @@ PrismaticJoint::MakeMobilizerForJoint( const math::RotationMatrixd R_JinF = // Also R_JoutM, since Jp=Jc at q=0. math::RotationMatrixd::MakeFromOneUnitVector(axis, *which_axis); F = &tree->AddEphemeralFrame(std::make_unique>( - this->MakeUniqueOffsetFrameName(Jin, "F"), Jin, + this->MakeUniqueOffsetFrameName(*Jin, "F"), *Jin, math::RigidTransformd(R_JinF), this->model_instance())); M = &tree->AddEphemeralFrame(std::make_unique>( - this->MakeUniqueOffsetFrameName(Jout, "M"), Jout, + this->MakeUniqueOffsetFrameName(*Jout, "M"), *Jout, math::RigidTransformd(R_JinF), this->model_instance())); } else { - F = &Jin; - M = &Jout; + F = Jin; + M = Jout; } std::unique_ptr> prismatic_mobilizer; diff --git a/multibody/tree/revolute_joint.cc b/multibody/tree/revolute_joint.cc index d0628a16440b..f267feb9106e 100644 --- a/multibody/tree/revolute_joint.cc +++ b/multibody/tree/revolute_joint.cc @@ -124,10 +124,7 @@ std::unique_ptr> RevoluteJoint::MakeMobilizerForJoint( // These are the joint's parent and child frames, but adjusted for // reversal to locate them on the inboard and outboard bodies. We may also // need to reverse the axis so that q will retain its expected sign. - const Frame& Jin = - reverse ? this->frame_on_child() : this->frame_on_parent(); - const Frame& Jout = - reverse ? this->frame_on_parent() : this->frame_on_child(); + const auto [Jin, Jout] = this->tree_frames(reverse); const Eigen::Vector3d axis = reverse ? -axis_ : axis_; // a unit vector // Determine whether the axis is one of +x, +y, +z, or something else. @@ -146,14 +143,14 @@ std::unique_ptr> RevoluteJoint::MakeMobilizerForJoint( const math::RotationMatrixd R_JinF = // Also R_JoutM, since Jp=Jc at q=0. math::RotationMatrixd::MakeFromOneUnitVector(axis, *which_axis); F = &tree->AddEphemeralFrame(std::make_unique>( - this->MakeUniqueOffsetFrameName(Jin, "F"), Jin, + this->MakeUniqueOffsetFrameName(*Jin, "F"), *Jin, math::RigidTransformd(R_JinF), this->model_instance())); M = &tree->AddEphemeralFrame(std::make_unique>( - this->MakeUniqueOffsetFrameName(Jout, "M"), Jout, + this->MakeUniqueOffsetFrameName(*Jout, "M"), *Jout, math::RigidTransformd(R_JinF), this->model_instance())); } else { - F = &Jin; - M = &Jout; + F = Jin; + M = Jout; } std::unique_ptr> revolute_mobilizer; diff --git a/multibody/tree/shadow_frame.cc b/multibody/tree/shadow_frame.cc new file mode 100644 index 000000000000..c06658d868c3 --- /dev/null +++ b/multibody/tree/shadow_frame.cc @@ -0,0 +1,89 @@ +#include "drake/multibody/tree/shadow_frame.h" + +#include +#include + +#include "drake/multibody/tree/multibody_tree.h" +#include "drake/multibody/tree/rigid_body.h" + +namespace drake { +namespace multibody { +namespace internal { + +template +ShadowFrame::ShadowFrame(const std::string& name, const Link& shadow_link, + const Frame& source_frame, + std::optional model_instance) + : Frame(name, shadow_link, model_instance), + source_frame_(source_frame) {} + +template +ShadowFrame::~ShadowFrame() = default; + +template +template +std::unique_ptr> ShadowFrame::TemplatedDoCloneToScalar( + const MultibodyTree& tree_clone) const { + const Link& shadow_link_clone = + tree_clone.get_variant(this->link()); + const Frame& source_frame_clone = + tree_clone.get_variant(source_frame_); + // A shadow frame's model instance is the loop joint's, which need not be the + // shadow link's, so it has to be copied rather than defaulted. + auto new_frame = std::make_unique>( + this->name(), shadow_link_clone, source_frame_clone, + this->model_instance()); + new_frame->set_is_ephemeral(this->is_ephemeral()); + return new_frame; +} + +template +std::unique_ptr> ShadowFrame::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> ShadowFrame::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> ShadowFrame::DoCloneToScalar( + const MultibodyTree& tree_clone) const { + return TemplatedDoCloneToScalar(tree_clone); +} + +template +std::unique_ptr> ShadowFrame::DoShallowClone() const { + auto new_frame = std::make_unique>( + this->name(), this->link(), source_frame_, this->model_instance()); + new_frame->set_is_ephemeral(this->is_ephemeral()); + return new_frame; +} + +template +math::RigidTransform ShadowFrame::DoCalcPoseInBodyFrame( + const systems::Parameters& parameters) const { + // Our link frame coincides with the source frame's link frame, so our X_LF + // is exactly the source frame's X_LF. Delegating (rather than storing a + // parameter of our own) is what keeps the two frames in agreement when the + // source frame's pose is changed at runtime. + return source_frame_.CalcOffsetPoseInBody( + parameters, math::RigidTransform::Identity()); +} + +template +math::RotationMatrix ShadowFrame::DoCalcRotationMatrixInBodyFrame( + const systems::Parameters& parameters) const { + return source_frame_.CalcOffsetRotationMatrixInBody( + parameters, math::RotationMatrix::Identity()); +} + +} // namespace internal +} // namespace multibody +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::multibody::internal::ShadowFrame); diff --git a/multibody/tree/shadow_frame.h b/multibody/tree/shadow_frame.h new file mode 100644 index 000000000000..8cd7b72d382b --- /dev/null +++ b/multibody/tree/shadow_frame.h @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include + +#include "drake/common/default_scalars.h" +#include "drake/common/drake_copyable.h" +#include "drake/multibody/tree/frame.h" + +namespace drake { +namespace multibody { +namespace internal { + +/* A Frame fixed to an ephemeral shadow link, coincident at all times with a +"source" Frame fixed to that shadow's primary link. + +When we model a closed kinematic loop we break the loop by splitting a link +into a primary link and one or more ephemeral shadow links, retargeting one of +the loop joint's frames onto a shadow (see LinkJointGraph). The joint's frame +was authored on the primary link, so the mobilizer needs an equivalent frame on +the shadow link. Because a shadow's link frame is intended to coincide with its +primary's (when the weld constraint is satisfied), "equivalent" here just means +having the same local pose: only the link the frame is fixed to differs, not +its pose on the link to which it is fixed. + +%ShadowFrame delegates its pose to the source frame rather than storing a pose +of its own, so it declares no Context parameters. That makes the source frame +the single source of truth: a runtime change to the source frame's pose (say +FixedOffsetFrame::SetPoseInParentFrame()) is reflected on the shadow side +automatically. The two frames are therefore coincident when the weld +constraint is satisfied. + +@tparam_default_scalar */ +template +class ShadowFrame final : public Frame { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ShadowFrame); + + /* Creates a frame named `name`, fixed to `shadow_link` and coincident with + `source_frame`. If `model_instance` is unspecified, uses `shadow_link`'s. + @pre `source_frame` is fixed to `shadow_link`'s primary link. */ + ShadowFrame(const std::string& name, const Link& shadow_link, + const Frame& source_frame, + std::optional model_instance = {}); + + ~ShadowFrame() final; + + /* Returns the frame on the primary link that this frame mirrors. */ + const Frame& source_frame() const { return source_frame_; } + + math::RigidTransform GetFixedPoseInBodyFrame() const final { + // The shadow link frame coincides with the primary's, so the source + // frame's pose in its own link frame is also this frame's pose in ours. + return source_frame_.GetFixedPoseInBodyFrame(); + } + + math::RotationMatrix GetFixedRotationMatrixInBodyFrame() const final { + return source_frame_.GetFixedRotationMatrixInBodyFrame(); + } + + protected: + /* @pre The source frame already has a clone in `tree_clone`. */ + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const final; + + /* @pre The source frame already has a clone in `tree_clone`. */ + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const final; + + std::unique_ptr> DoCloneToScalar( + const MultibodyTree& tree_clone) const final; + + std::unique_ptr> DoShallowClone() const final; + + math::RigidTransform DoCalcPoseInBodyFrame( + const systems::Parameters& parameters) const final; + + math::RotationMatrix DoCalcRotationMatrixInBodyFrame( + const systems::Parameters& parameters) const final; + + private: + // Helper method to make a clone templated on ToScalar. + template + std::unique_ptr> TemplatedDoCloneToScalar( + const MultibodyTree& tree_clone) const; + + // The frame on the primary link whose pose this frame mirrors. + const Frame& source_frame_; +}; + +} // namespace internal +} // namespace multibody +} // namespace drake + +DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class drake::multibody::internal::ShadowFrame); diff --git a/multibody/tree/weld_joint.cc b/multibody/tree/weld_joint.cc index b2376c3d3342..4e4a644374fe 100644 --- a/multibody/tree/weld_joint.cc +++ b/multibody/tree/weld_joint.cc @@ -77,27 +77,26 @@ std::unique_ptr> WeldJoint::MakeMobilizerForJoint( const internal::SpanningForest::Mobod& mobod, internal::MultibodyTree* tree) const { DRAKE_DEMAND(tree != nullptr); - const Frame& Jp = this->frame_on_parent(); - const Frame& Jc = this->frame_on_child(); - const bool X_JpJc_is_identity = X_JpJc_.IsExactlyIdentity(); - - const Frame* F{}; - const Frame* M{}; - if (mobod.is_reversed()) { - M = &Jp; // The reversed case: outboard==parent, inboard==child. - F = X_JpJc_is_identity - ? &Jc - : &tree->AddEphemeralFrame(std::make_unique>( - this->MakeUniqueOffsetFrameName(Jc, "F"), Jc, - X_JpJc_.inverse(), this->model_instance())); - } else { - M = &Jc; // The normal case: outboard==child, inboard==parent. - F = X_JpJc_is_identity - ? &Jp - : &tree->AddEphemeralFrame(std::make_unique>( - this->MakeUniqueOffsetFrameName(Jp, "F"), Jp, X_JpJc_, - this->model_instance())); - } + const bool reverse = mobod.is_reversed(); + // These are the joint's parent and child frames, but adjusted for reversal + // to locate them on the inboard and outboard bodies. Reversed means + // outboard==parent and inboard==child; normally outboard==child and + // inboard==parent. + const auto [Jin, Jout] = this->tree_frames(reverse); + // The offset from the inboard frame to the outboard one, which is X_JcJp = + // (X_JpJc)⁻¹ in the reversed case. + const math::RigidTransform X_JinJout = + reverse ? X_JpJc_.inverse() : X_JpJc_; + + // M is the outboard frame; F is colocated with the outboard frame but fixed + // to the inboard body, so that X_FM is the identity (see above). + const Frame* M = Jout; + const Frame* F = + X_JpJc_.IsExactlyIdentity() + ? Jin + : &tree->AddEphemeralFrame(std::make_unique>( + this->MakeUniqueOffsetFrameName(*Jin, "F"), *Jin, X_JinJout, + this->model_instance())); auto weld_mobilizer = std::make_unique>(mobod, *F, *M);