Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer. #23030

mitiguy · 2025-05-21T02:03:50Z

This is one in a series of PRs to help address issue #22630. It creates DoCalcNDotMatrix() and DoCalcNplusDotMatrix() for an RPY ball mobilizer.

PR #22950 was already merged to handle simple mobilizers for the aforementioned methods. Subsequent PRs will create similar functions for other complex mobilizers (e.g., quaternion_floating_mobilizer). A follow-on PR will create MapQDDotToAcceleration() and MapAccelerationToQDDot() for the rpy_ball_mobilizer.

FYI: Since mobilizers are Drake internal classes, after the internal mobilizer work is complete, there will be PRs (code and testing) for the public API in MultibodyPlant to address issue #22630.

This change is

mitiguy

Feature review +@sherm1

Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

sherm1

First feature review pass done, PTAL.

Reviewed 3 of 3 files at r1, all commit messages.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.h line 309 at r1 (raw file):

  // Certain roll pitch yaw calculations (e.g., calculating the N(q) matrix)
  // have  a singularity (divide-by-zero error) when cos(pitch) ≈ 0.

nit: extra space between "have" and "a"

multibody/tree/rpy_ball_mobilizer.cc line 135 at r1 (raw file):

template <typename T>
void RpyBallMobilizer<T>::ThrowSinceCosPitchIsNearZero(const T& pitch) const {

BTW consider passing the API name to include at the start of the error message. It won't be obvious to a user which API they called that caused this message. I'd use "CalcNMatrix(): ..." rather than DoCalcNMatrix() though.

multibody/tree/rpy_ball_mobilizer.cc line 217 at r1 (raw file):

  // N(q) = [                  -sin(y),                    cos(y),  0]
  //        [ sin(p) * cos(y) / cos(p),  sin(p) * sin(y) / cos(p),  1]
  //

BTW consider writing out Ṅ here in the comment like you did for N. That would make it easier to verify that the code below is correct.

multibody/tree/rpy_ball_mobilizer.cc line 248 at r1 (raw file):

  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;

BTW because Eigen stores matrices in column order, it might be better practice to fill them in that way also (i.e. 00, 10, 20, 01, 11, etc). (Possibly the compiler would be able to reorder these but I'm not confident it would.)

multibody/tree/rpy_ball_mobilizer.cc line 264 at r1 (raw file):

  // N⁺(q) = [ sin(y) * cos(p),   cos(y),  0]
  //         [         -sin(p),        0,  1]
  //

BTW would be helpful to have Ṅ⁺ written out here also.

multibody/tree/rpy_ball_mobilizer.cc line 291 at r1 (raw file):

  NplusDot->coeffRef(2, 0) = -cp * pdot;
  NplusDot->coeffRef(2, 1) = 0;
  NplusDot->coeffRef(2, 2) = 0;

BTW column order?

multibody/tree/test/rpy_ball_mobilizer_test.cc line 149 at r1 (raw file):

                              MatrixCompareType::relative));
  EXPECT_TRUE(CompareMatrices(NDot_Nplus, -N_NplusDot, kTolerance,
                              MatrixCompareType::relative));

BTW this is a very cool test. But is it sufficient? If we accidently made NDot or NPlusDot zero, I think this test would pass.

mitiguy

Hoping this passes CI and all is well.

Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.h line 309 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: extra space between "have" and "a"

Done.

multibody/tree/rpy_ball_mobilizer.cc line 135 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider passing the API name to include at the start of the error message. It won't be obvious to a user which API they called that caused this message. I'd use "CalcNMatrix(): ..." rather than DoCalcNMatrix() though.

Done.

multibody/tree/rpy_ball_mobilizer.cc line 217 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider writing out Ṅ here in the comment like you did for N. That would make it easier to verify that the code below is correct.

Done.

multibody/tree/rpy_ball_mobilizer.cc line 248 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW because Eigen stores matrices in column order, it might be better practice to fill them in that way also (i.e. 00, 10, 20, 01, 11, etc). (Possibly the compiler would be able to reorder these but I'm not confident it would.)

Done.

multibody/tree/rpy_ball_mobilizer.cc line 264 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW would be helpful to have Ṅ⁺ written out here also.

Done.

multibody/tree/rpy_ball_mobilizer.cc line 291 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW column order?

Done.

multibody/tree/test/rpy_ball_mobilizer_test.cc line 149 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this is a very cool test. But is it sufficient? If we accidently made NDot or NPlusDot zero, I think this test would pass.

Done.

mitiguy

@sherm1 -- Passed CI and is ready for you (when you have time).

Reviewable status: LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

sherm1

Feature pending some comment clarifications
+@sammy-tri for platform review per rotation, please

Reviewed 4 of 4 files at r2, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.cc line 168 at r2 (raw file):

  const T cp = cos(angles[1]);
  if (abs(cp) < 1.0e-3) {
    const char* function_name_less_Do = __func__ + 2;

BTW clever!

multibody/tree/rpy_ball_mobilizer.cc line 225 at r2 (raw file):

  //          ⌈ -sy/cp ẏ + cy sp/cp² ṗ    cy/cp ẏ + sy sp/cp² ṗ,   0 ⌉
  // Ṅ(q,q̇) = |                  -cy ẏ,                   -sy ẏ,   0 |
  //          ⌊  cy/cp² ṗ̇ - sp sy/cp ẏ,   sy/cp² ṗ + sp cy/cp ẏ,   0 ⌋

typo: pdotdot ->pdot

multibody/tree/rpy_ball_mobilizer.cc line 229 at r2 (raw file):

  // where cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y).
  // Note: Ṅ[2, 0] simplifies using: cp cy/cp ṗ + sp² cy/cp² ṗ = cy/cp² ṗ.
  // Note: Ṅ[2, 1] simplifies using: cp sy/cp ṗ̇ + sp² sy/cp² ṗ = sy/cp² ṗ.

typo: pdotdot ->pdot

multibody/tree/rpy_ball_mobilizer.cc line 229 at r2 (raw file):

  // where cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y).
  // Note: Ṅ[2, 0] simplifies using: cp cy/cp ṗ + sp² cy/cp² ṗ = cy/cp² ṗ.
  // Note: Ṅ[2, 1] simplifies using: cp sy/cp ṗ̇ + sp² sy/cp² ṗ = sy/cp² ṗ.

minor: I don't understand either of these alleged simplifications, and I don't see them reflected in the code below either.

multibody/tree/rpy_ball_mobilizer.cc line 287 at r2 (raw file):

  // where cp = cos(p), sp = sin(p), cy = cos(y), sy = sin(y).
  // Note: Ṅ[2, 0] simplifies using: cp cy/cp ṗ + sp² cy/cp² ṗ = cy/cp² ṗ.
  // Note: Ṅ[2, 1] simplifies using: cp sy/cp ṗ̇ + sp² sy/cp² ṗ = sy/cp² ṗ.

typo: pdotdot -> pdot

Again I don't understand these simplifications or see them in the code.

sammy-tri

Reviewed 4 of 4 files at r2, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

mitiguy

@sherm1 Updated -- PTAL.

Reviewable status: LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.cc line 225 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: pdotdot ->pdot

Done.

multibody/tree/rpy_ball_mobilizer.cc line 229 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: pdotdot ->pdot

Done.

multibody/tree/rpy_ball_mobilizer.cc line 229 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: I don't understand either of these alleged simplifications, and I don't see them reflected in the code below either.

Done. Updated language to clarify. Does this help?

multibody/tree/rpy_ball_mobilizer.cc line 287 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

typo: pdotdot -> pdot

Again I don't understand these simplifications or see them in the code.

Done. Removed notes as they are mistaken remnants of a copy/paste.

sherm1

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.cc line 229 at r2 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Done. Updated language to clarify. Does this help?

Still doesn't seem to match the code?

multibody/tree/rpy_ball_mobilizer.cc line 230 at r3 (raw file):

  // Note: Although the elements of Ṅ(q,q̇) are simply the time-derivatives of
  // the corresponding elements of N(q), the result for Ṅ[2, 0] was shortened
  // as cp cy/cp ṗ + sp² cy/cp² ṗ simplifies to cy/cp² ṗ. Similarly for Ṅ[2, 1].

But when I look below at the entries for Ṅ(2, 0) and (2, 1) I don't see these simplifications.

mitiguy

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

multibody/tree/rpy_ball_mobilizer.cc line 230 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

But when I look below at the entries for Ṅ(2, 0) and (2, 1) I don't see these simplifications.

Element Ṅ[2, 0] = cy * cpiSqr_pdot - sy * sp_cpi_ydot;
The first term in Ṅ[2, 0] is cy * cpiSqr_pdot which is written in the comment above as cy/cp² ṗ
cy/cp² ṗ was simplified from cp cy/cp ṗ + sp² cy/cp² ṗ s.

Perhaps this allegedly illuminating comment should be eliminated.
It may be more confusing than helpful.

sherm1

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.cc line 230 at r3 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Element Ṅ[2, 0] = cy * cpiSqr_pdot - sy * sp_cpi_ydot;
The first term in Ṅ[2, 0] is cy * cpiSqr_pdot which is written in the comment above as cy/cp² ṗ
cy/cp² ṗ was simplified from cp cy/cp ṗ + sp² cy/cp² ṗ s.

Perhaps this allegedly illuminating comment should be eliminated.
It may be more confusing than helpful.

Sorry, I'm still confused. For example, the original expression for Ṅ[2, 0] above has sy and ẏ in it but the simplification does not.

sherm1

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

multibody/tree/rpy_ball_mobilizer.cc line 230 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Sorry, I'm still confused. For example, the original expression for Ṅ[2, 0] above has sy and ẏ in it but the simplification does not.

Likely will save time to discuss f2f on Tuesday!

sammy-tri

@sherm1 I'm not 100% clear from the discussions (which are marked resolved) that you're entirely satisfied with the changes. I'm going to hold of merging for the moment, let me know if we're good or feel free to merge yourself.

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

sammy-tri

oh, there is actually one unresolved. that makes things clearer.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sammy-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

sammy-tri

I'll let you two sort out the last issue and merge.

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

mitiguy

Reviewable status: 1 unresolved discussion, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

multibody/tree/rpy_ball_mobilizer.cc line 229 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Still doesn't seem to match the code?

Updated per f2f.

multibody/tree/rpy_ball_mobilizer.cc line 230 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Likely will save time to discuss f2f on Tuesday!

Hopefully, good to go?

sherm1

Thanks, Sam!
I get it now, Paul -- thanks! I was able to verify the simplifications & code.

Reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mitiguy)

mitiguy added priority: medium status: do not review release notes: none This pull request should not be mentioned in the release notes labels May 21, 2025

mitiguy force-pushed the CalcNDotMatrixRPYBallMobilizer branch from b59a950 to 8381e83 Compare May 22, 2025 22:19

mitiguy changed the title ~~[WIP] Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer.~~ Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer. May 22, 2025

mitiguy removed the status: do not review label May 22, 2025

mitiguy assigned sherm1 May 22, 2025

mitiguy commented May 22, 2025

View reviewed changes

sherm1 reviewed May 23, 2025

View reviewed changes

mitiguy force-pushed the CalcNDotMatrixRPYBallMobilizer branch from 8d89fc4 to 57f640a Compare May 30, 2025 01:13

mitiguy commented May 30, 2025

View reviewed changes

sherm1 assigned sammy-tri May 30, 2025

sherm1 reviewed May 30, 2025

View reviewed changes

sammy-tri reviewed Jun 2, 2025

View reviewed changes

mitiguy force-pushed the CalcNDotMatrixRPYBallMobilizer branch from 4c68215 to 7077180 Compare June 2, 2025 22:07

mitiguy commented Jun 3, 2025

View reviewed changes

sherm1 reviewed Jun 3, 2025

View reviewed changes

mitiguy commented Jun 3, 2025

View reviewed changes

sherm1 reviewed Jun 3, 2025

View reviewed changes

sammy-tri reviewed Jun 3, 2025

View reviewed changes

Paul Mitiguy added 7 commits June 3, 2025 11:53

Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer.

2590863

Fix unit test.

6391f29

Improve documentation.

d4cd220

Respond to some of Sherm's comments.

cff4c09

Add function name to error message when there is a singularity.

d0c158e

Add better tests, including testing exception messages.

0aab111

Remove unused function in rpy_ball_mobilizer.h

1265875

Respond to Sherm's comments.

5bb63c6

sammy-tri reviewed Jun 3, 2025

View reviewed changes

Improve documentation on simplification.

8cf41fc

mitiguy force-pushed the CalcNDotMatrixRPYBallMobilizer branch from 7077180 to 8cf41fc Compare June 3, 2025 19:19

Change pddot to pdot.

3e8eb88

mitiguy commented Jun 3, 2025

View reviewed changes

sherm1 reviewed Jun 3, 2025

View reviewed changes

sherm1 merged commit 0b421dc into RobotLocomotion:master Jun 3, 2025
9 of 10 checks passed

Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer. #23030

Create CalcNDotMatrix() and CalcNplusDotMatrix() for rpy_ball_mobilizer. #23030

Uh oh!

Conversation

mitiguy commented May 21, 2025 • edited by sherm1 Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

sammy-tri left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

sammy-tri left a comment

Choose a reason for hiding this comment

Uh oh!

sammy-tri left a comment

Choose a reason for hiding this comment

Uh oh!

sammy-tri left a comment

Choose a reason for hiding this comment

Uh oh!

mitiguy left a comment

Choose a reason for hiding this comment

Uh oh!

sherm1 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

mitiguy commented May 21, 2025 •

edited by sherm1

Loading