clip arcsin to prevent locationd orientation NaN (#20868)

* clip arcsin * can of course be negative
4 years ago · 0a34900fec
parent decbcc5ae8
commit 0a34900fec
1 changed files with 2 additions and 1 deletions
--- a/common/transformations/orientation.cc
+++ b/common/transformations/orientation.cc
@ -30,7 +30,8 @@ Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){
  // Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 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 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()));
  return {gamma, theta, psi};
 }