Beispiel #1
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
Beispiel #2
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
Beispiel #3
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
Beispiel #4
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
Beispiel #5
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
Beispiel #6
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
Beispiel #7
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)
Beispiel #8
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)