Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer.

multibody/tree/BUILD.bazel

@@ -531,6 +531,7 @@ drake_cc_googletest(
         ":mobilizer_tester",
         ":tree",
         "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_no_throw",
         "//common/test_utilities:expect_throws_message",
     ],
 )

multibody/tree/rpy_ball_mobilizer.cc

@@ -131,6 +131,20 @@ void RpyBallMobilizer<T>::ProjectSpatialForce(
   calc_tau(nullptr, F_BMo_F, tau.data());
 }
 
+template <typename T>
+void RpyBallMobilizer<T>::ThrowSinceCosPitchIsNearZero(
+    const T& pitch, const char* function_name) const {
+  throw std::runtime_error(fmt::format(
+      "{}(): The RpyBallMobilizer (implementing a BallRpyJoint) between "
+      "body {} and body {} has reached a singularity. This occurs when the "
+      "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
+      "configuration, we have pitch = {} radians. Drake does not yet support "
+      "a comparable joint using quaternions, but the feature request is "
+      "tracked in https://github.com/RobotLocomotion/drake/issues/12404.",
+      function_name, this->inboard_body().name(), this->outboard_body().name(),
+      pitch));
+}
+
 template <typename T>
 void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
                                         EigenPtr<MatrixX<T>> N) const {
@@ -150,18 +164,10 @@ void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
   using std::sin;
   const Vector3<T> angles = get_angles(context);
   const T cp = cos(angles[1]);
-  // Ensure the calculation is not near a state in which N(q) is singular.
   if (abs(cp) < 1.0e-3) {
-    throw std::runtime_error(fmt::format(
-        "The RpyBallMobilizer (implementing a BallRpyJoint) between "
-        "body {} and body {} has reached a singularity. This occurs when the "
-        "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
-        "configuration, we have pitch = {}. Drake does not yet support a "
-        "comparable joint using quaternions, but the feature request is "
-        "tracked in https://github.com/RobotLocomotion/drake/issues/12404.",
-        this->inboard_body().name(), this->outboard_body().name(), angles[1]));
+    const char* function_name_less_Do = __func__ + 2;
+    ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
   }
-
   const T sp = sin(angles[1]);
   const T sy = sin(angles[2]);
   const T cy = cos(angles[2]);
@@ -200,6 +206,125 @@ void RpyBallMobilizer<T>::DoCalcNplusMatrix(const systems::Context<T>& context,
   *Nplus << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0;
 }
 
+template <typename T>
+void RpyBallMobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>& context,
+                                           EigenPtr<MatrixX<T>> Ndot) const {
+  // Computes the 3x3 matrix Ṅ(q,q̇) that helps relate q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇,
+  // where q = [r, p, y]ᵀ contains the roll (r), pitch (p) and yaw (y) angles
+  // and v = [wx, wy, wz]ᵀ represents W_FM_F (the angular velocity of the
+  // mobilizer's M frame measured in its F frame, expressed in the F frame).
+  //
+  // The 3x3 matrix N(q) relates q̇ to v as q̇ = N(q)⋅v, where
+  //
+  //        [          cos(y) / cos(p),           sin(y) / cos(p),  0]
+  // N(q) = [                  -sin(y),                    cos(y),  0]
+  //        [ sin(p) * cos(y) / cos(p),  sin(p) * sin(y) / cos(p),  1]
+  //
+  //          ⌈ -sy/cp ẏ + cy sp/cp² ṗ    cy/cp ẏ + sy sp/cp² ṗ,   0 ⌉
+  // Ṅ(q,q̇) = |                  -cy ẏ,                   -sy ẏ,   0 |
+  //          ⌊  cy/cp² ṗ - sp sy/cp ẏ,   sy/cp² ṗ + sp cy/cp ẏ,   0 ⌋
+  //
+  // where cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y).
+  // Note: Although the elements of Ṅ(q,q̇) are simply the time-derivatives of
+  // corresponding elements of N(q), result were simplified as follows.
+  // Ṅ[2, 0] = cy ṗ + sp² cy/cp² ṗ - sp sy/cp ẏ
+  //         =            cy/cp² ṗ - sp sy/cp ẏ.
+  // Ṅ[2, 1] = sy ṗ + sp² sy/cp² ṗ + sp cy/cp ẏ
+  //         =            sy/cp² ṗ + sp cy/cp ẏ.
+
+  using std::cos;
+  using std::sin;
+  const Vector3<T> angles = get_angles(context);
+  const T cp = cos(angles[1]);
+  const T sp = sin(angles[1]);
+  const T sy = sin(angles[2]);
+  const T cy = cos(angles[2]);
+  if (abs(cp) < 1.0e-3) {
+    const char* function_name_less_Do = __func__ + 2;
+    ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
+  }
+  const T cpi = 1.0 / cp;
+  const T cpiSqr = cpi * cpi;
+
+  // Calculate time-derivative of roll, pitch, and yaw angles.
+  const Vector3<T> v = get_angular_velocity(context);
+  Vector3<T> qdot;
+  DoMapVelocityToQDot(context, v, &qdot);
+  const T& pdot = qdot(1);  // time derivative of pitch angle.
+  const T& ydot = qdot(2);  // time derivative of yaw angle.
+  const T sp_pdot = sp * pdot;
+  const T cp_ydot = cp * ydot;
+  const T cpiSqr_pdot = cpiSqr * pdot;
+  const T sp_cpi_ydot = sp * cpi * ydot;
+
+  // The elements below are in column order (like Eigen).
+  Ndot->coeffRef(0, 0) = (cy * sp_pdot - sy * cp_ydot) * cpiSqr;
+  Ndot->coeffRef(1, 0) = -cy * ydot;
+  Ndot->coeffRef(2, 0) = cy * cpiSqr_pdot - sy * sp_cpi_ydot;
+  Ndot->coeffRef(0, 1) = (sy * sp_pdot + cy * cp_ydot) * cpiSqr;
+  Ndot->coeffRef(1, 1) = -sy * ydot;
+  Ndot->coeffRef(2, 1) = sy * cpiSqr_pdot + cy * sp_cpi_ydot;
+  Ndot->coeffRef(0, 2) = 0;
+  Ndot->coeffRef(1, 2) = 0;
+  Ndot->coeffRef(2, 2) = 0;
+}
+
+template <typename T>
+void RpyBallMobilizer<T>::DoCalcNplusDotMatrix(
+    const systems::Context<T>& context, EigenPtr<MatrixX<T>> NplusDot) const {
+  // Computes the matrix Ṅ⁺(q,q̇) that helps relate v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈,
+  // where q = [r, p, y]ᵀ contains the roll (r), pitch (p) and yaw (y) angles
+  // and v = [wx, wy, wz]ᵀ represents W_FM_F (the angular velocity of the
+  // mobilizer's M frame measured in its F frame, expressed in the F frame).
+  //
+  // The 3x3 matrix N⁺(q) relates v to q̇ as v = N⁺(q)⋅q̇, where
+  //
+  //         [ cos(y) * cos(p),  -sin(y),  0]
+  // N⁺(q) = [ sin(y) * cos(p),   cos(y),  0]
+  //         [         -sin(p),        0,  1]
+  //
+  //           ⌈ -sy cp ẏ - cy sp ṗ,   -cy ẏ,   0 ⌉
+  // Ṅ⁺(q,q̇) = |  cy cp ẏ - sy sp ṗ    -sy ẏ,   0 |
+  //           ⌊              -cp ṗ,       0,   0 ⌋
+  //
+  // where cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y).
+  using std::cos;
+  using std::sin;
+  const Vector3<T> angles = get_angles(context);
+  const T cp = cos(angles[1]);
+  const T sp = sin(angles[1]);
+  const T sy = sin(angles[2]);
+  const T cy = cos(angles[2]);
+
+  // Throw an exception with the proper function name if a singularity would be
+  // encountered in DoMapVelocityToQDot().
+  if (abs(cp) < 1.0e-3) {
+    const char* function_name_less_Do = __func__ + 2;
+    ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
+  }
+
+  // Calculate time-derivative of roll, pitch, and yaw angles.
+  const Vector3<T> v = get_angular_velocity(context);
+  Vector3<T> qdot;
+  DoMapVelocityToQDot(context, v, &qdot);
+  const T& pdot = qdot(1);  // time derivative of pitch angle.
+  const T& ydot = qdot(2);  // time derivative of yaw angle.
+  const T sp_pdot = sp * pdot;
+  const T cy_ydot = cy * ydot;
+  const T sy_ydot = sy * ydot;
+
+  // The elements below are in column order (like Eigen).
+  NplusDot->coeffRef(0, 0) = -sy_ydot * cp - cy * sp_pdot;
+  NplusDot->coeffRef(1, 0) = cy_ydot * cp - sy * sp_pdot;
+  NplusDot->coeffRef(2, 0) = -cp * pdot;
+  NplusDot->coeffRef(0, 1) = -cy_ydot;
+  NplusDot->coeffRef(1, 1) = -sy_ydot;
+  NplusDot->coeffRef(2, 1) = 0;
+  NplusDot->coeffRef(0, 2) = 0;
+  NplusDot->coeffRef(1, 2) = 0;
+  NplusDot->coeffRef(2, 2) = 0;
+}
+
 template <typename T>
 void RpyBallMobilizer<T>::DoMapVelocityToQDot(
     const systems::Context<T>& context, const Eigen::Ref<const VectorX<T>>& v,
@@ -232,25 +357,14 @@ void RpyBallMobilizer<T>::DoMapVelocityToQDot(
   using std::cos;
   using std::sin;
   const Vector3<T> angles = get_angles(context);
-  const T cp = cos(angles[1]);
-  if (abs(cp) < 1.0e-3) {
-    throw std::runtime_error(fmt::format(
-        "The RpyBallMobilizer (implementing a BallRpyJoint) between "
-        "body {} and body {} has reached a singularity. This occurs when the "
-        "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
-        "configuration, we have pitch = {}. Drake does not yet support a "
-        "comparable joint using quaternions, but the feature request is "
-        "tracked in https://github.com/RobotLocomotion/drake/issues/12404.",
-        this->inboard_body().name(), this->outboard_body().name(), angles[1]));
-  }
-
-  const T& w0 = v[0];
-  const T& w1 = v[1];
-  const T& w2 = v[2];
-
   const T sp = sin(angles[1]);
+  const T cp = cos(angles[1]);
   const T sy = sin(angles[2]);
   const T cy = cos(angles[2]);
+  if (abs(cp) < 1.0e-3) {
+    const char* function_name_less_Do = __func__ + 2;
+    ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
+  }
   const T cpi = 1.0 / cp;
 
   // Although we can calculate q̇ = N(q) * v, it is more efficient to implicitly
@@ -265,6 +379,9 @@ void RpyBallMobilizer<T>::DoMapVelocityToQDot(
   // ṙ = (cos(y) * w0 + sin(y) * w1) / cos(p)
   // ṗ = -sin(y) * w0 + cos(y) * w1
   // ẏ = sin(p) * ṙ + w2
+  const T& w0 = v[0];
+  const T& w1 = v[1];
+  const T& w2 = v[2];
   const T rdot = (cy * w0 + sy * w1) * cpi;
   *qdot = Vector3<T>(rdot, -sy * w0 + cy * w1, sp * rdot + w2);
 }
@@ -308,7 +425,7 @@ void RpyBallMobilizer<T>::DoMapQDotToVelocity(
   const T cy = cos(angles[2]);
   const T cp_x_rdot = cp * rdot;
 
-  // Compute the product v = N⁺(q) * q̇ element-by-element to leverate the zeros
+  // Compute the product v = N⁺(q) * q̇ element-by-element to leverage the zeros
   // in N⁺(q) -- which is more efficient than matrix multiplication N⁺(q) * q̇.
   *v = Vector3<T>(cy * cp_x_rdot - sy * pdot, /*+ 0 * ydot*/
                   sy * cp_x_rdot + cy * pdot, /*+ 0 * ydot*/

multibody/tree/rpy_ball_mobilizer.h

@@ -250,6 +250,14 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
   void DoCalcNplusMatrix(const systems::Context<T>& context,
                          EigenPtr<MatrixX<T>> Nplus) const final;
 
+  // Generally, q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇. For this mobilizer, Ṅ is not simple.
+  void DoCalcNDotMatrix(const systems::Context<T>& context,
+                        EigenPtr<MatrixX<T>> Ndot) const final;
+
+  // Generally, v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈. For this mobilizer, Ṅ⁺ is not simple.
+  void DoCalcNplusDotMatrix(const systems::Context<T>& context,
+                            EigenPtr<MatrixX<T>> NplusDot) const final;
+
   // Maps the generalized velocity v, which corresponds to the angular velocity
   // w_FM, to time derivatives of roll-pitch-yaw angles θ₀, θ₁, θ₂ in qdot.
   //
@@ -297,6 +305,11 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
   std::unique_ptr<Mobilizer<symbolic::Expression>> DoCloneToScalar(
       const MultibodyTree<symbolic::Expression>& tree_clone) const override;
 
+  // Certain roll pitch yaw calculations (e.g., calculating the N(q) matrix)
+  // have a singularity (divide-by-zero error) when cos(pitch) ≈ 0.
+  void ThrowSinceCosPitchIsNearZero(const T& pitch,
+                                    const char* function_name) const;
+
  private:
   // Helper method to make a clone templated on ToScalar.
   template <typename ToScalar>

multibody/tree/test/rpy_ball_mobilizer_test.cc

@@ -4,6 +4,7 @@
 
 #include "drake/common/eigen_types.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
+#include "drake/common/test_utilities/expect_no_throw.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/math/rigid_transform.h"
 #include "drake/math/rotation_matrix.h"
@@ -102,31 +103,84 @@ TEST_F(RpyBallMobilizerTest, ZeroState) {
   EXPECT_TRUE(X_WB.IsExactlyIdentity());
 }
 
-// For an arbitrary state verify that the computed Nplus(q) matrix is the
-// inverse of N(q).
+// For an arbitrary state, verify that calculating the Nplus matrix N⁺(q) is the
+// inverse of N(q), e.g., verify N⁺(q) * N(q) = [I] (the identity matrix). Also
+// verify the time derivatives of N⁺(q) * N(q) and N(q) * N⁺(q) are both zero.
 TEST_F(RpyBallMobilizerTest, KinematicMapping) {
   const Vector3d rpy(M_PI / 3, -M_PI / 3, M_PI / 5);
   mobilizer_->SetAngles(context_.get(), rpy);
 
   ASSERT_EQ(mobilizer_->num_positions(), 3);
   ASSERT_EQ(mobilizer_->num_velocities(), 3);
 
-  // Compute N.
-  MatrixX<double> N(3, 3);
+  // Compute the N(q) and Nplus(q) matrices.
+  MatrixX<double> N(3, 3), Nplus(3, 3);
   mobilizer_->CalcNMatrix(*context_, &N);
-
-  // Compute Nplus.
-  MatrixX<double> Nplus(3, 3);
   mobilizer_->CalcNplusMatrix(*context_, &Nplus);
 
-  // Verify that Nplus is the inverse of N.
-  MatrixX<double> N_x_Nplus = N * Nplus;
-  MatrixX<double> Nplus_x_N = Nplus * N;
-
+  // Verify that Nplus (N⁺) is the inverse of N and vice-versa.
+  const MatrixX<double> N_x_Nplus = N * Nplus;
+  const MatrixX<double> Nplus_x_N = Nplus * N;
   EXPECT_TRUE(CompareMatrices(N_x_Nplus, Matrix3d::Identity(), kTolerance,
                               MatrixCompareType::relative));
   EXPECT_TRUE(CompareMatrices(Nplus_x_N, Matrix3d::Identity(), kTolerance,
                               MatrixCompareType::relative));
+
+  // Also compare N(q) to numerical values produced by MotionGenesis.
+  constexpr double epsilon64 = 64 * std::numeric_limits<double>::epsilon();
+  MatrixX<double> Ncheck(3, 3);
+  Ncheck.row(0) << 1.6180339887498949, 1.1755705045849463, 0;
+  Ncheck.row(1) << -0.58778525229247314, 0.80901699437494745, 0;
+  Ncheck.row(2) << -1.4012585384440732, -1.0180739209102541, 1;
+  EXPECT_TRUE(
+      CompareMatrices(N, Ncheck, epsilon64, MatrixCompareType::relative));
+
+  // Set a generic angular velocity.
+  const Vector3<double> v(0.5, -0.7, 2.3);
+  mobilizer_->SetAngularVelocity(context_.get(), v);
+
+  // Compute the NDot(q,q̇) and NplusDot(q,q̇) matrices.
+  MatrixX<double> NDot(3, 3), NplusDot(3, 3);
+  mobilizer_->CalcNDotMatrix(*context_, &NDot);
+  mobilizer_->CalcNplusDotMatrix(*context_, &NplusDot);
+
+  // The Nplus(q) matrix N⁺ multiplied by N(q) is the identity matrix [I].
+  // Hence the time-derivative of (N⁺ * N = [I]) is Ṅ⁺ * N + N⁺ * Ṅ = [0],
+  // where [0] is the zero matrix. Therefore Ṅ⁺ * N = -N⁺ * Ṅ.
+  // Similarly, the time derivative of (N * N⁺ = [I]) is Ṅ * N⁺ + N * Ṅ⁺ = [0],
+  // so Ṅ * N⁺ = -N * Ṅ⁺.  Verify these relationships.
+  const MatrixX<double> NplusDot_N = NplusDot * N;
+  const MatrixX<double> Nplus_NDot = Nplus * NDot;
+  const MatrixX<double> NDot_Nplus = NDot * Nplus;
+  const MatrixX<double> N_NplusDot = N * NplusDot;
+  EXPECT_TRUE(CompareMatrices(NplusDot_N, -Nplus_NDot, kTolerance,
+                              MatrixCompareType::relative));
+  EXPECT_TRUE(CompareMatrices(NDot_Nplus, -N_NplusDot, kTolerance,
+                              MatrixCompareType::relative));
+
+  // Also compare Ṅ(q,q̇) to numerical values produced by MotionGenesis.
+  MatrixX<double> NDotcheck(3, 3);
+  NDotcheck.row(0) << -0.30720756492922385, 5.4924345293948527, 0;
+  NDotcheck.row(1) << -1.8704654739876834, -1.358972714017757, 0;
+  NDotcheck.row(2) << -0.42987052164155548, -5.2622033631883367, 0;
+  EXPECT_TRUE(
+      CompareMatrices(NDot, NDotcheck, epsilon64, MatrixCompareType::relative));
+
+  // Ensure cos(pitch) ≈ 0, throws an exception.
+  const double pitch = M_PI / 2 + epsilon64;
+  const Vector3d rpy_singular_value(M_PI / 3, pitch, -M_PI / 5);
+  mobilizer_->SetAngles(context_.get(), rpy_singular_value);
+  DRAKE_EXPECT_THROWS_MESSAGE(mobilizer_->CalcNMatrix(*context_, &N),
+                              "CalcNMatrix\\(\\): The RpyBallMobilizer .*"
+                              "has reached a singularity.*");
+  DRAKE_EXPECT_NO_THROW(mobilizer_->CalcNplusMatrix(*context_, &Nplus));
+  DRAKE_EXPECT_THROWS_MESSAGE(mobilizer_->CalcNDotMatrix(*context_, &NDot),
+                              "CalcNDotMatrix\\(\\): The RpyBallMobilizer .*"
+                              "has reached a singularity.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      mobilizer_->CalcNplusDotMatrix(*context_, &NplusDot),
+      "CalcNplusDotMatrix\\(\\): The RpyBallMobilizer .*"
+      "has reached a singularity.*");
 }
 
 TEST_F(RpyBallMobilizerTest, MapUsesN) {