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 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
Пример #3
0
    def build(self, real_time_rate=0, is_visualizing=False):
        # Create manipulation station simulator
        self.manip_station_sim = ManipulationStationSimulator(
            time_step=5e-3,
            object_file_path=object_file_path,
            object_base_link_name="base_link",
        )

        # Create plan runner
        plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram(
            station=self.manip_station_sim.station,
            kuka_plans=[],
            gripper_setpoint_list=[],
            rl_environment=True)
        self.manip_station_sim.plan_runner = plan_runner

        # Create builder and add systems
        builder = DiagramBuilder()
        builder.AddSystem(self.manip_station_sim.station)
        builder.AddSystem(plan_runner)

        # Connect systems
        builder.Connect(
            plan_runner.GetOutputPort("gripper_setpoint"),
            self.manip_station_sim.station.GetInputPort("wsg_position"))
        builder.Connect(
            plan_runner.GetOutputPort("force_limit"),
            self.manip_station_sim.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.manip_station_sim.station.GetInputPort("iiwa_position"))
        builder.Connect(
            demux.get_output_port(1),
            self.manip_station_sim.station.GetInputPort(
                "iiwa_feedforward_torque"))
        builder.Connect(
            self.manip_station_sim.station.GetOutputPort(
                "iiwa_position_measured"),
            plan_runner.GetInputPort("iiwa_position"))
        builder.Connect(
            self.manip_station_sim.station.GetOutputPort(
                "iiwa_velocity_estimated"),
            plan_runner.GetInputPort("iiwa_velocity"))

        # Add meshcat visualizer
        if is_visualizing:
            scene_graph = self.manip_station_sim.station.get_mutable_scene_graph(
            )
            viz = MeshcatVisualizer(scene_graph,
                                    is_drawing_contact_force=False,
                                    plant=self.manip_station_sim.plant)
            builder.AddSystem(viz)
            builder.Connect(
                self.manip_station_sim.station.GetOutputPort("pose_bundle"),
                viz.GetInputPort("lcm_visualization"))

        # Build diagram
        self.diagram = builder.Build()
        if is_visualizing:
            print("Setting up visualizer...")
            viz.load()
            time.sleep(2.0)

        # Construct Simulator
        self.simulator = Simulator(self.diagram)
        self.manip_station_sim.simulator = self.simulator

        self.simulator.set_publish_every_time_step(False)
        self.simulator.set_target_realtime_rate(real_time_rate)

        self.context = self.diagram.GetMutableSubsystemContext(
            self.manip_station_sim.station,
            self.simulator.get_mutable_context())

        self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName(
            "left_door_hinge")
        self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName(
            "right_door_hinge")

        # Goal for training
        self.goal_position = np.array([0.85, 0, 0.31])

        # Object body
        self.obj = self.manip_station_sim.plant.GetBodyByName(
            self.manip_station_sim.object_base_link_name,
            self.manip_station_sim.object)

        # Properties for RL
        max_action = np.ones(8) * 0.1
        max_action[-1] = 0.03
        low_action = -1 * max_action
        low_action[-1] = 0
        self.action_space = ActionSpace(low=low_action, high=max_action)
        self.state_dim = self._getObservation().shape[0]
        self._episode_steps = 0
        self._max_episode_steps = 75

        # Set initial state of the robot
        self.reset_sim = False
        self.reset()