Adds M-frame methods for curvilinear mobilizer.

[sherm1]

This is a yak shave for the upcoming M-frame inverse dynamics PR (see #22253).

The new Curvilinear mobilizer (and its underlying PiecewiseConstantCurvatureTrajectory) provide velocity, acceleration, and force projection in the "fixed" frame rather than the moving M frame. Additional methods required for M frame support are added here. Incidentally, everything is much nicer in M for this mobilizer.

There was a terminology conflict between the trajectory which used frames A (fixed) and F (moving) and the mobilizer -- all mobilizers use frames F (fixed) and M (moving). I renamed the moving frame in the trajectory to M to avoid the inevitable disaster of using "F" for opposite meanings!

All code here is internal so there is no release note needed.

cc @mshalm

---

This change is 

[sherm1]

+@amcastro-tri for feature review, please
@mshalm please feel free to comment also

You may want to start with the comment in mobilizer_impl.h to see the definition of the functions that have to be added.

Reviewable status: LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

[amcastro-tri]

This looks great Sherm! I love the updated docs.
As for the newly introduced APIs for this mobilizer only, I'd much rather prefer that to be ported separately when you land your M-frame machinery.

Reviewed 7 of 8 files at r1, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/tree/curvilinear_mobilizer.cc line 140 at r1 (raw file):

  const T v(1.);
  // Computes tau = H_FM_M(q)⋅F_Mo_M, equivalent to V_FM_M(q, 1)⋅F_BMo_M.
  tau[0] = calc_V_FM_M(X_FM, q, &v).dot(F_BMo_M);

fyi, this is ρ(s)⋅ Tz_M + Fx_M if you wanted to optimize it.
Obviously probably no need for it so non-blocking

---

multibody/tree/mobilizer_impl.h line 71 at r1 (raw file):

  // Returns A_FM_M = H_FM_M(q)⋅vdot + Hdot_FM_M(q,v)⋅v
  SpatialAcceleration<T> calc_A_FM_M(const math::RigidTransform<T>& X_FM,
                                     const T* q, const T* v, const T* vdot)

I understand you want to document the M frame for Mathew, but IMO it doesn't justify landing this code without the big PR that introduces these APIs.

I do love the documentation updates in this PR, but I'd stop there. This can land with the rest of the M-frame PR train separately for a review in context (e.g., Even though I can guess, I have no idea why passing X_FM is a good idea when in principle we should be working with that quantity in operator form, not explicitly, I think?. Again, not to be discussed here, I don't think)

---

multibody/tree/curvilinear_mobilizer.h line 30 at r1 (raw file):

 The path is specified as a PiecewiseConstantCurvatureTrajectory, refer to that
 class documentation for further details on parameterization, conventions and
 notation used. (The "A" frame there corresponds to the "F" frame here; "M" is

That bottered you enough that you refactored "F -> M" in this PR. Consider refactoring "A -> F" as well?

---

multibody/tree/curvilinear_mobilizer.h line 38 at r1 (raw file):

 frame M is defined according to the convention documented in
 PiecewiseConstantCurvatureTrajectory. That is, axis Mx is the tangent to the
 trajectory, Mz equals the (constant) normal p̂ to the plane, and My = Mz⨯ Mx.

nit, spacing

Code quote:

 Mz⨯ Mx.

---

multibody/tree/curvilinear_mobilizer.h line 58 at r1 (raw file):

 This is of course much nicer in M:
    H_FM_M(q) = [0 0 ρ(q) 1 0 0]ᵀ     Hdot_FM_M = [ 0₆ ]ᵀ

yay for M-frame math!

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 73 at r1 (raw file):

  spatial_velocity.translational() = R_AM.col(kCurveTangentIndex) * s_dot;

  return spatial_velocity;  // V_AM_A

here instead of rewriting the function below and its comments, I'd just write return R_AM * CalcSpatialVelocityInM()

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 126 at r1 (raw file):

      R_AM.col(kCurveTangentIndex) * s_ddot;

  return spatial_acceleration;  // A_AM_A

ditto comment for V_AM_A

[sherm1]

Thanks, Alejandro. All comments addressed, PTAL.

As for the newly introduced APIs for this mobilizer only ...

I'm trying to manage the size of PRs in this train to get good-quality reviews (like this one!) I intentionally pulled out the Curvilinear changes since they are more complicated and specialized than most mobilizers. The math & optimizations in the M-frame stuff here have a lot more to do with the specifics of Curvilinear than they do with the general M-frame algorithmic changes. IMO this is the right place to put all the Curvilinear-related changes (and I'm very happy with your detailed review comments on them). All the code & docs here are internal and easily changed later, but fully unit tested so able to stand alone even though not used elsewhere yet. There is a lot more code coming with a variety of twists and turns that need careful review and have nothing to do with mobilizer specifics -- I think we should get Curvilinear out of the way now. WDYT?

Reviewable status: 5 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 73 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

here instead of rewriting the function below and its comments, I'd just write return R_AM * CalcSpatialVelocityInM()

Done (duh!). This is why it's so great to get fresh eyes on code. I produced the "InM" methods by copying the other ones -- never occurred to me to go back and nuke the originals!

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 126 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ditto comment for V_AM_A

Done

---

multibody/tree/curvilinear_mobilizer.h line 30 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

That bottered you enough that you refactored "F -> M" in this PR. Consider refactoring "A -> F" as well?

I did think about it! Reversing the meaning of F in one PR is too much -- here I was able to make sure I got rid of every "F" so it's easy to see if it's right. A->F could be done in a subsequent PR. But I don't think it is actually worth worrying about now. It's easy (IMO) to think "A is F" -- much harder keep straight "A is F and F is M" while reviewing complicated frame-oriented math full of F's.

---

multibody/tree/curvilinear_mobilizer.h line 38 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, spacing

Done

---

multibody/tree/curvilinear_mobilizer.h line 58 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

yay for M-frame math!

:) it's so nice!

---

multibody/tree/curvilinear_mobilizer.cc line 140 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

fyi, this is ρ(s)⋅ Tz_M + Fx_M if you wanted to optimize it.
Obviously probably no need for it so non-blocking

Done, nice! This required exposing a public curvature(s) function in the trajectory class but that's a good addition, PTAL there.

---

multibody/tree/mobilizer_impl.h line 71 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I understand you want to document the M frame for Mathew, but IMO it doesn't justify landing this code without the big PR that introduces these APIs.

I do love the documentation updates in this PR, but I'd stop there. This can land with the rest of the M-frame PR train separately for a review in context (e.g., Even though I can guess, I have no idea why passing X_FM is a good idea when in principle we should be working with that quantity in operator form, not explicitly, I think?. Again, not to be discussed here, I don't think)

See my comment above for why I think it is important to get the Curvilinear code in now. I added a note making the situation more clear, PTAL.

[amcastro-tri]

up to you, I was trying to decouple PRs and simplify the review. If you want this PR as is I can of course review the new MobilizerImpl APIs (I already left a question for you below)

Reviewed 1 of 8 files at r1, 6 of 6 files at r2.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/tree/curvilinear_mobilizer.h line 30 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I did think about it! Reversing the meaning of F in one PR is too much -- here I was able to make sure I got rid of every "F" so it's easy to see if it's right. A->F could be done in a subsequent PR. But I don't think it is actually worth worrying about now. It's easy (IMO) to think "A is F" -- much harder keep straight "A is F and F is M" while reviewing complicated frame-oriented math full of F's.

Interesting to see how your mind works man. IMO symbols didn't necessarily need to match between the trajectory class and the mobilizer so not even "F/M" bothered me. Anyway, non-blocking.

---

multibody/tree/curvilinear_mobilizer.cc line 140 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done, nice! This required exposing a public curvature(s) function in the trajectory class but that's a good addition, PTAL there.

noise!

---

multibody/tree/mobilizer_impl.h line 71 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

See my comment above for why I think it is important to get the Curvilinear code in now. I added a note making the situation more clear, PTAL.

what I am proposing is that you do land the cleanup for curvilinear code.
Along that, I do propose you do not land the new calc_foo() in the moblizer impl. That's not a requirement for this PR.

---

multibody/tree/mobilizer_impl.h line 65 at r2 (raw file):

  // Returns A_FM_M = H_FM_M(q)⋅vdot + Hdot_FM_M(q,v)⋅v
  SpatialAcceleration<T> calc_A_FM_M(const math::RigidTransform<T>& X_FM,

if you plan to land this new API in this PR, could you elaborate (in Reviewable) why we'd pass a pressuablly expensive and easy to optimize object like X_FM as an argument? e.g. for revolute joints Featherstone writes operations with X_FM in operator form, never forming the 4x4 expensive pose object explicitly.

[sherm1]

Ah, I misunderstood your comment. No need to add the MobilizerImpl API spec yet, I just want the Curvilinear computations out of the way. Fixed, PTAL.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/tree/curvilinear_mobilizer.h line 30 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Interesting to see how your mind works man. IMO symbols didn't necessarily need to match between the trajectory class and the mobilizer so not even "F/M" bothered me. Anyway, non-blocking.

Yeah, it's very strange in here!

---

multibody/tree/curvilinear_mobilizer.cc line 140 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

noise!

I totally agree that the performance change here is just noise. But, speaking of how my mind works ... I feel it's important to set a good example of how to make use of the M frame stuff to simplify the low-level computations where possible. So though it's unlikely to matter here it could very well matter in some code that is cargo-culted from here. Doing it this way is just good hygiene when the whole point of these M-frame functions is to squeeze out more speed. I'm glad you suggested it.

As it happens, the Curvilinear mobilizer is one of the cleanest examples where working in the M frame is so much nicer than the F frame.

---

multibody/tree/mobilizer_impl.h line 71 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

what I am proposing is that you do land the cleanup for curvilinear code.
Along that, I do propose you do not land the new calc_foo() in the moblizer impl. That's not a requirement for this PR.

Done, gotcha -- I missed your point before, sorry. No need for a general definition of the API now.

---

multibody/tree/mobilizer_impl.h line 65 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

if you plan to land this new API in this PR, could you elaborate (in Reviewable) why we'd pass a pressuablly expensive and easy to optimize object like X_FM as an argument? e.g. for revolute joints Featherstone writes operations with X_FM in operator form, never forming the 4x4 expensive pose object explicitly.

Done. I removed the new definitions for now. The Curvilinear code is fine without all this.

BTW the X_FM is just a crutch so I could get the M-frame measurements done without writing a bunch of unneeded code. Because these are inline methods templated on mobilizer type, none of the M-frame code will work unless all mobilizers provide these methods (because calls to them show up in the M-frame algorithms). A few of the existing Drake mobilizer types (especially U-joint) require a lot of work to implement M frame methods. Passing X_FM let me hack those in terms of the old F frame methods, which doesn't matter for CassieBench timings.

For most of the mobilizers in the prototype code the supplied X_FM isn't used at all, and since everything is inline the compiler eliminates any cost of having it in the signature. But it shouldn't be there at all, I agree. I had a TODO in the comment here to remove X_FM altogether once we fully commit to M frame implementations. It will just require some pounding on obscure mobilizer types.

[sherm1]

+@rpoyner-tri for platform review per rotation, please

Reviewable status: 2 unresolved discussions, LGTM missing from assignees amcastro-tri,rpoyner-tri(platform)

[amcastro-tri]

This is great @sherm1, thanks!

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee rpoyner-tri(platform)

---

multibody/tree/curvilinear_mobilizer.h line 30 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yeah, it's very strange in here!

😆
If you could only see in here!

---

multibody/tree/curvilinear_mobilizer.cc line 140 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I totally agree that the performance change here is just noise. But, speaking of how my mind works ... I feel it's important to set a good example of how to make use of the M frame stuff to simplify the low-level computations where possible. So though it's unlikely to matter here it could very well matter in some code that is cargo-culted from here. Doing it this way is just good hygiene when the whole point of these M-frame functions is to squeeze out more speed. I'm glad you suggested it.

As it happens, the Curvilinear mobilizer is one of the cleanest examples where working in the M frame is so much nicer than the F frame.

bahaha! Sorry, I picked that up from Eric Cousineau's texts. "Noise" (maybe bad choice of spelling) is Eric's slang for "nice" (though pronounced "noise") 😆

(Don't ever text with the younger generations!)

---

multibody/tree/mobilizer_impl.h line 65 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done. I removed the new definitions for now. The Curvilinear code is fine without all this.

BTW the X_FM is just a crutch so I could get the M-frame measurements done without writing a bunch of unneeded code. Because these are inline methods templated on mobilizer type, none of the M-frame code will work unless all mobilizers provide these methods (because calls to them show up in the M-frame algorithms). A few of the existing Drake mobilizer types (especially U-joint) require a lot of work to implement M frame methods. Passing X_FM let me hack those in terms of the old F frame methods, which doesn't matter for CassieBench timings.

For most of the mobilizers in the prototype code the supplied X_FM isn't used at all, and since everything is inline the compiler eliminates any cost of having it in the signature. But it shouldn't be there at all, I agree. I had a TODO in the comment here to remove X_FM altogether once we fully commit to M frame implementations. It will just require some pounding on obscure mobilizer types.

I see, thanks for the explanation! can't wait to see all this cool stuff to land!

---

multibody/tree/curvilinear_mobilizer.h line 149 at r3 (raw file):

  /* Alternate function that returns the spatial velocity expressed in M. */
  SpatialVelocity<T> calc_V_FM_M(const math::RigidTransform<T>&, const T* q,

IMO these don't need to land if not used, but I'll let platform decide.
Acording to my understanding of our best pracices, we do never land code that does not get exercised (99% of the times I do agree with that)

[sherm1]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee rpoyner-tri(platform)

---

multibody/tree/curvilinear_mobilizer.h line 149 at r3 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

IMO these don't need to land if not used, but I'll let platform decide.
Acording to my understanding of our best pracices, we do never land code that does not get exercised (99% of the times I do agree with that)

Although internal, these are unit tested and I need them in order to move forward with the rest of the M-frame PR train. IMO they deserve separate review from the many other related mobilizer functions coming next.

---

multibody/tree/curvilinear_mobilizer.cc line 140 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

bahaha! Sorry, I picked that up from Eric Cousineau's texts. "Noise" (maybe bad choice of spelling) is Eric's slang for "nice" (though pronounced "noise") 😆

(Don't ever text with the younger generations!)

:) So I wrote that whole paragraph for nothing!

[rpoyner-tri]

Reviewed 2 of 8 files at r1, 4 of 6 files at r2, 2 of 2 files at r3, all commit messages.
Reviewable status: 2 unresolved discussions

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 102 at r3 (raw file):

  multibody::SpatialAcceleration<T> spatial_acceleration;

  // The angular velocity is ρᵢ⋅Mz⋅ds/dt. As Mz is constant everywyere, and ρᵢ

typo

Suggestion:

everywhere

[sherm1]

Reviewable status: complete! all discussions resolved, LGTM from assignees rpoyner-tri(platform),amcastro-tri

---

common/trajectories/piecewise_constant_curvature_trajectory.cc line 102 at r3 (raw file):

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

typo

Done

[rpoyner-tri]

Reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees rpoyner-tri(platform),amcastro-tri