示例#1
0
def get_place_position_for_puzzle(destination, orientation):
    rospy.logdebug("get_place_position_for_puzzle")
    place_poses = []
    pitch = pi / 2
    if orientation_to_vector(
            rotate_quaternion(deepcopy(orientation), 0, pitch, 0)).z > 0:
        pitch = -pitch

    place_pose_1 = PoseStamped()
    place_pose_1.header.frame_id = "/odom_combined"
    place_pose_1.pose.position = deepcopy(destination.point)
    place_pose_1.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
                                                      pitch, 0)
    place_poses.append(place_pose_1)

    #roll = add_point(deepcopy(destination.point),Point(-1, 0, 0))
    place_pose_2 = deepcopy(place_pose_1)
    place_pose_2.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
                                                      pitch, pi / 2)
    place_poses.append(place_pose_2)

    #roll = add_point(deepcopy(destination.point),Point(0, 1, 0))
    place_pose_3 = deepcopy(place_pose_1)
    place_pose_3.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
                                                      pitch, pi)
    place_poses.append(place_pose_3)

    #roll = add_point(deepcopy(destination.point),Point(0, -1, 0))
    place_pose_4 = deepcopy(place_pose_1)
    place_pose_4.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
                                                      pitch, pi * 1.5)
    place_poses.append(place_pose_4)

    return place_poses
示例#2
0
def get_place_position_for_puzzle(destination, orientation):
    rospy.logdebug("get_place_position_for_puzzle")
    place_poses = []
    pitch = pi/2
    if orientation_to_vector(rotate_quaternion(deepcopy(orientation), 0, pitch, 0)).z > 0:
        pitch = -pitch

    place_pose_1 = PoseStamped()
    place_pose_1.header.frame_id = "/odom_combined"
    place_pose_1.pose.position = deepcopy(destination.point)
    place_pose_1.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, 0)
    place_poses.append(place_pose_1)

    #roll = add_point(deepcopy(destination.point),Point(-1, 0, 0))
    place_pose_2 = deepcopy(place_pose_1)
    place_pose_2.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, pi/2)
    place_poses.append(place_pose_2)

    #roll = add_point(deepcopy(destination.point),Point(0, 1, 0))
    place_pose_3 = deepcopy(place_pose_1)
    place_pose_3.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, pi)
    place_poses.append(place_pose_3)

    #roll = add_point(deepcopy(destination.point),Point(0, -1, 0))
    place_pose_4 = deepcopy(place_pose_1)
    place_pose_4.pose.orientation = rotate_quaternion(deepcopy(orientation), 0, pitch, pi*1.5)
    place_poses.append(place_pose_4)

    return place_poses
示例#3
0
def get_place_position_for_single_object(collision_object, destination, transform_func,  grasp, d):
    """
    Calculates place positions for a non composition of collision objects.
    :param collision_object: object to be placed
    :type: CollisionObject
    :param destination:
    :type: PointStamped
    :param transform_func(object, frame_id): a function to transform objects to different frame_ids. (use Manipulation.transform_to)
    :param grasp: PoseStamped
    :type: PoseStamped
    :param d: dist from the grasped point to the tcp frame
    :type: float
    :return: list of possible place positions
    :type: [PoseStamped]
    """
    angle = get_pitch(grasp.pose.orientation)
    z_o = abs(grasp.pose.position.z) - (sin(angle) * d)

    place_pose = destination.point
    place_pose.z = z_o + safe_place
    diff = abs(pi/2 - angle)
    if 0 <= diff <= 0.1:
        place_poses = make_scan_pose(place_pose, d, angle, n=2)
    else:
        place_poses = make_scan_pose(place_pose, d, angle)

    p2 = PointStamped()
    p2.header.frame_id = collision_object.id
    p2 = transform_func(p2, "tcp")
    rospy.logwarn(p2)
    if p2.point.y < 0:
        for i in range(0, len(place_poses)):
            place_poses[i].pose.orientation = rotate_quaternion(place_poses[i].pose.orientation, pi, 0, 0)

    return place_poses
示例#4
0
def get_place_position_handle(collision_object, destination, transform_func,
                              d):
    """
    Calculates place positions for a composition of collision objects.
    :param collision_object: object to be placed
    :type: CollisionObject
    :param destination:
    :type: PointStamped
    :param transform_func(object, frame_id): a function to transform objects to different frame_ids. (use Manipulation.transform_to)
    :param grasp: PoseStamped
    :type: PoseStamped
    :param d: dist from the grasped point to the tcp frame
    :type: float
    :return: list of possible place positions
    :type: [PoseStamped]
    """
    angle = 0

    place_pose = destination.point

    p2 = PointStamped()
    p2.header.frame_id = collision_object.id
    p2 = transform_func(p2, "tcp")

    place_pose.z = collision_object.primitives[0].dimensions[
                                         shape_msgs.msg.SolidPrimitive.CYLINDER_HEIGHT] / 2 + \
                                     collision_object.primitives[1].dimensions[
                                         shape_msgs.msg.SolidPrimitive.BOX_X] + safe_place
    place_pose.z += abs(p2.point.y)

    place_poses = make_scan_pose(place_pose, d, angle)
    #rotate place poses, depending on where the centroid of the object is, relative to the gripper
    if p2.point.y < 0:
        for i in range(0, len(place_poses)):
            place_poses[i].pose.orientation = rotate_quaternion(
                place_poses[i].pose.orientation, pi, 0, 0)

    return place_poses
示例#5
0
def get_place_position_for_single_object(collision_object, destination,
                                         transform_func, grasp, d):
    """
    Calculates place positions for a non composition of collision objects.
    :param collision_object: object to be placed
    :type: CollisionObject
    :param destination:
    :type: PointStamped
    :param transform_func(object, frame_id): a function to transform objects to different frame_ids. (use Manipulation.transform_to)
    :param grasp: PoseStamped
    :type: PoseStamped
    :param d: dist from the grasped point to the tcp frame
    :type: float
    :return: list of possible place positions
    :type: [PoseStamped]
    """
    angle = get_pitch(grasp.pose.orientation)
    z_o = abs(grasp.pose.position.z) - (sin(angle) * d)

    place_pose = destination.point
    place_pose.z = z_o + safe_place
    diff = abs(pi / 2 - angle)
    if 0 <= diff <= 0.1:
        place_poses = make_scan_pose(place_pose, d, angle, n=2)
    else:
        place_poses = make_scan_pose(place_pose, d, angle)

    p2 = PointStamped()
    p2.header.frame_id = collision_object.id
    p2 = transform_func(p2, "tcp")
    rospy.logwarn(p2)
    if p2.point.y < 0:
        for i in range(0, len(place_poses)):
            place_poses[i].pose.orientation = rotate_quaternion(
                place_poses[i].pose.orientation, pi, 0, 0)

    return place_poses
示例#6
0
def get_place_position_handle(collision_object, destination, transform_func, d):
    """
    Calculates place positions for a composition of collision objects.
    :param collision_object: object to be placed
    :type: CollisionObject
    :param destination:
    :type: PointStamped
    :param transform_func(object, frame_id): a function to transform objects to different frame_ids. (use Manipulation.transform_to)
    :param grasp: PoseStamped
    :type: PoseStamped
    :param d: dist from the grasped point to the tcp frame
    :type: float
    :return: list of possible place positions
    :type: [PoseStamped]
    """
    angle = 0

    place_pose = destination.point

    p2 = PointStamped()
    p2.header.frame_id = collision_object.id
    p2 = transform_func(p2, "tcp")

    place_pose.z = collision_object.primitives[0].dimensions[
                                         shape_msgs.msg.SolidPrimitive.CYLINDER_HEIGHT] / 2 + \
                                     collision_object.primitives[1].dimensions[
                                         shape_msgs.msg.SolidPrimitive.BOX_X] + safe_place
    place_pose.z += abs(p2.point.y)

    place_poses = make_scan_pose(place_pose, d, angle)
    #rotate place poses, depending on where the centroid of the object is, relative to the gripper
    if p2.point.y < 0:
        for i in range(0, len(place_poses)):
            place_poses[i].pose.orientation = rotate_quaternion(place_poses[i].pose.orientation, pi, 0, 0)

    return place_poses