def localCb(self, data): self.localPose.setPoseStamped(data) if(not (self.enuTickPose.isNone() or self.offset is None)): t = self.localPose.getTranslation() q = self.enuTickPose.getQuaternion() q = quaternion_matrix(q) t = translation_matrix(t) self.localEnuPose.setMatrix(numpy.dot(q,t)) t = self.localEnuPose.getTranslation() t[0] -= self.offset[0] t[1] -= self.offset[1] t[2] -= self.offset[2] q = self.localEnuPose.getQuaternion() self.localEnuPose.setTranslationQuaternion(t, q) p = PointStamped() p.point.x = self.offset[0] p.point.y = self.offset[1] p.point.z = self.offset[2] p.header = Header(0, rospy.rostime.get_rostime(), "world") self.offsetPub.publish(p) self.localEnuPub.publish(self.localEnuPose.getPoseStamped())
def get_base_origin(self):
"""
:return: The centre of the arm's base
:type: PointStamped
"""
current_pose = self.__base_group.get_current_joint_values()
result = PointStamped()
result.header.frame_id = "/odom_combined"
result.point = Point(current_pose[0], current_pose[1], 0)
return result
def get_base_origin(self):
"""
:return: The centre of the arm's base
:type: PointStamped
"""
current_pose = self.__base_group.get_current_joint_values()
result = PointStamped()
result.header.frame_id = "/odom_combined"
result.point = Point(current_pose[0], current_pose[1], 0)
return result
def get_fingertip(hand_pose):
"""
Calculates the point between the finger tips, where objects will be hold.
:param hand_pose: hand pose
:type: PoseStamped
:return: point between fingers
:type: PointStamped
"""
grasp_point = PointStamped()
grasp_point.point = set_vector_length(hand_length + finger_length, Point(1, 0, 0))
grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point)
grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point)
grasp_point.header.frame_id = hand_pose.header.frame_id
return grasp_point
def get_fingertip(hand_pose):
"""
Calculates the point between the finger tips, where objects will be hold.
:param hand_pose: hand pose
:type: PoseStamped
:return: point between fingers
:type: PointStamped
"""
grasp_point = PointStamped()
grasp_point.point = set_vector_length(hand_length + finger_length,
Point(1, 0, 0))
grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point)
grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point)
grasp_point.header.frame_id = hand_pose.header.frame_id
return grasp_point
def get_center_of_mass(self, collision_object):
"""
Calculates the centre of a collision object.
:param collision_object: CollisionObject
:type: CollisionObject
:return: centre of mass
:type: PointStamped
"""
p = PointStamped()
p.header.frame_id = "/odom_combined"
for pose in collision_object.primitive_poses:
p.point = add_point(p.point, pose.position)
p.point = multiply_point(1.0 / len(collision_object.primitive_poses), p.point)
return p
def get_center_of_mass(self, collision_object):
"""
Calculates the centre of a collision object.
:param collision_object: CollisionObject
:type: CollisionObject
:return: centre of mass
:type: PointStamped
"""
p = PointStamped()
p.header.frame_id = "/odom_combined"
for pose in collision_object.primitive_poses:
p.point = add_point(p.point, pose.position)
p.point = multiply_point(1.0 / len(collision_object.primitive_poses),
p.point)
return p
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 test_task3(mani):
dest = PointStamped()
dest.header.frame_id = "/odom_combined"
dest.point = Point(0.85, -0.85, 0)
mani.place_and_move(dest)
def test_task3(mani):
dest = PointStamped()
dest.header.frame_id = "/odom_combined"
dest.point = Point(0.85, -0.85, 0)
mani.place_and_move(dest)