diff --git a/urdf_model/include/urdf_model/pose.h b/urdf_model/include/urdf_model/pose.h index 6ed7422..90e74a8 100644 --- a/urdf_model/include/urdf_model/pose.h +++ b/urdf_model/include/urdf_model/pose.h @@ -37,14 +37,6 @@ #ifndef URDF_INTERFACE_POSE_H #define URDF_INTERFACE_POSE_H -//For using the M_PI macro in visual studio it -//is necessary to define _USE_MATH_DEFINES -#ifdef _MSC_VER -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif -#endif - #include #include #include @@ -126,12 +118,13 @@ class Rotation // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ double sarg = -2 * (this->x*this->z - this->w*this->y); + const double pi_2 = 1.57079632679489661923; if (sarg <= -0.99999) { - pitch = -0.5*M_PI; + pitch = -pi_2; roll = 0; yaw = 2 * atan2(this->x, -this->y); } else if (sarg >= 0.99999) { - pitch = 0.5*M_PI; + pitch = pi_2; roll = 0; yaw = 2 * atan2(-this->x, this->y); } else {