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
station.Finalize() builder = DiagramBuilder() builder.AddSystem(station) ConnectMeshcatVisualizer(builder, scene_graph=station.get_scene_graph(), output_port=station.GetOutputPort("query_object")) diagram = builder.Build() #%% context = diagram.CreateDefaultContext() context_station = diagram.GetSubsystemContext(station, context) context_plant = station.GetSubsystemContext(station.get_multibody_plant(), context_station) #%% plant = station.get_multibody_plant() model_iiwa = plant.GetModelInstanceByName("iiwa") model_gripper = plant.GetModelInstanceByName("gripper") model_bin1 = plant.GetModelInstanceByName("bin1") model_bin2 = plant.GetModelInstanceByName("bin2") def get_default_joint_positions(model_idx): q_default = [] for joint_idx in plant.GetJointIndices(model_idx): default_positions = plant.get_joint(joint_idx).default_positions() n = len(default_positions) if n > 0: