From 6da604701843c4c14194ab90922e3b966343eb5e Mon Sep 17 00:00:00 2001
From: Sean Curtis <sean.curtis@tri.global>
Date: Tue, 16 Apr 2024 11:59:47 -0700
Subject: [PATCH] wip

---
 ...ear_distance_and_interpolation_provider.cc |  7 +---
 .../scene_graph_collision_checker_test.cc     | 32 +++++++++++++++++++
 2 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/planning/linear_distance_and_interpolation_provider.cc b/planning/linear_distance_and_interpolation_provider.cc
index 8ced97b813c6..bc19b432bc0f 100644
--- a/planning/linear_distance_and_interpolation_provider.cc
+++ b/planning/linear_distance_and_interpolation_provider.cc
@@ -20,15 +20,10 @@ namespace {
 
 std::vector<int> GetQuaternionDofStartIndices(
     const MultibodyPlant<double>& plant) {
-  // TODO(SeanCurtis-TRI) RigidBody::has_quaternion_dofs() is actually a
-  // misnomer for is_quaternion_floating(). The name implies general quaternion
-  // awareness but its documentation doesn't guarantee that. We should
-  // re-express this in terms of joints so that we can catch quaternions in any
-  // kind of joint.
   std::vector<int> quaternion_dof_start_indices;
   for (BodyIndex body_index(0); body_index < plant.num_bodies(); ++body_index) {
     const RigidBody<double>& body = plant.get_body(body_index);
-    if (body.has_quaternion_dofs()) {
+    if (body.is_floating() && body.has_quaternion_dofs()) {
       quaternion_dof_start_indices.push_back(body.floating_positions_start());
     }
   }
diff --git a/planning/test/scene_graph_collision_checker_test.cc b/planning/test/scene_graph_collision_checker_test.cc
index 6ec1ffb8a3cf..6f1ac81b8607 100644
--- a/planning/test/scene_graph_collision_checker_test.cc
+++ b/planning/test/scene_graph_collision_checker_test.cc
@@ -503,6 +503,38 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, CollisionFilterUpdate) {
   }
 }
 
+// Reproduction of issue from
+// https://stackoverflow.com/questions/78208761/drake-faking-attachment-between-floating-objects-with-locking-joints-is-it-po
+GTEST_TEST(SceneGraphCollisionCheckerTest, QuaternionFloatingJoint) {
+  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();
+  CollisionCheckerParams params;
+  params.model = builder.Build();
+  params.robot_model_instances.push_back(plant.GetModelInstanceByName("arm"));
+  params.edge_step_size = 0.05;
+  SceneGraphCollisionChecker dut(std::move(params));
+
+  // As constructed, collision filters must be consistent.
+  EXPECT_NO_THROW(EnforceCollisionFilterConsistency(dut));
+}
+
 }  // namespace test
 }  // namespace planning
 }  // namespace drake