void getEulerYPR(const geometry_msgs::Quaternion &q, double &yaw, double &pitch, double &roll)
{
double sqw;
double sqx;
double sqy;
double sqz;
sqx = q.x * q.x;
sqy = q.y * q.y;
sqz = q.z * q.z;
sqw = q.w * q.w;
// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
double sarg = -2 * (q.x * q.z - q.w * q.y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
if (sarg <= -0.99999)
{
pitch = -0.5 * M_PI;
roll = 0;
yaw = -2 * atan2(q.y, q.x);
}
else if (sarg >= 0.99999)
{
pitch = 0.5 * M_PI;
roll = 0;
yaw = 2 * atan2(q.y, q.x);
}
else
{
pitch = asin(sarg);
roll = atan2(2 * (q.y * q.z + q.w * q.x), sqw - sqx - sqy + sqz);
yaw = atan2(2 * (q.x * q.y + q.w * q.z), sqw + sqx - sqy - sqz);
}
};
四元数转欧拉角函数
最新推荐文章于 2024-04-16 19:59:55 发布