def calculate_grasp_position_list(collision_object, transform_func): """ Calculates grasp positions for a composition of collision objects. :param collision_object: object composition to be grapsed :type: CollisionObject :param transform_func(object, frame_id): transform function :return: list of graps positions :type: [PoseStamped] """ grasp_positions = [] for i in range(0, len(collision_object.primitives)): co = CollisionObject() co.id = collision_object.id co.primitives.append(collision_object.primitives[i]) co.header = collision_object.header co.primitive_poses.append(collision_object.primitive_poses[i]) temp = calculate_grasp_position(co, transform_func, False, 4) for j in range(0, len(temp)): tmp_pose = transform_func(co, "/" + collision_object.id).primitive_poses[0].position temp[j].pose.position = add_point(temp[j].pose.position, tmp_pose) grasp_positions.extend(temp) return grasp_positions
def make_box(name, pose, size): """ Creates a box collision object. :param name: name of the box :type: str :param pose: position of the box :type: PoseStamped :param size: box size :type: [float(x), float(y), float(z)] :return: box collision object :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = list(size) co.primitives = [box] co.primitive_poses = [pose.pose] return co
def make_cylinder(name, pose, size): """ Creates a cylinder collision object. :param name: name of the cylinder :type: str :param pose: position of the cylinder :type: PoseStamped :param size: cylinder size :type: [float(height), float(radius)] :return: cylinder collisionobject :type: CollisionObject """ co = CollisionObject() co.operation = CollisionObject.ADD co.id = name co.header = pose.header cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = list(size) co.primitives = [cylinder] co.primitive_poses = [pose.pose] return co