[multibody] Add shadow links and distribute mass properties (closed-loop topology) - #24864
Conversation
sherm1
added
priority: medium
release notes: none
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
+a:@mitiguy for feature review, please
Reviewer notes:
- most of the code by far is the new test cases
- the mass-splitting code is in multibody_tree.cc
@sherm1 made 1 comment.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).
There was a problem hiding this comment.
Pull request overview
Adds preliminary closed-loop topology support by materializing shadow links and distributing mass properties.
Changes:
- Adds an opt-in loop-topology setting.
- Creates shadow links with split inertia and geometry bookkeeping.
- Adds four-bar linkage coverage and generated documentation.
Reviewed changes
Copilot reviewed 11 out of 11 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
multibody/tree/rigid_body.h |
Adds shadow-aware inertia access and cloning. |
multibody/tree/rigid_body.cc |
Handles parameterless shadow links. |
multibody/tree/multibody_tree.h |
Declares loop topology support. |
multibody/tree/multibody_tree.cc |
Materializes shadows and splits inertia. |
multibody/plant/multibody_plant.h |
Exposes the opt-in setting. |
multibody/plant/multibody_plant.cc |
Adds setting and geometry bookkeeping. |
multibody/plant/test/multibody_plant_test.cc |
Tests option plumbing. |
multibody/plant/test/closed_topology_test.cc |
Tests four-bar shadow behavior. |
multibody/plant/BUILD.bazel |
Registers the new test. |
bindings/generated_docstrings/multibody_tree.h |
Updates generated body documentation. |
bindings/generated_docstrings/multibody_plant.h |
Updates generated plant documentation. |
💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
| /// loops (a "closed topology"). When enabled, Finalize() breaks each loop by | ||
| /// introducing a "shadow" body and a loop-closure constraint, using the | ||
| /// automatic loop-breaking already performed by Drake's internal topology |
2ed638b to
79e95ff
Compare
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
@sherm1 made 4 comments and resolved 4 discussions.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on mitiguy).
| /// loops (a "closed topology"). When enabled, Finalize() breaks each loop by | ||
| /// introducing a "shadow" body and a loop-closure constraint, using the | ||
| /// automatic loop-breaking already performed by Drake's internal topology |
mitiguy
left a comment
mitiguy
left a comment
There was a problem hiding this comment.
@sherm1 I undid many of my requested changes as they will slow us down. Here is a skim-pass to keep things moving for now. I'll pick it up again later tomorrow.
@mitiguy reviewed 3 files and all commit messages, and made 13 comments.
Reviewable status: 12 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on sherm1).
multibody/plant/multibody_plant.cc line 1173 at r3 (raw file):
// This array must be dense over num_bodies(); anything that adds a body // without extending it (e.g. ephemeral links materialized during Finalize()) // would turn the lookup below into an out-of-range read.
nit More explicit and consistent?
- Is "ephemeral link" synonymous with "shadow link". If so, simplify, use "shadow" consistently.
I am confused, as ephemeral seems to mean "temporary"?
Don't shadow links last consistently after Finalize()? - "Materialized" seems borrowed from Star-Trek. Perhaps "created" ?
Suggestion:
// Ensure the visual_geometries_ array has the proper size, accounting
// for any shadow links created for closed-topologies during Finalize().multibody/plant/multibody_plant.cc line 1219 at r3 (raw file):
// See the comment in GetVisualGeometriesForBody(); checking the index against // num_bodies() alone is not enough, since it is exactly the array-vs-body // count agreement that a newly added body can break.
nit Seems difficult to keep comments in synch (relying on other comments not changing).
I do not think I grok this comment as is.
Suggestion:
// Ensure the collision_geometries_ array has the proper size, accounting
// for any shadow links created for closed-topologies during Finalize().multibody/plant/multibody_plant.cc line 1482 at r3 (raw file):
template <typename T> void MultibodyPlant<T>::SetAllowLoopTopology(bool allow) { mutable_tree().SetAllowLoopTopology(allow);
nit allow -> enable
Suggestion:
void MultibodyPlant<T>::SetAllowLoopTopology(bool enable) {
mutable_tree().SetAllowLoopTopology(enable);multibody/plant/multibody_plant.cc line 1523 at r3 (raw file):
internal::MultibodyTreeSystem<T>::Finalize(); // Finalizing the tree may have materialized ephemeral shadow links (when loop
nit More explicit language.
Also, is the word ephemeral needed/helpful anywhere.
Suggestion:
// At Finalize(), multibody tree may create shadow links (when loopmultibody/tree/multibody_tree.h line 1002 at r3 (raw file):
// See MultibodyPlant API. void SetAllowLoopTopology(bool allow);
nit allow -> enable
Suggestion:
void SetAllowLoopTopology(bool enable);multibody/plant/multibody_plant.h line 1832 at r3 (raw file):
/// automatic loop-breaking already performed by Drake's internal topology /// analysis. When disabled, Finalize() throws if the model contains any such /// loops.
nit This features is about whether Drake automatically handles kinematic loops.
Suggestion:
/// (Internal use only for now) For systems whose bodies and joints
/// form one or more kinematic loops (a "closed topology"), controls
/// whether Finalize() automatically deals with these loops by breaking
/// each loop via a weld-constrained split-body (i.e., introducing a
/// "shadow" body and a weld loop-closure constraint). When disabled,
/// Finalize() throws if the system contains any such kinematic loops.multibody/plant/multibody_plant.h line 1838 at r3 (raw file):
/// Unlike SetFuseWeldedLinks() and SetBaseBodyJointType(), this is a single /// global setting (a kinematic loop can span multiple model instances) and /// therefore does not take a model instance argument.
nit Unnecessary. Remove this comment.
multibody/plant/multibody_plant.h line 1841 at r3 (raw file):
/// /// The default setting for Drake is _not_ to model looped systems /// automatically.
nit Shorten
Suggestion:
/// The default setting is _not_ to deal with kinematic loops
/// (if one is encountered, an exception is thrown).multibody/plant/multibody_plant.h line 1844 at r3 (raw file):
/// /// @param[in] allow Whether Finalize() should automatically model closed /// kinematic loops rather than throwing.
nit wording. The word "enable" is used in documentation in other parts of this PR in conjunction with this method. I think "enable" is the better choice of bool than "allow".
For example, here is the first line in closed_topology_test.cc
/* Tests for automatic modeling of closed-topology (looped) systems, enabled via
MultibodyPlant::SetAllowLoopTopology(). When enabled, Finalize() breaks each
kinematic loop using the shadow links and loop constraints produced by the ...
Suggestion:
/// @param[in] enable instructs Finalize() to automatically
/// deal with kinematic loops rather than throwing.multibody/plant/multibody_plant.h line 1847 at r3 (raw file):
/// @throws std::exception if called after Finalize(). /// @see GetAllowLoopTopology(), Finalize() void SetAllowLoopTopology(bool allow);
nit Replace allow with enable.
Suggestion:
void SetAllowLoopTopology(bool enable);multibody/plant/multibody_plant.h line 1874 at r3 (raw file):
/// (Internal use only for now) Returns the current setting for whether /// Finalize() will automatically model closed-topology (looped) systems.
nit Language improvement.
Suggestion:
/// (Internal use only for now) Returns the current setting for whether
/// Finalize() automatically deals with closed-topology (looped) systems.multibody/plant/BUILD.bazel line 463 at r3 (raw file):
drake_cc_googletest( name = "closed_topology_test",
nit I am trying to think of a more descriptive name for this test.
Here Drake is doing the work of choosing how to deal with the closed_topology, rather than having a test of how a user might be able to deal with a closed_topology (e.g., by bushings or other)..
The proposed name below is a prototype.
Suggestion:
name = "closed_topology_split_bodies_test",
79e95ff to
44bb20e
Compare
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
Thanks, Paul. First batch of comments addressed, PTAL.
Please feel free to start an issue with anything you think we should change but can defer until after the full functionality is in.
@sherm1 made 13 comments and resolved 12 discussions.
Reviewable status: LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).
multibody/plant/BUILD.bazel line 463 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit I am trying to think of a more descriptive name for this test.
Here Drake is doing the work of choosing how to deal with the closed_topology, rather than having a test of how a user might be able to deal with a closed_topology (e.g., by bushings or other)..The proposed name below is a prototype.
That doesn't really do it either since a user can split bodies manually. I guess the point here is that it is done automatically. How about "auto_closed_topology_test"?
multibody/plant/multibody_plant.h line 1832 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit This features is about whether Drake automatically handles kinematic loops.
Done. I split out the details as being less relevant than whether loops are handled automatically or not.
multibody/plant/multibody_plant.h line 1838 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Unnecessary. Remove this comment.
Done. Claude loves this irrelevant comments!
multibody/plant/multibody_plant.h line 1841 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Shorten
Done. Moved to the first paragraph.
multibody/plant/multibody_plant.h line 1844 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit wording. The word "enable" is used in documentation in other parts of this PR in conjunction with this method. I think "enable" is the better choice of bool than "allow".
For example, here is the first line in closed_topology_test.cc
/* Tests for automatic modeling of closed-topology (looped) systems, enabled via
MultibodyPlant::SetAllowLoopTopology(). When enabled, Finalize() breaks each
kinematic loop using the shadow links and loop constraints produced by the ...
Done
multibody/plant/multibody_plant.h line 1847 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Replace allow with enable.
Done
multibody/plant/multibody_plant.h line 1874 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Language improvement.
Done
multibody/plant/multibody_plant.cc line 1173 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit and consistent?
- Is "ephemeral link" synonymous with "shadow link". If so, simplify, use "shadow" consistently.
I am confused, as ephemeral seems to mean "temporary"?
Don't shadow links last consistently after Finalize()?- "Materialized" seems borrowed from Star-Trek. Perhaps "created" ?
Done
multibody/plant/multibody_plant.cc line 1219 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Seems difficult to keep comments in synch (relying on other comments not changing).
I do not think I grok this comment as is.
Done. Apparently Claude doesn't understand that!
multibody/plant/multibody_plant.cc line 1482 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit allow -> enable
Done
multibody/plant/multibody_plant.cc line 1523 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit language.
Also, is the word ephemeral needed/helpful anywhere.
Done. I agree that "ephemeral" isn't helpful. That term is used in the topology code to distinguish user-created stuff (permanent) and the Drake-added stuff (ephemeral because it depends on how we model the user stuff and evaporates if anything changes.) But in MultibodyPlant nothing changes after Finalize().
multibody/tree/multibody_tree.h line 1002 at r3 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit allow -> enable
Done
mitiguy
left a comment
mitiguy
left a comment
There was a problem hiding this comment.
@sherm1. More for you in auto_closed_topology_test.cc
I'll continue with the remainder of the file tomorrow.
@mitiguy partially reviewed 4 files and all commit messages, and made 29 comments.
Reviewable status: 27 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).
multibody/plant/BUILD.bazel line 463 at r3 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
That doesn't really do it either since a user can split bodies manually. I guess the point here is that it is done automatically. How about "auto_closed_topology_test"?
Yes -- that sounds good.
multibody/plant/test/auto_closed_topology_test.cc line 1 at r4 (raw file):
/* Tests for automatic modeling of closed-topology (looped) systems, enabled via
nit Prefer "handling" to "modeling".
Suggestion:
/* Tests for automatic handling of closed-topology (looped) systems, enabled viamultibody/plant/test/auto_closed_topology_test.cc line 5 at r4 (raw file):
kinematic loop using the shadow links and loop constraints produced by the underlying LinkJointGraph/SpanningForest. This file focuses on the ephemeral shadow links and their (evenly-split) mass properties.
nit. No need for "ephemeral".
More explicit language about primary and shadow is helpful.
Suggestion:
underlying LinkJointGraph/SpanningForest. This file focuses on adding a
shadow link to each primary link so each primary link's mass properties is
halved, giving the shadow link half the primary link's mass properties.multibody/plant/test/auto_closed_topology_test.cc line 39 at r4 (raw file):
revolute joints; all revolute axes point in +z (out of the page). Below, the links are drawn unassembled and lined up with the World axes; "*" marks the connection points and the attached frames are named.
nit Improve the language
Suggestion:
/* Shown below is an unassembled (broken) planar four-bar linkage which
is taken from examples/multibody/four_bar/dev/four_bar_loop.sdf.
There are three moving links (driver, coupler, rocker) plus World and four
revolute joints; all revolute axes are parallel to +Wz (World +z direction).
The unassembled links are aligned with the World axes; "*" marks the
connection points and the attached frames are named.multibody/plant/test/auto_closed_topology_test.cc line 62 at r4 (raw file):
In parent-child order the joints connect Wd-Do, Wr-Ro, Dc-Co, Cr-Rc. link mass centers are at their midpoints; inertias are those of thin rods (ML²/12). link masses: driver 1 kg, coupler 1 kg, rocker 2 kg.
nit missing comma in first sentence and capitalization issue.
Simpler and more explicit language.
Suggestion:
In parent-child order, the joints connect Wd-Do, Wr-Ro, Dc-Co, Cr-Rc.
The links are uniform-density thin rods with masses: driver 1 kg, coupler
1 kg, rocker 2 kg. Each link's central moment of inertia is Izz = mL²/12,
where m is the link's mass and L is the link's length.multibody/plant/test/auto_closed_topology_test.cc line 67 at r4 (raw file):
attaching the World-fixed frames Wd/Wr directly to "world" (which libsdformat's frame graph won't resolve for a bare model), we add a "world2" link welded to World and attach the frames to "world2" instead. */
Nit. I do not understand the previous comment or the need for the link world2.
Perhaps asked Claude if it world2 is necessary. It is unclear to me why it is needed.
If not necessary, is it helpful? Here is what I queried in Gemini and its response.
In SDF, how to attach a frame to world?
Answer:
<!-- Coordinate frame offset from the world origin -->
<frame name="my_world_frame" attached_to="world">
<pose>1.0 2.0 0.5 0 0 0</pose>
</frame>
From what I understand, that means you can skip world2, and instead just use something like (with less comments).
<frame name="Wd" attached_to="world">
<pose>-2 0 0 0 0 0</pose>
</frame>
<frame name="Wr" attached_to="world">
<pose>2 0 0 0 0 0</pose>
</frame>
nit Prefer "handling" to "modeling".
Suggestion:
/* Builds and finalizes the four-bar loop plant, with automatic handling of
loop constraints enabled. */multibody/plant/test/auto_closed_topology_test.cc line 175 at r4 (raw file):
/* As above, but registers the plant as a geometry source for `scene_graph` and gives the driver and the coupler (the link we know gets split; see CouplerIsSplit) one visual and one collision geometry apiece. */
nit As above is not great for keeping documentation in synch.
Also, why is the rocker not given one visual and one collision geometry?
Suggestion:
/* * Builds and finalizes the four-bar loop plant, with automatic handling
of loop constraints enabled. Registers the plant as a geometry source for
`scene_graph` and gives the driver, coupler, and rocker one visual and one
collision geometry apiece. */multibody/plant/test/auto_closed_topology_test.cc line 182 at r4 (raw file):
plant->RegisterAsSourceForSceneGraph(scene_graph); Parser(plant.get()).AddModelsFromString(kFourBarLoopSdf, "sdf"); for (const std::string name : {"driver", "coupler"}) {
Why not rocker too?
Suggestion:
for (const std::string name : {"driver", "coupler", "rocker"}) {multibody/plant/test/auto_closed_topology_test.cc line 185 at r4 (raw file):
const Link<double>& link = plant->GetBodyByName(name); plant->RegisterVisualGeometry(link, math::RigidTransformd(), geometry::Sphere(0.1), name + "_visual");
nit: Why are visual geometry and collision geometry spheres? If this makes sense add a comment. Otherwise, I would think they are rods?
Code quote:
geometry::Sphere(0.1), name + "_visual");multibody/plant/test/auto_closed_topology_test.cc line 196 at r4 (raw file):
/* Returns the single ephemeral (shadow) link in `plant`. We'll hunt for it here and then verify below that it meets our expectations (which is that the coupler was split). */
nit Lose ephermeral.
Suggestion:
/* Returns the single shadow link in `plant`, i.e., the link that is
created when the coupler is split at Finalize(). */multibody/plant/test/auto_closed_topology_test.cc line 212 at r4 (raw file):
yielding two length-2 branches. We therefore know a priori that primary = coupler and shadow = coupler$1, and the tests below rely on that so we verify here. */
nit More explicit.
For example, it helps to point out that coupler$1 is the name given to the coupler shadow link.
Suggestion:
/* The four-bar's kinematic loop is handled by splitting the coupler link's mass
equally into a primary and shadow link. The coupler link is split because the
SpanningForest minimizes the maximum branch length (hence it splits the middle
link): the coupler's primary link is reached via the driver and the coupler's
shadow link is reached via the rocker, yielding two length-2 branches. The
primary link is the coupler except half of its mass properties are given to
the coupler shadow link (which is named coupler$1). */multibody/plant/test/auto_closed_topology_test.cc line 217 at r4 (raw file):
// World + world2 + driver + coupler + rocker + the coupler's shadow. EXPECT_EQ(plant->num_bodies(), 6);
nit Lose world2
Suggestion:
// World, driver,coupler primary, coupler shadow, rocker.
EXPECT_EQ(plant->num_bodies(), 5);multibody/plant/test/auto_closed_topology_test.cc line 225 at r4 (raw file):
// Every user-defined link remains non-ephemeral. for (const char* name : {"world2", "driver", "coupler", "rocker"}) {
nit Lose world2
Suggestion:
for (const char* name : {"driver", "coupler", "rocker"}) {multibody/plant/test/auto_closed_topology_test.cc line 233 at r4 (raw file):
MultibodyElement::is_ephemeral() contract -- including the frames, which is easy to overlook because a link frame is created as a side effect of creating its link rather than by an explicit "add frame" call. */
nit The current comment is confusing.
Hopefully, this is better.
Suggestion:
/* Objects auto-created during Finalize() have to marked as ephemeral, per
the MultibodyElement::is_ephemeral() contract, including link frames which
are auto-created as a side effect of creating a link rather than by an
explicit "add frame" call. */multibody/plant/test/auto_closed_topology_test.cc line 242 at r4 (raw file):
// was necessarily created during Finalize(). For now that is just the // shadow's own link frame; when loop joints get retargeted onto the shadow // they will bring their mobilizer frames along too.
nit More explicit
Suggestion:
// Since shadow links are auto-created (not user-created), every frame
// fixed to a shadow link is auto-created during Finalize().multibody/plant/test/auto_closed_topology_test.cc line 259 at r4 (raw file):
// Scalar conversion must carry the flags over; unlike the pre-finalize path, // it creates the shadow's link frame by cloning rather than by adding a link.
nit More explicit.
Hopefully I understand what is being tested here.
Suggestion:
// Ensure scalar conversion properly manages is_ephermeral() data by creating
// the shadow's link frame by cloning rather than by adding a link.multibody/plant/test/auto_closed_topology_test.cc line 278 at r4 (raw file):
(its full declared 1 kg) while the ephemeral shadow's default reflects its half share; the effective (split) inertia checked here is what drives the dynamics. The unsplit links (driver and rocker) are unaffected. */
nit Too much noise?
Suggestion:
/* Ensure the coupler link's mass properties is split evenly with its shadow
link. Note: The algorithm used here splits individual links, not mobods.
(Reminder: mobod mass properties may include multiple fused links). Here, the
coupler primary link and the coupler's shadow link coupler$1 each carry half of
the coupler's inertia (and are identical to each other, since their link frames
coincide). The user-facing per-link (default) mass is unchanged for the coupler
(its full declared 1 kg) while the shadow link's default reflects its half
share; the effective (split) inertia checked here is what drives the dynamics.
Also ensure the unsplit links (driver and rocker) are unaffected. */multibody/plant/test/auto_closed_topology_test.cc line 303 at r4 (raw file):
// expressed in its link frame L (M_LLo_L == M_BBo_B, since the link frame is // the body frame). This public accessor reads the loop-split value from the // FrameBodyPoseCache.
nit More info.
Suggestion:
// the body frame). This Lambda function returns split-link mass properties
// from FrameBodyPoseCache (which is internal code, not user-facing code).multibody/plant/test/auto_closed_topology_test.cc line 311 at r4 (raw file):
// The coupler link and its shadow each carry half of the coupler's 1 kg, with // otherwise identical spatial inertia (their link frames coincide).
nit I think it helpful to differentiate the coupler's "primary link" from the "full coupler", particularly when it involves mass properties.
Suggestion:
// The coupler's primary link and its shadow link each carry half of
// the coupler's 1 kg, have colocated centers of mass, and have
// equal rotational inertia (their link frames coincide).multibody/plant/test/auto_closed_topology_test.cc line 321 at r4 (raw file):
// Moreover, each is exactly half of the coupler's declared (default) // rotational inertia about Lo, expressed in L.
nit More explicit.
Suggestion:
// The coupler's primary link and its shadow link each have half the
// user-declared coupler's rotational inertia about Lo, expressed in L.multibody/plant/test/auto_closed_topology_test.cc line 354 at r4 (raw file):
// 1 kg for the primary; see ShadowMassIsSplitEvenly.) EXPECT_NEAR(coupler.get_mass(*context), 0.5, kTol); EXPECT_NEAR(shadow.get_mass(*context), 0.5, kTol);
Hmmm. I am not sure why coupler.get_mass(*context) should return 0.5.
I thought get_mass() was a user-facing function?
Is this related to being in a "link" internal world, rather than "body" external world?
multibody/plant/test/auto_closed_topology_test.cc line 359 at r4 (raw file):
// 0.5 shadow = 1 kg), not double-count the shadow. The physical mass is // driver 1 + rocker 2 + coupler 1 = 4 kg (the world2 helper link is // massless).
nit Improve language.
Suggestion:
// Ensure the system's total mass properly accounts for the split-link
// coupler (0.5 * primary_link_mass + 0.5 * shadow_link_mass = 1 kg),
// so total mass is: driver 1 kg + rocker 2 kg + coupler 1 kg = 4 kg.multibody/plant/test/auto_closed_topology_test.cc line 382 at r4 (raw file):
EXPECT_NEAR(coupler.get_mass(*context), 1.5, kTol); EXPECT_NEAR(shadow.get_mass(*context), 1.5, kTol); EXPECT_NEAR(plant->CalcTotalMass(*context), 6.0, kTol);
Similar confusion here. Maybe more documentation?
It seems SetMass() works fine on the original primary link, but immediately thereafter get_mass() returns half its value. This is surprising if this was to be a user-facing function? No?
multibody/plant/test/auto_closed_topology_test.cc line 394 at r4 (raw file):
must stay dense over num_bodies(). Shadow links are created inside MultibodyTree::Finalize() rather than by MultibodyPlant::AddRigidBody() (which is what normally extends those arrays), so Finalize() has to extend them. */
nit improve language slightly.
Suggestion:
/* A shadow link carries no geometry of its own. It is an internal modeling
artifact coincident with its primary link, but it must still have an (empty)
entry in the plant's per-body geometry arrays, which are indexed by BodyIndex
and so must stay dense over num_bodies(). Shadow links are created inside
MultibodyTree::Finalize() rather than by MultibodyPlant::AddRigidBody() (which
is what normally extends those arrays), so Finalize() has to extend them. */multibody/plant/test/auto_closed_topology_test.cc line 403 at r4 (raw file):
const Link<double>& shadow = plant->GetBodyByName("coupler$1"); // The primary keeps the geometry registered on it ...
nit Put link consistently after primary.
Suggestion:
// The primary link keeps the geometry registered on it.multibody/plant/test/auto_closed_topology_test.cc line 408 at r4 (raw file):
// ... and its shadow has an entry, which is empty. Without that entry these // two lookups would read past the end of the per-body arrays.
nit Add "link" after shadow.
Suggestion:
// The shadow link has an empty entry. Without that empty entry,
// these two lookups would read past the end of the per-body arrays.multibody/plant/test/auto_closed_topology_test.cc line 415 at r4 (raw file):
// and one collision geometry each. EXPECT_EQ(plant->num_visual_geometries(), 2); EXPECT_EQ(plant->num_collision_geometries(), 2);
nit As previously noted, add geometry to the rocker so it is easier to understand the test.
Suggestion:
// Ensure that handling a kinematic constraint does not invent geometry.
// There should be one visual and one collision geometry for each of
// the four-bar's driver link, coupler link, and rocker link.
EXPECT_EQ(plant->num_visual_geometries(), 2);
EXPECT_EQ(plant->num_collision_geometries(), 2);
44bb20e to
aff1e55
Compare
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
All comments so far addressed, PTAL. I switched to a much simpler sdf in the test case and got rid of world2.
@sherm1 made 27 comments and resolved 24 discussions.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).
multibody/plant/test/auto_closed_topology_test.cc line 5 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit. No need for "ephemeral".
More explicit language about primary and shadow is helpful.
Done
multibody/plant/test/auto_closed_topology_test.cc line 39 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve the language
Moot now -- rewritten.
multibody/plant/test/auto_closed_topology_test.cc line 62 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit missing comma in first sentence and capitalization issue.
Simpler and more explicit language.
Done (rewritten)
multibody/plant/test/auto_closed_topology_test.cc line 67 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
Nit. I do not understand the previous comment or the need for the link world2.
Perhaps asked Claude if it world2 is necessary. It is unclear to me why it is needed.
If not necessary, is it helpful? Here is what I queried in Gemini and its response.In SDF, how to attach a frame to world?
Answer:<!-- Coordinate frame offset from the world origin --> <frame name="my_world_frame" attached_to="world"> <pose>1.0 2.0 0.5 0 0 0</pose> </frame>From what I understand, that means you can skip world2, and instead just use something like (with less comments).
<frame name="Wd" attached_to="world"> <pose>-2 0 0 0 0 0</pose> </frame> <frame name="Wr" attached_to="world"> <pose>2 0 0 0 0 0</pose> </frame>
Done. Per f2f Gemini's idea doesn't work but this simplified sdf doesn't need world2.
multibody/plant/test/auto_closed_topology_test.cc line 164 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Prefer "handling" to "modeling".
Done
multibody/plant/test/auto_closed_topology_test.cc line 175 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit As above is not great for keeping documentation in synch.
Also, why is the rocker not given one visual and one collision geometry?
Done
multibody/plant/test/auto_closed_topology_test.cc line 182 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
Why not rocker too?
Done
multibody/plant/test/auto_closed_topology_test.cc line 185 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit: Why are visual geometry and collision geometry spheres? If this makes sense add a comment. Otherwise, I would think they are rods?
Comment added. We're just checking geometry registration, not looking at anything.
multibody/plant/test/auto_closed_topology_test.cc line 196 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Lose ephermeral.
Better to keep it as is. The code literally checks is_ephemeral()! These test cases are for MultibodyPlant development, not for end users. I don't think we should be trying to hide the ephemeral terminology here.
multibody/plant/test/auto_closed_topology_test.cc line 212 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit.
For example, it helps to point out that coupler$1 is the name given to the coupler shadow link.
Done
multibody/plant/test/auto_closed_topology_test.cc line 217 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Lose world2
Done
multibody/plant/test/auto_closed_topology_test.cc line 225 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Lose world2
Done
multibody/plant/test/auto_closed_topology_test.cc line 233 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit The current comment is confusing.
Hopefully, this is better.
Done
multibody/plant/test/auto_closed_topology_test.cc line 242 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit
Done (kept the second sentence also)
multibody/plant/test/auto_closed_topology_test.cc line 259 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit.
Hopefully I understand what is being tested here.
I'm keeping the original. It does a better job of explaining why we have to do this test.
multibody/plant/test/auto_closed_topology_test.cc line 278 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Too much noise?
Done
multibody/plant/test/auto_closed_topology_test.cc line 303 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More info.
Done, reworked.
multibody/plant/test/auto_closed_topology_test.cc line 311 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit I think it helpful to differentiate the coupler's "primary link" from the "full coupler", particularly when it involves mass properties.
Done
multibody/plant/test/auto_closed_topology_test.cc line 321 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit.
Done
multibody/plant/test/auto_closed_topology_test.cc line 354 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
Hmmm. I am not sure why coupler.get_mass(*context) should return 0.5.
I thought get_mass() was a user-facing function?
Is this related to being in a "link" internal world, rather than "body" external world?
No, that's the actual mass of the coupler link read out of the context. There is no separate primary link -- that's the original link modified to deal with loop breaking. If get_mass() didn't return the actual physical mass then adding up all the links' masses wouldn't give the total system mass. That seems bad! (This has nothing to with link vs mobod since we're only dealing with link properties.)
multibody/plant/test/auto_closed_topology_test.cc line 359 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve language.
Done
multibody/plant/test/auto_closed_topology_test.cc line 382 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
Similar confusion here. Maybe more documentation?
It seems SetMass() works fine on the original primary link, but immediately thereafter get_mass() returns half its value. This is surprising if this was to be a user-facing function? No?
get_mass() and CalcTotalMass() continue to work as they always have -- they report the actual mass properties of the physical system. But we have modified the coupler and added a shadow. I agree it's a little weird but I think it is adequately documented above. The alternative of not returning the actual physical mass seems worse.
multibody/plant/test/auto_closed_topology_test.cc line 394 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit improve language slightly.
Done
multibody/plant/test/auto_closed_topology_test.cc line 403 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Put link consistently after primary.
Done
multibody/plant/test/auto_closed_topology_test.cc line 408 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Add "link" after shadow.
Done
multibody/plant/test/auto_closed_topology_test.cc line 415 at r4 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit As previously noted, add geometry to the rocker so it is easier to understand the test.
Done
aff1e55 to
a6f9558
Compare
mitiguy
left a comment
mitiguy
left a comment
There was a problem hiding this comment.
@mitiguy partially reviewed 6 files and made 18 comments.
Reviewable status: 21 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on sherm1).
multibody/plant/test/multibody_plant_test.cc line 4859 at r5 (raw file):
// only exercises the option plumbing on a loop-free model; automatic modeling // of looped systems is not implemented yet (see the TODO in // MultibodyTree::Finalize()).
nit Improve comment.
Suggestion:
// Verify that SetEnableLoopTopology() properly sets its setting and that it
// cannot be used post-Finalize(). Verify that when enabled, there is proper
// scalar conversion (which clones the underlying MultibodyTree).
// Note: for now, the system being tested does NOT have a kinematic loop
// since automatically dealing with kinematic loops is not yet implemented.
// TODO(Sherm) Verify this function for a system with a kinematic loop.multibody/tree/rigid_body.h line 448 at r5 (raw file):
// its shadows) report their share of the even mass split. For an ordinary // (unsplit) link this equals the parameter value. See // CalcSpatialInertiaInBodyFrame() for details.
nit Need a TODO with this?
Suggestion:
// CalcSpatialInertiaInBodyFrame() for details.
// TODO(Sherm) For public-facing functions, users should not know
// whether we split bodies. As of now, this function may return a
// smaller mass if it is a primary link with a shadow body.multibody/tree/rigid_body.h line 591 at r5 (raw file):
// way to query their (split) inertia. The cache itself is built in // CalcFrameBodyPoses() from each primary's parameter, read via // CalcSpatialInertiaInBodyFrameFromParameters().
nit Need a TODO with this?
Suggestion:
// CalcSpatialInertiaInBodyFrameFromParameters().
// TODO(Sherm) For public-facing functions, users should not know
// whether we split bodies. As of now, this function may return a
// smaller mass if it is a primary link with a shadow body.multibody/plant/test/multibody_plant_test.cc line 4863 at r5 (raw file):
MultibodyPlant<double> plant(0.0); // The default is off.
nit More explicit.
Suggestion:
// Verify the default setting is disabled (for now).multibody/tree/multibody_tree.h line 2584 at r5 (raw file):
// If `is_ephemeral` is true this Link was created during modeling (e.g. a // shadow link used to break a loop) and is therefore already present in the // LinkJointGraph, so we don't register it again there.
nit Improve language.
Suggestion:
// If `is_ephemeral` is true this Link was auto-created (not user-created),
// e.g. a shadow link used to break a kinematic loop. If auto-created, the
// Link is already in the LinkJointGraph, so don't register it again there.multibody/tree/multibody_tree.h line 2597 at r5 (raw file):
// inertia is never read by the dynamics. The new body's index is guaranteed // to match the graph's shadow link index provided ephemeral links are added // in graph order immediately after all user links.
nit Improve language.
Suggestion:
// (Internal use only) Add a shadow Link (RigidBody) that is auto-created
// when dealing with a kinematic loop (such links are called "ephemeral").
// @param[in] name is the shadow link's name, which is based on the name
// of its primary link. For example, if the primary link is named Fred
// then the first shadow link would be called Fred$1.
// @param[in] model_instance The index of the model instance.
// @param[in] M_BBo_B is the shadow link's default spatial inertia, which
// (at best) is informational ONLY. For internal computational purposes,
// the actual spatial inertia of the shadow link is computed in
// CalcFrameBodyPoses() by dividing the primary link's parameterized
// spatial inertia and assigning an appropriate amount to the shadow link
// The new shadow link's index is guaranteed to match the graph's shadow
// link index provided ephemeral links are added in graph order
// immediately after all user links.
// @throws std::exception if model_instance is not a valid index.multibody/tree/multibody_tree.h line 2959 at r5 (raw file):
// per-model-instance option. // TODO(sherm1) Remove this option and always model loops once the feature is // mature.
nit Improve language.
Suggestion:
// If true, Finalize() will deal with kinematic loops (closed-topologies)
// by automatically creating shadow links and loop constraints via the
// underlying LinkJointGraph/SpanningForest. If false (the default),
// Finalize() throws when the graph contains loops. Dealing with kinematic
// loops is a whole-graph policy, hence a single global setting rather
// than a per-model-instance option.
// TODO(sherm1) Set the default setting to truee when this feature matures.multibody/plant/test/multibody_plant_test.cc line 4866 at r5 (raw file):
EXPECT_FALSE(plant.GetEnableLoopTopology()); // We can turn it on and back off pre-Finalize().
nit Improve language.
Suggestion:
// Verify that pre-Finalize(), the setting can be enabled and/or disabled.multibody/tree/rigid_body.cc line 143 at r5 (raw file):
systems::Parameters<T>* parameters) const { // Ephemeral shadow links have no spatial-inertia parameter (see // DoDeclareParameters()), so there is nothing to default.
nit Should the next line throw an exception rather than return?
multibody/plant/test/multibody_plant_test.cc line 4880 at r5 (raw file):
// The setting cannot be changed post-Finalize(). DRAKE_EXPECT_THROWS_MESSAGE(plant.SetEnableLoopTopology(false), ".*is_finalized.*");
nit Consolidate the next few lines and make language more accurate.
Show that any call to the function SetEnableLoopTechnology() throws an exception, not just trying to change the setting.
Suggestion:
// Verify that post-Finalize(), cannot _call_ SetEnableLoopTopology().
plant.SetEnableLoopTopology(true);
plant.AddRigidBody("body", SpatialInertia<double>::MakeUnitary());
plant.Finalize();
EXPECT_TRUE(plant.GetEnableLoopTopology());
DRAKE_EXPECT_THROWS_MESSAGE(plant.SetEnableLoopTopology(false),
".*is_finalized.*");
DRAKE_EXPECT_THROWS_MESSAGE(plant.SetEnableLoopTopology(true),
".*is_finalized.*");multibody/tree/multibody_tree.cc line 556 at r5 (raw file):
// Ephemeral links are added during Finalize() (post-BuildForest), when the // tree is not yet marked finalized; all other links must be added // pre-finalize.
nit Confusing comment.
Suggestion:
// Auto-created ("ephemeral") links can be added during Finalize()
// (post-BuildForest). All other links must be added pre-finalize.multibody/tree/multibody_tree.cc line 594 at r5 (raw file):
link_frame->set_parent_tree(this, link_frame_index); // A link's frame comes into existence with the link, so it is ephemeral // exactly when the link is; see MultibodyElement::is_ephemeral().
nit Semantic improvement.
Suggestion:
// Whether or not a link_frame is "ephemeral" depends on its link
// since the link_frame comes into existence with the link.multibody/tree/multibody_tree.cc line 609 at r5 (raw file):
// effective runtime inertia is still recomputed in CalcFrameBodyPoses(). return AddLinkImpl(std::make_unique<Link<T>>(name, model_instance, M_BBo_B), true /* ephemeral */);
nit Remove comment. I think suggested documentation in .h file (in this review) supercedes comment.
Also add a check that model_instance makes sense.
Suggestion:
DRAKE_THROW_UNLESS(model_instance < num_model_instances());
return AddLinkImpl(std::make_unique<Link<T>>(name, model_instance, M_BBo_B),
true /* ephemeral */);multibody/tree/multibody_tree.cc line 1063 at r5 (raw file):
// flag (default off). When disabled we reject looped graphs as before; when // enabled we make use of the shadow links and loop constraints that // BuildForest() already produced for us. (Not functional yet)
nit Unnecessary comment.
multibody/tree/multibody_tree.cc line 1082 at r5 (raw file):
links) and before building BodyNodes. We add them in graph order, immediately after the user links, so that each new body's index matches its graph shadow-link index.
nit Confusing language. My attempt at cleanup.
Suggestion:
/* Add the shadow Links. BuildForest() creates a shadow Link for each
kinematic loop it breaks and that shadow link is needed for a corresponding
subsequent Joint implementations (which will reference the shadow link).
This is all needed before building BodyNodes. We add them in graph order,
immediately after the user links, so that each new shadow link's index
matches its graph shadow-link index.multibody/tree/multibody_tree.cc line 1091 at r5 (raw file):
shadows is still (re)computed in CalcFrameBodyPoses() by dividing the primary's parameterized inertia, which remains the single source of truth for the split (so runtime changes to the primary's mass re-split correctly). */
nit Add TODO
Suggestion:
the split (so runtime changes to the primary's mass re-split correctly). */
// TODO(Sherm) Change M_shadow to NaN. A shadow link's default spatial
// inertia should never be used. It is calculated in CalcFrameBodyPoses().
multibody/tree/multibody_tree.cc line 1813 at r5 (raw file):
// Mass properties are split evenly among a primary link and any shadow // links that were created to break loops. We source the inertia from the // primary (a shadow's own placeholder inertia is ignored) and divide by
nit wording.
Suggestion:
// primary link (a shadow link's default spatial inertia is ignored) and divide bymultibody/tree/multibody_tree.cc line 4534 at r5 (raw file):
tree_clone->link_joint_graph_ = this->link_joint_graph_; // Copy the loop-topology opt-in flag so GetEnableLoopTopology() reports the
nit More "Drake-like" language.
Suggestion:
// Copy the enable_loop_topology setting so GetEnableLoopTopology() reports the
a6f9558 to
72559d9
Compare
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
All feature review comments addressed, PTAL.
@sherm1 made 19 comments and resolved 18 discussions.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee mitiguy, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on mitiguy).
multibody/plant/test/multibody_plant_test.cc line 4859 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve comment.
Done. I shortened the comment -- it won't ever make sense to do an actual kinematic loop here. This is just testing that MultibodyPlant handles the flag correctly. Testing loop behavior belongs elsewhere (auto_closed_topology_test.cc).
multibody/plant/test/multibody_plant_test.cc line 4863 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More explicit.
Done
multibody/plant/test/multibody_plant_test.cc line 4866 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve language.
Done
multibody/plant/test/multibody_plant_test.cc line 4880 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Consolidate the next few lines and make language more accurate.
Show that any call to the function SetEnableLoopTechnology() throws an exception, not just trying to change the setting.
Done
multibody/tree/multibody_tree.h line 2584 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve language.
Done
multibody/tree/multibody_tree.h line 2597 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve language.
Done
multibody/tree/multibody_tree.h line 2959 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Improve language.
Done
multibody/tree/multibody_tree.cc line 556 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Confusing comment.
Done
multibody/tree/multibody_tree.cc line 594 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Semantic improvement.
Done
multibody/tree/multibody_tree.cc line 609 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Remove comment. I think suggested documentation in .h file (in this review) supercedes comment.
Also add a check that model_instance makes sense.
Done
multibody/tree/multibody_tree.cc line 1063 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Unnecessary comment.
Done
multibody/tree/multibody_tree.cc line 1082 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Confusing language. My attempt at cleanup.
Done
multibody/tree/multibody_tree.cc line 1091 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Add TODO
Done
multibody/tree/multibody_tree.cc line 1813 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit wording.
Done
multibody/tree/multibody_tree.cc line 4534 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit More "Drake-like" language.
Done
multibody/tree/rigid_body.h line 448 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Need a TODO with this?
Done
multibody/tree/rigid_body.h line 591 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Need a TODO with this?
Done
multibody/tree/rigid_body.cc line 143 at r5 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Should the next line throw an exception rather than return?
No, this is an internal function that gets called on all Links (part of the general MultibodyTree boilerplate that applies to all multibody elements). It is just supposed to do nothing when called on shadow links.
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
+a:@SeanCurtis-TRI for platform review, please. I'm hoping to get this series of PRs into Drake as an experimental feature at least in time for RoboSim where I'll be talking about it! Feature review hasn't stamped yet but everything's been addressed.
@sherm1 made 1 comment.
Reviewable status: 3 unresolved discussions, LGTM missing from assignees mitiguy,SeanCurtis-TRI(platform) (waiting on mitiguy and SeanCurtis-TRI).
|
I probably won't finish this until Friday. But it's a high priority. |
SeanCurtis-TRI
left a comment
SeanCurtis-TRI
left a comment
There was a problem hiding this comment.
First pass done.
@SeanCurtis-TRI partially reviewed 11 files and all commit messages, and made 21 comments.
Reviewable status: 23 unresolved discussions, LGTM missing from assignees mitiguy,SeanCurtis-TRI(platform) (waiting on mitiguy and sherm1).
multibody/plant/test/multibody_plant_test.cc at r7 (raw file):
Given the bespoke work required in cloning (and converting), should we be testing that targeted values survive cloning and scalar conversion?
multibody/plant/test/multibody_plant_test.cc line 4884 at r7 (raw file):
std::unique_ptr<MultibodyPlant<AutoDiffXd>> ad_plant = systems::System<double>::ToAutoDiffXd(plant); EXPECT_TRUE(ad_plant->GetEnableLoopTopology());
nit: Ostensibly, fields on body frames should also successfully report their ephemeral status after cloning/conversion.
I just noticed it exists in auto_closed_topology_test.cc. So, that makes me wonder two things:
- Does this scalar conversion test belong in this unit test?
- Does this unit test belong in this file, or should the unique portions of this unit test also be in the
auto_closed_topology_test.cc?
multibody/tree/rigid_body.h line 688 at r7 (raw file):
const SpatialInertia<T>& M_Bo_B) const { DRAKE_THROW_UNLESS(context != nullptr); ThrowIfEphemeralInertia(__func__);
nit: This function gets the same "throw on ephemeral", but didn't get the same documentation change.
This likewise applies to the functions that picked up ThrowIfEphemeralInertia() in their implementations in the .cc file.
multibody/tree/rigid_body.h line 834 at r7 (raw file):
// properties mirror its primary link and are managed internally). The // invoking method should pass its name for the error message. void ThrowIfEphemeralInertia(const char* source_method) const;
BTW Have you considered using std::source_location?
multibody/plant/test/auto_closed_topology_test.cc at r7 (raw file):
As part of our f2f, probably worth having a test that shadows a joint multiple times. It'd be nice to know that's working under test.
Also, other scenarios:
- Are things ok if the link that gets shadowed is welded to the world? (either directly or via a welded path?)
- What's the relationship between fusing and shadowing?
multibody/plant/test/auto_closed_topology_test.cc line 157 at r7 (raw file):
} /* Builds and finalizes the four-bar loop plant, with automatic handling
BTW The phrase "automatic handling of loop constraints enabled" seems odd. Handling implies they already existed. Surely this should say, "automatic handling of loops enabled" (by introducing constraints and links).
multibody/plant/test/auto_closed_topology_test.cc line 169 at r7 (raw file):
plant->RegisterAsSourceForSceneGraph(scene_graph); Parser(plant.get()).AddModelsFromString(kFourBarLoopSdf, "sdf"); for (const std::string name : {"driver", "coupler", "rocker"}) {
Why add geometry at all? And if geometry, why just tiny spheres as the com?
multibody/plant/test/auto_closed_topology_test.cc line 181 at r7 (raw file):
} /* Returns the single ephemeral (shadow) link in `plant`. We'll hunt for it here
nit: Why hunt? So much of the code has been written to make the ephemeral links come after all the user-declared links. Why not use this test to provide regression on that functionality? Or have we opted not to document that or guarantee it?
multibody/plant/test/auto_closed_topology_test.cc line 200 at r7 (raw file):
primary link is the coupler except half of its mass properties are given to the coupler shadow link (which is named coupler$1). */ GTEST_TEST(ClosedTopologyTest, CouplerIsSplit) {
BTW In contrast to the previous test (where we're not affirming that the shadow link is last), he're we are affirming where the split must happen. But, surely, that's not documented. So, I'm not entirely clear what the underlying testing philosophy is. This feels like it would better be a test of the topology code. This should simply assert that the couple is ephemeral and rely on topology testing to know if something is wrong that would ultimately lead the assertion here to fail.
multibody/plant/test/auto_closed_topology_test.cc line 209 at r7 (raw file):
const Link<double>& shadow = GetSoleShadowLink(*plant); EXPECT_EQ(shadow.name(), "coupler$1"); EXPECT_TRUE(shadow.is_ephemeral());
BTW if this test also confirmed the body_frames are/aren't ephemeral based on the classification of the bodies, would the next test hold that much value?
multibody/plant/test/auto_closed_topology_test.cc line 247 at r7 (raw file):
// Scalar conversion must carry the flags over; unlike the pre-finalize path, // it creates the shadow's link frame by cloning rather than by adding a link. std::unique_ptr<MultibodyPlant<AutoDiffXd>> plant_ad =
nit: This seems to supplant the scalar conversion logic in the new test in multibody_plant_test.cc. (It duplicates the basic conversion and then also tests the body frame).
multibody/plant/test/auto_closed_topology_test.cc line 374 at r7 (raw file):
/* A shadow link carries no geometry of its own. It is an internal modeling artifact coincident with its primary link, but it must still have an (empty) entry in the plant's per-body geometry arrays, which are indexed by BodyIndex
BTW Will this change if you pull the trigger on throwing on any query on shadow links? It seems, simply throwing on ephemeral links would simplify this PR.
Also, would it be better if MbP::num_bodies() returned the user bodies? And a new method returned all bodies? I think there's a debate to be made about the documentation. But the advantage of doing so is that num_bodies() only included user bodies before shadows. ANd it would continue to do that after shadows. So, from that semantic perspective, it hasn't really changed. Just our description of the indices it returns. And, as you've carefully made sure that the user bodies are nicely coalesced before the shadow links, that's a very reasonable API.
multibody/plant/test/auto_closed_topology_test.cc line 421 at r7 (raw file):
} /* Several post-finalize consumers (visualization helpers in particular) walk
So, is the expectation that every consumer should be ignorant of shadow links and just rely on the fact that they report zero collision/visual geometry as the mechanism?
For my own edification, which consumers are accessing geometry via multibody plant? I'd like to take them out behind the barn and shoot them.
multibody/tree/multibody_tree.h line 2961 at r7 (raw file):
default_body_poses_; // If true, Finalize() will deal with kinematic loops (closed topologies)
BTW It feels like this duplicates the documentation on MultibodyPlant. Perhaps it would be better if it simply referenced the documentation on SetEnableLoopTopology().
multibody/tree/multibody_tree.cc line 1061 at r7 (raw file):
link_joint_graph_.InvalidateForest(); throw std::runtime_error(fmt::format( "The bodies and joints of this system form one or "
BTW Claude hates wrapping strings.
throw std::runtime_error(fmt::format(
"The bodies and joints of this system form one or more loops in the "
"system graph. Automatic modeling of such systems is disabled by "
"default; you can enable it by calling "
"MultibodyPlant::SetEnableLoopTopology(true) before Finalize() "
"but: the feature is in development and not yet functional even when "
"enabled. For now, loops can be modeled with some input changes. See "
"https://drake.mit.edu/troubleshooting.html#mbp-loops-in-graph "
"for advice on how to model systems with loops."));Of particular note is putting the URL all on one line so it's much easier to see and reason about the url.
multibody/tree/multibody_tree.cc line 1083 at r7 (raw file):
default reflects the split rather than being zero. This default is informational only; the effective runtime inertia of both the primary and its shadows is still (re)computed in CalcFrameBodyPoses() by dividing the
nit grammar
Suggestion:
informational only; the effective runtime inertias of both the primary and its
shadows are still (re)computed in CalcFrameBodyPoses() by dividing themultibody/tree/multibody_tree.cc line 1086 at r7 (raw file):
primary's parameterized inertia, which remains the single source of truth for the split (so runtime changes to the primary's mass re-split correctly). */ // TODO(sherm1) Consider making shadow link inertias NaN and prohibiting
BTW Do I need to reconcile this todo's "they aren't used" with the statement that the shadow inertias are "computed in CalcFrameBodyPoses()"?
When you say the "shadow link inertias", I assume you mean these dummy "default" values which don't get used in practice? Setting to NaN will be a good way to figure out if they ever do accidentally get used. :)
multibody/tree/multibody_tree.cc line 1090 at r7 (raw file):
for (LinkOrdinal ordinal(graph.num_user_links()); ordinal < graph.num_links(); ++ordinal) { const LinkJointGraph::Link& graph_link = graph.links(ordinal);
BTW There's no way this link is world, right?
multibody/tree/multibody_tree.cc line 1826 at r7 (raw file):
source_link.CalcSpatialInertiaInBodyFrameFromParameters(context); if (num_copies > 1) { M_LLo_L =
BTW There's no way to simply set the mass in the copy? Sadness. :(
multibody/plant/multibody_plant.cc line 1171 at r7 (raw file):
const std::vector<geometry::GeometryId>& MultibodyPlant<T>::GetVisualGeometriesForBody(const RigidBody<T>& body) const { // Check that visual_geometries_ has been sized correctly and that the
BTW It really breaks my heart to see this method changing. If anything, it should go away and people should be asking SceneGraph. Same for collision geometries. We may never be able to back this hack out. :(
mitiguy
left a comment
mitiguy
left a comment
There was a problem hiding this comment.
@mitiguy partially reviewed 5 files and all commit messages, made 7 comments, and resolved 3 discussions.
Reviewable status: 23 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform) (waiting on SeanCurtis-TRI and sherm1).
multibody/plant/test/auto_closed_topology_test.cc line 354 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
No, that's the actual mass of the coupler link read out of the context. There is no separate primary link -- that's the original link modified to deal with loop breaking. If get_mass() didn't return the actual physical mass then adding up all the links' masses wouldn't give the total system mass. That seems bad! (This has nothing to with link vs mobod since we're only dealing with link properties.)
nit I added issue #24908 to investigate this more.
multibody/plant/test/auto_closed_topology_test.cc line 382 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
get_mass() and CalcTotalMass() continue to work as they always have -- they report the actual mass properties of the physical system. But we have modified the coupler and added a shadow. I agree it's a little weird but I think it is adequately documented above. The alternative of not returning the actual physical mass seems worse.
nit Per our discussion, I added issue #24908 to investigate this more. I agree that the aggregate mass (or spatial inertia or spatial momentum or gravitational/kinetic energy of a system is unchanged. However, it is odd if a user sets an link's mass to 3.0 kg, and it mysteriously returns 1.5 kg. Similarly for strange artifacts when querying individual links' spatial inertia, spatial momentum, kinetic energy, etc. The user does not know which links are split - or why, and really should not need to know. These internal details about how we handle individual links need to be internal only.
multibody/plant/test/auto_closed_topology_test.cc line 374 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Will this change if you pull the trigger on throwing on any query on shadow links? It seems, simply throwing on ephemeral links would simplify this PR.
Also, would it be better if
MbP::num_bodies()returned the user bodies? And a new method returned all bodies? I think there's a debate to be made about the documentation. But the advantage of doing so is thatnum_bodies()only included user bodies before shadows. ANd it would continue to do that after shadows. So, from that semantic perspective, it hasn't really changed. Just our description of the indices it returns. And, as you've carefully made sure that the user bodies are nicely coalesced before the shadow links, that's a very reasonable API.
nit Sean -- perhaps add more to issue #24908 ?
multibody/plant/test/auto_closed_topology_test.cc line 183 at r7 (raw file):
/* Returns the single ephemeral (shadow) link in `plant`. We'll hunt for it here and then verify below that it meets our expectations (which is that the coupler was split). */
nit. Ok with ephemeral. Shorten and clarify the comment?
Suggestion:
/* Ensure there is one ephemeral (shadow) link in `plant`, i.e., the
link created when the coupler is split at Finalize() - and return it. */multibody/plant/test/auto_closed_topology_test.cc line 220 at r7 (raw file):
the MultibodyElement::is_ephemeral() contract. That includes link frames which are auto-created as a side effect of creating a link rather than by an explicit "add frame" call. */
nit Small improvement on earlier suggestion?
Suggestion:
are co-created implicitly whenever a link is auto-created (as
contrasted by an explicit user call to "add frame"). */multibody/plant/test/auto_closed_topology_test.cc line 227 at r7 (raw file):
// Since shadow links are auto-created (not user-created), every frame // fixed to a shadow link is auto-created during Finalize(). For now that
nit missing comma after now.
If sentence spill-over is an issue, consider "with Finalize()" or "in Finalize()" nstead of "during Finalize".
Suggestion:
// fixed to a shadow link is auto-created during Finalize(). For now, that
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
Here's a first crack and response to your comments, Sean -- thanks for the speedy work! PTAL
@sherm1 made 24 comments and resolved 16 discussions.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform) (waiting on SeanCurtis-TRI).
multibody/plant/multibody_plant.cc line 1171 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW It really breaks my heart to see this method changing. If anything, it should go away and people should be asking SceneGraph. Same for collision geometries. We may never be able to back this hack out. :(
Well, I guess adding these asserts doesn't make it any worse! I could remove the changes to both functions if you want though, LMK.
multibody/plant/test/multibody_plant_test.cc line 4884 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: Ostensibly, fields on body frames should also successfully report their ephemeral status after cloning/conversion.
I just noticed it exists in
auto_closed_topology_test.cc. So, that makes me wonder two things:
- Does this scalar conversion test belong in this unit test?
- Does this unit test belong in this file, or should the unique portions of this unit test also be in the
auto_closed_topology_test.cc?
This is just testing the MbP option. I think it makes sense to place it with other MbP API tests, but I don't feel strongly. It really doesn't have anything to do with loop topology, just whether MbP tracks the option flag correctly. I could move it to the auto_closed_topology_test file if you don't like it here for some reason.
multibody/plant/test/multibody_plant_test.cc at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
Given the bespoke work required in cloning (and converting), should we be testing that targeted values survive cloning and scalar conversion?
I'm not sure what you're referring to here? There is a lot more covered in the auto_closed_topology_test but the test here is just for the "enable loops" option flag. Please clarify.
multibody/tree/multibody_tree.h line 2961 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW It feels like this duplicates the documentation on
MultibodyPlant. Perhaps it would be better if it simply referenced the documentation onSetEnableLoopTopology().
Done
multibody/tree/multibody_tree.cc line 1061 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Claude hates wrapping strings.
throw std::runtime_error(fmt::format( "The bodies and joints of this system form one or more loops in the " "system graph. Automatic modeling of such systems is disabled by " "default; you can enable it by calling " "MultibodyPlant::SetEnableLoopTopology(true) before Finalize() " "but: the feature is in development and not yet functional even when " "enabled. For now, loops can be modeled with some input changes. See " "https://drake.mit.edu/troubleshooting.html#mbp-loops-in-graph " "for advice on how to model systems with loops."));Of particular note is putting the URL all on one line so it's much easier to see and reason about the url.
Done, thanks.
multibody/tree/multibody_tree.cc line 1083 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit grammar
Done
multibody/tree/multibody_tree.cc line 1086 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Do I need to reconcile this todo's "they aren't used" with the statement that the shadow inertias are "computed in CalcFrameBodyPoses()"?
When you say the "shadow link inertias", I assume you mean these dummy "default" values which don't get used in practice? Setting to NaN will be a good way to figure out if they ever do accidentally get used. :)
Clarified. I'm leaning in the direction of prohibiting anyone from asking about shadow mass properties at all if we can maintain the fiction that all the mass belongs to the primary (as discussed in the issue Paul opened).
multibody/tree/multibody_tree.cc line 1090 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW There's no way this link is world, right?
Not any more! (PR #24909). Added some DRAKE_DEMANDs to make that clear here.
multibody/tree/multibody_tree.cc line 1826 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW There's no way to simply set the mass in the copy? Sadness. :(
Not 100% sure I understand your question. Does this help: Because of the fact that our mass properties are parameterized, we need the parameters (which the user sees) to reflect the full mass properties and then we have to split up the result. The shadows can't have their own parameters because they need to track whatever the user writes into the primary's parameters. And in this case we really do throw if a user attempts to set mass props directly on a shadow link.
If I missed you point, please LMK.
multibody/tree/rigid_body.h line 688 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: This function gets the same "throw on ephemeral", but didn't get the same documentation change.
This likewise applies to the functions that picked up
ThrowIfEphemeralInertia()in their implementations in the .cc file.
Done
multibody/tree/rigid_body.h line 834 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Have you considered using
std::source_location?
I had forgotten about that, thanks for the reminder. My impression is that it is overkill for user-directed messages. I don't want to include developer fluff like the file and line number. I'm happy with just the function name. I did check and __func__ isn't deprecated and appears likely to stick around forever.
multibody/plant/test/auto_closed_topology_test.cc line 157 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW The phrase "automatic handling of loop constraints enabled" seems odd. Handling implies they already existed. Surely this should say, "automatic handling of loops enabled" (by introducing constraints and links).
Done (here and above)
multibody/plant/test/auto_closed_topology_test.cc line 169 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
Why add geometry at all? And if geometry, why just tiny spheres as the com?
This is because of the special handling we had to do to make the geometry arrays the right size. The test here verifies that nothing bad happens when we ask about geometry on shadows. We're not looking at the geometry at all so the specifics don't matter; we just need a place holder and spheres are easiest. (There is a function comment that says that.)
A later PR will do more with shadow geometry (you helped me figure that out) so that it is possible to visualize shadow bodies in Meshcat.
multibody/plant/test/auto_closed_topology_test.cc line 181 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: Why hunt? So much of the code has been written to make the ephemeral links come after all the user-declared links. Why not use this test to provide regression on that functionality? Or have we opted not to document that or guarantee it?
Done. I like the hunting to verify that there weren't any surprise shadows. But the one we find better be the highest-numbered index. Added a check.
multibody/plant/test/auto_closed_topology_test.cc line 183 at r7 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit. Ok with ephemeral. Shorten and clarify the comment?
Done
multibody/plant/test/auto_closed_topology_test.cc line 200 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW In contrast to the previous test (where we're not affirming that the shadow link is last), he're we are affirming where the split must happen. But, surely, that's not documented. So, I'm not entirely clear what the underlying testing philosophy is. This feels like it would better be a test of the topology code. This should simply assert that the couple is ephemeral and rely on topology testing to know if something is wrong that would ultimately lead the assertion here to fail.
We're testing MbP's ability to build a model that correctly reproduces the modeling instructions dictated by multibody/topology. That code is thoroughly tested already but we don't know that MbP does the right thing with it. To test that we need to understand what topology did so we can check it. I think it is fine to have this test use knowledge of how the topology is built (minimize maximum branch length, etc.). It's true that if the topology algorithm changed this test would fail, but (a) that's unlikely, and (b) it would be easily detected and fixed.
What would cause the assertions here to fail would be broken MbP code so I think it is good to have this test here.
multibody/plant/test/auto_closed_topology_test.cc line 209 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW if this test also confirmed the body_frames are/aren't ephemeral based on the classification of the bodies, would the next test hold that much value?
Probably not but I like having the separate small tests better.
multibody/plant/test/auto_closed_topology_test.cc line 220 at r7 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Small improvement on earlier suggestion?
Done
multibody/plant/test/auto_closed_topology_test.cc line 227 at r7 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit missing comma after now.
If sentence spill-over is an issue, consider "with Finalize()" or "in Finalize()" nstead of "during Finalize".
Done
multibody/plant/test/auto_closed_topology_test.cc line 247 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: This seems to supplant the scalar conversion logic in the new test in
multibody_plant_test.cc. (It duplicates the basic conversion and then also tests the body frame).
The multibody_plant_test is just for the "enable closed loops" option flag (just a dumb bool), and it verifies that the flag survives scalar conversion. Here we are checking that the model survives scalar conversion, including the ephemeral flags which are a different code path. So I don't think there is much duplication if any.
multibody/plant/test/auto_closed_topology_test.cc line 374 at r7 (raw file):
Previously, mitiguy (Mitiguy) wrote…
nit Sean -- perhaps add more to issue #24908 ?
A worthy consideration. I suspect there will be pluses and minuses to whether we make the various APIs reflect what should be present vs. what is actually present. I added your idea above to the issue Paul opened.
multibody/plant/test/auto_closed_topology_test.cc line 421 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
So, is the expectation that every consumer should be ignorant of shadow links and just rely on the fact that they report zero collision/visual geometry as the mechanism?
For my own edification, which consumers are accessing geometry via multibody plant? I'd like to take them out behind the barn and shoot them.
I'm not sure. The current setup is consistent: num_bodies() has all of them and the zillion loops to num_bodies()-1 do the right thing. If we adopt your idea of using num_bodies() to hide the shadows, then we have to audit everything that depends on num_bodies() but actually needs the new num_links_including_shadows(). I'm sure Claude will be happy to do that when the time comes!
One of the advantages of the current scheme is that by mildly exposing the existence of ephemeral elements (shadow links, free joints, frames) vast amounts of existing code continues to work correctly.
multibody/plant/test/auto_closed_topology_test.cc at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
As part of our f2f, probably worth having a test that shadows a joint multiple times. It'd be nice to know that's working under test.
Also, other scenarios:
- Are things ok if the link that gets shadowed is welded to the world? (either directly or via a welded path?)
- What's the relationship between fusing and shadowing?
I agree on the multiple loops -- should be a test where a single primary has more than one shadow. I'll add that.
- There aren't any special cases involving unfused weld joints -- they are just 0 dof joints and are treated like any other joint with respect to splitting loops.
- Fusing does affect loop cutting since a set of fused links is just treated as a single level when balancing branch lengths. However, that is all handled by the topology code which is not being tested here. On the MbP side, fusing has to do with putting links onto a common mobod, while making shadow links is done prior to anyone thinking about how to model them. So none of the code in this PR knows whether fusing will occur. I could still add a test case combining fusing and loop breaking if you'd like to see one.
d8856b1 to
83c684b
Compare
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
@sherm1 made 1 comment.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform) (waiting on mitiguy and SeanCurtis-TRI).
multibody/plant/test/auto_closed_topology_test.cc at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
I agree on the multiple loops -- should be a test where a single primary has more than one shadow. I'll add that.
- There aren't any special cases involving unfused weld joints -- they are just 0 dof joints and are treated like any other joint with respect to splitting loops.
- Fusing does affect loop cutting since a set of fused links is just treated as a single level when balancing branch lengths. However, that is all handled by the topology code which is not being tested here. On the MbP side, fusing has to do with putting links onto a common mobod, while making shadow links is done prior to anyone thinking about how to model them. So none of the code in this PR knows whether fusing will occur. I could still add a test case combining fusing and loop breaking if you'd like to see one.
Done (multiple loops). LMK if you'd also like to see one with fusing.
SeanCurtis-TRI
left a comment
SeanCurtis-TRI
left a comment
There was a problem hiding this comment.
@SeanCurtis-TRI partially reviewed 5 files and all commit messages, made 11 comments, and resolved 7 discussions.
Reviewable status: 2 unresolved discussions (waiting on sherm1).
multibody/plant/test/multibody_plant_test.cc at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
I'm not sure what you're referring to here? There is a lot more covered in the auto_closed_topology_test but the test here is just for the "enable loops" option flag. Please clarify.
Sorry, I think I got my wires crossed. MultibodyTree has bespoke code for scalar converting and cloning -- the is_ephmeral() state is not copied for free. But I see that the bespoke code is tested in the auto_closed_topology_test.
multibody/tree/multibody_tree.cc line 1826 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Not 100% sure I understand your question. Does this help: Because of the fact that our mass properties are parameterized, we need the parameters (which the user sees) to reflect the full mass properties and then we have to split up the result. The shadows can't have their own parameters because they need to track whatever the user writes into the primary's parameters. And in this case we really do throw if a user attempts to set mass props directly on a shadow link.
If I missed you point, please LMK.
M_LLo_L is a unique instance of SpatialInertia. It has already been copied from the CalcSpatial.....(). Its center of mass and unit inertia are already correct. It's a shame we can't simply mutate the mass in place. As it is, we're going to create another copy of the center of mass and unit inertia. That's the shame to which I alluded.
multibody/plant/test/auto_closed_topology_test.cc line 169 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
This is because of the special handling we had to do to make the geometry arrays the right size. The test here verifies that nothing bad happens when we ask about geometry on shadows. We're not looking at the geometry at all so the specifics don't matter; we just need a place holder and spheres are easiest. (There is a function comment that says that.)
A later PR will do more with shadow geometry (you helped me figure that out) so that it is possible to visualize shadow bodies in Meshcat.
The following will not be a blocking comment, but I felt it would be worth getting on record.
I am 100% against the shadow geometry leaking out through the MbP API. Shadow links should not appear in the output ports. THey shouldn't be reported in the num_bodies(). Accessing them via their index should likewise not be possible -- it should be treated as an invalid body index.
If someone really needs advanced access to those links, then they should get access to the MultibodyTree and use those unguarded APIs.
I'm 99% sure @mitiguy feels the same. (Although, I may be reading things into his response from a previous discussion). I sincerely hope that before this feature goes "live", that shadow links will be invisible to the MbP APIs.
-- FOLLOW UP --
After writing this, I did see that Paul had broached this subject as well and you've tagged it in the issue for the evolution of this feature. I'm content that the view has been well captured but didn't feel like deleting what I wrote. :D
multibody/plant/test/auto_closed_topology_test.cc line 181 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Done. I like the hunting to verify that there weren't any surprise shadows. But the one we find better be the highest-numbered index. Added a check.
Alternatively, you could test index (N-1) (is shadow) and index (N-2) isn't a shadow. But, the fact that you're asserting that the shadow has the highest index is definitively an improvement to the test.
multibody/plant/test/auto_closed_topology_test.cc line 200 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
We're testing MbP's ability to build a model that correctly reproduces the modeling instructions dictated by multibody/topology. That code is thoroughly tested already but we don't know that MbP does the right thing with it. To test that we need to understand what topology did so we can check it. I think it is fine to have this test use knowledge of how the topology is built (minimize maximum branch length, etc.). It's true that if the topology algorithm changed this test would fail, but (a) that's unlikely, and (b) it would be easily detected and fixed.
What would cause the assertions here to fail would be broken MbP code so I think it is good to have this test here.
For the record, "MbP's ability to build a model" consists wholly of (1) passing the "enable loop handling" flag and (2) adding a bunch of DRAKE_DEMANDs. It doesn't have any logic for building such a model.
As you point out, that code is tested and we don't know that it is passing triggering that code. Given that MbP does almost nothing explicit to make sure the that it does "the right thing" with that code, a simple smoke test should be enough -- if it works at all, it works correctly. So, any evidence that a shadow exists should be more than enough.
Nevertheless, I'll defer to author on this one. (Although, I frequently finding myself wondering about nickle-and-diming our CI to death with test redundancy.)
multibody/plant/test/auto_closed_topology_test.cc line 209 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Probably not but I like having the separate small tests better.
Do, rather than one more line in this test, you prefer the 30 lines in the next? Different strokes for different folks, I guess.
multibody/plant/test/auto_closed_topology_test.cc line 247 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
The multibody_plant_test is just for the "enable closed loops" option flag (just a dumb bool), and it verifies that the flag survives scalar conversion. Here we are checking that the model survives scalar conversion, including the
ephemeralflags which are a different code path. So I don't think there is much duplication if any.
Ah...so if this actually added one line where it checked that the bool was still set, that test wouldn't have any value. Seems to be a continuation of a theme. However, you've got a consistent approach, so I'll treat this the same as the previous. I'm simply getting out of the way while casting a skeptical glare.... ;)
multibody/plant/test/auto_closed_topology_test.cc line 421 at r7 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
I'm not sure. The current setup is consistent: num_bodies() has all of them and the zillion loops to num_bodies()-1 do the right thing. If we adopt your idea of using num_bodies() to hide the shadows, then we have to audit everything that depends on num_bodies() but actually needs the new num_links_including_shadows(). I'm sure Claude will be happy to do that when the time comes!
One of the advantages of the current scheme is that by mildly exposing the existence of ephemeral elements (shadow links, free joints, frames) vast amounts of existing code continues to work correctly.
This might resolve itself with the whole "keep the shadow links in the shadows" rant above. So, for now, I'm getting out of the way.
multibody/tree/multibody_tree.cc line 12 at r8 (raw file):
#include <utility> #include "../topology/link_joint_graph.h"
nit: I think claude fooled you on this one.
multibody/plant/test/auto_closed_topology_test.cc line 502 at r8 (raw file):
plant.Finalize(); // Before testing MultibodyPlant's handling of multiple shadows, confirm our
BTW A passing thought. Our expectation is that when the dut works, all of these assertions are a waste of time. Something to consider:
Test for the existence of body link6$2. Only if that doesn't exist should you do all of the assertion so that we have an insight to why our fundamental assumption failed. But only do the work when we already know the assumption is invalid.
83c684b to
efb5409
Compare
sherm1
left a comment
sherm1
left a comment
There was a problem hiding this comment.
All comments resolved, thanks @SeanCurtis-TRI & @mitiguy !
@sherm1 made 7 comments and resolved 2 discussions.
Reviewable status:complete! all discussions resolved, LGTM from assignees mitiguy,SeanCurtis-TRI(platform) (waiting on sherm1).
multibody/tree/multibody_tree.cc line 1826 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
M_LLo_Lis a unique instance ofSpatialInertia. It has already been copied from theCalcSpatial.....(). Its center of mass and unit inertia are already correct. It's a shame we can't simply mutate the mass in place. As it is, we're going to create another copy of the center of mass and unit inertia. That's the shame to which I alluded.
I see your point now! We could have a SpatialInertia::SetMass() that would be more efficient (performance not an issue in this spot, but still!). OTOH it's a little tricky for an end user who likely doesn't know how we represent inertia internally. Would they be surprised that the inertia changes when they asked only to change the mass? Could make the API SetMassAndUpdateInertia(). As discretion is the better part of valor, I'd say we should hold off until we have a performance-sensitive use case and live with the ugliness for now.
multibody/tree/multibody_tree.cc line 12 at r8 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: I think claude fooled you on this one.
??? thanks!
multibody/plant/test/auto_closed_topology_test.cc line 169 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
The following will not be a blocking comment, but I felt it would be worth getting on record.
I am 100% against the shadow geometry leaking out through the MbP API. Shadow links should not appear in the output ports. THey shouldn't be reported in the
num_bodies(). Accessing them via their index should likewise not be possible -- it should be treated as an invalid body index.If someone really needs advanced access to those links, then they should get access to the
MultibodyTreeand use those unguarded APIs.I'm 99% sure @mitiguy feels the same. (Although, I may be reading things into his response from a previous discussion). I sincerely hope that before this feature goes "live", that shadow links will be invisible to the MbP APIs.
-- FOLLOW UP --
After writing this, I did see that Paul had broached this subject as well and you've tagged it in the issue for the evolution of this feature. I'm content that the view has been well captured but didn't feel like deleting what I wrote. :D
Yes, issue #24908 for reference.
multibody/plant/test/auto_closed_topology_test.cc line 200 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
For the record, "MbP's ability to build a model" consists wholly of (1) passing the "enable loop handling" flag and (2) adding a bunch of DRAKE_DEMANDs. It doesn't have any logic for building such a model.
As you point out, that code is tested and we don't know that it is passing triggering that code. Given that MbP does almost nothing explicit to make sure the that it does "the right thing" with that code, a simple smoke test should be enough -- if it works at all, it works correctly. So, any evidence that a shadow exists should be more than enough.
Nevertheless, I'll defer to author on this one. (Although, I frequently finding myself wondering about nickle-and-diming our CI to death with test redundancy.)
FYI Many of these tests reflect incremental development -- the option code got written first along with a test to make sure it works. More code got added later with more features, and those tests do depend on the option code working so you could argue they are sufficient. However, I like it better this way with small tests that just verify one feature at a time. I worry more about not enough tests than too many.
multibody/plant/test/auto_closed_topology_test.cc line 209 at r7 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
Do, rather than one more line in this test, you prefer the 30 lines in the next? Different strokes for different folks, I guess.
OK, I took another look at these two tests to see whether one more line would do it. It won't. There is very little overlap between the tests, and the frame tests are much more involved and take a lot more code than the CouplerIsSplit test. Their purposes are different, the implementing code is distinct, and IMO it is nice to have a separate test on which to hang the test comment explaining what's being tested. I suspect we are optimizing over different loss functions!
multibody/plant/test/auto_closed_topology_test.cc line 502 at r8 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW A passing thought. Our expectation is that when the dut works, all of these assertions are a waste of time. Something to consider:
Test for the existence of body
link6$2. Only if that doesn't exist should you do all of the assertion so that we have an insight to why our fundamental assumption failed. But only do the work when we already know the assumption is invalid.
Done. Good point -- the existence of link6$2 is enough evidence that we got the right system to test! I chopped out almost all the other ASSERTs which I agree were a waste of time.
This is the first yak-shave PR on the way to allowing Drake to automatically handle systems with closed-loop topologies (see WIP PR #24843).
See issue #18803 and PR #18829 for discussion.
This one does the following:
multibody/plant/test/auto_closed_topology_test.ccwith a four-bar linkage inspired by [parsing] Adds aspirational sdfs for kinematic loop mechanism #18829 as the test subject.This PR is limited to creating the shadows and distributing the mass properties. To follow in subsequent PRs:
There are no user-visible changes here except those marked "internal use only". No release notes are needed.
This change is