Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
May 14, 2025
Merged

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

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0d350e8
Replace E matrix with standard N matrix in rpy mobilizer.
May 14, 2025
c747ebf
Change rddot to rdot.
May 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
184 changes: 69 additions & 115 deletions multibody/tree/rpy_ball_mobilizer.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 "
Expand All @@ -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;

Expand All @@ -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]);
Expand All @@ -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) {
Expand All @@ -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];
Expand All @@ -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);
Expand Down