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
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
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
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
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
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