[multibody] Some minor performance tweaks & prep for M-frame algorithms #23061

sherm1 · 2025-06-02T19:09:11Z

During development of M-frame algorithms in #22253 I made many small changes in performance, testing, formatting, and documentation that applied to both the new M-frame algorithms and the existing W-frame ones. This PR extracts all the changes from that branch that affect existing code (i.e. none of the M-frame stuff), with some small performance improvements, mostly via inlining of some small inertia methods.

The main purpose of this PR is to allow the upcoming M-frame PRs (which will be a lot of code) to be focused on the new algorithms without the distraction of generic changes.

The notation changes in the Composite Body Inertia are similar to those in #23036 -- avoid the confusion caused by multiple uses of "C" to mean "Composite" and "Child". This notation is now used consistently for both the existing W-frame algorithm and faster M-frame algorithm (see #22253). Also, a number of names now add "in_world" or "in_W", to distinguish them from the "in_M"s of the near future. A few other implied frames are made explicit in names and documentation.

The Cassie benchmark now separates out Composite Body Inertia timing from Mass Matrix times, and adds timing for the (very slow) Mass Matrix via Inverse Dynamics method.

All the visible code changes are internal:: so no release notes are needed.

This change is

sherm1 · 2025-06-02T22:06:07Z

Here are measurements taken with this branch on 2025-06-02 on my old Puget (Xeon ES-2650 [email protected], gcc 11.4.0). The first group shows that this PR helps a little with Composite Body Inertia (used in Mass Matrix calculation). The second group is just to track overall progress since the start of the MbP speedup project (vs. this PR). These are Cassiebench times in μs. (Note: 50% speedup means 2X faster run time)

	P	P+CBI	P+CBI+MM	P + V	P + V + ID	P + V + FD
master	2.2	7.05	12	4.33	12.9	40.8
this PR	2.17	6.14	11.2	4.33	12.8	40.7
speedup	1%	13%	7%	--	1%	--

start of project	4.93	10.1	18.5	9.58	21.6	47.3
speedup	56%	39%	39%	55%	41%	14%

P=position kinematics, V=velocity kinematics, CBI=composite body inertia, MM=mass matrix, ID=inverse dynamics, FD=forward dynamics (I haven't worked on FD yet).

sherm1

+@mitiguy for feature review, please (mostly notation, documentation, and name changes)

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

mitiguy

@sherm1 I am sending some preliminary feedback early.

Reviewed 6 of 31 files at r1, 1 of 2 files at r2, all commit messages.
Reviewable status: 12 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers

multibody/plant/test/multibody_plant_test.cc line 2994 at r2 (raw file):

  const double theta = M_PI / 3;

  MatrixX<double> M_id(1, 1), M_in_world(1, 1);

Consider
M_viaWorld

Also, since id is so connected to Identification, perhaps change
M_id to M_invDyn

multibody/tree/body_node.h line 299 at r2 (raw file):

  // sweep down to World filling in its diagonal contributions as in the
  // inner loop of algorithm 9.3, using the appropriate
  // CalcMassMatrixOffDiagonalInWorldHelper().

// CalcMassMatrixOffDiagonalViaWorldHelper().

multibody/tree/body_node.h line 300 at r2 (raw file):

  // inner loop of algorithm 9.3, using the appropriate
  // CalcMassMatrixOffDiagonalInWorldHelper().
  virtual void CalcMassMatrixContributionInWorld_TipToBase(

// CalcMassMatrixContributionViaWorldHelper().

multibody/tree/body_node.h line 310 at r2 (raw file):

  // contribute here). This allows us to use fixed-size 2d matrices in the
  // implementation as we sweep the inboard bodies. Use the separate
  // dispatcher class CalcMassMatrixOffDiagonalInWorldDispatcher (defined below)

CalcMassMatrixOffDiagonalViaWorldDispatcher

Code quote:

CalcMassMatrixOffDiagonalInWorldDispatcher

multibody/tree/body_node.h line 312 at r2 (raw file):

  // dispatcher class CalcMassMatrixOffDiagonalInWorldDispatcher (defined below)
  // to generate the properly sized call for body B(k).
#define DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_IN_WORLD(Bnv)            \

Here and blow.
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_VIA_WORLD

multibody/tree/body_node.h line 696 at r2 (raw file):

};

// During CalcMassMatrix() (in World), this dispatcher is invoked by the

CalcMassMatrix() (via World Frame),

multibody/tree/body_node.h line 702 at r2 (raw file):

class CalcMassMatrixOffDiagonalInWorldDispatcher;

#define SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER(Bnv)                    \

SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER(Bnv) to
SPECIALIZE_MASS_MATRIX_VIA_WORLD_DISPATCHER(Bnv)

multibody/tree/body_node.h line 704 at r2 (raw file):

#define SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER(Bnv)                    \
  template <typename T>                                                    \
  class CalcMassMatrixOffDiagonalInWorldDispatcher<T, Bnv> {               \

class CalcMassMatrixOffDiagonalInWorldDispatcher<T, Bnv> to
class CalcMassMatrixOffDiagonalViaWorldDispatcher<T, Bnv>

multibody/tree/body_node.h line 715 at r2 (raw file):

  }

SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER(1);

Here and below.
SPECIALIZE_MASS_MATRIX_VIA_WORLD_DISPATCHER(1);

Code quote:

SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER(1);

multibody/plant/test/multibody_plant_mass_matrix_test.cc line 26 at r2 (raw file):

// We verify the computation of the mass matrix by comparing two significantly
// different implementations:
//   - CalcMassMatrix(): Composite Body Algorithm, in the World frame.

Consider a more detailed discussion of "in the World frame" this somewhere in the code (or elsewhere), perhaps with a link to it.

Per f2f, consider changing "in the World frame" to "via World Frame" or something similar.
Does "via M Frame" sound reasonable too?

multibody/plant/test/multibody_plant_mass_matrix_test.cc line 63 at r2 (raw file):

  // CalcMassMatrixViaInverseDynamics().
  void VerifyMassMatrixComputation(const Context<double>& context) {
    // Compute mass matrix via the Composite Body Algorithm in World.

via World Frame.

Code quote:

in World.

multibody/plant/test/multibody_plant_mass_matrix_test.cc line 64 at r2 (raw file):

  void VerifyMassMatrixComputation(const Context<double>& context) {
    // Compute mass matrix via the Composite Body Algorithm in World.
    MatrixX<double> Mcba_in_W(plant_.num_velocities(), plant_.num_velocities());

Here and below, consider something like
Mcba_viaWorld

Code quote:

 Mcba_in_W

sherm1

All the "via" changes made, PTAL.

Reviewable status: 12 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers

multibody/plant/test/multibody_plant_mass_matrix_test.cc line 26 at r2 (raw file):