def RunSimulation(self, real_time_rate=1.0): ''' The Princess Diaries was a good movie. ''' builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # object_file_path = FindResourceOrThrow( # "drake/examples/manipulation_station/models/061_foam_brick.sdf") # sdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf") # urdf_file = FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.urdf") sdf_file = "assets/acrobot.sdf" urdf_file = "assets/acrobot.urdf" plant = builder.AddSystem(MultibodyPlant()) plant.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant, scene_graph).AddModelFromFile(sdf_file) plant.Finalize(scene_graph) assert plant.geometry_source_is_registered() builder.Connect( plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Add nn_system = NNSystem(self.pytorch_nn_object) builder.AddSystem(nn_system) # NN -> plant builder.Connect(nn_system.NN_out_output_port, plant.get_actuation_input_port()) # plant -> NN builder.Connect(plant.get_continuous_state_output_port(), nn_system.NN_in_input_port) # Add meshcat visualizer meshcat = MeshcatVisualizer(scene_graph) builder.AddSystem(meshcat) # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"), builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.GetInputPort("lcm_visualization")) # build diagram diagram = builder.Build() meshcat.load() # time.sleep(2.0) RenderSystemWithGraphviz(diagram) # construct simulator simulator = Simulator(diagram) # context = diagram.GetMutableSubsystemContext( # self.station, simulator.get_mutable_context()) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(real_time_rate) simulator.Initialize() sim_duration = 5. simulator.StepTo(sim_duration) print("stepping complete")
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
def RunSimulation(self, q0_iiwa, door_angles, plan_list, gripper_setpoint_list, extra_time=0, real_time_rate=1.0, is_visualizing=True, gripper_setpoint_initial=0.025): """ 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, gripper_setpoint_initial=gripper_setpoint_initial) 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.set_publish_period(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) self.meshcat_vis = viz 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() self.diagram = diagram # 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.03) 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=door_angles[0]) right_hinge_joint = self.plant.GetJointByName("right_door_hinge") right_hinge_joint.set_angle( context=self.station.GetMutableSubsystemContext(self.plant, context), angle=door_angles[1]) 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)