clip arcsin to prevent locationd orientation NaN (#20868)
* clip arcsin * can of course be negativealbatross
parent
decbcc5ae8
commit
0a34900fec
|
@ -30,7 +30,8 @@ Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){
|
||||||
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
|
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
|
||||||
// return {euler(2), euler(1), euler(0)};
|
// return {euler(2), euler(1), euler(0)};
|
||||||
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
|
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
|
||||||
double theta = asin(2 * (quat.w() * quat.y() - quat.z() * quat.x()));
|
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
|
||||||
|
double theta = asin(asin_arg_clipped);
|
||||||
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
|
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
|
||||||
return {gamma, theta, psi};
|
return {gamma, theta, psi};
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue