-
Notifications
You must be signed in to change notification settings - Fork 1.3k
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
[multibody] Templatize body node #21862
Conversation
ac5cb91
to
3524248
Compare
3524248
to
b11a350
Compare
There was a problem hiding this 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
af0beb1
to
99d0021
Compare
There was a problem hiding this 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.
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
Replaced for now with ugly but serviceable |
Thanks to @jwnimmer-tri for helping me reproduce this locally. |
@drake-jenkins-bot mac-arm-ventura-clang-bazel-experimental-release please |
99d0021
to
a0b568a
Compare
!!! 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. |
923dccc
to
dcd2640
Compare
There was a problem hiding this 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.
There was a problem hiding this 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.
dcd2640
to
e1ff828
Compare
There was a problem hiding this 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>&,
There was a problem hiding this 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.
There was a problem hiding this 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 {
There was a problem hiding this 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 andMap
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.
There was a problem hiding this 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 generalEigen::Ref
is Eigen's solution to being able to pass different Eigen types having the same layout.
For instance, we see inbody_node_impl.cc:90
, that we need to instantiate aMap
before callingcalc_V_FM()
. I was wondering if passing byEigen::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.
There was a problem hiding this 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.
There was a problem hiding this 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.
There was a problem hiding this 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 theq_ptr
orv_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.
There was a problem hiding this 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.
Done (by Jeremy!)
There was a problem hiding this 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)
15071b8
to
9049eba
Compare
There was a problem hiding this 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 theDontAlign
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 thecontext
with aEigen::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?
There was a problem hiding this 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.)
9049eba
to
ecad91a
Compare
There was a problem hiding this 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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this 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
ecad91a
to
4d408ed
Compare
There was a problem hiding this 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
There was a problem hiding this 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
4d408ed
to
9ff8057
Compare
There was a problem hiding this 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.
9ff8057
to
8a2cdb6
Compare
There was a problem hiding this 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
?
There was a problem hiding this 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.
Reviewable status: 1 unresolved discussion
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Reviewable status: 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.
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
calc_X_FM()
andcalc_V_FM()
. The existing virtual methods are reimplemented as calls to the non-virtuals.What's not here
internal::
)Notes for reviewers
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.)
*
times from pre-#21853 which applied similar changes to Frames.This change is