Skip to content

Commit

Permalink
Adds careful handling of massless links to avoid singular forests.
Browse files Browse the repository at this point in the history
  • Loading branch information
@sherm1
sherm1 committed Jul 23, 2024
1 parent 60d7abc commit 05ab84b
Show file tree
Hide file tree
Showing 3 changed files with 115 additions and 64 deletions.
2 changes: 1 addition & 1 deletion multibody/topology/link_joint_graph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ namespace multibody {
namespace internal {

// TODO(sherm1) The class comment describes the complete functionality of
// PR #20225; only part of that code is actually here.
// PR #20225; some functionality is currently missing.

// TODO(sherm1) During the PR train leading up to MbP using this code in Drake
// master, I'm using Doxygen comments /** despite the fact that this is
Expand Down
134 changes: 95 additions & 39 deletions multibody/topology/spanning_forest.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,9 @@ void SpanningForest::Clear() {
// subset of the algorithm described.
// What's here: the ability to model tree-structured graphs, add joints to
// world as needed, grow breadth-first and then renumber depth-first,
// assign coordinates, break loops.
// What's not here (see #20225): treat massless bodies specially, use a single
// Mobod for composites. The related options are allowed but ignored.
// assign coordinates, break loops, treat massless bodies specially.
// What's not here (see #20225): Use a single Mobod for composites. The related
// option is allowed but ignored.

/* This is the algorithm that takes an arbitrary link-joint graph and models
it as a spanning forest of mobilized bodies plus loop-closing constraints.
Expand Down Expand Up @@ -411,12 +411,14 @@ void SpanningForest::ChooseBaseBodiesAndAddTrees(int* num_unprocessed_links) {

/* Should be nothing left now except unjointed (single) Links. We'll attach
with either a floating joint or a weld joint depending on modeling options.
If a weld and we're optimizing composites the Link will just join the World
Mobod. Otherwise it gets a new Mobod that serves as the base body of a new
It gets a new Mobod that serves as the base body of a new
Tree. Although static links and links belonging to a static model instance
get welded to World at the start of forest building, it is still possible
to need a weld here for a lone link that is in a "use fixed base" model
instance (that's not technically a static link). (1.5) */
// TODO(sherm1) If the required joint here is a weld and we're optimizing
// composites, the Link should just join the World Mobod and the joint is
// unmodeled. Coming soon.
DRAKE_DEMAND(*num_unprocessed_links == ssize(unjointed_links));

for (LinkOrdinal unjointed_link : unjointed_links) {
Expand All @@ -429,6 +431,7 @@ void SpanningForest::ChooseBaseBodiesAndAddTrees(int* num_unprocessed_links) {
unjointed_link);
const JointOrdinal next_joint_ordinal =
graph().index_to_ordinal(next_joint_index);

AddNewMobod(unjointed_link, next_joint_ordinal, world_mobod().index(),
false); // Not reversed; World is parent
/* No tree to extend here. */
Expand All @@ -448,11 +451,13 @@ void SpanningForest::ExtendTrees(const std::vector<JointIndex>& joints_to_model,
}
}

/* Grows each Tree by one level. */
// TODO(sherm1) Exceptions stubbed out: (1) keep going for massless links so we
// don't ever leave one as a leaf when breaking loops, and (2) for composites
// implemented with a single Mobod keep going until all those links are done.
// See PR #20225 for the whole story.
/* Grows each Tree by one level with two exceptions:
- TODO(sherm1) if we're optimizing composites, keep growing LinkComposites
as far as possible since they use only a single Mobod, and
- if we encounter a "treat as massless" Link it can't be a terminal body
of a branch. In that case we keep growing that branch until we can end
with something massful. Note that a LinkComposite can be terminal if _any_
of its Links are massful. */
void SpanningForest::ExtendTreesOneLevel(
const std::vector<JointIndex>& joints_to_model, int* num_unprocessed_links,
std::vector<JointIndex>* joints_to_model_next) {
Expand Down Expand Up @@ -493,9 +498,8 @@ void SpanningForest::ExtendTreesOneLevel(

// TODO(sherm1) Combining composites stubbed out here (E.4)

/* The modeled_joint might connect to any Link of an inboard LinkComposite
that follows inboard_mobod. That's the Link we want as inboard Link; not
the Composite's active Link. */
const Joint& modeled_joint = joints(modeled_joint_ordinal);

const auto [modeled_inboard_link_ordinal, modeled_outboard_link_ordinal,
is_reversed] =
graph().FindInboardOutboardLinks(inboard_mobod_index,
Expand All @@ -509,45 +513,97 @@ void SpanningForest::ExtendTreesOneLevel(
continue;
}

/* Note: this invalidates references to Mobods. */
/* There isn't a loop and the outboard Link isn't part of a combined
LinkComposite so we can model it with a Mobod.
Note: this invalidates references to Mobods. */
AddNewMobod(modeled_outboard_link_ordinal, modeled_joint_ordinal,
inboard_mobod_index,
is_reversed); // Is mobilizer reversed from Joint?
--(*num_unprocessed_links);

const Link& modeled_outboard_link = links(modeled_outboard_link_ordinal);
const Joint& modeled_joint = joints(modeled_joint_ordinal);
/* Now determine if we can stop here or if we have to keep extending
the branch we're on. If the Mobod we just added was a massless body
on an articulated (non-weld) mobilizer, we may need to extend "one more
level" from here. This can continue recursively if we run into
more massless Links. */
const Link& modeled_link = links(modeled_outboard_link_ordinal);
if (modeled_joint.is_weld() || !modeled_link.treat_as_massless()) {
/* We can stop here. Collect up the Joints for the next level and
go on to the next Joint to model at this level. */
for (JointIndex next_joint_index : modeled_link.joints()) {
const JointOrdinal next_joint_ordinal =
graph().index_to_ordinal(next_joint_index);
if (!joints(next_joint_ordinal).has_been_processed())
joints_to_model_next->push_back(next_joint_index);
}
continue;
}

/* The Link we just added to the branch is massless and articulated; we
don't want to terminate the branch with it. Three possibilities:
1 If it has outboard joints and at least one leads to a massful link,
we're fine and can end here knowing that the massless link will get
covered at the next level.
2 If the link has no outboard joints we're stuck and will just have to
declare this as a "no dynamics" model.
3 If all the outboard joints lead to massless links, we must not stop
here but instead need to move on to the next level in the hope that
we'll eventually run into something massful. */
std::vector<JointIndex> massless_link_unmodeled_joints;
bool has_outboard_massful_link = false;
for (JointIndex massless_link_joint_index : modeled_link.joints()) {
const JointOrdinal massless_link_joint_ordinal =
graph().index_to_ordinal(massless_link_joint_index);
if (joints(massless_link_joint_ordinal).has_been_processed()) continue;
massless_link_unmodeled_joints.push_back(massless_link_joint_index);

if (!has_outboard_massful_link) {
const Joint& outboard_joint = joints(massless_link_joint_ordinal);
const Link& modeled_outboard_link =
links(modeled_outboard_link_ordinal);
const BodyIndex outboard_link_index =
outboard_joint.other_link_index(modeled_outboard_link.index());
has_outboard_massful_link =
!link_by_index(outboard_link_index).treat_as_massless();
}
}

/* Case 1: all good. */
if (has_outboard_massful_link) {
/* The outboard joints we just collected are the ones we should
process at the next level. */
joints_to_model_next->insert(joints_to_model_next->end(),
massless_link_unmodeled_joints.begin(),
massless_link_unmodeled_joints.end());
continue;
}

/* If we just added a massless Mobod on an articulated (non-weld) joint,
we're in trouble if there are no outboard joints. In that case the forest
can't be used for dynamics. Record the first occurrence but continue
building the forest. */
if (!modeled_joint.is_weld() && modeled_outboard_link.treat_as_massless()) {
/* We just added an articulated massless body. If the only joint it has
is the one we just processed, it's terminal. */
if (ssize(modeled_outboard_link.joints()) == 1 && data_.dynamics_ok) {
const LinkJointGraph::JointTraits& modeled_joint_type =
/* Case 2: we're not going to be able to do dynamics :(. */
if (massless_link_unmodeled_joints.empty()) {
/* If this is the first problem we've seen, record it. */
if (data_.dynamics_ok) {
const LinkJointGraph::JointTraits& modeled_joint_traits =
graph().joint_traits(modeled_joint.traits_index());
data_.dynamics_ok = false;
data_.why_no_dynamics = fmt::format(
"Link {} on {} joint {} is a terminal, articulated, massless link. "
"The resulting multibody system will have a singular mass matrix "
"so cannot be used for dynamics.",
modeled_outboard_link.name(), modeled_joint_type.name,
"Link {} on {} joint {} is a terminal, articulated, massless "
"link. The resulting multibody system will have a singular "
"mass matrix so cannot be used for dynamics.",
modeled_link.name(), modeled_joint_traits.name,
modeled_joint.name());
}

// TODO(sherm1) If the massless body is non-terminal there is still hope
// if we keep extending this branch. That's stubbed out here so for now
// we assume we can always stop after doing one level. (E.6)
continue;
}

/* We can stop here. Collect up the Joints for the next level and go on to
the next Joint to model at this level. */
for (JointIndex next_joint_index : modeled_outboard_link.joints()) {
if (!joint_by_index(next_joint_index).has_been_processed())
joints_to_model_next->push_back(next_joint_index);
}
/* Case 3: there is still hope. The massless link does have some
not-yet-modeled joints, but they all lead to more massless links. Extend
further and collect up the outboard joints at the next level. */
std::vector<JointIndex> next_level_outboard_joints;
ExtendTreesOneLevel(massless_link_unmodeled_joints, &*num_unprocessed_links,
&next_level_outboard_joints);
joints_to_model_next->insert(joints_to_model_next->end(),
next_level_outboard_joints.begin(),
next_level_outboard_joints.end());
}
}

Expand Down
43 changes: 19 additions & 24 deletions multibody/topology/test/spanning_forest_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -269,6 +269,7 @@ GTEST_TEST(SpanningForest, MultipleBranchesDefaultOptions) {

// Build with default options.
EXPECT_TRUE(graph.BuildForest());

EXPECT_EQ(forest.options(), ForestBuildingOptions::kDefault);
EXPECT_EQ(forest.options(left_instance), ForestBuildingOptions::kDefault);
EXPECT_EQ(forest.options(right_instance), ForestBuildingOptions::kDefault);
Expand Down Expand Up @@ -966,6 +967,7 @@ GTEST_TEST(SpanningForest, SimpleTrees) {
// We should report that the resulting forest is unsuited for dynamics due
// to a terminal massless body. Specifically, it should complain about link
// 4 rather than link 2 since 4 is at a lower level and should be seen first.
// (Tests Case 2 in ExtendTreesOneLevel())
EXPECT_FALSE(graph.BuildForest());
const SpanningForest& forest = graph.forest();
EXPECT_FALSE(forest.dynamics_ok());
Expand All @@ -975,7 +977,7 @@ GTEST_TEST(SpanningForest, SimpleTrees) {
"singular.*cannot be used for dynamics.*"));

// Change link 4's joint type to "weld". That should shift the complaint to
// link 2.
// link 2. (Tests Case 2 in ExtendTreesOneLevel())
graph.ChangeJointType(JointIndex(5), "weld");
EXPECT_FALSE(graph.BuildForest());
EXPECT_FALSE(forest.dynamics_ok());
Expand Down Expand Up @@ -1010,23 +1012,14 @@ With all massful Links, loop should be broken at Joint 3 (between links
{3} and {4} since that minimizes the maximum chain length. In that case we
have trees {1234s} and {564} where {4s} is the shadow of {4}.
If we make {3}* massless we'll have to extend the first chain
to link {4} before breaking the loop at Joint 6, giving trees
{1234} and {564s} ({4} is still split since it is the child of Joint 6).
TODO(sherm1) With massless handling stubbed out, the algorithm will _not_
extend the first chain when it sees massless {3} so produces the same two
trees as the massful case above. As it happens, that's still a good forest.
If we make just {3}* massless we'll get the same Forest since we can see
that there is an outboard massful body {4}. When we split {4} both halves are
massful, so we'll still get {1234s} and {564}.
If we make _both_ {3}* and {4}* massless, modeling should start with {12} and
{56} but then next extend the first tree to {12346s} because we can't stop at
{3} or {4}. Joint 6 is the loop joint but the mobilizer has to be reversed so
that we end with a massful shadow link {6s} rather than the massless {4}.
TODO(sherm1) With massless handling stubbed out, we once again get the same
two trees but now the forest can't be used for dynamics due to terminal
massless bodies 4 and 4s.
*/
that we end with a massful shadow link {6s} rather than the massless {4}. */
GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) {
LinkJointGraph graph;
graph.RegisterJointType("revolute", 1, 1);
Expand Down Expand Up @@ -1063,6 +1056,7 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) {
BodyIndex(7));

// Changing just 3 to massless results in the same forest.
// (Tests Case 1 in ExtendTreesOneLevel())
graph.ChangeLinkFlags(BodyIndex(3), LinkFlags::kTreatAsMassless);
EXPECT_TRUE(graph.BuildForest());

Expand All @@ -1077,20 +1071,21 @@ GTEST_TEST(SpanningForest, MasslessLinksChangeLoopBreaking) {
EXPECT_EQ(graph.links(forest.mobods(MobodIndex(4)).link_ordinal()).index(),
BodyIndex(7));

// TODO(sherm1) With both 3 and 4 massless and massless handling stubbed,
// we'll end up with a no-dynamics model. Should not happen once this gets
// un-stubbed.
// Changing both 3 and 4 to massless breaks the loop at 6 instead of 4.
// (Tests Case 3 in ExtendTreesOneLevel())
graph.ChangeLinkFlags(BodyIndex(4), LinkFlags::kTreatAsMassless);
EXPECT_FALSE(graph.BuildForest()); // Indicates "no dynamics".
EXPECT_TRUE(graph.BuildForest());

EXPECT_THAT(
forest.why_no_dynamics(),
testing::MatchesRegex("Loop.*joint3.*massless.*link3.*link4.*singular.*"
"cannot be used for dynamics.*"));
EXPECT_EQ(ssize(graph.links()), 8);
EXPECT_EQ(ssize(graph.joints()), 7);
EXPECT_TRUE(graph.link_by_index(BodyIndex(7)).is_shadow());
EXPECT_EQ(graph.link_by_index(BodyIndex(7)).primary_link(), BodyIndex(6));

EXPECT_EQ(ssize(forest.trees()), 2);
EXPECT_EQ(forest.trees()[0].num_mobods(), 3);
EXPECT_EQ(forest.trees()[1].num_mobods(), 4);
EXPECT_EQ(forest.trees()[0].num_mobods(), 5);
EXPECT_EQ(forest.trees()[1].num_mobods(), 2);
EXPECT_EQ(graph.links(forest.mobods(MobodIndex(5)).link_ordinal()).index(),
BodyIndex(7));
}

/* Here is a tricky case that should be handled correctly and without warnings.
Expand Down

0 comments on commit 05ab84b

Please sign in to comment.