def ConnectPortsAndAddLoggers(builder, station, plan_runner):
        builder.AddSystem(plan_runner)
        builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"),
                        station.GetInputPort("wsg_position"))
        builder.Connect(plan_runner.GetOutputPort("force_limit"),
                        station.GetInputPort("wsg_force_limit"))
        builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"),
                        station.GetInputPort("iiwa_position"))
        builder.Connect(plan_runner.GetOutputPort("iiwa_torque_command"),
                        station.GetInputPort("iiwa_feedforward_torque"))

        builder.Connect(station.GetOutputPort("iiwa_position_measured"),
                        plan_runner.GetInputPort("iiwa_position"))
        builder.Connect(station.GetOutputPort("iiwa_position_commanded"),
                        plan_runner.GetInputPort("iiwa_position_cmd"))
        builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
                        plan_runner.GetInputPort("iiwa_velocity"))
        builder.Connect(station.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.01)

        iiwa_external_torque_log = LogOutput(
            station.GetOutputPort("iiwa_torque_external"), builder)
        iiwa_external_torque_log.DeclarePeriodicPublish(0.01)

        iiwa_position_measured_log = LogOutput(
            station.GetOutputPort("iiwa_position_measured"), builder)
        iiwa_position_measured_log.DeclarePeriodicPublish(0.01)

        iiwa_velocity_estimated_log = LogOutput(
            station.GetOutputPort("iiwa_velocity_estimated"), builder)
        iiwa_velocity_estimated_log.DeclarePeriodicPublish(0.005)

        return (iiwa_position_command_log, iiwa_position_measured_log,
                iiwa_external_torque_log, iiwa_velocity_estimated_log)
    def RunSimulation(self,
                      q0_iiwa,
                      plan_list,
                      gripper_setpoint_list,
                      extra_time=0,
                      real_time_rate=1.0,
                      is_visualizing=True):
        """
        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_iiwa: 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.
        @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_visualizing:
            print_period = 1.0
        else:
            print_period = np.inf

        plan_runner = IiwaPlanRunner(
            iiwa_plans=plan_list,
            gripper_setpoint_list=gripper_setpoint_list,
            print_period=print_period)
        self.plan_runner = plan_runner

        iiwa_position_command_log, iiwa_position_measured_log, \
        iiwa_external_torque_log, iiwa_velocity_estimated_log = \
            self.ConnectPortsAndAddLoggers(builder, self.station, plan_runner)

        plant_state_log = LogOutput(
            self.station.GetOutputPort("plant_continuous_state"), builder)
        plant_state_log.DeclarePeriodicPublish(0.01)

        if is_visualizing:
            # Add meshcat visualizer
            scene_graph = self.station.get_mutable_scene_graph()
            viz = MeshcatVisualizer(scene_graph,
                                    zmq_url="tcp://127.0.0.1:6000",
                                    frames_to_draw=self.frames_to_draw,
                                    frames_opacity=0.8)
            builder.AddSystem(viz)
            builder.Connect(self.station.GetOutputPort("pose_bundle"),
                            viz.GetInputPort("lcm_visualization"))

            contact_viz = MeshcatContactVisualizer(meshcat_viz=viz,
                                                   plant=self.plant)
            builder.AddSystem(contact_viz)
            builder.Connect(self.station.GetOutputPort("pose_bundle"),
                            contact_viz.GetInputPort("pose_bundle"))
            builder.Connect(self.station.GetOutputPort("contact_results"),
                            contact_viz.GetInputPort("contact_results"))

        # build diagram
        diagram = builder.Build()
        # 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(context, q0_iiwa)
        self.station.SetIiwaVelocity(context, np.zeros(7))
        self.station.SetWsgPosition(context, 0.05)
        self.station.SetWsgVelocity(context, 0)

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

        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)
        sim_duration = t_plan[-1] + extra_time
        if is_visualizing:
            print("simulation duration", sim_duration)
            print("plan starting times\n", t_plan)

        self.SetInitialPlanRunnerState(plan_runner, simulator, diagram)
        simulator.Initialize()
        simulator.AdvanceTo(sim_duration)

        return (iiwa_position_command_log, iiwa_position_measured_log,
                iiwa_external_torque_log, iiwa_velocity_estimated_log,
                plant_state_log, t_plan)