[planning] Fix GetQuaternionDofStartIndices to loop over joints, not bodies

[calderpg-tri]

I believe this resolves #21215, @SeanCurtis-TRI did you have something else in mind in your TODO?

+@SeanCurtis-TRI for feature review, thanks!

---

This change is 

[jwnimmer-tri]

My read of the TODO was that we'd prefer a spelling where the forthcoming BallQuaternionJoint (#20841) would also be identified as using a quaternion (i.e., a Joint::has_quaternion_dofs() base class function), with no additional changes to this class. I think that's fine to leave as a TODO though -- this is still an improvement.

(And of course the other thing this PR needs is a unit test that fails without the fix.)

[SeanCurtis-TRI]

To be perfectly frank, I'm not 100% sure how to interpret either. It's been a while since I wrote it. What @jwnimmer-tri has an air of familiarity. However, as I review the current documentation on the code, it's quite so obvious to me.

1. RigidBody::has_quaternion_dofs() clearly documents that, if true is returned, that the first four values in the generalized positions are the quaternion. It makes no promises about the remaining values (either to count or semantics).
2. As I read QuaternionFloatingJoint, I don't seem similar promises about the layout of its position.

Therefore, in some sense, this seems a step backwards. :( Although, that may simply be a defect in the joint's documentation. Perhaps waiting until the floating ball joint exists would give us better insight into what the solution here should be (should we be doing string compares or relying on virtual methods?)

Sorry I can't be more definitive.

Reviewed 1 of 1 files at r1, all commit messages.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers

[jwnimmer-tri]

@sherm1 could you weigh in on the dofs ordering, please? As I understand it, we do already intend to promise that the first four q in a QuaterionFloatingJoint configuration are always the quaternion portion, but we just haven't written into the API docs yet. Is that right?

[calderpg-tri]

I don't seem similar promises about the layout of its position

As I understand it, a body with has_quaternion_dof() == true also has a corresponding QuaternionFloatingJoint, both of which have to agree on position start index and thus they must have the same position layout. An equivalent lack of documentation clarity came up originally in #19105 with RigidBody positions not being promised in documentation (but being confirmed f2f with dynamics).

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers

[jwnimmer-tri]

I think we have basically two paths forward:

(1) Document that Joint::position_start on a quaternion-holding joint is the quaternion portion.

or

(2) Add an optional<int> Joint::quaternion_position_start method on Joint that returns the position of the quaternion (in the whole-plant dofs) if there is one, or else nullopt if the joint doesn't have a quaternion.

We can let @sherm1 pick which one he likes best.

[jwnimmer-tri]

I suppose for completeness I should mention the third option, but it's too big to be a short-term fix:

(3) The MbP exposes the quaternion norm constraint as a SystemConstraint, and places like CollisionChecker (and elsewhere) that need to know about constraints like that use the SystemConstraint language to interrogate the plant for specifics. This removes the need to loop over joints, and instead uses the fully generic constraint language we already have in place, which can then flex to cases more complicated than unit norm (e.g., mimic, coupler, etc.).

[sherm1]

@sherm could you weigh in on the dofs ordering, please? As I understand it, we do already intend to promise that the first four q in a QuaternionFloatingJoint configuration are always the quaternion portion, but we just haven't written into the API docs yet. Is that right?

Right! It is my intention that any joint with a quaternion has it in the first four q's. And as Calder pointed out the promise for a floating body is just the promise about the joint we'll use for it. The documentation for the quaternion floating joint could be clearer but it does say introduces four generalized positions for the quaternion and three for the position. It should add "in that order" but it would be odd to describe them in that order and store them reversed.

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers

[SeanCurtis-TRI]

On the other hand, if I put on my Supreme Court Justice wig, one might argue that where the authors have *explicitly called something out in one place, its absence at other sites becomes conspicuous. :)

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers

[jwnimmer-tri]

=> #21309 for the docs fix. This PR can just focus on the collision checker fix + unit test.

[calderpg-tri]

Test case with explicitly-added-to-model QuaternionFloatingJoint added in r2.

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

[SeanCurtis-TRI]

And of course the other thing this PR needs is a unit test that fails without the fix.

I don't think such a test exists. The reason for that is I don't think there's any way for a body to report true on has_quaternion_dofs() that isn't currently perfectly synonymous with this alternative joint spelling. So, this is less a fix and more a respelling. The current tests are complete.

This only gets interesting when has_quaternion_dofs() does not mean QuaternionFloatingJoint. And, at that point, not only will we need a test, but we'll have to revisit this code, because this code will ignore any other joint type that would cause a body to
report that it has quaternion_dofs.

This is why this feels like a step backward. The TODO being eliminated may either be wrong from the beginning or have grown stale vis a vis the state of the MbP ecosystem.

Whatever the state of the code and the state of my mind may have been back when I wrote that TODO, it seems, today at least, the contract for RigidBody::has_quaternion_dofs() is intended to serve as a Joint-type-erased view into the Joint in question and it should continue to work even when we add any other quaternion-based joint type (assuming that the joint/mobilizer is implemented in (a) correctly reporting that it has quaternion dofs and (b) keeps them as the first four elements of its state vector.

As such, I think the only real fix necessary here is removing to remove the TODO and leave everything else intact.

Reviewed 1 of 1 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

---

planning/test/linear_distance_and_interpolation_provider_test.cc line 410 at r2 (raw file):

}

GTEST_TEST(FloatingJointIiwaTest, Test) {

I'm pretty sure this test doesn't add anything.

The reason is that multibody_tree.cc:727already adds a QuaternionFloatingJoint between the world and flying_robot_base. That is how "floating bodies" are now represented. So, the scene set up by this test should be indistinguishable from the previous test.

I reverted the first commit in this branch and ran the test and, unsurprisingly, both tests pass. So, this test does not serve the purpose of uniquely testing the new code.

[jwnimmer-tri]

The original issue has a crash repro. That is probably a good starting point for making a unit test?

[SeanCurtis-TRI]

Ah....I hadn't noticed that. That does matke what needs to happen in the test a bit more obvious.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

---

planning/test/linear_distance_and_interpolation_provider_test.cc line 435 at r2 (raw file):

  auto joint = std::make_unique<multibody::QuaternionFloatingJoint<double>>(
      "robot_base_floating_joint", builder.plant().GetFrameByName("world"),

nit: As per the linked issue, it shouldn't be joined to world. It needs to be linked to some non-world frame.

You can actually do that in the yaml above if you like. If you want to go that route, take a look at the documentation for the AddModel model directive.

[SeanCurtis-TRI]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

---

planning/test/linear_distance_and_interpolation_provider_test.cc line 435 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: As per the linked issue, it shouldn't be joined to world. It needs to be linked to some non-world frame.

You can actually do that in the yaml above if you like. If you want to go that route, take a look at the documentation for the AddModel model directive.

In fact, the issue includes a unit test. :) Again, the same set up can be achieved in just yaml, but if you just steal the test from the issue, you're home free.

[SeanCurtis-TRI]

Take a look at #21315 as an alternative.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

[SeanCurtis-TRI]

planning/linear_distance_and_interpolation_provider.cc line 30 at r2 (raw file):

       ++joint_index) {
    const Joint<double>& joint = plant.get_joint(joint_index);
    if (joint.type_name() == QuaternionFloatingJoint<double>::kTypeName) {

BTW It seems unfortunate that this will break if/when BallQuaternionJoint is introduced (if it fails to be explicitly accounted for here). Kind of feels like planting a land mine for the future.

Any thoughts on how we can reduce the odds of that outcome? Beyond just having the requisite APIs on the Joint class?

[calderpg-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

---

planning/linear_distance_and_interpolation_provider.cc line 30 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW It seems unfortunate that this will break if/when BallQuaternionJoint is introduced (if it fails to be explicitly accounted for here). Kind of feels like planting a land mine for the future.

Any thoughts on how we can reduce the odds of that outcome? Beyond just having the requisite APIs on the Joint class?

Having the right API in Joint is really the right way to do it. In general, any new joint type that requires a specific interpolation/distance operation would cause a problem if omitted in LinearDistanceAndInterpolationProvider, and an inescapable part of adding a new joint type is considering and addressing uses throughout Drake.

Absent a compile-time approach to ensuring that every joint type is considered here, the best approach is probably to create a stress-test plant that exercises all reasonable features a CollisionChecker should support, but doing so really just moves the problem around to keeping the stress-test model up to date.

---

planning/test/linear_distance_and_interpolation_provider_test.cc line 410 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm pretty sure this test doesn't add anything.

The reason is that multibody_tree.cc:727already adds a QuaternionFloatingJoint between the world and flying_robot_base. That is how "floating bodies" are now represented. So, the scene set up by this test should be indistinguishable from the previous test.

I reverted the first commit in this branch and ran the test and, unsurprisingly, both tests pass. So, this test does not serve the purpose of uniquely testing the new code.

Switched to using the test case from #21315

---

planning/test/linear_distance_and_interpolation_provider_test.cc line 435 at r2 (raw file):
Switched to using the test case from #21315

As per the linked issue, it shouldn't be joined to world. It needs to be linked to some non-world frame.

Note that I don't see the non-world aspect called out anywhere in the issue or the SO post. To me at least, a difference between floating-body and floating-joint-to-body behavior that only affects some cases is much worse that a difference that affects all of them.

[SeanCurtis-TRI]

With one more swipe at testing.

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: 2 unresolved discussions, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

---

planning/linear_distance_and_interpolation_provider.cc line 30 at r2 (raw file):

an inescapable part of adding a new joint type is considering and addressing uses throughout Drake.

That's the challenge. I've added an issue (#21320) and we'll let it go on this PR.

---

planning/test/linear_distance_and_interpolation_provider_test.cc line 435 at r2 (raw file):

Previously, calderpg-tri wrote…

Switched to using the test case from #21315

As per the linked issue, it shouldn't be joined to world. It needs to be linked to some non-world frame.

Note that I don't see the non-world aspect called out anywhere in the issue or the SO post. To me at least, a difference between floating-body and floating-joint-to-body behavior that only affects some cases is much worse that a difference that affects all of them.

There are some shiboleths:

• The title refers to "# QuaternionFloatingJoint (for a non-floating body)". The definition of a "floating" body is a body that has a QFJ between it and the world (and only the world). If it between that body and any other body it is no longer a "floating" body.
• In the specific test included in the description, the joint was explicitly added between two arbitrary bodies (no world included). That's not coincidence.

But, prior to a couple of weeks ago, I would've completely missed that. It's only the sheer happenstance that pushed me into that world that I learned that little bit of trivia.

---

planning/test/scene_graph_collision_checker_test.cc line 522 at r3 (raw file):

    default_free_body_pose:
        base_link_cracker:
            base_frame: arm::iiwa_link_ee

BTW This is the voodoo for creating a floating joint between base_link_cracker and arm::iiwa_link_ee. Being able to do this in yaml is a recent feature.

Note to myself: ultimately, the fact that this creates a floating joint between the two bodies is a horrible hack which will eventually go away and this test will break. But, meh. I think we've set someone up to come yelling at us for breaking their spacebar heater. Hyrum's law is going to bite us.

---

planning/test/scene_graph_collision_checker_test.cc line 533 at r3 (raw file):

  // The default LinearDistanceAndInterpolationProvider must not throw.
  EXPECT_NO_THROW(SceneGraphCollisionChecker(std::move(params)));

i think the test, as is, isn't quite enough.

As everyone observed in #21315, a wrong solution passed this test. This test just cares that things don't crash. The wrong solution passed this test by omitting the quaternion dofs. Your proper solution includes them and this test should confirm that the dofs exist.

I'd recommend putting the following in the lin_dist_and_intp_provider_test.cc; it directly tests what's being fixed here and distinguishes between my wrong fix and your correct fix:

// Root cause of the bug reported in issue #21215.
GTEST_TEST(LinearDistanceAndInterpolationproviderTest,
           NonFloatingBodyWithQuaternionJoint) {
  RobotDiagramBuilder<double> builder;
  const std::string model_directives = R"""(
directives:
- add_model:
    name: arm
    file: package://drake_models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf
- add_weld:
    parent: world
    child: arm::base
- add_model:
    name: cracker
    file: package://drake_models/ycb/003_cracker_box.sdf
    default_free_body_pose:
        base_link_cracker:
            base_frame: arm::iiwa_link_ee
)""";
  builder.parser().AddModelsFromString(model_directives, "dmd.yaml");

  const auto& plant = builder.plant();
  auto model = builder.Build();

  LinearDistanceAndInterpolationProvider provider(plant);

  // We should have a single floating joint between the cracker box and end
  // effector (named for the base_link_cracker body).
  const auto& joint = plant.GetJointByName("base_link_cracker");
  ASSERT_EQ(provider.quaternion_dof_start_indices().size(), 1);
  EXPECT_EQ(provider.quaternion_dof_start_indices()[0], joint.position_start());
}

We could leave the test here as confirmation that it addresses the bug at the level the issue observed the problem, I'm open to that but leave it up to you.

[calderpg-tri]

Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

---

planning/test/scene_graph_collision_checker_test.cc line 533 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

i think the test, as is, isn't quite enough.

As everyone observed in #21315, a wrong solution passed this test. This test just cares that things don't crash. The wrong solution passed this test by omitting the quaternion dofs. Your proper solution includes them and this test should confirm that the dofs exist.

I'd recommend putting the following in the lin_dist_and_intp_provider_test.cc; it directly tests what's being fixed here and distinguishes between my wrong fix and your correct fix:

// Root cause of the bug reported in issue #21215.
GTEST_TEST(LinearDistanceAndInterpolationproviderTest,
           NonFloatingBodyWithQuaternionJoint) {
  RobotDiagramBuilder<double> builder;
  const std::string model_directives = R"""(
directives:
- add_model:
    name: arm
    file: package://drake_models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf
- add_weld:
    parent: world
    child: arm::base
- add_model:
    name: cracker
    file: package://drake_models/ycb/003_cracker_box.sdf
    default_free_body_pose:
        base_link_cracker:
            base_frame: arm::iiwa_link_ee
)""";
  builder.parser().AddModelsFromString(model_directives, "dmd.yaml");

  const auto& plant = builder.plant();
  auto model = builder.Build();

  LinearDistanceAndInterpolationProvider provider(plant);

  // We should have a single floating joint between the cracker box and end
  // effector (named for the base_link_cracker body).
  const auto& joint = plant.GetJointByName("base_link_cracker");
  ASSERT_EQ(provider.quaternion_dof_start_indices().size(), 1);
  EXPECT_EQ(provider.quaternion_dof_start_indices()[0], joint.position_start());
}

We could leave the test here as confirmation that it addresses the bug at the level the issue observed the problem, I'm open to that but leave it up to you.

I've switched back to the test directly on LinearDistanceAndInterpolationProvider, I don't think the duplication is particularly useful.

[calderpg-tri]

+@xuchenhan-tri for platform review per schedule, please

Reviewable status: 1 unresolved discussion, LGTM missing from assignee xuchenhan-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

[SeanCurtis-TRI]

Reviewed 3 of 3 files at r4, all commit messages.
Reviewable status: LGTM missing from assignee xuchenhan-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)

[xuchenhan-tri]

Reviewed 1 of 1 files at r1, 1 of 2 files at r3, 3 of 3 files at r4, all commit messages.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @calderpg-tri)