fixes degeneracies when doing atan2(0,0)

This fixes the unit test in urdfdom at ros/urdfdom#49
vrabaud · May 18, 2015 · 96b8a4b · 96b8a4b
1 parent 6d0174c
commit 96b8a4b
Showing 1 changed file with 14 additions and 3 deletions.
diff --git a/urdf_model/include/urdf_model/pose.h b/urdf_model/include/urdf_model/pose.h
@@ -112,10 +112,21 @@ class Rotation
     sqz = this->z * this->z;
     sqw = this->w * this->w;
 
-    roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
+    // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
     double sarg = -2 * (this->x*this->z - this->w*this->y);
-    pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
-    yaw   = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
+    if (sarg <= -0.99999) {
+      pitch = -0.5*M_PI;
+      roll  = 0;
+      yaw   = -2 * atan2(this->y, this->x);
+    } else if (sarg >= 0.99999) {
+      pitch = 0.5*M_PI;
+      roll  = 0;
+      yaw   = 2 * atan2(this->y, this->x);
+    } else {
+      pitch = asin(sarg);
+      roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
+      yaw   = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
+    }
 
   };
   void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)