通过Euler角度输入旋转四元数 [英] Rotate a quaternion by Euler angles input
问题描述
我正在编写代码来控制3D空间中的机械臂。机械臂通过四元数来处理旋转,但是我希望用户通过更改偏航角,俯仰和滚动来控制它,因为它对人类使用这些更敏感。
I am writing a code to control robotic arm in 3D space. The robotic arm handle the rotation by quaternion but I want user to control it by changing yaw, pitch and roll since its more sensible for human to use these.
我编写了函数获取用户想要在每个方向(滚动,俯仰,偏航)上旋转手臂并输出新的四元数的量。我将current_quaternion保存为全局变量。
I wrote function to get the amount that user wants to rotate the arm in each of directions(roll, pitch, yaw) and output the new quaternion. I saved the current_quaternion as a global variable.
我正在使用C ++和Eigen。
I am using C++ and 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()的顺序的方法,
I tried many things changing the order of multiplying angles and multiplying UnitX(), UnitY() and UnitZ() by current_q.toRotationMatrix() but non of them worked.
推荐答案
您的示例几乎与示例
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。
Have you tried printing the result of that combined rotation matrix? I will bet it is diagonal 1,1,1 when the angles are zero.
我对您使用current_q感到困惑。如果滚动,俯仰,偏航对应于某个固定的参考方向,则q将是整个旋转。在这种情况下,
I'm confused about your use of current_q. If roll, pitch, yaw corresponds to some fixed reference direction, then q will be the whole rotation. In that case, this:
current_q=q*current_q;
return current_q;
应该只是
current_q=q;
return current_q;
如果是侧倾,俯仰,偏航
当更改当前的欧拉旋转角度(从某个固定参考方向开始)时,存储这些角度并进行相应更改会更容易:
if roll, pitch, yaw
are meant as changes to the current euler rotation angles (which start from some fixed reference direction), it's easier to store these angles and change them accordingly:
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的评论中也建议
This is also suggested in a comment by sbabbi
这篇关于通过Euler角度输入旋转四元数的文章就介绍到这了,希望我们推荐的答案对大家有所帮助,也希望大家多多支持IT屋!