Повернуть кватернион по входным значениям углов Эйлера

Question

Повернуть кватернион по входным значениям углов Эйлера

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

Я написал функцию, чтобы получить количество, которое пользователь хочет повернуть в каждом направлении (поворот, шаг, рыскание) и вывести новый кватернион. Я сохранил current_quaternion как глобальную переменную.

Я использую C ++ и Eigen.

Eigen::Quaterniond euler2Quaternion( const double roll,
const double pitch,
const double yaw)
{

Eigen::AngleAxisd rollAngle(roll,Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(pitch,Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(yaw,Eigen::Vector3d::UnitZ());

Eigen::Quaterniond q = rollAngle*pitchAngle*yawAngle;
current_q=q*current_q;
return current_q;
}

Я пробовал много вещей, меняя порядок умножения углов и умножая UnitX (), UnitY () и UnitZ () на current_q.toRotationMatrix (), но ни один из них не работал.

3

c++eigen euler-angles quaternions transformation

Решение

Другие решения

Почему бы не построить кватернионы напрямую?

Eigen::Quaterniond rollQuaternion(cos(0.5*roll), sin(0.5*roll), 0.0, 0.0);
Eigen::Quaterniond pitchQuaternion(cos(0.5*roll), 0.0, sin(0.5*roll), 0.0);
Eigen::Quaterniond yawQuaternion(cos(0.5*roll), 0.0, 0.0, sin(0.5*roll));
Eigen::Quaterniond finalOrientation = rollQuaternion*pitchQuaternion*yawQuaternion*current_q;

0

Источник

Accepted Answer

Ваш пример почти идентичен пример

Matrix3f m;
m = AngleAxisf(0.25*M_PI, Vector3f::UnitX())
* AngleAxisf(0.5*M_PI,  Vector3f::UnitY())
* AngleAxisf(0.33*M_PI, Vector3f::UnitZ());

Вы пытались напечатать результат этой комбинированной матрицы вращения? Держу пари, что диагональ 1,1,1, когда углы равны нулю.

Я запутался по поводу использования вами current_q. Если крен, тангаж, рыскание соответствует некоторому фиксированному опорному направлению, то д будет все вращениями. В этом случае это:

current_q=q*current_q;
return current_q;

должно быть просто

current_q=q;
return current_q;

если roll, pitch, yaw подразумеваются изменения в текущих углов поворота Эйлер (которые начинаются с некоторого фиксированного опорного направления), это легче хранить эти углы и изменить их соответствующим образом:

double m_roll=0, m_pitch=0, m_yaw=0;
. . .
Eigen::Quaterniond euler2Quaternion(double roll,
double pitch,
double yaw)
{
m_roll+=roll;
m_pitch+=pitch;
m_yaw+=yaw;

Eigen::AngleAxisd rollAngle(m_roll,Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(m_pitch,Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(m_yaw,Eigen::Vector3d::UnitZ());

Eigen::Quaterniond q = rollAngle*pitchAngle*yawAngle;
current_q=q;
return current_q;
}

Это также предлагается в комментарии sbabbi

3