Optimize F & M frames for prismatic joint's mobilizer. (#22925)

Author: sherm1

math/rigid_transform.h

@@ -478,7 +478,8 @@ class RigidTransform {
   /// `X_AC = X_AB * atX_BC`. This requires only 6 floating point operations.
   /// @param[in] atX_BC An axial translation transform about the indicated
   ///   `axis`.
-  /// @param[out] X_AC Preallocated space for the result.
+  /// @param[out] X_AC Preallocated space for the result, which will be a
+  ///   general transform.
   /// @tparam axis 0, 1, or 2 corresponding to +x, +y, or +z rotation axis.
   /// @pre atX_BC is an @ref special_xform_def "Axial translation transform".
   template <int axis>
@@ -494,6 +495,51 @@ class RigidTransform {
     X_AC->set_translation(p_AoBo_A_ + p_BC[axis] * R_AB_.col(axis));
   }
 
+  /// (Internal use only) With `this` a general transform X_BC, and given an
+  /// axial translation transform atX_AB, efficiently calculates
+  /// `X_AC = atX_AB * X_BC`. This requires just 1 floating point operation.
+  /// @param[in] atX_AB An axial translation transform along the indicated
+  ///   `axis`.
+  /// @param[out] X_AC Preallocated space for the result, which will be a
+  ///   general transform.
+  /// @tparam axis 0, 1, or 2 corresponding to +x, +y, or +z translation axis.
+  /// @pre atX_AB is an @ref special_xform_def "Axial translation transform"
+  template <int axis>
+#ifndef DRAKE_DOXYGEN_CXX
+    requires(0 <= axis && axis <= 2)
+#endif
+  void PreMultiplyByAxialTranslation(const RigidTransform<T>& atX_AB,
+                                     RigidTransform<T>* X_AC) const {
+    DRAKE_ASSERT(X_AC != nullptr);
+    DRAKE_ASSERT_VOID(atX_AB.IsAxialTranslationOnlyOrThrow<axis>());
+    // Note that R_AB = I.
+    const RigidTransform<T>& X_BC = *this;  // Rename to keep frames straight.
+    X_AC->set_rotation(X_BC.rotation());    // R_AC = R_AB * R_BC = R_BC.
+    X_AC->p_AoBo_A_ = X_BC.translation();   // Initialize to p_BC_B.
+    X_AC->p_AoBo_A_[axis] += atX_AB.translation()[axis];  // The only flop.
+  }
+
+  /// (Internal use only) Given a new `distance`, updates the axial translation
+  /// transform atX_BC to represent the new translation by that amount. We
+  /// expect that atX_BC was already such a transform (about the given x, y, or
+  /// z axis). Only the 1 active element is modified; the other 11 remain
+  /// unchanged. No floating point operations are needed.
+  /// @param[in] distance the component of p_BC along the `axis` direction.
+  /// @param[in,out] atX_BC the axial translation transform matrix to be
+  ///   updated.
+  /// @tparam axis 0, 1, or 2 corresponding to +x, +y, or +z translation axis.
+  /// @pre atX_BC is an @ref special_xform_def "Axial translation transform".
+  template <int axis>
+#ifndef DRAKE_DOXYGEN_CXX
+    requires(0 <= axis && axis <= 2)
+#endif
+  static void UpdateAxialTranslation(const T& distance,
+                                     RigidTransform<T>* atX_BC) {
+    DRAKE_ASSERT(atX_BC != nullptr);
+    DRAKE_ASSERT_VOID(atX_BC->IsAxialTranslationOnlyOrThrow<axis>());
+    atX_BC->p_AoBo_A_[axis] = distance;
+  }
+
   /// (Internal use only) Composes `this` general transform X_AB with a given
   /// rotation-only transform rX_BC to efficiently calculate
   /// `X_AC = X_AB * rX_BC`. This requires 45 floating point operations.

math/test/rigid_transform_test.cc

@@ -874,12 +874,19 @@ GTEST_TEST(RigidTransform, SpecializedTransformOperators) {
   EXPECT_TRUE(X_BC_update.IsNearlyEqualTo(
       Xform::MakeAxialRotation<2>(4 * theta), kTol));
 
+  // Make an axial translation transform (this one is y-axial).
+  const Xform ytX_BC(Vector3d(0, -2, 0));  // d = -2
+
+  // Test UpdateAxialTranslation().
+  Xform ytX_BC_update = ytX_BC;
+  Xform::UpdateAxialTranslation<1>(5, &ytX_BC_update);  // d = 5
+  EXPECT_TRUE(ytX_BC_update.IsExactlyEqualTo(Xform(Vector3d(0, 5, 0))));
+
   // Make general, rotation-only, and translation-only transforms.
   const Xform X_AB(RollPitchYaw(1.0, 2.0, 3.0), Vector3d(1.5, 2.5, 3.5));
   const Xform rX_BC(RollPitchYaw(0.5, 1.5, -2.5), Vector3d::Zero());
   const Xform tX_BC(Vector3d(-1.0, 2.0, -3.0));
-  const Xform ytX_BC(Vector3d(0.0, -2.0, 0.0));  // y-axial translation
-  Xform X_AC;                                    // reusable result
+  Xform X_AC;  // reusable result
 
   // Test PostMultiplyByRotation() and PostMultiplyBy[Axial]Translation().
   X_AB.PostMultiplyByRotation(rX_BC, &X_AC);
@@ -899,7 +906,7 @@ GTEST_TEST(RigidTransform, SpecializedTransformOperators) {
   X_AB.PostMultiplyByAxialRotation<2>(zrX_BC, &X_AC);
   EXPECT_TRUE(X_AC.IsNearlyEqualTo(X_AB * zrX_BC, kTol));
 
-  // Test PreMultiplyByAxialRotation().
+  // Test PreMultiplyByAxialRotation(), PreMultiplyByAxialTranslation().
   const Xform X_CD(RollPitchYaw(1.0, 2.0, 3.0), Vector3d(1.5, 2.5, 3.5));
   Xform X_BD;  // reusable result
   X_CD.PreMultiplyByAxialRotation<0>(xrX_BC, &X_BD);
@@ -908,6 +915,8 @@ GTEST_TEST(RigidTransform, SpecializedTransformOperators) {
   EXPECT_TRUE(X_BD.IsNearlyEqualTo(yrX_BC * X_CD, kTol));
   X_CD.PreMultiplyByAxialRotation<2>(zrX_BC, &X_BD);
   EXPECT_TRUE(X_BD.IsNearlyEqualTo(zrX_BC * X_CD, kTol));
+  X_CD.PreMultiplyByAxialTranslation<1>(ytX_BC, &X_BD);
+  EXPECT_TRUE(X_BD.IsNearlyEqualTo(ytX_BC * X_CD, kTol));
 }
 
 GTEST_TEST(RigidTransform, TestMemoryLayoutOfRigidTransformDouble) {

multibody/tree/body_node_impl.cc

@@ -935,7 +935,9 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAccelerationBias(
 #define EXPLICITLY_INSTANTIATE_IMPLS(T)                           \
   template class BodyNodeImpl<T, CurvilinearMobilizer<T>>;        \
   template class BodyNodeImpl<T, PlanarMobilizer<T>>;             \
-  template class BodyNodeImpl<T, PrismaticMobilizer<T>>;          \
+  template class BodyNodeImpl<T, PrismaticMobilizerAxial<T, 0>>;  \
+  template class BodyNodeImpl<T, PrismaticMobilizerAxial<T, 1>>;  \
+  template class BodyNodeImpl<T, PrismaticMobilizerAxial<T, 2>>;  \
   template class BodyNodeImpl<T, QuaternionFloatingMobilizer<T>>; \
   template class BodyNodeImpl<T, RevoluteMobilizerAxial<T, 0>>;   \
   template class BodyNodeImpl<T, RevoluteMobilizerAxial<T, 1>>;   \

multibody/tree/body_node_impl_mass_matrix.cc

@@ -142,7 +142,9 @@ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6)
 #define EXPLICITLY_INSTANTIATE_IMPLS(T)                           \
   template class BodyNodeImpl<T, CurvilinearMobilizer<T>>;        \
   template class BodyNodeImpl<T, PlanarMobilizer<T>>;             \
-  template class BodyNodeImpl<T, PrismaticMobilizer<T>>;          \
+  template class BodyNodeImpl<T, PrismaticMobilizerAxial<T, 0>>;  \
+  template class BodyNodeImpl<T, PrismaticMobilizerAxial<T, 1>>;  \
+  template class BodyNodeImpl<T, PrismaticMobilizerAxial<T, 2>>;  \
   template class BodyNodeImpl<T, QuaternionFloatingMobilizer<T>>; \
   template class BodyNodeImpl<T, RevoluteMobilizerAxial<T, 0>>;   \
   template class BodyNodeImpl<T, RevoluteMobilizerAxial<T, 1>>;   \

multibody/tree/joint.cc

@@ -115,6 +115,18 @@ std::unique_ptr<Joint<T>> Joint<T>::DoShallowClone() const {
       "The {} joint failed to override DoShallowClone()", type_name()));
 }
 
+template <typename T>
+std::string Joint<T>::MakeUniqueOffsetFrameName(
+    const Frame<T>& parent_frame, const std::string& suffix) const {
+  const internal::MultibodyTree<T>& tree = this->get_parent_tree();
+  std::string new_name =
+      fmt::format("{}_{}_{}", this->name(), parent_frame.name(), suffix);
+  while (tree.HasFrameNamed(new_name, this->model_instance())) {
+    new_name = "_" + new_name;
+  }
+  return new_name;
+}
+
 template <typename T>
 void Joint<T>::SetSpatialVelocityImpl(systems::Context<T>* context,
                                       const SpatialVelocity<T>& V_FM,

multibody/tree/joint.h

@@ -779,6 +779,16 @@ class Joint : public MultibodyElement<T> {
     DRAKE_DEMAND(has_mobilizer());
     return *mobilizer_;
   }
+
+  // (Internal use only) This utility generates a unique name for an offset
+  // frame of the form jointname_parentframename_suffix. This is intended for
+  // creating F and M mobilizer frames that are offset from joint frames Jp and
+  // Jc. The name is guaranteed to be unique within this Joint's model instance.
+  // If necessary, leading underscores are prepended until uniqueness is
+  // achieved. Be sure to create the new frame in the _Joint's_ model instance
+  // to avoid name clashes.
+  std::string MakeUniqueOffsetFrameName(const Frame<T>& parent_frame,
+                                        const std::string& suffix) const;
 #endif
   // End of hidden Doxygen section.
 

multibody/tree/mobilizer_impl.h

@@ -29,7 +29,10 @@ Every concrete Mobilizer derived from MobilizerImpl must implement the
 following (ideally inline) methods (some have defaults; see below). Note that
 these are not virtual methods so we have to document them here in a comment
 rather than as declarations in the header file. The code won't compile if
-any mobilizer fails to implement the non-defaulted methods.
+any mobilizer fails to implement the non-defaulted methods. These are const
+member functions (rather than static members) so are permitted to depend on
+mobilizer-specific data members, though in many cases they don't require any
+such data.
 
 @note The coordinate pointers q and v are guaranteed to point to the kNq or kNv
 state variables for the particular mobilizer. They are only 8-byte aligned so be

multibody/tree/prismatic_joint.cc

@@ -3,7 +3,7 @@
 #include <memory>
 #include <stdexcept>
 
-#include "drake/multibody/tree/multibody_tree.h"
+#include "drake/multibody/tree/multibody_tree-inl.h"
 
 namespace drake {
 namespace multibody {
@@ -30,8 +30,7 @@ PrismaticJoint<T>::PrismaticJoint(const std::string& name,
   const double kEpsilon = std::sqrt(std::numeric_limits<double>::epsilon());
   if (axis.isZero(kEpsilon)) {
     throw std::logic_error(
-        "Prismatic joint axis vector must have nonzero "
-        "length.");
+        "Prismatic joint axis vector must have nonzero length.");
   }
   if (damping < 0) {
     throw std::logic_error("Prismatic joint damping must be nonnegative.");
@@ -97,6 +96,94 @@ std::unique_ptr<Joint<T>> PrismaticJoint<T>::DoShallowClone() const {
       this->position_upper_limit(), this->default_damping());
 }
 
+/* For a prismatic joint, we are given Jp on parent P, Jc on child C, and a
+translation unit vector â whose components are identical in Jp and Jc. At q=0,
+Jp and Jc are coincident. At all times, the coordinate axes of Jp and Jc remain
+aligned, and vector â has constant and equal components when expressed in these
+frames. Jc translates with respect to Jp by a translation distance q meters
+along the translation vector â. The origins Jpo and Jco are thus separated by
+a vector q⋅â so that Jco = Jpo + q⋅â.
+
+We need to implement this joint with one of three available prismatic
+mobilizers. Every mobilizer has an inboard frame F, and an outboard frame M. The
+available prismatic mobilizers translate along one of the coordinate axes x, y,
+or z. If â happens already to be a coordinate axis of Jp (and Jc), we are golden
+and can use Jp and Jc as F and M. Otherwise we are going to have to create new
+frames F and M such that one of their coordinate axes is aligned with â.
+
+To create new frames, we use the MakeFromOneVector() function to create a
+RotationMatrix R_JpF (R_JcM) such that F(M)'s z axis is aligned with â (or -â if
+the mobilizer is reversed from the joint). That rotation matrix is what we need
+to create FixedOffsetFrame F from Jp (or Jc if reversed) and M from Jc (or Jp
+if reversed). Then we use the z-axial prismatic mobilizer to implement the
+joint. */
+template <typename T>
+std::unique_ptr<internal::Mobilizer<T>>
+PrismaticJoint<T>::MakeMobilizerForJoint(
+    const internal::SpanningForest::Mobod& mobod,
+    internal::MultibodyTree<T>* tree) const {
+  DRAKE_DEMAND(tree != nullptr);
+  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. 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 Eigen::Vector3d axis = reverse ? -axis_ : axis_;  // a unit vector
+
+  // Determine whether the axis is one of +x, +y, +z, or something else.
+  // In the latter case we'll change this to +z below.
+  std::optional<int> which_axis = [&axis]() -> std::optional<int> {
+    for (int i = 0; i < 3; ++i) {
+      if (axis[i] == 1.0) return i;  // exact match only
+    }
+    return {};
+  }();
+
+  const Frame<T>* F{};
+  const Frame<T>* M{};
+  if (!which_axis) {
+    which_axis = 2;  // Arbitrarily using the z axis mobilizer.
+    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,
+        math::RigidTransformd(R_JinF), this->model_instance()));
+    M = &tree->AddEphemeralFrame(std::make_unique<FixedOffsetFrame<T>>(
+        this->MakeUniqueOffsetFrameName(Jout, "M"), Jout,
+        math::RigidTransformd(R_JinF), this->model_instance()));
+  } else {
+    F = &Jin;
+    M = &Jout;
+  }
+
+  std::unique_ptr<internal::PrismaticMobilizer<T>> prismatic_mobilizer;
+
+  switch (*which_axis) {
+    case 0:
+      prismatic_mobilizer =
+          std::make_unique<internal::PrismaticMobilizerAxial<T, 0>>(mobod, *F,
+                                                                    *M);
+      break;
+    case 1:
+      prismatic_mobilizer =
+          std::make_unique<internal::PrismaticMobilizerAxial<T, 1>>(mobod, *F,
+                                                                    *M);
+      break;
+    case 2:
+      prismatic_mobilizer =
+          std::make_unique<internal::PrismaticMobilizerAxial<T, 2>>(mobod, *F,
+                                                                    *M);
+      break;
+  }
+
+  DRAKE_DEMAND(prismatic_mobilizer != nullptr);
+  prismatic_mobilizer->set_default_position(this->default_positions());
+  return prismatic_mobilizer;
+}
+
 }  // namespace multibody
 }  // namespace drake
 

multibody/tree/prismatic_joint.h

@@ -16,13 +16,12 @@ namespace multibody {
 
 /// This Joint allows two bodies to translate relative to one another along a
 /// common axis.
-/// That is, given a frame F attached to the parent body P and a frame M
-/// attached to the child body B (see the Joint class's documentation),
-/// this Joint allows frames F and M to translate with respect to each other
+/// That is, given a frame Jp attached to the parent body P and a frame Jc
+/// attached to the child body C (see the Joint class's documentation),
+/// this Joint allows frames Jp and Jc to translate with respect to each other
 /// along an axis â. The translation distance is defined positive when child
-/// body B translates along the direction of â.
-/// Axis â is constant and has the same measures in both frames F and M, that
-/// is, `â_F = â_M`.
+/// body C translates along the direction of â. Axis vector â is constant and
+/// has the same components in both frames Jp and Jc, that is, `â_Jp = â_Jc`.
 ///
 /// @tparam_default_scalar
 template <typename T>
@@ -36,19 +35,19 @@ class PrismaticJoint final : public Joint<T> {
   static const char kTypeName[];
 
   /// Constructor to create a prismatic joint between two bodies so that
-  /// frame F attached to the parent body P and frame M attached to the child
-  /// body B, translate relatively to one another along a common axis. See this
+  /// frame Jp attached to the parent body P and frame Jc attached to the child
+  /// body C, translate relatively to one another along a common axis. See this
   /// class's documentation for further details on the definition of these
   /// frames and translation distance.
   /// The first three arguments to this constructor are those of the Joint class
   /// constructor. See the Joint class's documentation for details.
   /// The additional parameter `axis` is:
   /// @param[in] axis
   ///   A vector in ℝ³ specifying the translation axis for this joint. Given
-  ///   that frame M only translates with respect to F and there is no relative
-  ///   rotation, the measures of `axis` in either frame F or M
-  ///   are exactly the same, that is, `axis_F = axis_M`.
-  ///   This vector can have any length, only the direction is used.
+  ///   that frame Jc only translates with respect to Jp and there is no
+  ///   relative rotation, the components of `axis` in either frame Jp or Jc
+  ///   are exactly the same, that is, `axis_Jp = axis_Jc`. This vector can have
+  ///   any length, only the direction is used.
   /// @param[in] pos_lower_limit
   ///   Lower position limit, in meters, for the translation coordinate
   ///   (see get_translation()).
@@ -76,9 +75,9 @@ class PrismaticJoint final : public Joint<T> {
   const std::string& type_name() const final;
 
   /// Returns the axis of translation for `this` joint as a unit vector.
-  /// Since the measures of this axis in either frame F or M are the same (see
-  /// this class's documentation for frame definitions) then,
-  /// `axis = axis_F = axis_M`.
+  /// Since the components of this axis in either frame Jp or Jc are the same
+  /// (see this class's documentation for frame definitions) then,
+  /// `axis = axis_Jp = axis_Jc`.
   const Vector3<double>& translation_axis() const { return axis_; }
 
   /// Returns `this` joint's default damping constant in N⋅s/m.
@@ -243,6 +242,7 @@ class PrismaticJoint final : public Joint<T> {
                        MultibodyForces<T>* forces) const final {
     // Right now we assume all the forces in joint_tau go into a single
     // mobilizer.
+    DRAKE_DEMAND(joint_dof == 0);
     Eigen::Ref<VectorX<T>> tau_mob =
         get_mobilizer().get_mutable_generalized_forces_from_array(
             &forces->mutable_generalized_forces());
@@ -300,16 +300,7 @@ class PrismaticJoint final : public Joint<T> {
   // Joint<T> finals:
   std::unique_ptr<internal::Mobilizer<T>> MakeMobilizerForJoint(
       const internal::SpanningForest::Mobod& mobod,
-      internal::MultibodyTree<T>*) const final {
-    const auto [inboard_frame, outboard_frame] =
-        this->tree_frames(mobod.is_reversed());
-    // TODO(sherm1) The mobilizer needs to be reversed, not just the frames.
-    auto prismatic_mobilizer =
-        std::make_unique<internal::PrismaticMobilizer<T>>(
-            mobod, *inboard_frame, *outboard_frame, axis_);
-    prismatic_mobilizer->set_default_position(this->default_positions());
-    return prismatic_mobilizer;
-  }
+      internal::MultibodyTree<T>*) const final;
 
   std::unique_ptr<Joint<double>> DoCloneToScalar(
       const internal::MultibodyTree<double>& tree_clone) const final;