def test_system_monitor(self): x = Variable("x") sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3]) simulator = Simulator(sys) def monitor(root_context): context = sys.GetMyContextFromRoot(root_context) if context.get_time() >= 1.: return EventStatus.ReachedTermination(sys, "Time reached") else: return EventStatus.DidNothing() self.assertIsNone(simulator.get_monitor()) simulator.set_monitor(monitor) self.assertIsNotNone(simulator.get_monitor()) status = simulator.AdvanceTo(2.) self.assertEqual( status.reason(), SimulatorStatus.ReturnReason.kReachedTerminationCondition) self.assertLess(status.return_time(), 1.1) simulator.clear_monitor() self.assertIsNone(simulator.get_monitor())
def test_exploding_iiwa_sim(self): """ Shows a simulation of a falling + exploding IIWA which "changes topology" by being rebuilt without joints. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") Parser(plant).AddModelFromFile(iiwa_file, "iiwa") # Add ground plane. X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0]) plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) plant.Finalize() # Loosey-goosey - no control. for model in me.get_model_instances(plant): util.build_with_no_control(builder, plant, model) if VISUALIZE: print("test_exploding_iiwa_sim") role = Role.kIllustration DrakeVisualizer.AddToBuilder( builder, scene_graph, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph) diagram = builder.Build() # Set up context. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) # - Hoist IIWA up in the air. plant.SetFreeBodyPose(context, plant.GetBodyByName("base"), RigidTransform([0, 0, 1.])) # - Set joint velocities to "spin" it in the air. for joint in me.get_joints(plant): if isinstance(joint, RevoluteJoint): me.set_joint_positions(plant, context, joint, 0.7) me.set_joint_velocities(plant, context, joint, -5.) def monitor(d_context): # Stop the simulation once there's any contact. context = plant.GetMyContextFromRoot(d_context) query_object = plant.get_geometry_query_input_port().Eval(context) if query_object.HasCollisions(): return EventStatus.ReachedTermination(plant, "Contact") else: return EventStatus.DidNothing() # Forward simulate. simulator = Simulator(diagram, d_context) simulator.Initialize() simulator.set_monitor(monitor) if VISUALIZE: 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. builder_new = DiagramBuilder() plant_new, scene_graph_new = AddMultibodyPlantSceneGraph( builder_new, time_step=plant.time_step()) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant, scene_graph)) # Remove all joints; make them floating bodies. for joint in me.get_joints(plant): subgraph.remove_joint(joint) # Remove massless bodies. # For more info, see: https://stackoverflow.com/a/62035705/7829525 for body in me.get_bodies(plant): if body is plant.world_body(): continue if body.default_mass() == 0.: subgraph.remove_body(body) # Finalize. to_new = subgraph.add_to(plant_new, scene_graph_new) plant_new.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder( builder_new, scene_graph_new, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder_new, plant_new, scene_graph_new) diagram_new = builder_new.Build() # Remap state. d_context_new = diagram_new.CreateDefaultContext() d_context_new.SetTime(d_context.get_time()) context_new = plant_new.GetMyContextFromRoot(d_context_new) to_new.copy_state(context, context_new) # Simulate. simulator_new = Simulator(diagram_new, d_context_new) simulator_new.Initialize() diagram_new.Publish(d_context_new) if VISUALIZE: simulator_new.set_target_realtime_rate(1.) simulator_new.AdvanceTo(context_new.get_time() + 2) if VISUALIZE: input(" Press enter...")
def do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src): # Show a simulation which "changes state" by being rebuilt. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) subgraph_src = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(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(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) plant.Finalize() # Loosey-goosey - no control. for model in me.get_model_instances(plant): util.no_control(builder, plant, model) if VISUALIZE: print("do_exploding_iiwa_sim") role = Role.kIllustration ConnectDrakeVisualizer(builder, scene_graph, role=role) ConnectContactResultsToDrakeVisualizer(builder, plant) diagram = builder.Build() # Set up context. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) to_plant.copy_state(context_src, context) # - Hoist IIWA up in the air. plant.SetFreeBodyPose(context, plant.GetBodyByName("base"), RigidTransform([0, 0, 1.])) # - Set joint velocities to "spin" it in the air. for joint in me.get_joints(plant): if isinstance(joint, RevoluteJoint): me.set_joint_positions(plant, context, joint, 0.7) me.set_joint_velocities(plant, context, joint, -5.) def monitor(d_context): # Stop the simulation once there's any contact. context = plant.GetMyContextFromRoot(d_context) query_object = plant.get_geometry_query_input_port().Eval(context) if query_object.HasCollisions(): return EventStatus.ReachedTermination(plant, "Contact") else: return EventStatus.DidNothing() # Forward simulate. simulator = Simulator(diagram, d_context) simulator.Initialize() simulator.set_monitor(monitor) if VISUALIZE: 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. builder_new = DiagramBuilder() plant_new, scene_graph_new = AddMultibodyPlantSceneGraph( builder_new, time_step=plant.time_step()) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant, scene_graph)) # Remove all joints; make them floating bodies. for joint in me.get_joints(plant): subgraph.remove_joint(joint) # Remove massless bodies. for body in me.get_bodies(plant): if body is plant.world_body(): continue if body.default_mass() == 0.: subgraph.remove_body(body) # Finalize. to_new = subgraph.add_to(plant_new, scene_graph_new) plant_new.Finalize() if VISUALIZE: ConnectDrakeVisualizer(builder_new, scene_graph_new, role=role) ConnectContactResultsToDrakeVisualizer(builder_new, plant_new) diagram_new = builder_new.Build() # Remap state. d_context_new = diagram_new.CreateDefaultContext() d_context_new.SetTime(d_context.get_time()) context_new = plant_new.GetMyContextFromRoot(d_context_new) to_new.copy_state(context, context_new) # Simulate. simulator_new = Simulator(diagram_new, d_context_new) simulator_new.Initialize() diagram_new.Publish(d_context_new) if VISUALIZE: simulator_new.set_target_realtime_rate(1.) simulator_new.AdvanceTo(context_new.get_time() + 2) if VISUALIZE: input(" Press enter...")