Преобразование углов
Что вы узнаете в этом разделе:
- Что такое кватернионы и углы Эйлера
- Зачем нужно преобразовывать кватернионы в углы
- Как преобразовать кватернион в угол
thetaдля 2D навигации - Как использовать библиотеку
tf_transformationsдля работы с углами - Готовые функции для преобразования углов в Python
Преобразование в углы Эйлера
Важная часть практического задания, это перевод углового положения робота из системы кватернионов в более понятные для движения в одной плоскости углы Эйлера.
Теоретическая часть этой задачи, довольно сложная и потребует углубленных знаний математики, материалы для изучения находятся по ссылкам в конце страницы.
Мы же поступим очень просто, возьмем готовые функции и поверим, что они делают то что нам нужно;
Обычно в двухмерной плоскости угол положения робота называется theta (тэтта). Для того чтобы перевести угол из кватерниона в угол theta, можно воспользоваться готовой функцией на python:
import math
from geometry_msgs.msg import Quaternion
def quaternion_to_theta(orientation: Quaternion):
t1 = +2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
t2 = +1.0 - 2.0 * (orientation.y ** 2 + orientation.z**2)
return math.atan2(t1, t2)
q = Quaternion(x=0, y=0, z=-0.282, w=-0.959)
print(quaternion_to_theta(q))
Если вам необходимы все углы Эйлера (углы крена, тангажа и рыскания), лучше воспользоваться библиотекой tf_transformations, которая также содержит дополнительные методы.
from tf_transformations import quaternion_from_euler, euler_from_quaternion
# Преобразование из Эйлера в кватернион
quaternion = quaternion_from_euler(0, 0, 3.14)
# Преобразование из кватерниона в углы Эйлера
euler = euler_from_quaternion(quaternion)
print(euler)