MapQDDotToAcceleration() and MapAccelerationToQDDot() for rpy mobilizer. (#23092)

Author: mitiguy

multibody/tree/rpy_ball_mobilizer.cc

@@ -132,17 +132,20 @@ void RpyBallMobilizer<T>::ProjectSpatialForce(
 }
 
 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));
+void RpyBallMobilizer<T>::ThrowIfCosPitchNearZero(
+    const T& cos_pitch, const T& pitch_angle, const char* function_name) const {
+  using std::abs;
+  if (abs(cos_pitch) < 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 = {} 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_angle));
+  }
 }
 
 template <typename T>
@@ -157,17 +160,14 @@ void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
   // ⌊ ẏ ⌋   ⌊ sin(p) * cos(y) / cos(p),  sin(p) * sin(y) / cos(p),  1 ⌋ ⌊ ω2 ⌋
   //
   // Note: N(q) is singular for p = π/2 + kπ, for k = ±1, ±2, ...
-  // See related code and comments in MapVelocityToQdot().
+  // See related code and comments in DoMapVelocityToQdot().
 
-  using std::abs;
   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) {
-    const char* function_name_less_Do = __func__ + 2;
-    ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
-  }
+  ThrowIfCosPitchNearZero(cp, angles[1], "CalcNMatrix");
+
   const T sp = sin(angles[1]);
   const T sy = sin(angles[2]);
   const T cy = cos(angles[2]);
@@ -196,7 +196,7 @@ void RpyBallMobilizer<T>::DoCalcNplusMatrix(const systems::Context<T>& context,
   // | ω1 | = | sin(y) * cos(p),   cos(y),  0 | | ṗ |
   // ⌊ ω2 ⌋   ⌊         -sin(p),        0,  1 ⌋ ⌊ ẏ ⌋
   //
-  // See related code and comments in MapQDotToVelocity().
+  // See related code and comments in DoMapQDotToVelocity().
   const Vector3<T> angles = get_angles(context);
   const T sp = sin(angles[1]);
   const T cp = cos(angles[1]);
@@ -239,10 +239,7 @@ void RpyBallMobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>& context,
   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);
-  }
+  ThrowIfCosPitchNearZero(cp, angles[1], "CalcNDotMatrix");
   const T cpi = 1.0 / cp;
   const T cpiSqr = cpi * cpi;
 
@@ -298,10 +295,7 @@ void RpyBallMobilizer<T>::DoCalcNplusDotMatrix(
 
   // 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);
-  }
+  ThrowIfCosPitchNearZero(cp, angles[1], "CalcNplusDotMatrix");
 
   // Calculate time-derivative of roll, pitch, and yaw angles.
   const Vector3<T> v = get_angular_velocity(context);
@@ -353,18 +347,14 @@ void RpyBallMobilizer<T>::DoMapVelocityToQDot(
   // [Mitiguy August 2019] Mitiguy, P., 2019. Advanced Dynamics & Motion
   //                       Simulation.
 
-  using std::abs;
   using std::cos;
   using std::sin;
   const Vector3<T> angles = get_angles(context);
   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);
-  }
+  ThrowIfCosPitchNearZero(cp, angles[1], "MapVelocityToQDot");
   const T cpi = 1.0 / cp;
 
   // Although we can calculate q̇ = N(q) * v, it is more efficient to implicitly
@@ -446,6 +436,95 @@ RpyBallMobilizer<T>::TemplatedDoCloneToScalar(
       outboard_frame_clone);
 }
 
+template <typename T>
+Vector3<T> RpyBallMobilizer<T>::CalcAccelerationBiasForQDDot(
+    const systems::Context<T>& context, const char* function_name) const {
+  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]);
+  ThrowIfCosPitchNearZero(cp, angles[1], function_name);
+
+  // The algorithm below calculates Ṅ⁺(q,q̇)⋅q̇. The algorithm was verified with
+  // MotionGenesis. It can also be verified by-hand, e.g., with documentation
+  // in DoCalcNplusDotMatrix which directly differentiates N⁺(q) to form
+  // Ṅ⁺(q,q̇). Thereafter, multiply by q̇ to form Ṅ⁺(q,q̇)⋅q̇ (and simplify).
+  const Vector3<T> v = get_angular_velocity(context);
+  Vector3<T> qdot;
+  DoMapVelocityToQDot(context, v, &qdot);
+  const T& rdot = qdot[0];
+  const T& pdot = qdot[1];
+  const T& ydot = qdot[2];
+  const T pdot_ydot = pdot * ydot;
+  const T rdot_pdot = rdot * pdot;
+  const T rdot_ydot = rdot * ydot;
+  const T sp_rdot_pdot = sp * rdot_pdot;
+  const T cp_rdot_ydot = cp * rdot_ydot;
+  return Vector3<T>(-cy * pdot_ydot - cy * sp_rdot_pdot - sy * cp_rdot_ydot,
+                    -sy * pdot_ydot - sy * sp_rdot_pdot + cy * cp_rdot_ydot,
+                    -cp * rdot_pdot);
+}
+
+template <typename T>
+void RpyBallMobilizer<T>::DoMapAccelerationToQDDot(
+    const systems::Context<T>& context,
+    const Eigen::Ref<const VectorX<T>>& vdot,
+    EigenPtr<VectorX<T>> qddot) const {
+  // As shown in DoMapVelocityToQDot(), the time-derivatives of generalized
+  // positions q̇ = [ṙ, ṗ, ẏ]ᵀ are related to the generalized velocities
+  // v = [ωx, ωy, ωz]ᵀ in the matrix form q̇ = N(q)⋅v, where N is a 3x3 matrix.
+  //
+  // For this mobilizer v = w_FM_F =  [ωx, ωy, ωz]ᵀ is the mobilizer M frame's
+  // angular velocity in the mobilizer F frame, expressed in frame F and
+  // q = [r, p, y]ᵀ  denote roll, pitch, yaw angles (generalized positions).
+  //
+  // There are various ways to get q̈ = [r̈, p̈, ÿ]ᵀ in terms of v̇ = [ω̇x, ω̇y, ω̇z]ᵀ.
+  // One way to is differentiate q̇ = N(q)⋅v to form q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.
+  // However, N⁺(q) is simpler than N(q) so Ṅ⁺(q,q̇) is much simpler than Ṅ(q,q̇).
+  // A calculation of q̈ that leverages the simplicity of Ṅ⁺(q,q̇) over Ṅ(q,q̇)
+  // and the available function CalcAccelerationBiasForQDDot() starts with
+  // v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈ and then solves as q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.
+  const Vector3<T> vdot_minus_NplusDotTimesQDot =
+      vdot - CalcAccelerationBiasForQDDot(context, __func__);
+
+  // Note: Although the function below was designed to calculate q̇ = N(q)⋅v,
+  // it can also be used to calculate q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.
+  DoMapVelocityToQDot(context, vdot_minus_NplusDotTimesQDot, qddot);
+}
+
+template <typename T>
+void RpyBallMobilizer<T>::DoMapQDDotToAcceleration(
+    const systems::Context<T>& context,
+    const Eigen::Ref<const VectorX<T>>& qddot,
+    EigenPtr<VectorX<T>> vdot) const {
+  // As seen in DoMapQDotToVelocity(), generalized velocities v = [ωx, ωy, ωz]ᵀ
+  // are related to q̇ = [ṙ, ṗ, ẏ]ᵀ (time-derivatives of generalized positions)
+  // in the matrix form v = N⁺(q)⋅q̇, where N⁺ is a 3x3 matrix.
+  //
+  // For this mobilizer v = w_FM_F =  [ωx, ωy, ωz]ᵀ is the mobilizer M frame's
+  // angular velocity in the mobilizer F frame, expressed in frame F and
+  // q = [r, p, y]ᵀ  denote roll, pitch, yaw angles (generalized positions).
+  //
+  // There are various ways to calculate v̇ = [ω̇x, ω̇y, ω̇z]ᵀ (the time-derivatives
+  // of the generalized velocities). The calculation below is straightforward in
+  // that it simply differentiates v = N⁺(q)⋅q̇ to form v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+
+  // Form the Ṅ⁺(q,q̇)⋅q̇ term of the result now (start of this function) so any
+  // singularity (if one exists) throws an exception referencing this function.
+  const Vector3<T> NplusDot_times_Qdot =
+      CalcAccelerationBiasForQDDot(context, __func__);
+
+  // Although the function below was designed to calculate v = N⁺(q)⋅q̇, it can
+  // also be used to calculate N⁺(q)⋅q̈.
+  DoMapQDotToVelocity(context, qddot, vdot);  // On return, vdot = N⁺(q)⋅q̈.
+
+  // Sum the previous terms to form v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  *vdot += NplusDot_times_Qdot;
+}
+
 template <typename T>
 std::unique_ptr<Mobilizer<double>> RpyBallMobilizer<T>::DoCloneToScalar(
     const MultibodyTree<double>& tree_clone) const {

multibody/tree/rpy_ball_mobilizer.h

@@ -246,7 +246,7 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
 
   bool is_velocity_equal_to_qdot() const override { return false; }
 
- protected:
+ private:
   void DoCalcNMatrix(const systems::Context<T>& context,
                      EigenPtr<MatrixX<T>> N) const final;
 
@@ -299,6 +299,22 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
                            const Eigen::Ref<const VectorX<T>>& qdot,
                            EigenPtr<VectorX<T>> v) const final;
 
+  // Maps vdot to qddot, which for this mobilizer is q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.
+  // For simple mobilizers q̈ = v̇. This mobilizer's N and Ṅ are more elaborate.
+  void DoMapAccelerationToQDDot(const systems::Context<T>& context,
+                                const Eigen::Ref<const VectorX<T>>& vdot,
+                                EigenPtr<VectorX<T>> qddot) const final;
+
+  // Maps qddot to vdot, which for this mobilizer is v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  // For simple mobilizers v̇ = q̈. This mobilizer's N and Ṅ⁺ are more elaborate.
+  void DoMapQDDotToAcceleration(const systems::Context<T>& context,
+                                const Eigen::Ref<const VectorX<T>>& qddot,
+                                EigenPtr<VectorX<T>> vdot) const final;
+
+  // Calculate the term Ṅ⁺(q,q̇)⋅q̇ which appears in v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
+  Vector3<T> CalcAccelerationBiasForQDDot(const systems::Context<T>& context,
+                                          const char* function_name) const;
+
   std::unique_ptr<Mobilizer<double>> DoCloneToScalar(
       const MultibodyTree<double>& tree_clone) const override;
 
@@ -310,10 +326,12 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
 
   // 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;
+  // The tolerance 1.0e-3 is used to test whether the cosine of the pitch angle
+  // is near zero, which occurs when the pitch angle ≈ π/2 ± n π (n=0, 1 2, …).
+  // Throw an exception if a pitch angle is within ≈ 0.057° of a singularity.
+  void ThrowIfCosPitchNearZero(const T& cos_pitch, const T& pitch_angle,
+                               const char* function_name) const;
 
- private:
   // Helper method to make a clone templated on ToScalar.
   template <typename ToScalar>
   std::unique_ptr<Mobilizer<ToScalar>> TemplatedDoCloneToScalar(

multibody/tree/test/rpy_ball_mobilizer_test.cc

@@ -223,6 +223,49 @@ TEST_F(RpyBallMobilizerTest, MapUsesNplus) {
                               MatrixCompareType::relative));
 }
 
+TEST_F(RpyBallMobilizerTest, MapAccelerationToQDDotAndViceVersa) {
+  // Set an arbitrary non-zero state.
+  const Vector3<double> rpy(M_PI / 3, -M_PI / 4, M_PI / 5);
+  const Vector3<double> wxyz(5.4, -9.8, 3.2);
+  mobilizer_->SetAngles(context_.get(), rpy);
+  mobilizer_->SetAngularVelocity(context_.get(), wxyz);
+
+  // Set an arbitrary v̇ and use MapAccelerationToQDDot() to calculate q̈.
+  const Vector3<double> vdot(0.3, -0.2, 0.9);  // v̇ = [ẇx, ẇy, ẇz].
+  Vector3<double> qddot;
+  mobilizer_->MapAccelerationToQDDot(*context_, vdot, &qddot);
+
+  // Compute the 3x3 N(q) matrix and its time-derivative Ṅ(q,q̇).
+  MatrixX<double> N(3, 3), Ndot(3, 3);
+  mobilizer_->CalcNMatrix(*context_, &N);
+  mobilizer_->CalcNDotMatrix(*context_, &Ndot);
+
+  // Verify equivalence of q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇ and MapAccelerationToQDDot().
+  const Vector3<double> qddot_expected = Ndot * wxyz + N * vdot;
+  EXPECT_TRUE(CompareMatrices(qddot, qddot_expected, kTolerance,
+                              MatrixCompareType::relative));
+
+  // Compute the 3x3 N⁺(q) matrix and its time-derivative Ṅ⁺(q,q̇).
+  MatrixX<double> Nplus(3, 3), Nplusdot(3, 3);
+  mobilizer_->CalcNplusMatrix(*context_, &Nplus);
+  mobilizer_->CalcNplusDotMatrix(*context_, &Nplusdot);
+
+  // Starting with the previous q̈, use MapQDDotToAcceleration() to calculate v̇.
+  Vector3<double> wdot;
+  mobilizer_->MapQDDotToAcceleration(*context_, qddot, &wdot);
+
+  // Verify equivalence of v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈ and MapQDDotToAcceleration().
+  Vector3<double> qdot;
+  mobilizer_->MapVelocityToQDot(*context_, wxyz, &qdot);
+  const Vector3<double> vdot_expected = Nplusdot * qdot + Nplus * qddot;
+  EXPECT_TRUE(CompareMatrices(vdot_expected, wdot, 16 * kTolerance,
+                              MatrixCompareType::relative));
+
+  // Verify MapQDDotToAcceleration() is the inverse of MapAccelerationToQDDot().
+  EXPECT_TRUE(CompareMatrices(vdot, wdot, 16 * kTolerance,
+                              MatrixCompareType::relative));
+}
+
 TEST_F(RpyBallMobilizerTest, SingularityError) {
   // Set state in singularity
   const Vector3d rpy_value(M_PI / 3, M_PI / 2, M_PI / 5);