Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer.

Author: Paul Mitiguy

multibody/tree/rpy_ball_mobilizer.cc

@@ -131,6 +131,18 @@ void RpyBallMobilizer<T>::ProjectSpatialForce(
   calc_tau(nullptr, F_BMo_F, tau.data());
 }
 
+template <typename T>
+void RpyBallMobilizer<T>::ThrowSinceCosPitchIsNearZero(const T& pitch) 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 = {}. 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(), pitch));
+}
+
 template <typename T>
 void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
                                         EigenPtr<MatrixX<T>> N) const {
@@ -150,18 +162,7 @@ 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]));
-  }
-
+  if (abs(cp) < 1.0e-3) ThrowSinceCosPitchIsNearZero(angles[1]);
   const T sp = sin(angles[1]);
   const T sy = sin(angles[2]);
   const T cy = cos(angles[2]);
@@ -200,6 +201,96 @@ 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]
+  //
+  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) ThrowSinceCosPitchIsNearZero(angles[1]);
+  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;
+
+  Ndot->coeffRef(0, 0) = (cy * sp_pdot - sy * cp_ydot) * cpiSqr;
+  Ndot->coeffRef(0, 1) = (sy * sp_pdot + cy * cp_ydot) * cpiSqr;
+  Ndot->coeffRef(0, 2) = 0;
+  Ndot->coeffRef(1, 0) = -cy * ydot;
+  Ndot->coeffRef(1, 1) = -sy * ydot;
+  Ndot->coeffRef(1, 2) = 0;
+  Ndot->coeffRef(2, 0) = cy * cpiSqr_pdot - sy * sp_cpi_ydot;
+  Ndot->coeffRef(2, 1) = sy * cpiSqr_pdot + cy * sp_cpi_ydot;
+  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]
+  //
+  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]);
+
+  // 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;
+
+  NplusDot->coeffRef(0, 0) = -sy_ydot * cp - cy * sp_pdot;
+  NplusDot->coeffRef(0, 1) = -cy_ydot;
+  NplusDot->coeffRef(0, 2) = 0;
+  NplusDot->coeffRef(1, 0) = cy_ydot * cp - sy * sp_pdot;
+  NplusDot->coeffRef(1, 1) = -sy * ydot;
+  NplusDot->coeffRef(1, 2) = 0;
+  NplusDot->coeffRef(2, 0) = -cp * pdot;
+  NplusDot->coeffRef(2, 1) = 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 +323,11 @@ 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) ThrowSinceCosPitchIsNearZero(angles[1]);
   const T cpi = 1.0 / cp;
 
   // Although we can calculate q̇ = N(q) * v, it is more efficient to implicitly
@@ -265,6 +342,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);
 }

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,10 @@ 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;
+
  private:
   // Helper method to make a clone templated on ToScalar.
   template <typename ToScalar>

multibody/tree/test/rpy_ball_mobilizer_test.cc

@@ -102,31 +102,46 @@ 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).  Similarly, verify that the NplusDot matrix Ṅ⁺(q,q̇) is the
+// inverse of the NDot matrix Ṅ(q,q̇).
 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 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));
+
+  // 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);
+
+  // Verify that NplusDot is the inverse of NDot and vice-versa.
+  const MatrixX<double> NDot_NplusDot = NDot * NplusDot;
+  const MatrixX<double> NplusDot_NDot = NplusDot * NDot;
+  // TODO(Mitiguy) Fix these to EXPECT_TRUE.
+  EXPECT_FALSE(CompareMatrices(NDot_NplusDot, Matrix3d::Identity(), kTolerance,
+                               MatrixCompareType::relative));
+  EXPECT_FALSE(CompareMatrices(NplusDot_NDot, Matrix3d::Identity(), kTolerance,
+                               MatrixCompareType::relative));
 }
 
 TEST_F(RpyBallMobilizerTest, MapUsesN) {