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
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
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, 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()