[multibody] Add CurvilinearJoint (#22196)

common/trajectories/piecewise_constant_curvature_trajectory.h

@@ -40,8 +40,8 @@ namespace trajectories {
  For constant curvature paths on a plane, the <a
  href="https://en.wikipedia.org/wiki/Frenet%E2%80%93Serret_formulas">Frenet–Serret
  formulas</a> simplify and we can write: <pre>
-     dFx/ds(s) =  ρ(s)⋅ Fy(s)
-     dFy/ds(s) = -ρ(s)⋅ Fx(s)
+     dFx/ds(s) =  ρ(s)⋅Fy(s)
+     dFy/ds(s) = -ρ(s)⋅Fx(s)
      dFz/ds(s) =  0
  </pre>
  for the entire trajectory.
@@ -69,6 +69,9 @@ class PiecewiseConstantCurvatureTrajectory final
   /** An empty piecewise constant curvature trajectory. */
   PiecewiseConstantCurvatureTrajectory() = default;
 
+  template <typename U>
+  using ScalarValueConverter = typename systems::scalar_conversion::template ValueConverter<T, U>;
+
   /** Constructs a piecewise constant curvature trajectory.
 
    Endpoints of each constant-curvature segments are defined by n breaks
@@ -90,7 +93,7 @@ class PiecewiseConstantCurvatureTrajectory final
    @param turning_rates A vector of n-1 turning rates ρᵢ for each segment.
    @param initial_curve_tangent The initial tangent of the curve expressed in
    the parent frame, t̂_A(s₀).
-   @param   The normal axis of the 2D plane in which the curve
+   @param plane_normal The normal axis of the 2D plane in which the curve
    lies, expressed in the parent frame, p̂_A.
    @param initial_position The initial position of the curve expressed in
    the parent frame, p_AoFo_A(s₀).
@@ -118,12 +121,12 @@ class PiecewiseConstantCurvatureTrajectory final
             other.get_initial_pose()
                 .rotation()
                 .col(kCurveTangentIndex)
-                .template cast<U>(),
+                .unaryExpr(ScalarValueConverter<U>{}),
             other.get_initial_pose()
                 .rotation()
                 .col(kPlaneNormalIndex)
-                .template cast<U>(),
-            other.get_initial_pose().translation().template cast<U>()) {}
+                .unaryExpr(ScalarValueConverter<U>{}),
+            other.get_initial_pose().translation().unaryExpr(ScalarValueConverter<U>{})) {}
 
   /** @returns the number of rows in the output of value(). */
   Eigen::Index rows() const override { return 3; }
@@ -221,7 +224,7 @@ class PiecewiseConstantCurvatureTrajectory final
   static std::vector<T> ScalarConvertStdVector(
       const std::vector<U>& segment_data) {
     std::vector<T> converted_segment_data;
-    systems::scalar_conversion::ValueConverter<U, T> converter;
+    ScalarValueConverter<U> converter;
     for (const U& segment : segment_data) {
       converted_segment_data.push_back(converter(segment));
     }

multibody/tree/BUILD.bazel

@@ -87,6 +87,8 @@ drake_cc_library(
         "body_node.cc",
         "body_node_impl.cc",
         "body_node_impl_mass_matrix.cc",
+        "curvilinear_joint.cc",
+        "curvilinear_mobilizer.cc",
         "door_hinge.cc",
         "element_collection.cc",
         "fixed_offset_frame.cc",
@@ -131,6 +133,8 @@ drake_cc_library(
         "body_node.h",
         "body_node_impl.h",
         "body_node_world.h",
+        "curvilinear_joint.h",
+        "curvilinear_mobilizer.h",
         "door_hinge.h",
         "element_collection.h",
         "fixed_offset_frame.h",
@@ -185,7 +189,9 @@ drake_cc_library(
         "//common:nice_type_name",
         "//common:string_container",
         "//common:unused",
+        "//common/trajectories:piecewise_constant_curvature_trajectory",
         "//math:geometric_transform",
+        "//math:wrap_to",
         "//multibody/topology",
         "//systems/framework:leaf_system",
     ],
@@ -446,6 +452,15 @@ drake_cc_library(
     ],
 )
 
+drake_cc_googletest(
+    name = "curvilinear_mobilizer_test",
+    deps = [
+        ":mobilizer_tester",
+        ":tree",
+        "//common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "planar_mobilizer_test",
     deps = [
@@ -611,6 +626,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "curvilinear_joint_test",
+    deps = [
+        ":tree",
+        "//common/test_utilities:expect_throws_message",
+    ],
+)
+
 drake_cc_googletest(
     name = "revolute_joint_test",
     deps = [

multibody/tree/body_node_impl.cc

@@ -1,6 +1,7 @@
 #include "drake/multibody/tree/body_node_impl.h"
 
 #include "drake/common/default_scalars.h"
+#include "drake/multibody/tree/curvilinear_mobilizer.h"
 #include "drake/multibody/tree/planar_mobilizer.h"
 #include "drake/multibody/tree/prismatic_mobilizer.h"
 #include "drake/multibody/tree/quaternion_floating_mobilizer.h"
@@ -864,6 +865,7 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAccelerationBias(
 
 // Macro used to explicitly instantiate implementations for every mobilizer.
 #define EXPLICITLY_INSTANTIATE_IMPLS(T)                        \
+  template class BodyNodeImpl<T, CurvilinearMobilizer>;        \
   template class BodyNodeImpl<T, PlanarMobilizer>;             \
   template class BodyNodeImpl<T, PrismaticMobilizer>;          \
   template class BodyNodeImpl<T, QuaternionFloatingMobilizer>; \

multibody/tree/body_node_impl_mass_matrix.cc

@@ -4,6 +4,7 @@
 /* clang-format on */
 
 #include "drake/common/default_scalars.h"
+#include "drake/multibody/tree/curvilinear_mobilizer.h"
 #include "drake/multibody/tree/planar_mobilizer.h"
 #include "drake/multibody/tree/prismatic_mobilizer.h"
 #include "drake/multibody/tree/quaternion_floating_mobilizer.h"
@@ -139,6 +140,7 @@ DEFINE_MASS_MATRIX_OFF_DIAGONAL_BLOCK(6)
 
 // Macro used to explicitly instantiate implementations for every mobilizer.
 #define EXPLICITLY_INSTANTIATE_IMPLS(T)                        \
+  template class BodyNodeImpl<T, CurvilinearMobilizer>;        \
   template class BodyNodeImpl<T, PlanarMobilizer>;             \
   template class BodyNodeImpl<T, PrismaticMobilizer>;          \
   template class BodyNodeImpl<T, QuaternionFloatingMobilizer>; \

multibody/tree/curvilinear_joint.cc

@@ -0,0 +1,94 @@
+#include "drake/multibody/tree/curvilinear_joint.h"
+
+#include <memory>
+#include <stdexcept>
+
+#include "drake/common/eigen_types.h"
+#include "drake/common/never_destroyed.h"
+#include "drake/multibody/tree/multibody_tree.h"
+
+using drake::never_destroyed;
+
+namespace drake {
+namespace multibody {
+
+template <typename T>
+CurvilinearJoint<T>::CurvilinearJoint(
+    const std::string& name, const Frame<T>& frame_on_parent,
+    const Frame<T>& frame_on_child,
+    const trajectories::PiecewiseConstantCurvatureTrajectory<double>
+        curvilinear_path,
+    bool is_periodic, double pos_lower_limit, double pos_upper_limit,
+    double damping)
+    : Joint<T>(
+          name, frame_on_parent, frame_on_child,
+          VectorX<double>::Constant(1, damping),
+          VectorX<double>::Constant(1, pos_lower_limit),
+          VectorX<double>::Constant(1, pos_upper_limit),
+          VectorX<double>::Constant(1,
+                                    -std::numeric_limits<double>::infinity()),
+          VectorX<double>::Constant(1, std::numeric_limits<double>::infinity()),
+          VectorX<double>::Constant(1,
+                                    -std::numeric_limits<double>::infinity()),
+          VectorX<double>::Constant(1,
+                                    std::numeric_limits<double>::infinity())),
+      curvilinear_path_(curvilinear_path),
+      is_periodic_(is_periodic) {}
+
+template <typename T>
+CurvilinearJoint<T>::~CurvilinearJoint() = default;
+
+template <typename T>
+const std::string& CurvilinearJoint<T>::type_name() const {
+  static const never_destroyed<std::string> name{kTypeName};
+  return name.access();
+}
+
+template <typename T>
+template <typename ToScalar>
+std::unique_ptr<Joint<ToScalar>> CurvilinearJoint<T>::TemplatedDoCloneToScalar(
+    const internal::MultibodyTree<ToScalar>& tree_clone) const {
+  const Frame<ToScalar>& frame_on_parent_body_clone =
+      tree_clone.get_variant(this->frame_on_parent());
+  const Frame<ToScalar>& frame_on_child_body_clone =
+      tree_clone.get_variant(this->frame_on_child());
+
+  // Make the Joint<T> clone.
+  auto joint_clone = std::make_unique<CurvilinearJoint<ToScalar>>(
+      this->name(), frame_on_parent_body_clone, frame_on_child_body_clone,
+      curvilinear_path_, is_periodic_, this->position_lower_limit(),
+      this->position_upper_limit(), this->default_damping());
+
+  joint_clone->set_velocity_limits(this->velocity_lower_limits(),
+                                   this->velocity_upper_limits());
+  joint_clone->set_acceleration_limits(this->acceleration_lower_limits(),
+                                       this->acceleration_upper_limits());
+  joint_clone->set_default_positions(this->default_positions());
+
+  return joint_clone;
+}
+
+template <typename T>
+std::unique_ptr<Joint<double>> CurvilinearJoint<T>::DoCloneToScalar(
+    const internal::MultibodyTree<double>& tree_clone) const {
+  return TemplatedDoCloneToScalar(tree_clone);
+}
+
+template <typename T>
+std::unique_ptr<Joint<AutoDiffXd>> CurvilinearJoint<T>::DoCloneToScalar(
+    const internal::MultibodyTree<AutoDiffXd>& tree_clone) const {
+  return TemplatedDoCloneToScalar(tree_clone);
+}
+
+template <typename T>
+std::unique_ptr<Joint<symbolic::Expression>>
+CurvilinearJoint<T>::DoCloneToScalar(
+    const internal::MultibodyTree<symbolic::Expression>& tree_clone) const {
+  return TemplatedDoCloneToScalar(tree_clone);
+}
+
+}  // namespace multibody
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::multibody::CurvilinearJoint);