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())
示例#2
0
 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
示例#3
0
 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
示例#4
0
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
示例#5
0
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
示例#6
0
    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
示例#7
0
    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
示例#8
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
示例#9
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
示例#10
0
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)
示例#11
0
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)