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)