[multibody] Simplify frames in MultibodyPlant

[sherm1]

This is a small step towards a high-performance multibody kernel, see issue #18442. An important goal is to eliminate Context spelunking and virtual methods from the multibody kernel; this moves us part way there and provides a modest performance boost (benchmarks below).

Background

The Frame class hierarchy was originally designed to support frames on deformable bodies, supporting the notion that a frame's pose on its body might not be constant. We don't need that abstraction now -- all our Frames have fixed poses after parameters have been set (they are either RigidBodyFrames or FixedOffsetFrames). There is overhead associated with that abstraction -- virtual methods, runtime recalculations of pose, and copies rather than references.

What's here

This PR removes the deformable-frame abstraction and then takes advantage of that to cache Frame pose information as soon as parameters are set rather than at run time. Specifically:

• Removes useless FrameBase class.
• Caches frame pose-in-body and inverse.
• Replaces runtime virtuals and computations with inline cache references.
• Now no kernel function looks in the Context for Frame body poses.

What's not here

• There are still many places where a Context is being passed in to kernel functions. All of those need to be replaced with precalculated cache entries so that the kernel operates only on numerical quantities. TODOs for now.
• BodyNode needs to be templatized by concrete Mobilizer to allow fixed-size vectors and inline calls rather than virtual functions.

Release note

Although FrameBase was a public class it had no functionality and served only as the useless base class for Frame. There are no direct uses of FrameBase in Drake or Anzu, and no Python bindings. So although this is technically a breaking change I'm not deprecating it because there is ~zero chance anyone will be affected.

Benchmarking

CassieBench measures only multibody kernel time, in microseconds.

Test	Master/GCC	This PR	Speedup	Master/Clang	ThisPR	Speedup
MassMatrix	15.7	13.6	13%	11.5	10.4	10%
InverseDynamics	18.3	15.3	16%	13	11	15%
ForwardDynamics	40.8	38.1	7%	37.1	35.5	4%

AutoDiff and Expression benchmarks were not measurably affected by this change.

• Puget Xeon E5-2699 v4 @2.2GHz
• gcc 11.4.0
• clang 15.0.7

---

This change is 

[sherm1]

+@joemasterjohn please LMK if you have time and interest to feature review this. It simplifies the Frame classes and caches poses so we can use inline references. Provides a modest performance boost, see PR description for benchmark data.

Reviewable status: LGTM missing from assignee joemasterjohn, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", missing label for release notes (waiting on @sherm1)

[joemasterjohn]

Taking a look now.

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

[joemasterjohn]

Very nice! This is super clean, so not much to say on my part. I sprinkled a few thoughts, but all nits.

Reviewed 14 of 14 files at r1, all commit messages.
Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers

---

multibody/tree/frame_body_pose_cache.h line 3 at r1 (raw file):

#pragma once

#include <memory>

nit: Unused?

Code quote:

#include <memory>

---

multibody/tree/multibody_tree.cc line 1397 at r1 (raw file):

    }
    frame_body_poses->set_X_BF(body_pose_index_in_cache,
                               frame->CalcPoseInBodyFrame(context));

Super nit picky, but calling CalcPoseInBodyFrame() on each frame ends up recomputing the frame compositions all the way up the antecedents each time. If we processed the frames in topological order or just memoized X_BP for a frame F with parent P we could avoid recomputing X_BP when computing X_BF (= X_BP * X_PF). Of course this is only becomes a performance issue with super long chains of offset frames. Maybe just worth noting in a comment though? Whatever you think.

Code quote:

    frame_body_poses->set_X_BF(body_pose_index_in_cache,
                               frame->CalcPoseInBodyFrame(context));

---

multibody/tree/test/frames_test.cc line 290 at r1 (raw file):

      R_BG, kEpsilon));

  // Change the poses of the parent P and frame Q and make sure those are

nit: Is there an easy way to detect that a cached quantity has not been recomputed from the context of user level code like this? I was just thinking it would be nice to assert that a change to state doesn't recompute X_BF when EvalPoseInBodyFrame() gets called, to show the more granular dependency is working.

[joemasterjohn]

+@xuchenhan-tri for platform review, please.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee xuchenhan-tri(platform)

[sherm1]

Feature review comments addressed, PTAL. Thanks @joemasterjohn !

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

---

multibody/tree/frame_body_pose_cache.h line 3 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: Unused?

Done

---

multibody/tree/multibody_tree.cc line 1397 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

Super nit picky, but calling CalcPoseInBodyFrame() on each frame ends up recomputing the frame compositions all the way up the antecedents each time. If we processed the frames in topological order or just memoized X_BP for a frame F with parent P we could avoid recomputing X_BP when computing X_BF (= X_BP * X_PF). Of course this is only becomes a performance issue with super long chains of offset frames. Maybe just worth noting in a comment though? Whatever you think.

Yeah, I thought of that too. I talked myself into doing this way based on (a) only doing this when parameters change, (b) likely only short offset frame stacks so not too much recalculation and (c) no convenient data structure around for doing this in a nice order. Added a TODO for now, PTAL. WDYT?

---

multibody/tree/test/frames_test.cc line 290 at r1 (raw file):

Previously, joemasterjohn (Joe Masterjohn) wrote…

nit: Is there an easy way to detect that a cached quantity has not been recomputed from the context of user level code like this? I was just thinking it would be nice to assert that a change to state doesn't recompute X_BF when EvalPoseInBodyFrame() gets called, to show the more granular dependency is working.

Done. Good idea. Requires a little caching-fu.

[xuchenhan-tri]

Platform with one comment.

Reviewed 13 of 14 files at r1, 3 of 3 files at r2, all commit messages.
Reviewable status: 2 unresolved discussions

---

multibody/tree/frame_base.h line 51 at r1 (raw file):

/// @tparam_default_scalar
template <typename T>
class FrameBase : public MultibodyElement<T> {

This class is a part of public APIs. Is there a good reason why we aren't deprecating this? In any case, the release note label should be updated.

[joemasterjohn]

Reviewed 3 of 3 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion

---

multibody/tree/multibody_tree.cc line 1397 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Yeah, I thought of that too. I talked myself into doing this way based on (a) only doing this when parameters change, (b) likely only short offset frame stacks so not too much recalculation and (c) no convenient data structure around for doing this in a nice order. Added a TODO for now, PTAL. WDYT?

Totally agree. Thanks for the comment.

---

multibody/tree/test/frames_test.cc line 290 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Done. Good idea. Requires a little caching-fu.

Awesome, looks great. I thought it might be doable given the access to the MultibodyTreeSystem in this unit test.

[sherm1]

Reviewable status: 1 unresolved discussion

---

multibody/tree/frame_base.h line 51 at r1 (raw file):

Previously, xuchenhan-tri wrote…

This class is a part of public APIs. Is there a good reason why we aren't deprecating this? In any case, the release note label should be updated.

Done, thanks. Good point. Technically this is a breaking change so I changed the release note to that. However, there were no uses of this class in Drake or Anzu, no Python bindings, and no functionality so I believe it is harmless and not worth deprecating. I added a note about that in the PR description for the next release, PTAL.

[sherm1]

Thanks, @xuchenhan-tri -- I addressed your comment, WDYT?

Reviewable status: 1 unresolved discussion

[xuchenhan-tri]

as long as you are confident that no one is depending on the FrameBase class.

Reviewable status: 1 unresolved discussion

---

-- commits line 4 at r2:
BTW, I think the release engineer only sees the commit message (but not the PR description), consider moving the note about the breaking change here.

---

multibody/tree/frame_base.h line 51 at r1 (raw file):
If we are sure that no one is relying this, then I think a breaking change is fine. Otherwise, we should deprecate because our guidelines do say that a breaking change is "very rare".

On very rare occasions it is impractical, too expensive, or logically unsound to meet the full three month deprecation window. In these cases the change will be announced in the “Breaking changes” section of the release notes.

[sherm1]

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

---

-- commits line 4 at r2:

Previously, xuchenhan-tri wrote…

BTW, I think the release engineer only sees the commit message (but not the PR description), consider moving the note about the breaking change here.

Done

---

multibody/tree/frame_base.h line 51 at r1 (raw file):

Previously, xuchenhan-tri wrote…

If we are sure that no one is relying this, then I think a breaking change is fine. Otherwise, we should deprecate because our guidelines do say that a breaking change is "very rare".

On very rare occasions it is impractical, too expensive, or logically unsound to meet the full three month deprecation window. In these cases the change will be announced in the “Breaking changes” section of the release notes.

I know we're not using it internally, and since the class has no functionality there is no sane reason to mention it rather than starting with Frame as in all our examples. So IMO no deprecation needed.

[xuchenhan-tri]

Reviewed all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees joemasterjohn,xuchenhan-tri(platform)

---

multibody/tree/frame_base.h line 51 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I know we're not using it internally, and since the class has no functionality there is no sane reason to mention it rather than starting with Frame as in all our examples. So IMO no deprecation needed.

SGTM!