[multibody] Add CurvilinearJoint (#22196)

common/trajectories/BUILD.bazel

@@ -156,6 +156,7 @@ drake_cc_library(
         "//common:essential",
         "//common:pointer_cast",
         "//math:geometric_transform",
+        "//math:wrap_to",
         "//multibody/math:spatial_algebra",
         "//systems/framework:system_scalar_converter",
     ],

common/trajectories/piecewise_constant_curvature_trajectory.cc

@@ -10,7 +10,7 @@ template <typename T>
 PiecewiseConstantCurvatureTrajectory<T>::PiecewiseConstantCurvatureTrajectory(
     const std::vector<T>& breaks, const std::vector<T>& turning_rates,
     const Vector3<T>& initial_curve_tangent, const Vector3<T>& plane_normal,
-    const Vector3<T>& initial_position)
+    const Vector3<T>& initial_position, double periodicity_tolerance)
     : PiecewiseTrajectory<T>(breaks), segment_turning_rates_(turning_rates) {
   if (turning_rates.size() != breaks.size() - 1) {
     throw std::logic_error(
@@ -37,24 +37,27 @@ PiecewiseConstantCurvatureTrajectory<T>::PiecewiseConstantCurvatureTrajectory(
   segment_start_poses_ = MakeSegmentStartPoses(
       MakeInitialPose(initial_curve_tangent, plane_normal, initial_position),
       breaks, turning_rates);
+  is_periodic_ = IsNearlyPeriodic(periodicity_tolerance);
 }
 
 template <typename T>
 math::RigidTransform<T> PiecewiseConstantCurvatureTrajectory<T>::CalcPose(
     const T& s) const {
-  int segment_index = this->get_segment_index(s);
+  const T w = maybe_wrap(s);
+  int segment_index = this->get_segment_index(w);
   const math::RigidTransform<T> X_FiF =
       CalcRelativePoseInSegment(segment_turning_rates_[segment_index],
-                                s - this->start_time(segment_index));
+                                w - this->start_time(segment_index));
   return segment_start_poses_[segment_index] * X_FiF;
 }
 
 template <typename T>
 multibody::SpatialVelocity<T>
 PiecewiseConstantCurvatureTrajectory<T>::CalcSpatialVelocity(
     const T& s, const T& s_dot) const {
-  const math::RotationMatrix<T> R_AF = CalcPose(s).rotation();
-  const T& rho_i = segment_turning_rates_[this->get_segment_index(s)];
+  const T w = maybe_wrap(s);
+  const math::RotationMatrix<T> R_AF = CalcPose(w).rotation();
+  const T& rho_i = segment_turning_rates_[this->get_segment_index(w)];
 
   multibody::SpatialVelocity<T> spatial_velocity;
   // From Frenet–Serret analysis and the class doc for
@@ -73,8 +76,9 @@ template <typename T>
 multibody::SpatialAcceleration<T>
 PiecewiseConstantCurvatureTrajectory<T>::CalcSpatialAcceleration(
     const T& s, const T& s_dot, const T& s_ddot) const {
-  const math::RotationMatrix<T> R_AF = CalcPose(s).rotation();
-  const T& rho_i = segment_turning_rates_[this->get_segment_index(s)];
+  const T w = maybe_wrap(s);
+  const math::RotationMatrix<T> R_AF = CalcPose(w).rotation();
+  const T& rho_i = segment_turning_rates_[this->get_segment_index(w)];
 
   // The spatial acceleration is the time derivative of the spatial velocity.
   // We compute the acceleration by applying the chain rule to the formulas

common/trajectories/piecewise_constant_curvature_trajectory.h

@@ -1,12 +1,14 @@
 #pragma once
 
+#include <limits>
 #include <memory>
 #include <vector>
 
 #include "drake/common/drake_copyable.h"
 #include "drake/common/eigen_types.h"
 #include "drake/common/trajectories/piecewise_trajectory.h"
 #include "drake/math/rigid_transform.h"
+#include "drake/math/wrap_to.h"
 #include "drake/multibody/math/spatial_algebra.h"
 #include "drake/systems/framework/scalar_conversion_traits.h"
 
@@ -32,6 +34,11 @@ namespace trajectories {
  are used to define a frame F along the curve with basis vectors Fx, Fy, Fz
  coincident with vectors t̂, n̂, p̂, respectively.
 
+ A trajectory is said to be periodic if the pose X_AF of frame F is a periodic
+ function of arclength, i.e. X_AF(s) = X_AF(s + k⋅L) ∀ k ∈ ℤ, where L is the
+ length of a single cycle along the trajectory. Periodicity is determined
+ at construction.
+
  @note Though similar, frame F is distinct from the Frenet–Serret frame defined
  by the tangent-normal-binormal vectors T̂, N̂, B̂ See <a
  href="https://en.wikipedia.org/wiki/Frenet%E2%80%93Serret_formulas">Frenet–Serret
@@ -40,8 +47,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 +76,10 @@ 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
@@ -94,6 +105,12 @@ class PiecewiseConstantCurvatureTrajectory final
    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₀).
+   @param periodicity_tolerance Tolerance used to determine if the resulting
+   trajectory is periodic, according to the metric defined by
+   IsNearlyPeriodic(). If IsNearlyPeriodic(periodicity_tolerance) is true, then
+   the newly constructed trajectory will be periodic. That is,
+   X_AF(s) = X_AF(s + k⋅L) ∀ k ∈ ℤ, where L equals length(). Subsequent calls to
+   is_periodic() will return `true`.
 
    @throws std::exception if the number of turning rates does not match
    the number of segments
@@ -106,7 +123,8 @@ class PiecewiseConstantCurvatureTrajectory final
                                        const std::vector<T>& turning_rates,
                                        const Vector3<T>& initial_curve_tangent,
                                        const Vector3<T>& plane_normal,
-                                       const Vector3<T>& initial_position);
+                                       const Vector3<T>& initial_position,
+                                       double periodicity_tolerance = 1e-8);
 
   /** Scalar conversion constructor. See @ref system_scalar_conversion. */
   template <typename U>
@@ -118,16 +136,21 @@ 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 total arclength of the curve in meters. */
   T length() const { return this->end_time(); }
 
+  /* Returns `true` if `this` trajectory is periodic.
+   That is, X_AF(s) = X_AF(s + k⋅L) ∀ k ∈ ℤ, where L equals length(). */
+  boolean<T> is_periodic() const { return is_periodic_; }
+
   /** Calculates the trajectory's pose X_AF(s) at the given arclength s.
 
    @note For s < 0 and s > length() the pose is extrapolated as if the curve
@@ -221,13 +244,19 @@ 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));
     }
     return converted_segment_data;
   }
 
+  /* If the trajectory is periodic, this helper wraps the distance coordinate s
+   to the path's length. If not periodic, then it simply returns s. */
+  T maybe_wrap(const T& s) const {
+    return is_periodic_ ? math::wrap_to(s, T(0.), length()) : s;
+  }
+
   /* Calculates pose X_FiF of the frame F at distance ds from the start of the
    i-th segment, relative to frame Fi at the start of the segment.
 
@@ -289,6 +318,7 @@ class PiecewiseConstantCurvatureTrajectory final
 
   std::vector<T> segment_turning_rates_;
   std::vector<math::RigidTransform<T>> segment_start_poses_;
+  boolean<T> is_periodic_{false};
 
   static inline constexpr size_t kCurveTangentIndex = 0;
   static inline constexpr size_t kCurveNormalIndex = 1;

common/trajectories/test/piecewise_constant_curvature_trajectory_test.cc

@@ -207,6 +207,7 @@ GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestRandomizedTrajectory) {
 
 GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestPeriodicity) {
   const double r = 2;
+  const double length = 2 * M_PI * r;
   const double kappa = 1 / r;
   const int num_segments = 10;
   std::vector<double> segment_breaks_periodic(num_segments + 1);
@@ -231,8 +232,19 @@ GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestPeriodicity) {
       segment_breaks_aperiodic, turning_rates, curve_tangent, plane_normal,
       Vector3d::Zero());
 
+  EXPECT_TRUE(periodic_trajectory.is_periodic());
   EXPECT_TRUE(periodic_trajectory.IsNearlyPeriodic(kTolerance));
+  EXPECT_FALSE(aperiodic_trajectory.is_periodic());
   EXPECT_FALSE(aperiodic_trajectory.IsNearlyPeriodic(kTolerance));
+
+  // Test periodicity for at arbitrary value.
+  const double s = 1.5;
+  const RigidTransformd X_AF_s0 = periodic_trajectory.CalcPose(s);
+  const RigidTransformd X_AF_s1 = periodic_trajectory.CalcPose(s + length);
+  const RigidTransformd X_AF_s3 =
+      periodic_trajectory.CalcPose(s + 3.0 * length);
+  EXPECT_TRUE(X_AF_s0.IsNearlyEqualTo(X_AF_s1, kTolerance));
+  EXPECT_TRUE(X_AF_s0.IsNearlyEqualTo(X_AF_s3, kTolerance));
 }
 
 GTEST_TEST(TestPiecewiseConstantCurvatureTrajectory, TestScalarConversion) {

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",
@@ -130,6 +132,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",
@@ -183,6 +187,7 @@ drake_cc_library(
         "//common:nice_type_name",
         "//common:string_container",
         "//common:unused",
+        "//common/trajectories:piecewise_constant_curvature_trajectory",
         "//math:geometric_transform",
         "//multibody/topology",
         "//systems/framework:leaf_system",
@@ -444,6 +449,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 = [
@@ -609,6 +623,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>; \