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