fixes missing multibody graph accounting (RobotLocomotion#14411)

Slumon · Dec 8, 2020 · 098e830 · 098e830
1 parent c0bb0ff
commit 098e830
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 10 deletions.
diff --git a/bindings/pydrake/multibody/test/plant_test.py b/bindings/pydrake/multibody/test/plant_test.py
@@ -320,12 +320,8 @@ def check_repr(element, expected):
         self.assertTrue(plant.HasModelInstanceNamed(name="acrobot"))
         self.assertEqual(
             model_instance, plant.GetModelInstanceByName(name="acrobot"))
-        if T == float:
-            # TODO(#14346): Re-enable for other scalar types once this is
-            # supported.
-            self.assertEqual(
-                plant.GetBodiesWeldedTo(plant.world_body()),
-                [plant.world_body()])
+        self.assertEqual(
+            plant.GetBodiesWeldedTo(plant.world_body()), [plant.world_body()])
         self.assertIsInstance(
             plant.get_actuation_input_port(), InputPort)
         self.assertIsInstance(

diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h
@@ -782,8 +782,19 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
     X_WB_default_list_ = other.X_WB_default_list_;
     contact_model_ = other.contact_model_;
     penetration_allowance_ = other.penetration_allowance_;
+
     DeclareSceneGraphPorts();
 
+    // Do accounting for MultibodyGraph
+    for (BodyIndex index(0); index < num_bodies(); ++index) {
+      const Body<T>& body = get_body(index);
+      multibody_graph_.AddBody(body.name(), body.model_instance());
+    }
+
+    for (JointIndex index(0); index < num_joints(); ++index) {
+      RegisterJointInGraph(get_joint(index));
+    }
+
     // MultibodyTree::CloneToScalar() already called MultibodyTree::Finalize()
     // on the new MultibodyTree on U. Therefore we only Finalize the plant's
     // internals (and not the MultibodyTree).

diff --git a/multibody/plant/test/multibody_plant_test.cc b/multibody/plant/test/multibody_plant_test.cc
@@ -1385,12 +1385,16 @@ GTEST_TEST(MultibodyPlantTest, GetBodiesWeldedTo) {
               UnorderedElementsAre(&upper, &lower));
 
   // Briefly test scalar conversion.
-  // TODO(#14346): Make this a positive test once it's supported.
   std::unique_ptr<MultibodyPlant<AutoDiffXd>> plant_ad =
       systems::System<double>::ToAutoDiffXd(plant);
-  EXPECT_THROW(
-      plant_ad->GetBodiesWeldedTo(plant_ad->world_body()),
-      std::runtime_error);
+  const Body<AutoDiffXd>& upper_ad = plant_ad->GetBodyByName("upper_section");
+  const Body<AutoDiffXd>& lower_ad = plant_ad->GetBodyByName("lower_section");
+  const Body<AutoDiffXd>& extra_ad = plant_ad->GetBodyByName("extra");
+
+  EXPECT_THAT(plant_ad->GetBodiesWeldedTo(plant_ad->world_body()),
+              UnorderedElementsAre(&plant_ad->world_body(), &extra_ad));
+  EXPECT_THAT(plant_ad->GetBodiesWeldedTo(lower_ad),
+              UnorderedElementsAre(&upper_ad, &lower_ad));
 }
 
 // Utility to verify that the only ports of MultibodyPlant that are feedthrough