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_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 test_task3(mani): dest = PointStamped() dest.header.frame_id = "/odom_combined" dest.point = Point(0.85, -0.85, 0) mani.place_and_move(dest)