Normalize angle to be within the interval [-pi,pi].
| 292 | |
| 293 | // Normalize angle to be within the interval [-pi,pi]. |
| 294 | double |
| 295 | standardRad(double t) |
| 296 | { |
| 297 | if (t >= 0.) { |
| 298 | t = std::fmod(t + M_PI, 2 * M_PI) - M_PI; |
| 299 | } |
| 300 | else { |
| 301 | t = std::fmod(t - M_PI, -2 * M_PI) + M_PI; |
| 302 | } |
| 303 | return t; |
| 304 | } |
| 305 | |
| 306 | void |
| 307 | wRo_to_euler(const Eigen::Matrix3f& wRo, double& yaw, double& pitch, double& roll) |