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