[multibody] Auto loop breaking: retarget joints

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 */ {

multibody/topology/link_joint_graph_link.h

@@ -107,6 +107,14 @@ class LinkJointGraph::Link {
   shadow links, if any. */
   const std::vector<LinkIndex>& 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<JointIndex>& 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

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",

multibody/tree/joint.h

@@ -767,6 +767,32 @@ class Joint : public MultibodyElement<T> {
       const internal::SpanningForest::Mobod& mobod,
       internal::MultibodyTree<T>* 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<T>& frame) {
+    effective_frame_on_parent_ = &frame;
+  }
+  void set_effective_frame_on_child(const Frame<T>& 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<T>& effective_frame_on_parent() const {
+    return effective_frame_on_parent_ != nullptr ? *effective_frame_on_parent_
+                                                 : frame_on_parent_;
+  }
+  const Frame<T>& 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<T> {
     std::unique_ptr<Joint<ToScalar>> 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<T> {
   /// 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<T>*, const Frame<T>*> 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<T> {
   const Frame<T>& frame_on_parent_;  // Frame Jp.
   const Frame<T>& 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<T>* effective_frame_on_parent_{nullptr};
+  const Frame<T>* effective_frame_on_child_{nullptr};
+
   VectorX<double> damping_;
 
   // Joint position limits. These vectors have zero size for joints with no

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<T>::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<double>& M_primary =
         links_.get_element(primary_index).default_spatial_inertia();
     const SpatialInertia<double> M_shadow(M_primary.get_mass() / num_copies,
                                           M_primary.get_com(),
                                           M_primary.get_unit_inertia());
-    const RigidBody<T>& shadow = AddEphemeralLink(
-        graph_link.name(), graph_link.model_instance(), M_shadow);
-    DRAKE_DEMAND(shadow.index() == graph_link.index());
+    const Link<T>& 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<T>& 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<T>& source_frame =
+        moved_child ? joint.frame_on_child() : joint.frame_on_parent();
+
+    const Frame<T>& shadow_frame =
+        AddEphemeralFrame(std::make_unique<ShadowFrame<T>>(
+            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. */

multibody/tree/prismatic_joint.cc

@@ -127,10 +127,7 @@ PrismaticJoint<T>::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<T>& Jin =
-      reverse ? this->frame_on_child() : this->frame_on_parent();
-  const Frame<T>& 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<T>::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<FixedOffsetFrame<T>>(
-        this->MakeUniqueOffsetFrameName(Jin, "F"), Jin,
+        this->MakeUniqueOffsetFrameName(*Jin, "F"), *Jin,
         math::RigidTransformd(R_JinF), this->model_instance()));
     M = &tree->AddEphemeralFrame(std::make_unique<FixedOffsetFrame<T>>(
-        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<internal::PrismaticMobilizer<T>> prismatic_mobilizer;

multibody/tree/revolute_joint.cc

@@ -124,10 +124,7 @@ std::unique_ptr<internal::Mobilizer<T>> RevoluteJoint<T>::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<T>& Jin =
-      reverse ? this->frame_on_child() : this->frame_on_parent();
-  const Frame<T>& 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<internal::Mobilizer<T>> RevoluteJoint<T>::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<FixedOffsetFrame<T>>(
-        this->MakeUniqueOffsetFrameName(Jin, "F"), Jin,
+        this->MakeUniqueOffsetFrameName(*Jin, "F"), *Jin,
         math::RigidTransformd(R_JinF), this->model_instance()));
     M = &tree->AddEphemeralFrame(std::make_unique<FixedOffsetFrame<T>>(
-        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<internal::RevoluteMobilizer<T>> revolute_mobilizer;