mawei/openpilot-meb
1
0
Fork 0
Code Issues Pull Requests Actions 7 Packages Projects Releases Wiki Activity

clip arcsin to prevent locationd orientation NaN (#20868)

Browse Source 
* clip arcsin

* can of course be negative
old-commit-hash: 0a34900fec799616002985ce8c15e94e904fa06c
This commit is contained in:
HaraldSchafer
2021-05-10 21:21:20 -06:00
committed by GitHub
parent 59c7bccf81
commit 3a38582efb
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
common/transformations/orientation.cc
View File

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