class ManipulationStationCollisionChecker: def __init__(self, is_visualizing=False): self.station = ManipulationStation() self.station.SetupManipulationClassStation( IiwaCollisionModel.kBoxCollision) self.station.Finalize() self.plant = self.station.get_mutable_multibody_plant() self.scene_graph = self.station.get_mutable_scene_graph() self.is_visualizing = is_visualizing # scene graph query output port. self.query_output_port = self.scene_graph.GetOutputPort("query") builder = DiagramBuilder() builder.AddSystem(self.station) # meshcat visualizer if is_visualizing: self.viz = MeshcatVisualizer(self.scene_graph, zmq_url="tcp://127.0.0.1:6000") # connect meshcat to manipulation station builder.AddSystem(self.viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), self.viz.GetInputPort("lcm_visualization")) self.diagram = builder.Build() # contexts self.context_diagram = self.diagram.CreateDefaultContext() self.context_station = self.diagram.GetSubsystemContext( self.station, self.context_diagram) self.context_scene_graph = self.station.GetSubsystemContext( self.scene_graph, self.context_station) self.context_plant = self.station.GetMutableSubsystemContext( self.plant, self.context_station) if is_visualizing: self.viz.load() self.context_meshcat = self.diagram.GetSubsystemContext( self.viz, self.context_diagram) def SetStationConfiguration(self, q_iiwa, gripper_setpoint, left_door_angle, right_door_angle): """ :param q_iiwa: (7,) numpy array, joint angle of robots in radian. :param gripper_setpoint: float, gripper opening distance in meters. :param left_door_angle: float, left door hinge angle, \in [0, pi/2]. :param right_door_angle: float, right door hinge angle, \in [0, pi/2]. :return: """ self.station.SetIiwaPosition(self.context_station, q_iiwa) self.station.SetWsgPosition(self.context_station, gripper_setpoint) # cabinet doors if left_door_angle > 0: left_door_angle *= -1 left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle(context=self.context_plant, angle=left_door_angle) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle(context=self.context_plant, angle=right_door_angle) def DrawStation(self, q_iiwa, gripper_setpoint, q_door_left, q_door_right): if not self.is_visualizing: print("collision checker is not initialized with visualization.") return self.SetStationConfiguration(q_iiwa, gripper_setpoint, q_door_left, q_door_right) self.viz.DoPublish(self.context_meshcat, []) def ExistsCollision(self, q_iiwa, gripper_setpoint, q_door_left, q_door_right): self.SetStationConfiguration(q_iiwa, gripper_setpoint, q_door_left, q_door_right) query_object = self.query_output_port.Eval(self.context_scene_graph) collision_paris = query_object.ComputePointPairPenetration() return len(collision_paris) > 0
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)
class ManipulationStationSimulator: def __init__( self, time_step, object_file_path=None, object_base_link_name=None, X_WObject=X_WObject_default, ): self.object_base_link_name = object_base_link_name self.time_step = time_step # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() if object_file_path is not None: self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() # Initial pose of the object self.X_WObject = X_WObject def RunSimulation(self, plans_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7), is_visualizing=True): """ Constructs a Diagram that sends commands to ManipulationStation. @param plans_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @param extra_time: Time in seconds that the simulation is run, in addition to the sum of the durations of all plans. @param real_time_rate: 1.0 means realtime; 0 means as fast as possible. @param q0_kuka: initial configuration of the robot. @param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False when running tests. @return: logs of robot configuration and MultibodyPlant, generated by simulation. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, self.station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, self.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), self.station.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), self.station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add meshcat visualizer if is_visualizing: scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=True, plant=self.plant) builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) builder.Connect(self.station.GetOutputPort("contact_results"), viz.GetInputPort("contact_results")) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( self.station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( self.station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) plant_state_log = LogOutput( self.station.GetOutputPort("plant_continuous_state"), builder) plant_state_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() if is_visualizing: viz.load() time.sleep(2.0) # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgPosition(0.05, context) self.station.SetWsgVelocity(0, context) # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-0.001) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=0.001) # set initial pose of the object if self.object_base_link_name is not None: self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), self.X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 sim_duration += extra_time print "simulation duration", sim_duration simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, \ iiwa_external_torque_log, \ plant_state_log def RunRealRobot(self, plans_list, gripper_setpoint_list): """ Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. @param plans_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() camera_ids = ["805212060544"] station_hardware = ManipulationStationHardwareInterface(camera_ids) station_hardware.Connect(wait_for_cameras=False) builder.AddSystem(station_hardware) # Add plan runner # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station_hardware.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station_hardware.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station_hardware.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), station_hardware.GetInputPort("iiwa_feedforward_torque")) builder.Connect( station_hardware.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect( station_hardware.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station_hardware.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station_hardware.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, iiwa_external_torque_log
class ManipulationStationSimulator: def __init__( self, time_step, object_file_path=None, object_base_link_name=None, X_WObject=X_WObject_default, ): self.object_base_link_name = object_base_link_name self.time_step = time_step # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.SetupDefaultStation() self.plant = self.station.get_mutable_multibody_plant() if object_file_path is not None: self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() self.simulator = None self.plan_runner = None # Initial pose of the object self.X_WObject = X_WObject def RunSimulation(self, plan_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7), is_visualizing=True, sim_duration=None, is_plan_runner_diagram=False): """ Constructs a Diagram that sends commands to ManipulationStation. @param plan_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @param extra_time: the amount of time for which commands are sent, in addition to the sum of the durations of all plans. @param real_time_rate: 1.0 means realtime; 0 means as fast as possible. @param q0_kuka: initial configuration of the robot. @param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False when running tests. @param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of all plans in plan_list plus extra_time. @param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version of PlanRunner. @return: logs of robot configuration and MultibodyPlant, generated by simulation. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner. if is_plan_runner_diagram: plan_runner, duration_multiplier = CreateManipStationPlanRunnerDiagram( kuka_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list) else: plan_runner = ManipStationPlanRunner( kuka_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list) duration_multiplier = plan_runner.kPlanDurationMultiplier self.plan_runner = plan_runner builder.AddSystem(plan_runner) builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"), self.station.GetInputPort("wsg_position")) builder.Connect(plan_runner.GetOutputPort("force_limit"), self.station.GetInputPort("wsg_force_limit")) builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"), self.station.GetInputPort("iiwa_position")) builder.Connect(plan_runner.GetOutputPort("iiwa_torque_command"), self.station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) builder.Connect(self.station.GetOutputPort("iiwa_torque_external"), plan_runner.GetInputPort("iiwa_torque_external")) # Add meshcat visualizer if is_visualizing: scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=True, plant=self.plant) builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) builder.Connect(self.station.GetOutputPort("contact_results"), viz.GetInputPort("contact_results")) # Add logger iiwa_position_command_log = LogOutput( plan_runner.GetOutputPort("iiwa_position_command"), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( self.station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( self.station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) plant_state_log = LogOutput( self.station.GetOutputPort("plant_continuous_state"), builder) plant_state_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() if is_visualizing: viz.load() time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) self.simulator = simulator context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgPosition(0.05, context) self.station.SetWsgVelocity(0, context) # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-0.001) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=0.001) # set initial pose of the object if self.object_base_link_name is not None: self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), self.X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) # calculate starting time for all plans. t_plan = GetPlanStartingTimes(plan_list, duration_multiplier) if sim_duration is None: sim_duration = t_plan[-1] + extra_time print "simulation duration", sim_duration print "plan starting times\n", t_plan simulator.Initialize() self.SetInitialPlanRunnerState(plan_runner, simulator, diagram) simulator.StepTo(sim_duration) return iiwa_position_command_log, iiwa_position_measured_log, \ iiwa_external_torque_log, plant_state_log, t_plan def RunRealRobot( self, plan_list, gripper_setpoint_list, sim_duration=None, extra_time=2.0, is_plan_runner_diagram=False, ): """ Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. @param plan_list: A list of Plans to be executed. @param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan. @param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of all plans in plan_list plus extra_time. @param extra_time: the amount of time for which commands are sent, in addition to the duration of all plans. @param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version of PlanRunner. @return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver. Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data(). """ builder = DiagramBuilder() camera_ids = ["805212060544"] station_hardware = ManipulationStationHardwareInterface(camera_ids) station_hardware.Connect(wait_for_cameras=False) builder.AddSystem(station_hardware) # Add plan runner. if is_plan_runner_diagram: plan_runner, duration_multiplier = CreateManipStationPlanRunnerDiagram( kuka_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list, print_period=0, ) else: plan_runner = ManipStationPlanRunner( kuka_plans=plan_list, gripper_setpoint_list=gripper_setpoint_list, print_period=0, ) duration_multiplier = plan_runner.kPlanDurationMultiplier builder.AddSystem(plan_runner) builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"), station_hardware.GetInputPort("wsg_position")) builder.Connect(plan_runner.GetOutputPort("force_limit"), station_hardware.GetInputPort("wsg_force_limit")) builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"), station_hardware.GetInputPort("iiwa_position")) builder.Connect( plan_runner.GetOutputPort("iiwa_torque_command"), station_hardware.GetInputPort("iiwa_feedforward_torque")) builder.Connect( station_hardware.GetOutputPort("iiwa_position_measured"), plan_runner.GetInputPort("iiwa_position")) builder.Connect( station_hardware.GetOutputPort("iiwa_velocity_estimated"), plan_runner.GetInputPort("iiwa_velocity")) builder.Connect(station_hardware.GetOutputPort("iiwa_torque_external"), plan_runner.GetInputPort("iiwa_torque_external")) # Add logger iiwa_position_command_log = LogOutput( plan_runner.GetOutputPort("iiwa_position_command"), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( station_hardware.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) iiwa_velocity_estimated_log = LogOutput( station_hardware.GetOutputPort("iiwa_velocity_estimated"), builder) iiwa_velocity_estimated_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station_hardware.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) # build diagram diagram = builder.Build() # RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) self.simulator = simulator simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) t_plan = GetPlanStartingTimes(plan_list, duration_multiplier) if sim_duration is None: sim_duration = t_plan[-1] + extra_time print "simulation duration", sim_duration print "plan starting times\n", t_plan print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.Initialize() self.SetInitialPlanRunnerState(plan_runner, simulator, diagram) simulator.StepTo(sim_duration) return iiwa_position_command_log, iiwa_position_measured_log, \ iiwa_velocity_estimated_log, iiwa_external_torque_log, t_plan def SetInitialPlanRunnerState(self, plan_runner, simulator, diagram): """ Sets iiwa_position_command, part of the discrete state of plan_runner, to the initial state of the robot at t=0. Otherwise the position command at t=0 is 0, driving the robot to its upright position, usually resulting in huge velocity. Calling this function after simulator.Initialize() puts a column of zeros at the beginning of iiwa_position_command_log, but that zero command doesn't seem to be sent to the robot. TODO: turning off publishing at initialization should remove those zeros, but the findings for that function doesn't exist yet. """ plan_runner_context = \ diagram.GetMutableSubsystemContext(plan_runner, simulator.get_mutable_context()) iiwa_position_input_port = plan_runner.GetInputPort("iiwa_position") q0_iiwa = plan_runner.EvalVectorInput( plan_runner_context, iiwa_position_input_port.get_index()).get_value() for i in range(7): plan_runner_context.get_mutable_discrete_state(0).SetAtIndex( i, q0_iiwa[i])
class ManipulationStationSimulator: def __init__(self, time_step, object_file_path, object_base_link_name, is_hardware=False): self.object_base_link_name = object_base_link_name self.time_step = time_step self.is_hardware = is_hardware # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() def RunSimulation(self, plans_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7)): ''' Constructs a Diagram that sends commands to ManipulationStation. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner plan_runner = KukaPlanRunner(self.plant) builder.AddSystem(plan_runner) builder.Connect(plan_runner.get_output_port(0), self.station.GetInputPort("iiwa_position")) # Add state machine. state_machine = ManipStateMachine( plant=self.plant, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(state_machine) builder.Connect(state_machine.kuka_plan_output_port, plan_runner.plan_input_port) builder.Connect(state_machine.hand_setpoint_output_port, self.station.GetInputPort("wsg_position")) builder.Connect(state_machine.gripper_force_limit_output_port, self.station.GetInputPort("wsg_force_limit")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), state_machine.iiwa_position_input_port) # Add meshcat visualizer from underactuated.meshcat_visualizer import MeshcatVisualizer scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph) builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.get_input_port(0)) # Add logger iiwa_position_command_log = builder.AddSystem( SignalLogger(self.station.GetInputPort("iiwa_position").size())) iiwa_position_command_log._DeclarePeriodicPublish(0.005) builder.Connect(plan_runner.get_output_port(0), iiwa_position_command_log.get_input_port(0)) # build diagram diagram = builder.Build() viz.load() time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgState(0.05, 0, context) # set initial hinge angles of the cupboard. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-np.pi / 2) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=np.pi / 2) # set initial pose of the object X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) # fix feedforward torque input to 0. context.FixInputPort( self.station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 sim_duration += extra_time print "simulation duration", sim_duration simulator.StepTo(sim_duration) return iiwa_position_command_log def RunRealRobot(self, plans_list, gripper_setpoint_list): ''' Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' from pydrake.examples.manipulation_station import ManipulationStationHardwareInterface builder = DiagramBuilder() self.station_hardware = ManipulationStationHardwareInterface() builder.AddSystem(self.station_hardware) # Add plan runner plan_runner = KukaPlanRunner(self.plant) builder.AddSystem(plan_runner) builder.Connect(plan_runner.get_output_port(0), self.station_hardware.GetInputPort("iiwa_position")) # Add state machine. state_machine = ManipStateMachine( plant=self.plant, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(state_machine) builder.Connect(state_machine.kuka_plan_output_port, plan_runner.plan_input_port) builder.Connect(state_machine.hand_setpoint_output_port, self.station_hardware.GetInputPort("wsg_position")) builder.Connect(state_machine.gripper_force_limit_output_port, self.station_hardware.GetInputPort("wsg_force_limit")) builder.Connect( self.station_hardware.GetOutputPort("iiwa_position_measured"), state_machine.iiwa_position_input_port) # Add logger iiwa_position_command_log = builder.AddSystem( SignalLogger( self.station_hardware.GetInputPort("iiwa_position").size())) builder.Connect(plan_runner.get_output_port(0), iiwa_position_command_log.get_input_port(0)) # build diagram diagram = builder.Build() RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station_hardware, simulator.get_mutable_context()) context.FixInputPort( self.station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7)) simulator.set_target_realtime_rate(1.0) simulator.Initialize() # simulator.set_publish_at_initialization(False) sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.StepTo(sim_duration) return iiwa_position_command_log
from demo import DepthCameraDemoSystem from pydrake.examples.manipulation_station import ( ManipulationStation, ManipulationStationHardwareInterface) from pydrake.systems.drawing import plot_system_graphviz station = ManipulationStation() station.SetupManipulationClassStation() # station.SetupClutterClearingStation() station.Finalize() context = station.CreateDefaultContext() camera_names = station.get_camera_names() index = 1 for name in camera_names: color_image = station.GetOutputPort( "camera_" + name + "_rgb_image").Eval(context) depth_image = station.GetOutputPort( "camera_" + name + "_depth_image").Eval(context) plt.subplot(len(camera_names), 2, index) plt.imshow(color_image.data) index += 1 plt.title('Color image') plt.subplot(len(camera_names), 2, index) plt.imshow(np.squeeze(depth_image.data)) index += 1 plt.title('Depth image') plt.show()
class ManipulationStationSimulator: def __init__(self, time_step, object_file_path, object_base_link_name, is_hardware=False): self.object_base_link_name = object_base_link_name self.time_step = time_step self.is_hardware = is_hardware # Finalize manipulation station by adding manipuland. self.station = ManipulationStation(self.time_step) self.station.AddCupboard() self.plant = self.station.get_mutable_multibody_plant() self.object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=self.station.get_mutable_multibody_plant(), scene_graph=self.station.get_mutable_scene_graph()) self.station.Finalize() # Initial pose of the object X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) self.X_WObject = X_WObject def RunSimulation(self, plans_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, q0_kuka=np.zeros(7)): ''' Constructs a Diagram that sends commands to ManipulationStation. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' builder = DiagramBuilder() builder.AddSystem(self.station) # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, self.station.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, self.station.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), self.station.GetInputPort("iiwa_position")) builder.Connect(demux.get_output_port(1), self.station.GetInputPort("iiwa_feedforward_torque")) builder.Connect(self.station.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add meshcat visualizer scene_graph = self.station.get_mutable_scene_graph() viz = MeshcatVisualizer(scene_graph, is_drawing_contact_force=True, plant=self.plant) self.viz = viz builder.AddSystem(viz) builder.Connect(self.station.GetOutputPort("pose_bundle"), viz.GetInputPort("lcm_visualization")) builder.Connect(self.station.GetOutputPort("contact_results"), viz.GetInputPort("contact_results")) # Add logger iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder) iiwa_position_command_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( self.station.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) iiwa_position_measured_log = LogOutput( self.station.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput( self.station.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DeclarePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) # build diagram diagram = builder.Build() viz.load() time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) context = diagram.GetMutableSubsystemContext( self.station, simulator.get_mutable_context()) # set initial state of the robot self.station.SetIiwaPosition(q0_kuka, context) self.station.SetIiwaVelocity(np.zeros(7), context) self.station.SetWsgPosition(0.05, context) self.station.SetWsgVelocity(0, context) # set initial hinge angles of the cupboard. # setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact # with small contact forces between the door and the cupboard body. left_hinge_joint = self.plant.GetJointByName("left_door_hinge") left_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=-0.001) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext( self.plant, context), angle=0.001) # set initial pose of the object self.plant.tree().SetFreeBodyPoseOrThrow( self.plant.GetBodyByName(self.object_base_link_name, self.object), self.X_WObject, self.station.GetMutableSubsystemContext(self.plant, context)) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 sim_duration += extra_time print "simulation duration", sim_duration simulator.StepTo(sim_duration) return iiwa_position_command_log, \ iiwa_position_measured_log, \ iiwa_external_torque_log, \ wsg_state_log, \ wsg_command_log def RunRealRobot(self, plans_list, gripper_setpoint_list): ''' Constructs a Diagram that sends commands to ManipulationStationHardwareInterface. :param plans_list: :param gripper_setpoint_list: :param extra_time: :param real_time_rate: :param q0_kuka: :return: ''' builder = DiagramBuilder() camera_ids = ["805212060544"] station_hardware = ManipulationStationHardwareInterface(camera_ids) station_hardware.Connect(wait_for_cameras=False) builder.AddSystem(station_hardware) # Add plan runner # Add plan runner. plan_runner = ManipStationPlanRunner( station=self.station, kuka_plans=plans_list, gripper_setpoint_list=gripper_setpoint_list) builder.AddSystem(plan_runner) builder.Connect(plan_runner.hand_setpoint_output_port, station_hardware.GetInputPort("wsg_position")) builder.Connect(plan_runner.gripper_force_limit_output_port, station_hardware.GetInputPort("wsg_force_limit")) demux = builder.AddSystem(Demultiplexer(14, 7)) builder.Connect( plan_runner.GetOutputPort("iiwa_position_and_torque_command"), demux.get_input_port(0)) builder.Connect(demux.get_output_port(0), station_hardware.GetInputPort("iiwa_position")) builder.Connect( demux.get_output_port(1), station_hardware.GetInputPort("iiwa_feedforward_torque")) builder.Connect( station_hardware.GetOutputPort("iiwa_position_measured"), plan_runner.iiwa_position_input_port) builder.Connect( station_hardware.GetOutputPort("iiwa_velocity_estimated"), plan_runner.iiwa_velocity_input_port) # Add logger iiwa_position_measured_log = LogOutput( station_hardware.GetOutputPort("iiwa_position_measured"), builder) iiwa_position_measured_log._DeclarePeriodicPublish(0.005) iiwa_external_torque_log = LogOutput( station_hardware.GetOutputPort("iiwa_torque_external"), builder) iiwa_external_torque_log._DeclarePeriodicPublish(0.005) wsg_state_log = LogOutput( station_hardware.GetOutputPort("wsg_state_measured"), builder) wsg_state_log._DecalrePeriodicPublish(0.1) wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder) wsg_command_log._DeclarePeriodicPublish(0.1) # build diagram diagram = builder.Build() RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) sim_duration = 0. for plan in plans_list: sim_duration += plan.get_duration() * 1.1 print "sending trajectories in 2 seconds..." time.sleep(1.0) print "sending trajectories in 1 second..." time.sleep(1.0) print "sending trajectories now!" simulator.StepTo(sim_duration) return iiwa_position_measured_log, iiwa_external_torque_log, wsg_state_log, wsg_command_log def Replay(self): self.viz.replay()
for name in model_names: models_object[name] = [] for i in range(3): models_object[name].append( parser.AddModelFromFile( os.path.join("cad_files", name + '_simplified.sdf'), name + str(i))) station.Finalize() builder = DiagramBuilder() builder.AddSystem(station) ConnectMeshcatVisualizer(builder, scene_graph=station.get_scene_graph(), output_port=station.GetOutputPort("query_object")) diagram = builder.Build() #%% context = diagram.CreateDefaultContext() context_station = diagram.GetSubsystemContext(station, context) context_plant = station.GetSubsystemContext(station.get_multibody_plant(), context_station) #%% plant = station.get_multibody_plant() model_iiwa = plant.GetModelInstanceByName("iiwa") model_gripper = plant.GetModelInstanceByName("gripper") model_bin1 = plant.GetModelInstanceByName("bin1") model_bin2 = plant.GetModelInstanceByName("bin2")
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") ConnectDrakeVisualizer(builder, scene_graph, station.GetOutputPort("pose_bundle")) 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] q_wsg = [-0.03, 0.03] plant.SetPositions(context, iiwa, q_iiwa) plant.SetPositions(context, wsg, q_wsg) # Build and visualize. control_builder = DiagramBuilder() control_plant, control_scene_graph = AddMultibodyPlantSceneGraph( control_builder, time_step=0.) if VISUALIZE: ConnectDrakeVisualizer(control_builder, control_scene_graph) # N.B. This has the same scene, but with all joints outside of the # IIWA frozen. 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, scene_graph_dest=control_scene_graph) self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap) control_plant.Finalize() self.assertEqual(control_plant.num_positions(), plant.num_positions(iiwa)) 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_frames(plant, context, control_plant, control_context, "iiwa_link_0", "iiwa_link_7") util.compare_frames(plant, context, control_plant, control_context, "body", "left_finger") # Visualize. if VISUALIZE: print(" Visualize original plant") Simulator(diagram, d_context.Clone()).Initialize() input(" Press enter...") print(" Visualize control plant") Simulator(control_diagram, control_d_context.Clone()).Initialize() input(" Press enter...") # For grins, ensure that we can copy everything, including world weld. control_plant_copy = MultibodyPlant(time_step=0.) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(control_plant)) subgraph.add_to(control_plant_copy) control_plant_copy.Finalize() self.assertEqual(control_plant_copy.num_positions(), control_plant.num_positions()) util.assert_plant_equals(control_plant, None, control_plant_copy, None)