通过欧拉角输入旋转四元数
Rotate a quaternion by Euler angles input
我正在编写代码来控制 3D 机械臂 space。机械臂通过四元数处理旋转,但我希望用户通过改变偏航、俯仰和滚动来控制它,因为人类使用这些更明智。
我编写函数来获取用户想要在每个方向(滚动、俯仰、偏航)上旋转手臂的量并输出新的四元数。我将 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(),但没有一个奏效。
您的示例与 the example 几乎相同
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 的使用感到困惑。如果roll、pitch、yaw对应于某个固定的参考方向,那么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 在评论中也提出了这一建议
为什么不直接构建四元数?
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;
我正在编写代码来控制 3D 机械臂 space。机械臂通过四元数处理旋转,但我希望用户通过改变偏航、俯仰和滚动来控制它,因为人类使用这些更明智。
我编写函数来获取用户想要在每个方向(滚动、俯仰、偏航)上旋转手臂的量并输出新的四元数。我将 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(),但没有一个奏效。
您的示例与 the example 几乎相同
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 的使用感到困惑。如果roll、pitch、yaw对应于某个固定的参考方向,那么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 在评论中也提出了这一建议
为什么不直接构建四元数?
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;