Skip to content
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

[multibody] Templatize body node #21862

Merged
merged 1 commit into from
Sep 19, 2024

Conversation

sherm1
Copy link
Member

@sherm1 sherm1 commented Aug 29, 2024

Templatizes BodyNodeImpl on concrete Mobilizer type to permit inlines rather than virtuals. This is part of the MbP performance epic #18442.

This PR doesn't change how we compute anything or attempt to reduce flop count. It just rearranges what's there to allow the compiler to generate better code. There are many further changes to come but I want to merge this now since the shuffling around would make it hard to review with substantive changes. This is a step towards a templatized kernel using only fixed-size objects, and reduces the amount of context-digging being done (but doesn't yet eliminate it all).

As a proof of concept, in addition to the newly-templatized BodyNodeImpl, I moved the position and velocity kinematics methods to take advantage of that. Everything else is unchanged. I measured 4% and 21% speedup for those with gcc, 11% and 32% with clang. Combined with the similar hacks to Frames in last week's #21853, position and velocity speedup is 28%/41% and 28%/44% resp. (See table below)
Note that these are the simplest computations with very few "up to 6" vectors and virtual function calls -- the dynamics computations have a lot more potential for speedup.

What's here

  • BodyNodeImpl (which was unused before) is now templated by concrete Mobilizer type, and partially filled in.
  • Every mobilizer gains two new inline, non-virtual methods calc_X_FM() and calc_V_FM(). The existing virtual methods are reimplemented as calls to the non-virtuals.
  • The per-node position & velocity kinematics functions are moved from the generic BodyNode to the templatized BodyNodeImpl where they can invoke the new inline methods directly.
  • The corresponding tree-walking algorithms in MultibodyTree are fixed to extract numerical info from the context outside the loop, and to allow the individual nodes to grab what they need (which they can do very efficiently).

What's not here

  • no algorithmic changes
  • no modifications to most of the existing computations (they are still stuck in BodyNode)
  • not every Context extraction at low levels has been removed yet, so the existing "kernel" is not yet purely numerical
  • other than modest speedups, there are no user-visible changes here (all code is internal::)

Notes for reviewers

  • I suggest starting with body_node_impl.h to see how it is templatized now, and the three virtual kinematics functions it implements. This is not CRTP -- it is very straightforward.
  • Then body_node_impl.cc has the implementations which are able to call directly into concrete mobilizer's inlines. These functions were moved over directly from BodyNode and then lightly modified to replace virtuals with inline calls.
  • Every mobilizer was changed in the same boilerplate manner to add the new inlines.

Benchmarking

CassieBench measures only multibody kernel time, in microseconds. (I did nothing to speed up Mass Matrix calculation here but clang sped it up anyway so I included those times.)

Test (gcc) Master Master* This PR Speedup Speedup*
Position only 2.68 3.58 2.58 4% 28%
Position&Velocity 5.55 7.52 4.41 21% 41%
Mass Matrix 12.5 14.5 12.2 2% 16%
Test (clang) Master Master* This PR Speedup Speedup*
Position only 2.52 3.12 2.25 11% 28%
Position&Velocity 5.19 6.3 3.55 32% 44%
Mass Matrix 9.55 10.7 8.88 7% 17%

* times from pre-#21853 which applied similar changes to Frames.

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

This change is Reviewable

@sherm1 sherm1 force-pushed the templatize_body_node branch 6 times, most recently from ac5cb91 to 3524248 Compare September 5, 2024 18:22
@sherm1 sherm1 added the release notes: none This pull request should not be mentioned in the release notes label Sep 5, 2024
Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge" (waiting on @sherm1)

a discussion (no related file):
BTW I was able to reproduce a failure locally on Jammy.

Here is my command line:

bazel test //multibody/plant:joint_locking_test --config=clang -c dbg --copt=-O2 --copt=-DNDEBUG --define=NO_CLARABEL=ON

The -c dbg --copt=-O2 --copt=-DNDEBUG part is turning on debug symbols, but still using an optimized build. (The error did not reproduce in a debug-only build without optimizations).

The --define=NO_CLARABEL=ON is a work-around until #21836 is fixed.

From gdb:

Program received signal SIGSEGV, Segmentation fault.
drake::multibody::internal::BodyNodeImpl<double, drake::multibody::internal::QuaternionFloatingMobilizer>::CalcVelocityKinematicsCache_BaseToTip (this=0x555557a57a60, context=..., pc=..., H_PB_W_cache=..., velocities=0x555557a6c058, vc=0x555557a643c0) at multibody/tree/body_node_impl.cc:170
170	  V_FM = mobilizer_->calc_V_FM(context, vm);
(gdb) bt
#0  drake::multibody::internal::BodyNodeImpl<double, drake::multibody::internal::QuaternionFloatingMobilizer>::CalcVelocityKinematicsCache_BaseToTip (this=0x555557a57a60, context=..., pc=..., H_PB_W_cache=..., velocities=0x555557a6c058, vc=0x555557a643c0) at multibody/tree/body_node_impl.cc:170
#1  0x0000555555c59e04 in drake::multibody::internal::MultibodyTree<double>::CalcVelocityKinematicsCache (this=<optimized out>, context=..., pc=..., vc=<optimized out>) at multibody/tree/multibody_tree.cc:1312

@sherm1 sherm1 force-pushed the templatize_body_node branch 2 times, most recently from af0beb1 to 99d0021 Compare September 10, 2024 00:40
Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)

a discussion (no related file):
BTW I guess I'm still subscribed to this "do not review" after posting my prior comment.

Anyway, here's a thought. Consider something like Eigen::Matrix<nq, 1, Eigen::DontAlign> or an Eigen::Block for the return type slices. Those are the canonical spellings of unaligned sub-views of slab memory.


@sherm1
Copy link
Member Author

sherm1 commented Sep 10, 2024

Arggh! Ubsan steered me straight on this one -- it was an Eigen alignment issue. All even-sized fixed Eigen vectors require 16-byte alignment. So if you try to take a plain double array like our velocity state v, and cleverly overlay fixed Eigen vectors on it to do per-mobilizer math (as I was attempting), a Vector1 works fine always but a Vector6 seg faults if:

  • the compiler generates SIMD instructions that have alignment restrictions, and
  • the mobilizer's v segment happens to start at an odd index into the overall v.

Replaced for now with ugly but serviceable std::array objects instead :(

@sherm1
Copy link
Member Author

sherm1 commented Sep 10, 2024

Thanks to @jwnimmer-tri for helping me reproduce this locally.

@sherm1
Copy link
Member Author

sherm1 commented Sep 10, 2024

@drake-jenkins-bot mac-arm-ventura-clang-bazel-experimental-release please

@sherm1
Copy link
Member Author

sherm1 commented Sep 10, 2024

Consider something like Eigen::Matrix<nq, 1, Eigen::DontAlign>

!!! That's just what I need. I went looking for that in Eigen's "alignment issues" documentation and didn't find it. Thanks! Will try.

@sherm1 sherm1 force-pushed the templatize_body_node branch 3 times, most recently from 923dccc to dcd2640 Compare September 10, 2024 20:41
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.

+@amcastro-tri for feature review, please. (See PR description for review suggestions.)

Reviewable status: 3 unresolved discussions, LGTM missing from assignee amcastro-tri, needs platform reviewer assigned, needs at least two assigned reviewers, labeled "do not merge"


multibody/tree/screw_mobilizer.h line 35 at r3 (raw file):

  T revolution_amount{z / screw_pitch};
  return revolution_amount * 2 * M_PI;
}

BTW these are just moved up from below so they can be used in now-inlined methods.

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.

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

a discussion (no related file):

Previously, sherm1 (Michael Sherman) wrote…

Consider something like Eigen::Matrix<nq, 1, Eigen::DontAlign>

!!! That's just what I need. I went looking for that in Eigen's "alignment issues" documentation and didn't find it. Thanks! Will try.

Eigen::DontAlign worked great. Updated performance figures in the PR description.


Copy link
Contributor

@amcastro-tri amcastro-tri left a comment

Choose a reason for hiding this comment

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

Reviewed 5 of 12 files at r8.
Reviewable status: 14 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)


multibody/tree/body_node_impl.h line 86 at r8 (raw file):

 private:
  // Given a pointer to the contiguous array of all q's in this system, returns
  // a reference to just the ones for this mobilizer, as a fixed-size vector.

nit, here and below, document precondition on the proper size of the arrays pinted to by these pointers.


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

  // Given this mobilizer's position coordinates q as a T*, overlay with the
  // appropriate-sized Eigen vector. See QVector comment re alignment.
  static Eigen::Map<const QVector> to_q_vector(const T* q_ptr) {

I forget if we discussed this, but why not to pass all qs by Eigen::Ref? smilarly for vs.

Code quote:

or(const T* q_ptr) {

multibody/tree/planar_mobilizer.h line 161 at r8 (raw file):

  /* Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame
   M measured and expressed in frame F as a function of the input velocity v. */
  SpatialVelocity<T> calc_V_FM(const systems::Context<T>&,

I guess we were expecting to probably pass parameters here as well?
But I think that's no longer true. So if all we need to pass are qs, maybe change the context with a Eigen::Ref?

ditto for all other mobilizers.

Code quote:

calc_V_FM(const systems::Context<T>&,

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

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


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I forget if we discussed this, but why not to pass all qs by Eigen::Ref? smilarly for vs.

Are you talking about the argument or the return value?

In any case, with Eigen::Ref you need to be somewhat careful to not accidentally copy things. It's a fine tool, but I think raw pointers and Map are a fair solution here.

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

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


multibody/tree/prismatic_mobilizer.h line 143 at r8 (raw file):

  // translational velocity v along this mobilizer's axis (see
  // translation_axis()).
  SpatialVelocity<T> calc_V_FM(const systems::Context<T>&,

BTW These new non-virtual functions now are a kind of "concept" that all mobilizer templates must implement, with exactly this signature, so that body_node_impl can call it.

I wonder if there is some way to document the required functions in mobilizer_impl.h. Maybe like this?

(We could write an API spec on the stubs too, I just didn't bother here.)

--- a/multibody/tree/mobilizer_impl.h
+++ b/multibody/tree/mobilizer_impl.h
@@ -163,6 +163,12 @@ class MobilizerImpl : public Mobilizer<T> {
     return Eigen::Map<HMatrix>(h_ptr->data());
   }
 
+  // These functions are required to be implemented by all subclasses, akin to
+  // pure virtual functions (but without the overhead of a vtable indirection).
+  math::RigidTransform<T> calc_X_FM(Eigen::Map<const QVector> q) const = delete;
+  SpatialVelocity<T> calc_V_FM(const systems::Context<T>&,
+                               Eigen::Map<const VVector> v) const = delete;
+
  protected:
   // Returns the zero configuration for the mobilizer.
   virtual Vector<double, kNq> get_zero_position() const {

Copy link
Contributor

@amcastro-tri amcastro-tri left a comment

Choose a reason for hiding this comment

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

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


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Are you talking about the argument or the return value?

In any case, with Eigen::Ref you need to be somewhat careful to not accidentally copy things. It's a fine tool, but I think raw pointers and Map are a fair solution here.

For the argument.
In general Eigen::Ref is Eigen's solution to being able to pass different Eigen types having the same layout.
For instance, we see in body_node_impl.cc:90, that we need to instantiate a Map before calling calc_V_FM(). I was wondering if passing by Eigen::Ref would cost the same and avoid having to worry about these explicit conversions.

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

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


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

For the argument.
In general Eigen::Ref is Eigen's solution to being able to pass different Eigen types having the same layout.
For instance, we see in body_node_impl.cc:90, that we need to instantiate a Map before calling calc_V_FM(). I was wondering if passing by Eigen::Ref would cost the same and avoid having to worry about these explicit conversions.

Yes, Eigen::Ref offers the upside of "I'll take anything!" so it's easier to use. The downside it that it can be less performant.

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.

Reviewable status: 15 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Yes, Eigen::Ref offers the upside of "I'll take anything!" so it's easier to use. The downside it that it can be less performant.

FYI I'm not liking the whole scheme now with Map in it (though the performance is good). I'm considering switching to T* at the low level and leaving optional mapping up to the concrete mobilizer. For example, the 1-dof mobilizers don't gain anything from working with Eigen::Map<const Vector<T, 1>> when they could work just with a scalar.

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

Reviewed all commit messages.
Reviewable status: 14 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @sherm1)


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

FYI I'm not liking the whole scheme now with Map in it (though the performance is good). I'm considering switching to T* at the low level and leaving optional mapping up to the concrete mobilizer. For example, the 1-dof mobilizers don't gain anything from working with Eigen::Map<const Vector<T, 1>> when they could work just with a scalar.

FWIW that sounds plausible to me.

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.

Reviewable status: 12 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)


multibody/tree/body_node.h line 97 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW At some convenient point in your PR train, please remove enable_clang_format_lint = False from the MbT BUILD file so that we never have this review chaff anymore.

Will do. It's really a pain this way. (As a standalone PR)


multibody/tree/mobilizer_impl.h line 157 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW I'm not sure that h_ptr is any more or less likely to be null than the q_ptr or v_ptr? Do we care about being consistent here?

That's just because we dereference h_ptr here to find the data. No deref is required for the q and v_ptrs so null is OK. Rethinking this anyway.

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.

Reviewable status: 12 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)


multibody/tree/body_node.h line 192 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Must be overridden.

That's what = 0 is for.

sherm1#7

Done (by Jeremy!)

Copy link
Contributor

@amcastro-tri amcastro-tri left a comment

Choose a reason for hiding this comment

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

Reviewed 2 of 3 files at r9.
Reviewable status: 12 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits)

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.

Thanks for the great reviews -- all comments addressed, PTAL.

Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)


multibody/tree/body_node_impl.h line 18 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW I imagine there is still more work to do here, but just double-checking that this TODO isn't solved yet?

Right, updated the message.


multibody/tree/body_node_impl.h line 26 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This overview is slightly out of date now. It's not wrong per se, but we're less focused now on the fixed sizes than actually just eating the mobilizer directly.

Done


multibody/tree/body_node_impl.h line 29 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Even in subclasses, I think we still prefer DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BodyNodeImpl); here for clarity.

Done


multibody/tree/body_node_impl.h line 57 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This cast is borderline for https://drake.mit.edu/styleguide/cppguide.html#Inline_Functions -- it comes down to whether a dynamic_cast is considered "cheap" or not. It's actually only eight instructions, ending with a call to __dynamic_cast function in the stdlib. However, given that this header file is only ever included from two cc files, I am OK if you'd rather leave it alone.

Done


multibody/tree/body_node_impl.h line 86 at r8 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, here and below, document precondition on the proper size of the arrays pinted to by these pointers.

Done


multibody/tree/body_node_impl.h line 131 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit We either need one more const, or a moustache on the non-const POD.

Done


multibody/tree/mobilizer_impl.h line 54 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

See https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html:

MapOptions specifies whether the pointer is Aligned or Unaligned. The default is Unaligned.

If the purpose of these typedefs is to be arguments to Eigen::Map, then the DontAlign is superfluous and thus confusing. Instead, we could amend the comment paragraph above to talk about the Map default of unaligned to help teach people what's going on.

Or, I wonder if doing using QVectorView = Eigen::Map<const Eigen::Matrix<T, kNq, 1>>; would be a more helpful sugar than what's here.

Possibly you also want to mark the HMatrix map as aligned. The extra one cycle latency probably washes out in the superscalar noise, but shrug.

Done. Since these are never overlaid directly now I don't need to say anything about their alignment. These are useful for allocating local variables of the right size when needed (some of which Eigen will align), and for properly parameterizing Eigen::Map when it is needed. I modified the get_H() methods to mark the Map as Aligned16.


multibody/tree/mobilizer_impl.h line 144 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

FWIW that sounds plausible to me.

Done. I nuked these methods as unnecessary now.


multibody/tree/mobilizer_impl.cc line 5 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Do we still need this here? It seems like it has moved into the individual mobilizers now.

Done


multibody/tree/planar_mobilizer.h line 161 at r8 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I guess we were expecting to probably pass parameters here as well?
But I think that's no longer true. So if all we need to pass are qs, maybe change the context with a Eigen::Ref?

ditto for all other mobilizers.

Right, we should be passing nothing but numerical arrays, Eigen vectors, or structs of those objects. Right now I still have to pass the Context because universal mobilizer uses it to recalculate Hw (unnecessarily) and I didn't want to rewrite that as part of the big shuffle here. I am going to be evolving all of these methods to a Context-free nirvana as we go, so this won't be the final signature.


multibody/tree/prismatic_mobilizer.h line 122 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

https://drake.mit.edu/styleguide/cppguide.html#Inline_Functions

None of these specific-mobilizer virtuals can move into the header. Ditto for both virtuals on all mobilizer subclasses.

(To be clear the new non-virtual functions like calc_X_FM are just fine, and a perfect use of inlining.)

Done. Back where they belong.


multibody/tree/prismatic_mobilizer.h line 143 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW These new non-virtual functions now are a kind of "concept" that all mobilizer templates must implement, with exactly this signature, so that body_node_impl can call it.

I wonder if there is some way to document the required functions in mobilizer_impl.h. Maybe like this?

(We could write an API spec on the stubs too, I just didn't bother here.)

--- a/multibody/tree/mobilizer_impl.h
+++ b/multibody/tree/mobilizer_impl.h
@@ -163,6 +163,12 @@ class MobilizerImpl : public Mobilizer<T> {
     return Eigen::Map<HMatrix>(h_ptr->data());
   }
 
+  // These functions are required to be implemented by all subclasses, akin to
+  // pure virtual functions (but without the overhead of a vtable indirection).
+  math::RigidTransform<T> calc_X_FM(Eigen::Map<const QVector> q) const = delete;
+  SpatialVelocity<T> calc_V_FM(const systems::Context<T>&,
+                               Eigen::Map<const VVector> v) const = delete;
+
  protected:
   // Returns the zero configuration for the mobilizer.
   virtual Vector<double, kNq> get_zero_position() const {

Done, partially. See the class doc for MoblizerImpl. For now I'll just grow the specification in a comment (these signatures are going to change for a while). I wonder if there is a way to enforce the implementations using actual C++20 concept directives?

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

Reviewed 2 of 3 files at r9, 23 of 23 files at r10, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)


multibody/tree/body_node_impl.h line 87 at r10 (raw file):

  // @pre `positions` is the full set of q's for this system
  const T* get_q(const T* positions) const {
    return  &positions[mobilizer_->position_start_in_q()];

nit Extra whitespace after return.


multibody/tree/planar_mobilizer.h line 41 at r10 (raw file):

  using MobilizerBase = MobilizerImpl<T, 3, 3>;
  using MobilizerBase::kNq, MobilizerBase::kNv, MobilizerBase::kNx;
  using typename MobilizerBase::QVector, typename MobilizerBase::VVector;

nit Are these Eigen typedefs here still necessary? They don't seem to be used...

Ditto for all of the concrete mobilizers.


multibody/tree/screw_mobilizer.h line 25 at r10 (raw file):

 the same relation, depended on the `screw_pitch` of a screw mobilizer. */
template <typename T>
inline T get_screw_translation_from_rotation(const T& theta,

(Edit: Ah oops, after typing this out I saw later that you just moved these. Still, no time like the present to make it right.)

These are not getters or setters, so are not allowed to use snake_case for the function name. I guess the problem here was that this code was written originally by an external contributor, and reviewers at the time didn't find everything.

My suggestion is to make them non-static member functions on the Mobilizer, and therefore lose screw_patch as an argument since the mobilizer knows its own pitch.

(I would also accept a TODO comment expressing your astonishment at the state of this code, with a call to action to clean it up in a future PR.)

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.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)


multibody/tree/body_node_impl.h line 87 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Extra whitespace after return.

Done


multibody/tree/planar_mobilizer.h line 41 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Are these Eigen typedefs here still necessary? They don't seem to be used...

Ditto for all of the concrete mobilizers.

Yes, they are currently used by BodyNodeImpl to learn the sizes from its ConcreteMobilizer template parameter. Also kNq and kNv are used in local mobilizer code to check sizes. VVector is used in some of the mobilizers now. I expect to use these locally more as more of the code gets inlined.


multibody/tree/screw_mobilizer.h line 25 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

(Edit: Ah oops, after typing this out I saw later that you just moved these. Still, no time like the present to make it right.)

These are not getters or setters, so are not allowed to use snake_case for the function name. I guess the problem here was that this code was written originally by an external contributor, and reviewers at the time didn't find everything.

My suggestion is to make them non-static member functions on the Mobilizer, and therefore lose screw_patch as an argument since the mobilizer knows its own pitch.

(I would also accept a TODO comment expressing your astonishment at the state of this code, with a call to action to clean it up in a future PR.)

Done. These are also used by ScrewJoint, so I left them in internal:: but cleaned them up.

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

:lgtm: but please let the feature reviewer catch back up with all of the rewrites before you merge.

Reviewed 4 of 4 files at r11, all commit messages.
Reviewable status: 3 unresolved discussions


multibody/tree/planar_mobilizer.h line 41 at r10 (raw file):

Yes, they are currently used by BodyNodeImpl ...

Not relevant AFAICT. These lines are just repeating the MobilizerImpl typedefs so that method bodies in these files can skip mention of the base class. BodyNode is already specifying the class qualifier, so would already see the base typedef in the base class.

Also kNq and kNv are used in local mobilizer code to check sizes.

The aim of my discussion was only the typedefs, not the constants.

I expect to use these locally more as more of the code gets inlined.

OK That's a fine reason to keep them.


multibody/tree/screw_mobilizer.h line 20 at r11 (raw file):

namespace internal {

/* These two methods are suitable for position, velocity, and acceleration

typo

Suggestion:

functions

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.

@amcastro-tri much hacking since you last saw this -- please take another look

Reviewable status: 3 unresolved discussions

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.

Reviewable status: 2 unresolved discussions


multibody/tree/screw_mobilizer.h line 20 at r11 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

Done

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

Reviewed 1 of 1 files at r12, all commit messages.
Reviewable status: 2 unresolved discussions

Copy link
Collaborator

@jwnimmer-tri jwnimmer-tri left a comment

Choose a reason for hiding this comment

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

Reviewed 1 of 1 files at r13, all commit messages.
Reviewable status: 2 unresolved discussions

Moved position & velocity kinematics to BodyNodeImpl.
Copy link
Contributor

@amcastro-tri amcastro-tri left a comment

Choose a reason for hiding this comment

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

Reviewed 20 of 23 files at r10, 3 of 4 files at r11, 1 of 1 files at r12, 1 of 1 files at r14, all commit messages.
Reviewable status: 1 unresolved discussion


multibody/tree/body_node_impl.h line 101 at r14 (raw file):

  // matrix. This matrix is 16-byte aligned because it is composed of
  // columns of Eigen::Vector6 objects which Eigen aligns.
  Eigen::Map<const HMatrix, Eigen::Aligned16> get_H(

question, non-blocking, if HMatrix is fixed size, should't Map have all the info it needs to align properly? do we still need to pass Eigen::Align16?

Copy link
Contributor

@amcastro-tri amcastro-tri left a comment

Choose a reason for hiding this comment

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

Per f2f, @sherm1 explained to me why the latest changes and tought me quite a bit about alignment.
:lgtm_strong:

Reviewable status: 1 unresolved discussion

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.

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


multibody/tree/body_node_impl.h line 101 at r14 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

question, non-blocking, if HMatrix is fixed size, should't Map have all the info it needs to align properly? do we still need to pass Eigen::Align16?

Per f2f: because Map is constructed with only a double* (which can be 8-byte aligned) it can't know at compile time that its argument has a better alignment than that unless you tell it explicitly.

@amcastro-tri amcastro-tri merged commit ebc55ba into RobotLocomotion:master Sep 19, 2024
9 checks passed
@amcastro-tri amcastro-tri deleted the templatize_body_node branch September 19, 2024 20:25
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.

3 participants