Какое преобразование кватернионов записано ниже в коде? - PullRequest
Новая
       128

Какое преобразование кватернионов записано ниже в коде?

0 голосов
/ 07 ноября 2019

Я уже спрашивал об этом при проверке кода, так как это код на С ++, который работает, но они сказали, что здесь лучше. Итак, у меня есть некоторый код, который вращает кватернион (orix, oriy, oriz, oriw) в другой кватернион, но этот поворот написан плохо, и кто-то может сделать это в меньшем количестве строк. Итак, у меня есть два вопроса: кто-нибудь знает, как вращается этот кватернион? Я думаю, что это вращается -pi / 2 вокруг y в углах Эйлера? И, во-вторых, кто-нибудь знает, как написать это лучше?

Это полный код, который выполняет преобразование (с импортом tf2).

Редактировать: да, tf2 - это библиотека. вот ссылка: docs.ros.org / melodic / api / tf2 / 

            tf2::Quaternion quat(orix, -oriy, oriz, oriw);
            tf2::Quaternion q_rot;
            tf2::Vector3 rotation_vector(0.7071068, 0, 0.7071068);
            q_rot.setRotation(rotation_vector, M_PI);
            quat = q_rot*quat;
            quat.normalize();   
            tf2::Matrix3x3 matrix(quat);
            tf2::Matrix3x3 change_y(1,0, 0, 0, -1,0 ,0 ,0, 1);
            matrix = change_y * matrix;

            double roll, pitch, yaw;
            matrix.getRPY(roll, pitch, yaw);
            quat.setRPY(roll,pitch,yaw);`

Ответы [ 2 ]

1 голос
/ 07 ноября 2019

rotation_vector фактически определяет ось вращения, поэтому вы вращаете M_PI вокруг этого вектора, определенного как [0.7 0 0.7], что было бы аналогично вращению вокруг оси y на M_PI/4 - таким образом, генерируя новыйкоординатную рамку с [x' y' z'] - и затем поверните M_PI вокруг текущей оси Z.

Возможно, это руководство по ROS может помочь.

И это должно бытьдостаточно для того, чтобы повернуть кватернион при желаемом крене, тангаже и повороте рыскания.

0 голосов
/ 09 ноября 2019

Строка ниже кодирует преобразование отражения на плоскости XZ оси вращения "quat": 

tf2::Quaternion quat(orix, -oriy, oriz, oriw);

Затем они просто определяют вращение pi / 2 рад вокруг оси вXZ-plane. 

tf2::Vector3 rotation_vector(0.7071068, 0, 0.7071068);
q_rot.setRotation(rotation_vector, M_PI);

Затем они выполняют композицию вращений (порядок имеет значение): 

quat = q_rot*quat;
quat.normalize();

Затем они меняют математическое представление с кватернионов на матричную алгебру: 

tf2::Matrix3x3 matrix(quat);

Затем они снова определяют преобразование отражения относительно плоскости XZ: 

tf2::Matrix3x3 change_y(1,0, 0, 0, -1,0 ,0 ,0, 1);

Следующая строка объединяет это отражение с текущим преобразованием: 

matrix = change_y * matrix;

Наконец онипереходя от матрицы к кватернионам: 

double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
quat.setRPY(roll,pitch,yaw);`

Поскольку вы можете представлять отражение, используя чистые кватернионы (см. https://www.euclideanspace.com/maths/geometry/affine/reflection/quaternion/index.htm), тогда я утверждаю, что следующее выражение эквивалентно приведенному выше коду: 

// pure quaternion for doing reflection
tf2::Quaternion j(0, 1, 0, 0); 
// j*X*j is the reflection of X on the plane with normal j
quat = j*q_rot*j*quat*j*j;

Что можно упростить до: 

quat = -j*q_rot*j*quat;

Трудно сказать, действительно ли эти выражения эквивалентны из-за изменения между матрицей и кватернионами, поэтомуможно было бы провести числовой тест для подтверждения того, что я не испортил вещи со знаками. Я не проверял это, так что принимайте это как есть.

...