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
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
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
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