MapQDDotToAcceleration() and MapAccelerationToQDDot() for rpy mobilizer.

[mitiguy]

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

PR #22698 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).

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 +@amcastro-tri
Alternatively, I am happy to get this small PR reviewed by @vincekurtz

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

[mitiguy]

@vincekurtz -- Since Sherm is out and it would be great to get your input on this, feel free to take a look and comment on code, comments, and whether the documentation is sufficient. There is a follow-up PR in the wings, so whatever we can do to make progress is appreciated.

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

[amcastro-tri]

Looking good @mitiguy. Mostly doc nits and refences.

On a major note, I think you want to check with @sherm1 on the design. He's been working real hard for a long time now to get rid of virtuals in the mobilizer classes.

Reviewed 2 of 3 files at r1.
Reviewable status: 12 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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 265 at r1 (raw file):

  // Calculate the term Ṅ⁺(q,q̇)⋅q̇ which appears in v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
  Vector3<T> CalcNplusDotTimesQdot(const systems::Context<T>& context,

Being the go-to person when naming things, you'll agree with me on keeping naming consistency across the entire code base.
We have a name for this we use all throoughout the tree directory; "bias".
Even yourself introduced this term in the public MbP APIs, see @anchor bias_acceleration_functions (which I believe you wrote?).

Therefore, why not ot name this something like CalcVelocityToQDotBias()? keeping also the parallel with DoMapVelocityToQDot()?

This current name is quite difficult to parse really, being the a conglomeration of abbreviations.

---

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

  //   A vector of generalized velocities for this Mobilizer which should
  //   correspond to a vector in ℝ³ for an angular velocity w_FM of M in F.
  void DoMapQDotToVelocity(const systems::Context<T>& context,

read the docs updated by @sherm1 in mobilizer_impl.h, @mitiguy.
Specifically, look for pre_multiply_by_X_FM() and see how Sherm resolved the dispatch within body_node_impl

Sherm has been working real hard to get rid of virtual dispatching for performance sensitive methods like these. I am pretty certain there is no good reason to introduce new virtual functions now that Sherm figured out how to get rid of them.

You might want to double check with @sherm1 on this now outdated design pattern.

---

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

                           EigenPtr<VectorX<T>> v) const final;

  // Maps vdot to qddot, which for this mobilizer is q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.

In practice, we rarely write the bias as these two separate terms, since each separate terms is more expesive to compute than their product.
You might consider giving the bias a name and wal away, say you write instead:

///  q̈ =  N(q)⋅v̇ +  bᴺ(q, v)

Ditto below for the inverse map:

///   v̇ =N⁺(q)⋅q̈ + bᴺ⁺(q,v)

Code quote:

 q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.

---

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

  const T sp = sin(angles[1]);
  const T sy = sin(angles[2]);
  const T cy = cos(angles[2]);

nit, you can move these to after the check for throwing, closer to the scope where they get used.

Code quote:

  const T sy = sin(angles[2]);
  const T cy = cos(angles[2]);

---

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

  if (abs(cp) < 1.0e-3) {
    ThrowSinceCosPitchIsNearZero(angles[1], function_name);
  }

this is the 5th time this exact code is copy/pasted withing this file.
I suggest that the 1e-3 threshold is moved within ThrowSinceCosPitchIsNearZero() so that this and the other 4 ocurrences ready like proposed below.

That will allow you to consolidated this hard-coded tolerance in a single place and even document in a single place why this value is chosen .

Suggestion:

ThrowSinceCosPitchIsNearZero(angles[1], function_name);

---

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

  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,

I'll have to thrust you on these. Could you add a reference to a book here?
This one is a classic:

• Diebel, J., 2006. Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix, 58(15-16), pp.1-35.

---

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

  // ⌈ ṙ ⌉   ⌈  cos(y) / cos(p)           sin(y) / cos(p)           0 ⌉ ⌈ ωx ⌉
  // | ṗ | = | -sin(y)                    cos(y)                    0 | | ωy |
  // ⌊ ẏ ⌋   ⌊  sin(p) * cos(y) / cos(p)  sin(p) * sin(y) / cos(p)  1 ⌋ ⌊ ωz ⌋

Copy/pasta from DoMapVelocityToQDot().
There is no reason to repeat here, it's actually distracting in this context.
I suggest you simple say N(q) and ference to docs in DoMapVelocityToQDot().

---

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

  // However, N⁺(q) is simpler than N(q) so Ṅ⁺(q,q̇) is much simpler than Ṅ(q,q̇).
  // Hence, a more efficient calculation of q̈ starts as v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈
  // and then solves as q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.

I like this! nice trick! Is there a reference to this efficiency?

Code quote:

  // However, N⁺(q) is simpler than N(q) so Ṅ⁺(q,q̇) is much simpler than Ṅ(q,q̇).
  // Hence, a more efficient calculation of q̈ starts as v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈
  // and then solves as q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.

---

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

  // Mathematical trick: Although the function below was designed to efficiently
  // calculate q̇ = N(q)⋅v, it can be used to calculate q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.

This does not look like a trick Paul, it's simply calling the operator form.
THat is, the comments add no additional information to your previous comment above.
Remove.

Code quote:

  // Mathematical trick: Although the function below was designed to efficiently
  // calculate q̇ = N(q)⋅v, it can be used to calculate q̈ = N(q) {v̇ - Ṅ⁺(q,q̇)⋅q̇}.

---

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

  // ⌈ ωx ⌉   ⌈cos(y) cos(p)  -sin(y)  0⌉ ⌈ ṙ ⌉
  // | ωy | = |sin(y) cos(p)   cos(y)  0| | ṗ |
  // ⌊ ωz ⌋   ⌊      -sin(p)       0   1⌋ ⌊ ẏ ⌋

Ditt, remove. Reference MapQDotToVelocity().

---

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

  //
  // There are various ways to calculate v̇ = [ω̇x, ω̇y, ω̇z]ᵀ (the time-derivatives
  // of the generalized velocities). A simple one is v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.

ditto, add reference to docs on this trick (yes, this one is a trick!, so please document accordingly)

Code quote:

  // There are various ways to calculate v̇ = [ω̇x, ω̇y, ω̇z]ᵀ (the time-derivatives
  // of the generalized velocities). A simple one is v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.

---

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

  // Mathematical trick: Although the function below was designed to efficiently
  // calculate v = N⁺(q)⋅q̇, it can be used to calculate N⁺(q)⋅q̈.

ditto, too berbose is distracting, and you already said that above.

Code quote:

  // Mathematical trick: Although the function below was designed to efficiently
  // calculate v = N⁺(q)⋅q̇, it can be used to calculate N⁺(q)⋅q̈.

[vincekurtz]

Added a few very small comments, mostly redundant with Alejandro's

Reviewed 1 of 3 files at r1, all commit messages.
Reviewable status: 15 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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.cc line 462 at r1 (raw file):

    ThrowSinceCosPitchIsNearZero(angles[1], function_name);
  }
  // The algorithm below efficiently calculates Ṅ⁺(q,q̇)⋅q̇.

minor: this comment isn't particularly informative. Does the algorithm below come from somewhere? Or is it a standard thing?

---

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

  // of the generalized velocities). A simple one is v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.
  // --------------------------------------------------------------------------
  // Calculate Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.

nit: this is just the first term, Ṅ⁺(q,q̇)⋅q̇, correct?

---

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

  DoMapQDotToVelocity(context, qddot, vdot);

  // Calculate v̇ = Ṅ⁺(q,q̇)⋅q̇ + N⁺(q)⋅q̈.

minor: can we not just do something like (*vdot) += NplusDotTimesQdot? (ignore if there's some EigenPtr weirdness I'm forgetting about)

[mitiguy]

Reviewable status: 15 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

Being the go-to person when naming things, you'll agree with me on keeping naming consistency across the entire code base.
We have a name for this we use all throoughout the tree directory; "bias".
Even yourself introduced this term in the public MbP APIs, see @anchor bias_acceleration_functions (which I believe you wrote?).

Therefore, why not ot name this something like CalcVelocityToQDotBias()? keeping also the parallel with DoMapVelocityToQDot()?

This current name is quite difficult to parse really, being the a conglomeration of abbreviations.

For now, I moved this function to the private part of the class as it is only used twice (it is simply a helper function and it's name in the two contexts that is used is easy to understand).

I like your point below as it may be more globally important. It may make sense to implement something like CalcAccelerationToQddotBias() for all mobilizers. Let me think that over for later, talk to @sherm1 when he has returned, and converge with you too. For now, this is not a show stopper.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

read the docs updated by @sherm1 in mobilizer_impl.h, @mitiguy.
Specifically, look for pre_multiply_by_X_FM() and see how Sherm resolved the dispatch within body_node_impl

Sherm has been working real hard to get rid of virtual dispatching for performance sensitive methods like these. I am pretty certain there is no good reason to introduce new virtual functions now that Sherm figured out how to get rid of them.

You might want to double check with @sherm1 on this now outdated design pattern.

I looked at pre_multiply_by_X_FM() and I agree that this may be worthwhile to optimize. I'll check with @sherm1 when he returns. He was highly involved (did the feature review) with the restructuring done to streamline the public function MapQDotToVelocity() with its NVI counterparts DoMapQDotToVelocity(). There is lots of work that needs to happen to get this feature done -- and most are private/protected methods, so we should not run into deprecation issues. We can revisit on his return as this is a larger question.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

In practice, we rarely write the bias as these two separate terms, since each separate terms is more expesive to compute than their product.
You might consider giving the bias a name and wal away, say you write instead:

///  q̈ =  N(q)⋅v̇ +  bᴺ(q, v)

Ditto below for the inverse map:

///   v̇ =N⁺(q)⋅q̈ + bᴺ⁺(q,v)

Agreed. We may want to give the bias a name such as CalcAccelerationToQddotBias(). This can wait until @sherm1 returns as it may affects this rpy_ball_mobilizer and a quaternion class, but simple mobilizers may not need it.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, you can move these to after the check for throwing, closer to the scope where they get used.

There are major inefficiencies with sine/cosine calculations that will be addressed in the subsequent PR. I discussed this with Rico on Wednesday. The next PR will address it. I'll keep you in the loop and it should happen right after this PR merges.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

this is the 5th time this exact code is copy/pasted withing this file.
I suggest that the 1e-3 threshold is moved within ThrowSinceCosPitchIsNearZero() so that this and the other 4 ocurrences ready like proposed below.

That will allow you to consolidated this hard-coded tolerance in a single place and even document in a single place why this value is chosen .

Yes -- and agreed. The subsequent PR will consolidate this as well as many of the calls to sine and cosine -- with some functions, due to their calling other functions, are calculating the same sines and cosines 3 times (expensive and unnecessary). The next PR will address it. I'll keep you in the loop.

---

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

Previously, vincekurtz (Vince Kurtz) wrote…

minor: this comment isn't particularly informative. Does the algorithm below come from somewhere? Or is it a standard thing?

Good. I added more to document how to form/verify this.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

I'll have to thrust you on these. Could you add a reference to a book here?
This one is a classic:

• Diebel, J., 2006. Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix, 58(15-16), pp.1-35.

This was more of a by-hand calculation. I documented the algorithm more. Also, I like Jame's work. It is good (but not used for this calculation).

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

Copy/pasta from DoMapVelocityToQDot().
There is no reason to repeat here, it's actually distracting in this context.
I suggest you simple say N(q) and ference to docs in DoMapVelocityToQDot().

Done.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

I like this! nice trick! Is there a reference to this efficiency?

Thanks Alejandro. I am not aware of a reference. I thought through various ways to do this, and this seemed reasonably efficient and straightforward. Perhaps later I'll add it to the textbook chapter titled "Angular velocity and kinematical ODEs" and it deals with 1st and 2nd-order ODEs (e.g., for quaternions, etc.).

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

This does not look like a trick Paul, it's simply calling the operator form.
THat is, the comments add no additional information to your previous comment above.
Remove.

Done. I updated the comment. I helps me remember that the public function MapVelocityToQdot() is being used in an efficient non-traditional way (the arguments do not match the documentation).

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

Ditt, remove. Reference MapQDotToVelocity().

Done.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto, add reference to docs on this trick (yes, this one is a trick!, so please document accordingly)

Done.

---

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

Previously, vincekurtz (Vince Kurtz) wrote…

nit: this is just the first term, Ṅ⁺(q,q̇)⋅q̇, correct?

Yes . Good eyes. That comment is misleading, so I updated to clarify. Better?

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto, too berbose is distracting, and you already said that above.

Done. A little less verbose.

---

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

Previously, vincekurtz (Vince Kurtz) wrote…

minor: can we not just do something like (*vdot) += NplusDotTimesQdot? (ignore if there's some EigenPtr weirdness I'm forgetting about)

Great catch -- thanks Vince.

[mitiguy]

@amcastro-tri and @vincekurtz thank you for reviewing this PR. It is ready for another look.
Alejandro, I like your suggestion about bias and think CalcAccelerationToQddotBias() makes sense. We could use it in this PR as a prototype name (in a private or protected part of this class) that may eventually bubble up to a public function.

Reviewable status: 15 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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 306 at r1 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Agreed. We may want to give the bias a name such as CalcAccelerationToQddotBias(). This can wait until @sherm1 returns as it may affects this rpy_ball_mobilizer and a quaternion class, but simple mobilizers may not need it.

Following up. I like your suggestion and think CalcAccelerationToQddotBias() makes sense. Let me know if you think we should use it here as a proxy for the name that may eventually bubble up to a public function.

[amcastro-tri]

This is almost ready to go Paul. Two majore comments:

1. This is Drake, no MotionGenesis nor other software. Lets stick to coding and documentation styles from Drake, see below.
2. Do not say "efficiently" or "more efficient" if there is no data supporting that claim. In this case it seems you took the "long path" calculation for efficiency, but it's not clear from this PR that it's even worth it. A hand crafted algebra for the bias term should be as fast if not faster, probably even easer for the compiler to optimize.

Reviewed 1 of 3 files at r2, 2 of 2 files at r3.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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.cc line 458 at r1 (raw file):

There are major inefficiencies with sine/cosine calculations that will be addressed in the subsequent PR.

I didn't say anything about inneficiencies Paul, this was merely a C++ styling request if you will.

---

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

The subsequent PR will consolidate this as well

This litereally would take 2 minutes of your time Paul, why wait for next PR?

---

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

and this seemed reasonably efficient

Without actual benchmarks, then there is no reason to say "efficient" in the comments, it is missleading.

There is nothing that supports why you didn't use the "simple" calculation reference above. Quoting you:

A simple calculation directly solves for q̈ as q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.

---

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

Previously, mitiguy (Mitiguy) wrote…

Done. I updated the comment. I helps me remember that the public function MapVelocityToQdot() is being used in an efficient non-traditional way (the arguments do not match the documentation).

please remove "efficient" from these comments. These methods are doing the algebra they are supposed to, with no apparent optimizations for speed. I'd use the word "efficient" if for instace there is a clever trick to reduce FLOPS or to use AVX instructions, but these are the algebraic expressions one would get from directly using the chain rule.

---

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

    const Eigen::Ref<const VectorX<T>>& vdot,
    EigenPtr<VectorX<T>> qddot) const {
  // --------------------------------------------------------------------------

nit, remove these all trhouout. We do not do that anywhere in Drake, no need to introduce new conventions/preferences.

[mitiguy]

Thanks @amcastro-tri. Improved per your comments. Ready for you.
@vincekurtz Please let me know if your comments were addressed.

Reviewable status: 6 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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.cc line 458 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

There are major inefficiencies with sine/cosine calculations that will be addressed in the subsequent PR.

I didn't say anything about inneficiencies Paul, this was merely a C++ styling request if you will.

Good point. My response should have been that I am keeping these together, as they are also coded together in the same way (clustered) in 6 other functions in this same file.

More background: Several of these clustered groups of , including the ones here, will disappear as they are unnecessarily repetitive. Separating the cluster here is a detriment to the reviewer for the next PR. Stay tuned.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

The subsequent PR will consolidate this as well

This litereally would take 2 minutes of your time Paul, why wait for next PR?

Done. Yes. The tolerance should (and now is) hard-coded in a single place.

Reminder notes to myself:

1. This repetitious 5 times that this exact code is copy/pasted within this file will be reduced, e.g., to 2 in a follow-up PR.
2. The test if( abs(cp)<1.0e-3 ) cannot be moved within the function unless the function name ThrowSinceCosPitchIsNearZero() is also changed and the function takes an additional argument cp.
3. Moving the test to another function may result in an otherwise unnecessary additional function call/return.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

and this seemed reasonably efficient

Without actual benchmarks, then there is no reason to say "efficient" in the comments, it is missleading.

There is nothing that supports why you didn't use the "simple" calculation reference above. Quoting you:

A simple calculation directly solves for q̈ as q̈ = Ṅ(q,q̇)⋅v + N(q)⋅v̇.

Done. Language was changed.

Note: An extra benefit is code reuse. This code leverages the existing function CalcAccelerationBiasForQDDot() (no need to create additional code).

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

please remove "efficient" from these comments. These methods are doing the algebra they are supposed to, with no apparent optimizations for speed. I'd use the word "efficient" if for instace there is a clever trick to reduce FLOPS or to use AVX instructions, but these are the algebraic expressions one would get from directly using the chain rule.

Done.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, remove these all trhouout. We do not do that anywhere in Drake, no need to introduce new conventions/preferences.

Done. Thanks for seeing these.

[vincekurtz]

@mitiguy My comments were addressed. Thanks Paul!

[mitiguy]

Thanks @vincekurtz -- your input is appreciated.
I think all is well for feature review pending @amcastro-tri

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

[amcastro-tri]

Thanks Paul!

I'd argue that you should address the repeated code now, since it really is very easy to do. Platfrom might disagree but usually we follow our rule of three (now 5!)

Reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, 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.cc line 461 at r1 (raw file):

Previously, mitiguy (Mitiguy) wrote…

Done. Yes. The tolerance should (and now is) hard-coded in a single place.

Reminder notes to myself:

1. This repetitious 5 times that this exact code is copy/pasted within this file will be reduced, e.g., to 2 in a follow-up PR.
2. The test if( abs(cp)<1.0e-3 ) cannot be moved within the function unless the function name ThrowSinceCosPitchIsNearZero() is also changed and the function takes an additional argument cp.
3. Moving the test to another function may result in an otherwise unnecessary additional function call/return.

I'd address this now. It literally costs nothing and it does go against our GSG. Naming and passing cp as arugments don't seem like a good reason to put this on hold?

---

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

Previously, mitiguy (Mitiguy) wrote…

Done. Language was changed.

Note: An extra benefit is code reuse. This code leverages the existing function CalcAccelerationBiasForQDDot() (no need to create additional code).

ah, that's fair. Say that then!
anyway, you do now. Thanks for the updated comment.

In terms of efficiency, I'd believe the handcrafted chain rule migh be more efficient. But again, no need to optimize something that doen't even show up in a full Anzu sim profile.

---

multibody/tree/rpy_ball_mobilizer.cc line 20 at r4 (raw file):

// is near zero, which occurs when the pitch angle ≈ π/2 ± n π (n=0, 1 2, …).
// An exception is thrown if a pitch angle is within ≈ 0.057° of a singularity.
constexpr double kToleranceCosPitchNearZero = 1.0e-3;

IMO, move this into ThrowSinceCosPitchIsNearZero() with updated signature to take cp.

[amcastro-tri]

+@rpoyner-tri for platform review please

[mitiguy]

Thanks @amcastro-tri. As I mentioned, the next PR will address the repeated code and consolidate (and reduce) the calculations of sine and cosines The repeated code was there before this PR and need to be fixed (agree and understood). Stay tuned. Also, except for the discussion on the repeated code, please let @rpoyner-tri know if this PR has your feature review LGTM.

Reviewable status: 2 unresolved discussions, LGTM missing from assignees rpoyner-tri(platform),amcastro-tri, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

[rpoyner-tri]

pending resolution of discussions

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

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

I'd address this now. It literally costs nothing and it does go against our GSG. Naming and passing cp as arugments don't seem like a good reason to put this on hold?

I concur with Alejandro. To your points:

1. 5x copy-paste of even a small item is certainly distracting, and an invitation to maintenance errors later.
2. Any functions to support this check should be either class-private or in an anonymous namespace in the cc file. The naming should be good/right, but needn't be public-api quality.
3. I don't buy arguments about micro-optimizations unless they are accompanied with measurements. As Sherm has found, the compiler is likely already smarter than we are.

---

multibody/tree/rpy_ball_mobilizer.cc line 527 at r4 (raw file):

  //
  // There are various ways to calculate v̇ = [ω̇x, ω̇y, ω̇z]ᵀ (the time-derivatives
  // of the generalized velocities). The calculation below is straighforward in

typo

Suggestion:

straightforward 

---

multibody/tree/rpy_ball_mobilizer.h line 249 at r4 (raw file):

  bool is_velocity_equal_to_qdot() const override { return false; }

 protected:

minor Not this PR's fault, but a class that is final (no further inheritance) never needs a protected section. All of these methods should just be private.

[mitiguy]

Thanks for the reviews @amcastro-tri and @rpoyner-tri .
I think all issues have been resolved.

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

---

multibody/tree/rpy_ball_mobilizer.h line 249 at r4 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

minor Not this PR's fault, but a class that is final (no further inheritance) never needs a protected section. All of these methods should just be private.

Done.

---

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

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

I concur with Alejandro. To your points:

1. 5x copy-paste of even a small item is certainly distracting, and an invitation to maintenance errors later.
2. Any functions to support this check should be either class-private or in an anonymous namespace in the cc file. The naming should be good/right, but needn't be public-api quality.
3. I don't buy arguments about micro-optimizations unless they are accompanied with measurements. As Sherm has found, the compiler is likely already smarter than we are.

Done.

---

multibody/tree/rpy_ball_mobilizer.cc line 20 at r4 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

IMO, move this into ThrowSinceCosPitchIsNearZero() with updated signature to take cp.

Done.

---

multibody/tree/rpy_ball_mobilizer.cc line 527 at r4 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

typo

Done.

[rpoyner-tri]

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

[amcastro-tri]

Thanks Paul, just a minor request on removing the unused function signature.

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

---

-- commits line 9 at r5:
fyi, remember to squash before mergin Paul

---

multibody/tree/rpy_ball_mobilizer.h line 332 at r5 (raw file):

  // 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 ThrowSinceCosPitchIsNearZero(const T& pitch,

This function is never called again, since the new signature is the one you use in code.
I suggest you remove this one and move the throw within ThrowIfCosPitchNearZero(). It also helps in that you only need to document one function instead of sharing this doc between the two.

Code quote:

 ThrowSinceCosPitchIsNearZero(

---

multibody/tree/rpy_ball_mobilizer.h line 336 at r5 (raw file):

  void ThrowIfCosPitchNearZero(const T& cos_pitch, const T& pitch_angle,
                               const char* function_name) const {
    if (abs(cos_pitch) < 1.0e-3)

nit, don't we need using std::abs; here for ADL? @rpoyner-tri, could you confirm?

also, consider moving into the cc file. If ThrowSinceCosPitchIsNearZero() does not inline, there's no hope for this one either.

---

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

  const T cp = cos(angles[1]);
  const char* function_name_less_Do = __func__ + 2;
  ThrowIfCosPitchNearZero(cp, angles[1], function_name_less_Do);

minor, this IMO is way too fancy for no good reason, and it depends on how __func__ formats things. Why not to write the function name you want directly? IMO it reads nicer also and, it fits into a single line.

Ditto in other places.

Suggestion:

ThrowIfCosPitchNearZero(cp, angles[1], "CalcNMatrix");

[amcastro-tri]

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

[rpoyner-tri]

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

---

multibody/tree/rpy_ball_mobilizer.h line 336 at r5 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, don't we need using std::abs; here for ADL? @rpoyner-tri, could you confirm?

also, consider moving into the cc file. If ThrowSinceCosPitchIsNearZero() does not inline, there's no hope for this one either.

Yes, using std::abs; follows the recommendation in common/autodiff_overloads.h.

[mitiguy]

On the one requested minor request -- please see the logic below. I have provided logic consistent with the style guide. If flawed, I'll make the requested change ASAP. If acceptable enough, I would like to proceed. The next PR builds off this one.

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

---

-- commits line 9 at r5:

Previously, amcastro-tri (Alejandro Castro) wrote…

fyi, remember to squash before mergin Paul

Thanks -- OK.

---

multibody/tree/rpy_ball_mobilizer.h line 332 at r5 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This function is never called again, since the new signature is the one you use in code.
I suggest you remove this one and move the throw within ThrowIfCosPitchNearZero(). It also helps in that you only need to document one function instead of sharing this doc between the two.

@amcastro-tri and @rpoyner-tri
Hmmmm... My intent was to follow the Drake Style Guide: Inline Functions. I appreciate your correction if what I am doing is inconsistent with the style guide.

My logic: It seems prudent to have two functions, namely ThrowIfCosPitchNearZero() and ThrowSinceCosPitchIsNearZero(). Implicitly inlining the function ThrowIfCosPitchNearZero() by leaving its definition in the header file is consistent with the style guide:
a. The function is non-virtual,
b. The function is not recursive,
c. The function is very short (less than 10 lines).
d. The function is called multiple times in this mobilizer class, and it should be fast. The function call overhead seems plausibly performance-sensitive when measured against the cost of the function as a whole (the whose function cost ideally becomes small as Sherm/all of us eliminate other slower performing functions).

Note: My understanding is that compilers typically inline in a "top-down" manner, meaning they inline the calling function first. If the compiler inlines the calling function, it may (or may not) inline functions called within the inlined function. There seems to be no reason to inline ThrowSinceCosPitchIsNearZero() as it just formats and throws an exception (it can be very slow). Alternatively, it seems logical and beneficial to encourage the compiler to inline ThrowIfCosPitchNearZero() since it is called in multiple places inside this mobilizer class, and the purpose of ThrowIfCosPitchNearZero() is to run a lightning quick test, and in the very rare situation in which a singularity is encountered, call ``ThrowSinceCosPitchIsNearZero()`.

---

multibody/tree/rpy_ball_mobilizer.h line 336 at r5 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Yes, using std::abs; follows the recommendation in common/autodiff_overloads.h.

Done. Fixed `using std::abs()'.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor, this IMO is way too fancy for no good reason, and it depends on how __func__ formats things. Why not to write the function name you want directly? IMO it reads nicer also and, it fits into a single line.

Ditto in other places.

Done. Sherm seemed to like the older version. I am fine with either.
I do like the new shorter single line.

[rpoyner-tri]

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

---

multibody/tree/rpy_ball_mobilizer.h line 332 at r5 (raw file):

Previously, mitiguy (Mitiguy) wrote…

@amcastro-tri and @rpoyner-tri
Hmmmm... My intent was to follow the Drake Style Guide: Inline Functions. I appreciate your correction if what I am doing is inconsistent with the style guide.

My logic: It seems prudent to have two functions, namely ThrowIfCosPitchNearZero() and ThrowSinceCosPitchIsNearZero(). Implicitly inlining the function ThrowIfCosPitchNearZero() by leaving its definition in the header file is consistent with the style guide:
a. The function is non-virtual,
b. The function is not recursive,
c. The function is very short (less than 10 lines).
d. The function is called multiple times in this mobilizer class, and it should be fast. The function call overhead seems plausibly performance-sensitive when measured against the cost of the function as a whole (the whose function cost ideally becomes small as Sherm/all of us eliminate other slower performing functions).

Note: My understanding is that compilers typically inline in a "top-down" manner, meaning they inline the calling function first. If the compiler inlines the calling function, it may (or may not) inline functions called within the inlined function. There seems to be no reason to inline ThrowSinceCosPitchIsNearZero() as it just formats and throws an exception (it can be very slow). Alternatively, it seems logical and beneficial to encourage the compiler to inline ThrowIfCosPitchNearZero() since it is called in multiple places inside this mobilizer class, and the purpose of ThrowIfCosPitchNearZero() is to run a lightning quick test, and in the very rare situation in which a singularity is encountered, call ``ThrowSinceCosPitchIsNearZero()`.

I buy all of Paul's assertions except (d). No one has shown actual benchmark measurements for this PR's changes, and frankly, I doubt that collecting them is worth the trouble.

It's worth doing a quick survey of all the functions in Drake named ThrowIf* -- there are a lot of them, and in fairly performance-sensitive places: system framework, multibody, etc. A very few of the call sites are wrapped in DRAKE_ASSERT_VOID(). Whether that was the result of benchmarks, or the "sherm complained" metric, I couldn't now say ;) .

I agree that inlining can't possibly be a red/blue function problem. If ever calling a non-inlined function prevented the caller's body from being inlined, almost no code would be inline-able. See also the survey of Drake ThrowIf.

What do I recommend then? In the end, I think we should do the simplest thing that could possibly work, especially in the absence of measurements: Fold all the code into the new-signature ThrowIfCosPitchNearZero, and move its body to the .cc file.

If some later optimization effort finds some of these to be hot spots (doubtful given their proximity to sines and cosines), then those later optimizers can complicate things as needed.