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 test_decomposition_controller_like_workflow(self): """Tests subgraphs (post-finalize) for decomposition, with a scene-graph. Also shows a workflow of replacing joints / welding joints.""" builder = DiagramBuilder() # N.B. I (Eric) am using ManipulationStation because it's currently # the simplest way to get a complex scene in pydrake. station = ManipulationStation(time_step=0.001) station.SetupManipulationClassStation() station.Finalize() builder.AddSystem(station) plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() iiwa = plant.GetModelInstanceByName("iiwa") wsg = plant.GetModelInstanceByName("gripper") if VISUALIZE: print("test_decomposition_controller_like_workflow") DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) diagram = builder.Build() # Set the context with which things should be computed. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7] ndof = 7 q_wsg = [-0.03, 0.03] plant.SetPositions(context, iiwa, q_iiwa) plant.SetPositions(context, wsg, q_wsg) # Add copy of only the IIWA to a control diagram. control_builder = DiagramBuilder() control_plant = control_builder.AddSystem(MultibodyPlant(time_step=0)) # N.B. This has the same scene, but with all joints outside of the # IIWA frozen # TODO(eric.cousineau): Re-investigate weird "build_with_no_control" # behavior (with scene graph) with default conditions and time_step=0 # - see Anzu commit 2cf08cfc3. to_control = mut.add_plant_with_articulated_subset_to( plant_src=plant, scene_graph_src=scene_graph, articulated_models_src=[iiwa], context_src=context, plant_dest=control_plant) self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap) control_iiwa = to_control.model_instances[iiwa] control_plant.Finalize() self.assertEqual(control_plant.num_positions(), plant.num_positions(iiwa)) kp = np.array([2000., 1500, 1500, 1500, 1500, 500, 500]) kd = np.sqrt(2 * kp) ki = np.zeros(7) controller = control_builder.AddSystem( InverseDynamicsController(robot=control_plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=False)) # N.B. Rather than use model instances for direct correspence, we could # use the mappings themselves within a custom system. control_builder.Connect( control_plant.get_state_output_port(control_iiwa), controller.get_input_port_estimated_state()) control_builder.Connect( controller.get_output_port_control(), control_plant.get_actuation_input_port(control_iiwa)) # Control to having the elbow slightly bent. q_iiwa_final = np.zeros(7) q_iiwa_final[3] = -np.pi / 2 t_start = 0. t_end = 1. nx = 2 * ndof def q_desired_interpolator(t: ContextTimeArg) -> VectorArg(nx): s = (t - t_start) / (t_end - t_start) ds = 1 / (t_end - t_start) q = q_iiwa + s * (q_iiwa_final - q_iiwa) v = ds * (q_iiwa_final - q_iiwa) x = np.hstack((q, v)) return x interpolator = control_builder.AddSystem( FunctionSystem(q_desired_interpolator)) control_builder.Connect(interpolator.get_output_port(0), controller.get_input_port_desired_state()) control_diagram = control_builder.Build() control_d_context = control_diagram.CreateDefaultContext() control_context = control_plant.GetMyContextFromRoot(control_d_context) to_control.copy_state(context, control_context) util.compare_frame_poses(plant, context, control_plant, control_context, "iiwa_link_0", "iiwa_link_7") util.compare_frame_poses(plant, context, control_plant, control_context, "body", "left_finger") from_control = to_control.inverse() def viz_monitor(control_d_context): # Simulate control, visualizing in original diagram. assert (control_context is control_plant.GetMyContextFromRoot(control_d_context)) from_control.copy_state(control_context, context) d_context.SetTime(control_d_context.get_time()) diagram.Publish(d_context) return EventStatus.DidNothing() simulator = Simulator(control_diagram, control_d_context) simulator.Initialize() if VISUALIZE: simulator.set_monitor(viz_monitor) simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(t_end)
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...")