class PickPlaceDemo(): def __init__(self): self._scene = PlanningSceneInterface('base_link') self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True) self._move_group = MoveGroupInterface('xarm5', 'base_link') def setup_scene(self): # remove previous objects for name in self._scene.getKnownCollisionObjects(): self._scene.removeCollisionObject(name, False) for name in self._scene.getKnownAttachedObjects(): self._scene.removeAttachedObject(name, False) self._scene.waitForSync() self.__addBoxWithOrientation('box', 0.25, 0.25, 0.25, -0.45, 0.1, 0.125, 0, 0, np.radians(45), wait=False) self._scene.waitForSync() def __addBoxWithOrientation(self, name, size_x, size_y, size_z, x, y, z, roll, pitch, yaw, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX ps = PoseStamped() ps.header.frame_id = self._scene._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) ps.pose.orientation.x = q[0] ps.pose.orientation.y = q[1] ps.pose.orientation.z = q[2] ps.pose.orientation.w = q[3] self._scene.addSolidPrimitive(name, s, ps.pose, wait) def pickup(self): g = Grasp() self._pickplace.pickup("box", [g], support_name="box")
def setup_scene(): scene = PlanningSceneInterface('base_link') scene.removeCollisionObject('box') scene.addCube('box', 0.25, -0.45, 0.1, 0.125) pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True) grasp = Grasp() # fill in g # setup object named object_name using PlanningSceneInterface pick_place.pickup('box', [grasp], support_name='supporting_surface') place_loc = PlaceLocation() # fill in l pick_place.place('box'[place_loc], goal_is_eef=True, support_name='supporting_surface')