clip arcsin to prevent locationd orientation NaN (#20868)

* clip arcsin

* can of course be negative
old-commit-hash: 0a34900fec

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};
 }