def apply(self, state, **kwargs): cone = self.attach.cone detach = Detach(self.attach.robot, self.attach.group, cone) for _ in detach.apply(state, **kwargs): yield del state.poses[cone] remove_body(cone) wait_for_duration(1e-2)
def apply(self, state, **kwargs): print(self.visual_data) with LockRenderer(): visual_id = visual_shape_from_data(self.visual_data[0]) cone = create_body(visual_id=visual_id) #cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) set_pose(cone, self.pose) wait_for_duration(self._duration) with LockRenderer(): remove_body(cone) wait_for_duration(1e-2) wait_for_duration(self._duration) # TODO: set to transparent before removing yield
def apply(self, state, **kwargs): # TODO: check if actually can register mesh, _ = get_detection_cone(self.robot, self.body, camera_link=self.camera_frame, depth=self.max_depth) if mesh is None: wait_for_user() assert ( mesh is not None ) # TODO: is_visible_aabb(body_aabb, **kwargs)=False sometimes without collisions with LockRenderer(): cone = create_mesh(mesh, color=apply_alpha(GREEN, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(1e-2) wait_for_duration(self._duration) remove_body(cone) wait_for_duration(1e-2) state.registered.add(self.body) yield