Correct and improve rpy_ball_mobilizer documentation, replace E matrix with standard N matrix. (#23006)

Correct and improve rpy_ball_mobilizer documentation, replace E matrix with standard N matrix. (#23006)

Co-authored-by: Paul Mitiguy <[email protected]>

Author: mitiguy

multibody/tree/rpy_ball_mobilizer.cc

@@ -134,28 +134,23 @@ void RpyBallMobilizer<T>::ProjectSpatialForce(
 template <typename T>
 void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
                                         EigenPtr<MatrixX<T>> N) const {
-  using std::abs;
-  using std::cos;
-  using std::sin;
-
-  // The linear map E_F(q) allows computing v from q̇ as:
-  // w_FM = E_F(q) * q̇; q̇ = [ṙ, ṗ, ẏ]ᵀ
+  // The matrix N(q) relates q̇ to v as q̇ = N(q) * v, where q̇ = [ṙ, ṗ, ẏ]ᵀ and
+  // v = w_FM_F = [ω0, ω1, ω2]ᵀ is the mobilizer M frame's angular velocity in
+  // the mobilizer F frame, expressed in the F frame.
   //
-  // Here, following a convention used by many dynamicists, we are calling the
-  // angles q0, q1, q2 as roll (r), pitch (p) and yaw (y), respectively.
+  // ⌈ ṙ ⌉   ⌈          cos(y) / cos(p),           sin(y) / cos(p),  0 ⌉ ⌈ ω0 ⌉
+  // | ṗ | = |                  -sin(y),                    cos(y),  0 | | ω1 |
+  // ⌊ ẏ ⌋   ⌊ sin(p) * cos(y) / cos(p),  sin(p) * sin(y) / cos(p),  1 ⌋ ⌊ ω2 ⌋
   //
-  // The linear map from v to q̇ is given by the inverse of E_F(q):
-  //          [          cos(y) / cos(p),          sin(y) / cos(p), 0]
-  // Einv_F = [                  -sin(y),                   cos(y), 0]
-  //          [ sin(p) * cos(y) / cos(p), sin(p) * sin(y) / cos(p), 1]
-  //
-  // such that q̇ = Einv_F(q) * w_FM; q̇ = [ṙ, ṗ, ẏ]ᵀ
-  // See developer notes in MapVelocityToQdot() for further details.
+  // Note: N(q) is singular for p = π/2 + kπ, for k = ±1, ±2, ...
+  // See related code and comments in MapVelocityToQdot().
 
+  using std::abs;
+  using std::cos;
+  using std::sin;
   const Vector3<T> angles = get_angles(context);
   const T cp = cos(angles[1]);
-  // Demand for the computation to be away from a state for which Einv_F is
-  // singular.
+  // 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 "
@@ -171,7 +166,6 @@ void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
   const T sy = sin(angles[2]);
   const T cy = cos(angles[2]);
   const T cpi = 1.0 / cp;
-
   const T cy_x_cpi = cy * cpi;
   const T sy_x_cpi = sy * cpi;
 
@@ -188,21 +182,16 @@ void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
 template <typename T>
 void RpyBallMobilizer<T>::DoCalcNplusMatrix(const systems::Context<T>& context,
                                             EigenPtr<MatrixX<T>> Nplus) const {
-  // The linear map between q̇ and v is given by matrix E_F(q) defined by:
-  //          [ cos(y) * cos(p), -sin(y), 0]
-  // E_F(q) = [ sin(y) * cos(p),  cos(y), 0]
-  //          [         -sin(p),       0, 1]
-  //
-  // w_FM = E_F(q) * q̇; q̇ = [ṙ, ṗ, ẏ]ᵀ
+  // The matrix N⁺(q) relates v to q̇ as v = N⁺(q) * q̇, where q̇ = [ṙ, ṗ, ẏ]ᵀ and
+  // v = w_FM_F = [ω0, ω1, ω2]ᵀ is the mobilizer M frame's angular velocity in
+  // the mobilizer F frame, expressed in the F frame (thus w_FM_F = N⁺(q) * q̇).
   //
-  // Here, following a convention used by many dynamicists, we are calling the
-  // angles q0, q1, q2 as roll (r), pitch (p) and yaw (y), respectively.
+  // ⌈ ω0 ⌉   ⌈ cos(y) * cos(p),  -sin(y),  0 ⌉ ⌈ ṙ ⌉
+  // | ω1 | = | sin(y) * cos(p),   cos(y),  0 | | ṗ |
+  // ⌊ ω2 ⌋   ⌊         -sin(p),        0,  1 ⌋ ⌊ ẏ ⌋
   //
-  // See detailed developer comments for E_F(q) in the implementation for
-  // MapQDotToVelocity().
-
+  // See related code and comments in MapQDotToVelocity().
   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]);
@@ -215,57 +204,33 @@ template <typename T>
 void RpyBallMobilizer<T>::DoMapVelocityToQDot(
     const systems::Context<T>& context, const Eigen::Ref<const VectorX<T>>& v,
     EigenPtr<VectorX<T>> qdot) const {
-  using std::abs;
-  using std::cos;
-  using std::sin;
-
-  // The linear map E_F(q) allows computing v from q̇ as:
-  // w_FM = E_F(q) * q̇; q̇ = [ṙ, ṗ, ẏ]ᵀ
+  // The matrix N(q) relates q̇ to v as q̇ = N(q) * v, where q̇ = [ṙ, ṗ, ẏ]ᵀ and
+  // v = w_FM_F = [ω0, ω1, ω2]ᵀ is the mobilizer M frame's angular velocity in
+  // the mobilizer F frame, expressed in the F frame.
   //
-  // Here, following a convention used by many dynamicists, we are calling the
-  // angles θ₁, θ₂, θ₃ as roll (r), pitch (p) and yaw (y), respectively.
+  // ⌈ ṙ ⌉   ⌈          cos(y) / cos(p),           sin(y) / cos(p),  0 ⌉ ⌈ ω0 ⌉
+  // | ṗ | = |                  -sin(y),                    cos(y),  0 | | ω1 |
+  // ⌊ ẏ ⌋   ⌊ sin(p) * cos(y) / cos(p),  sin(p) * sin(y) / cos(p),  1 ⌋ ⌊ ω2 ⌋
   //
-  // The linear map from v to q̇ is given by the inverse of E_F(q):
-  //          [          cos(y) / cos(p),          sin(y) / cos(p), 0]
-  // Einv_F = [                  -sin(y),                   cos(y), 0]
-  //          [ sin(p) * cos(y) / cos(p), sin(p) * sin(y) / cos(p), 1]
+  // Note: N(q) is singular for p = π/2 + kπ, for k = ±1, ±2, ...
+  // See related code and comments in CalcNMatrix().
+  // Note: The calculation below is more efficient than calculating N(q) * v.
   //
-  // such that q̇ = Einv_F(q) * w_FM; q̇ = [ṙ, ṗ, ẏ]ᵀ
-  // where we intentionally wrote the expression for Einv_F in terms of sines
-  // and cosines only to arrive to the more computationally efficient version
-  // below.
-  //
-  // Notice Einv_F is singular for p = π/2 + kπ, ∀ k ∈ ℤ.
-  //
-  // Note to developers:
-  // Matrix E_F(q) is obtained by computing w_FM as the composition of the
-  // angular velocity induced by each Euler angle rate in its respective
-  // body-fixed frame. This is outlined in [Diebel 2006, §5.2;
-  // Mitiguy (July 22) 2016, §9.3]. Notice however that our rotation matrix R_FM
-  // is the transpose of that in [Diebel 2006], Eq. 67, given the convention
-  // used there. Still, the expression for Einv_F in [Diebel 2006], Eq. 76, is
-  // exactly the same here presented.
-  //
-  // The expression for Einv_F was symbolically generated with the following
-  // Maxima script (which can be copy/pasted and executed as is):
-  //
-  // Rx:matrix([1,0,0],[0,cos(r),-sin(r)],[0,sin(r),cos(r)]);
-  // Ry:matrix([cos(p),0,sin(p)],[0,1,0],[-sin(p),0,cos(p)]);
-  // Rz:matrix([cos(y),-sin(y),0],[sin(y),cos(y),0],[0,0,1]);
-  // R_FM:Rz . Ry . Rx;
-  // R_MF:transpose(R_FM);
-  // E_F: transpose(append(transpose(
-  //             Rz . Ry . [1,0,0]),transpose(Rz . [0,1,0]),matrix([0,0,1])));
-  // detout: true$
-  // doallmxops: false$
-  // doscmxops: false$
-  // Einv_F: trigsimp(invert(E_F));
+  // Developer note: N(q) is calculated by first forming w_FM by adding three
+  // angular velocities, each related to an Euler angle rate (ṙ or ṗ or ẏ) in
+  // various frames (frame F, two intermediate frames, and frame M). This is
+  // discussed in [Diebel 2006, §5.2; Mitiguy (August 2019, §9.1].
+  // Note: Diebel's eq. 67 rotation matrix is the transpose of our R_FM. Still
+  // the expression for N(q) in [Diebel 2006], Eq. 76, is the same as herein.
   //
   // [Diebel 2006] Representing attitude: Euler angles, unit quaternions, and
   //               rotation vectors. Stanford University.
-  // [Mitiguy (July 22) 2016] Mitiguy, P., 2016. Advanced Dynamics & Motion
-  //                          Simulation.
+  // [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 cp = cos(angles[1]);
   if (abs(cp) < 1.0e-3) {
@@ -288,61 +253,50 @@ void RpyBallMobilizer<T>::DoMapVelocityToQDot(
   const T cy = cos(angles[2]);
   const T cpi = 1.0 / cp;
 
-  // Although the linear equations relating v to q̇ can be used to explicitly
-  // solve the equation w_FM = E_F(q) * q̇ for q̇, a more computational efficient
-  // solution results by implicit solution of those linear equations.
-  // Namely, the first two equations in w_FM = E_F(q) * q̇ are used to solve for
-  // ṙ and ṗ, then the third equation is used to solve for ẏ in terms of just
-  // ṙ and w2:
+  // Although we can calculate q̇ = N(q) * v, it is more efficient to implicitly
+  // invert the simpler equation v = N⁺(q) * q̇, whose matrix form is
+  //
+  // ⌈ ω0 ⌉   ⌈ cos(y) * cos(p),  -sin(y),  0 ⌉ ⌈ ṙ ⌉
+  // | ω1 | = | sin(y) * cos(p),   cos(y),  0 | | ṗ |
+  // ⌊ ω2 ⌋   ⌊         -sin(p),        0,  1 ⌋ ⌊ ẏ ⌋
+  //
+  // Namely, the first two equations are used to solve for ṙ and ṗ, then the
+  // third equation is used to solve for ẏ in terms of just ṙ and w2:
   // ṙ = (cos(y) * w0 + sin(y) * w1) / cos(p)
   // ṗ = -sin(y) * w0 + cos(y) * w1
   // ẏ = sin(p) * ṙ + w2
-  const T t = (cy * w0 + sy * w1) * cpi;  // Common factor.
-  *qdot = Vector3<T>(t, -sy * w0 + cy * w1, sp * t + w2);
+  const T rdot = (cy * w0 + sy * w1) * cpi;
+  *qdot = Vector3<T>(rdot, -sy * w0 + cy * w1, sp * rdot + w2);
 }
 
 template <typename T>
 void RpyBallMobilizer<T>::DoMapQDotToVelocity(
     const systems::Context<T>& context,
     const Eigen::Ref<const VectorX<T>>& qdot, EigenPtr<VectorX<T>> v) const {
-  using std::cos;
-  using std::sin;
-
-  // The linear map between q̇ and v is given by matrix E_F(q) defined by:
-  //          [ cos(y) * cos(p), -sin(y), 0]
-  // E_F(q) = [ sin(y) * cos(p),  cos(y), 0]
-  //          [         -sin(p),       0, 1]
+  // The matrix N⁺(q) relates v to q̇ as v = N⁺(q) * q̇, where q̇ = [ṙ, ṗ, ẏ]ᵀ and
+  // v = w_FM_F = [ω0, ω1, ω2]ᵀ is the mobilizer M frame's angular velocity in
+  // the mobilizer F frame, expressed in the F frame (thus w_FM_F = N⁺(q) * q̇).
   //
-  // w_FM = E_F(q) * q̇; q̇ = [ṙ, ṗ, ẏ]ᵀ
+  // ⌈ ω0 ⌉   ⌈ cos(y) * cos(p),  -sin(y),  0 ⌉ ⌈ ṙ ⌉
+  // | ω1 | = | sin(y) * cos(p),   cos(y),  0 | | ṗ |
+  // ⌊ ω2 ⌋   ⌊         -sin(p),        0,  1 ⌋ ⌊ ẏ ⌋
   //
-  // Here, following a convention used by many dynamicists, we are calling the
-  // angles θ₁, θ₂, θ₃ as roll (r), pitch (p) and yaw (y), respectively.
+  // See related code and comments in DoCalcNplusMatrix().
   //
-  // Note to developers:
-  // Matrix E_F(q) is obtained by computing w_FM as the composition of the
-  // angular velocity induced by each Euler angle rate in its respective
-  // body-fixed frame. This is outlined in [Diebel 2006, §5.2;
-  // Mitiguy (July 22) 2016, §9.3]. Notice however that our rotation matrix R_FM
-  // is the transpose of that in [Diebel 2006], Eq. 67, given the convention
-  // used there. Still, the expression for E_F in [Diebel 2006], Eq. 74, is
-  // exactly the same here presented.
-  //
-  // The expression for E_F was symbolically generated with the following
-  // Maxima script (which can be copy/pasted and executed as is):
-  //
-  // Rx:matrix([1,0,0],[0,cos(r),-sin(r)],[0,sin(r),cos(r)]);
-  // Ry:matrix([cos(p),0,sin(p)],[0,1,0],[-sin(p),0,cos(p)]);
-  // Rz:matrix([cos(y),-sin(y),0],[sin(y),cos(y),0],[0,0,1]);
-  // R_FM:Rz . Ry . Rx;
-  // R_MF:transpose(R_FM);
-  // E_F: transpose(append(transpose(
-  //             Rz . Ry . [1,0,0]),transpose(Rz . [0,1,0]),matrix([0,0,1])));
+  // Developer note: N(q) is calculated by first forming w_FM by adding three
+  // angular velocities, each related to an Euler angle rate (ṙ or ṗ or ẏ) in
+  // various frames (frame F, two intermediate frames, and frame M). This is
+  // discussed in [Diebel 2006, §5.2; Mitiguy (August 2019, §9.1].
+  // Note: Diebel's eq. 67 rotation matrix is the transpose of our R_FM. Still
+  // the expression for N(q) in [Diebel 2006], Eq. 76, is the same as herein.
   //
   // [Diebel 2006] Representing attitude: Euler angles, unit quaternions, and
   //               rotation vectors. Stanford University.
-  // [Mitiguy (July 22) 2016] Mitiguy, P., 2016. Advanced Dynamics & Motion
-  //                          Simulation.
+  // [Mitiguy August 2019] Mitiguy, P., 2019. Advanced Dynamics & Motion
+  //                       Simulation.
 
+  using std::cos;
+  using std::sin;
   const Vector3<T> angles = get_angles(context);
   const T& rdot = qdot[0];
   const T& pdot = qdot[1];
@@ -354,8 +308,8 @@ void RpyBallMobilizer<T>::DoMapQDotToVelocity(
   const T cy = cos(angles[2]);
   const T cp_x_rdot = cp * rdot;
 
-  // Compute the product w_FM = E_W * q̇ directly since it's cheaper than
-  // explicitly forming E_F and then multiplying with q̇.
+  // Compute the product v = N⁺(q) * q̇ element-by-element to leverate 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*/
                   -sp * rdot /*+   0 * pdot */ + ydot);