From 01be6e74dc0ba88264f2f3ce1b2c50b1e105cc36 Mon Sep 17 00:00:00 2001
From: Eric Cousineau <eric.cousineau@tri.global>
Date: Tue, 26 May 2020 20:55:24 -0400
Subject: [PATCH] Should actually remove bodies...

---
 .../multibody_plant_subgraph.py               |  2 ++
 .../test/multibody_plant_subgraph_test.py     | 35 +++++++++----------
 2 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py b/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py
index 3ce67ad8..d12748a5 100644
--- a/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py
+++ b/drake_stuff/multibody_plant_prototypes/multibody_plant_subgraph.py
@@ -828,6 +828,8 @@ def remove_body(self, body):
         """
         elem_src = self._elem_src
         elem_removed = MultibodyPlantElementsList()
+        elem_src.bodies.remove(body)
+        elem_removed.bodies.append(body)
         frames = [
             x for x in elem_src.frames
             if x.body() is body]
diff --git a/drake_stuff/multibody_plant_prototypes/test/multibody_plant_subgraph_test.py b/drake_stuff/multibody_plant_prototypes/test/multibody_plant_subgraph_test.py
index 55443de4..1dc7f9d4 100644
--- a/drake_stuff/multibody_plant_prototypes/test/multibody_plant_subgraph_test.py
+++ b/drake_stuff/multibody_plant_prototypes/test/multibody_plant_subgraph_test.py
@@ -262,8 +262,8 @@ def test_composition_gripper_workflow(self):
         self.do_exploding_iiwa_sim(plant, scene_graph, context)
 
     def do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src):
-        # role = Role.kIllustration
-        role = Role.kProximity
+        role = Role.kIllustration
+        # role = Role.kProximity
         # Show a simulation which "changes state" by being rebuilt.
         builder = DiagramBuilder()
         plant, scene_graph = AddMultibodyPlantSceneGraph(
@@ -272,9 +272,10 @@ def do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src):
             plant_src, scene_graph_src)
         to_plant = subgraph_src.add_to(plant, scene_graph)
         # Add ground plane.
+        X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0])
         plant.RegisterCollisionGeometry(
-            plant.world_body(), HalfSpace.MakePose([0, 0, 1], [0, 0, 0]),
-            HalfSpace(), "collision", CoulombFriction(1, 1))
+            plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision",
+            CoulombFriction(0.8, 0.3))
         plant.Finalize()
         # Loosey-goosey.
         for model in mut.get_model_instances(plant):
@@ -311,6 +312,9 @@ def monitor(d_context):
         simulator.set_monitor(monitor)
         simulator.set_target_realtime_rate(1.)
         simulator.AdvanceTo(2.)
+        # Try to push a bit further?
+        simulator.clear_monitor()
+        simulator.AdvanceTo(d_context.get_time() + 0.05)
         diagram.Publish(d_context)
 
         # Recreate simulator.
@@ -320,23 +324,18 @@ def monitor(d_context):
 
         subgraph = mut.MultibodyPlantSubgraph.from_plant(plant, scene_graph)
         # Remove all joints; make them floating bodies.
-        remove_joints = True  # False
-        # TODO(eric.cousineau): Why does `remove_joints = True` cause a TAMSI
-        # error? If the joints are removed
-        # Failure at external/drake/multibody/plant/tamsi_solver.cc:217 in
-        # SolveQuadraticForTheSmallestPositiveRoot(): condition 'Delta > 0'
-        # failed.
-        if remove_joints:
-            for joint in mut.get_joints(plant):
-                subgraph.remove_joint(joint)
+        for joint in mut.get_joints(plant):
+            subgraph.remove_joint(joint)
+        # Remove massless / low inertia bodies?
+        for body in mut.get_bodies(plant):
+            if body is plant.world_body():
+                continue
+            if body.default_mass() == 0.:
+                subgraph.remove_body(body)
 
         to_new = subgraph.add_to(plant_new, scene_graph_new)
         plant_new.Finalize()
 
-        if not remove_joints:
-            for model in mut.get_model_instances(plant_new):
-                no_control(builder_new, plant_new, model)
-
         if VISUALIZE:
             ConnectDrakeVisualizer(builder_new, scene_graph_new, role=role)
             ConnectContactResultsToDrakeVisualizer(builder_new, plant_new)
@@ -353,7 +352,7 @@ def monitor(d_context):
         simulator_new.Initialize()
         diagram_new.Publish(d_context_new)
         simulator_new.set_target_realtime_rate(1.)
-        simulator_new.AdvanceTo(context_new.get_time() + 0.5)
+        simulator_new.AdvanceTo(context_new.get_time() + 2)
 
     def test_decomposition_controller_like_workflow(self):
         """Tests subgraphs (post-finalize) for decomposition, with a