Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from

Conversation

sherm1
Copy link
Member

@sherm1 sherm1 commented Jun 2, 2025

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 Reviewable

@sherm1 sherm1 added status: do not merge status: do not review release notes: none This pull request should not be mentioned in the release notes labels Jun 2, 2025
@sherm1
Copy link
Member Author

sherm1 commented Jun 2, 2025

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 sherm1 force-pushed the some_performance_tweaks branch from 65dd9cc to 7036af3 Compare June 2, 2025 22:40
@sherm1 sherm1 marked this pull request as ready for review June 2, 2025 22:40
Copy link
Member Author

@sherm1 sherm1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

+@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

Copy link
Contributor

@mitiguy mitiguy left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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 sherm1 force-pushed the some_performance_tweaks branch from 7036af3 to 7fdf775 Compare June 3, 2025 23:33
Copy link
Member Author

@sherm1 sherm1 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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):

Previously, mitiguy (Mitiguy) wrote…

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?

Done.


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

Previously, mitiguy (Mitiguy) wrote…

via World Frame.

Done.


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

Previously, mitiguy (Mitiguy) wrote…

Here and below, consider something like
Mcba_viaWorld

Done (using via_W with a note about it)


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

Previously, mitiguy (Mitiguy) wrote…

Consider
M_viaWorld

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

Done. Using "via_id" to keep it short but with a comment saying what it is.


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

Previously, mitiguy (Mitiguy) wrote…

// CalcMassMatrixOffDiagonalViaWorldHelper().

Done.


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

Previously, mitiguy (Mitiguy) wrote…

// CalcMassMatrixContributionViaWorldHelper().

Done.


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

Previously, mitiguy (Mitiguy) wrote…

CalcMassMatrixOffDiagonalViaWorldDispatcher

Done.


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

Previously, mitiguy (Mitiguy) wrote…

Here and blow.
DECLARE_MASS_MATRIX_OFF_DIAGONAL_BLOCK_VIA_WORLD

Done.


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

Previously, mitiguy (Mitiguy) wrote…

CalcMassMatrix() (via World Frame),

Done.


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

Previously, mitiguy (Mitiguy) wrote…

SPECIALIZE_MASS_MATRIX_IN_WORLD_DISPATCHER(Bnv) to
SPECIALIZE_MASS_MATRIX_VIA_WORLD_DISPATCHER(Bnv)

Done.


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

Previously, mitiguy (Mitiguy) wrote…

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

Done.


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

Previously, mitiguy (Mitiguy) wrote…

Here and below.
SPECIALIZE_MASS_MATRIX_VIA_WORLD_DISPATCHER(1);

Done.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
priority: medium release notes: none This pull request should not be mentioned in the release notes
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants