Merge pull request #19 from traversaro/master

Fix handling rpy with pitch equal to +/-M_PI/2

urdf_model/include/urdf_model/pose.h

@@ -1,13 +1,13 @@
 /*********************************************************************
 * Software License Agreement (BSD License)
-* 
+*
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
-* 
+*
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
-* 
+*
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
@@ -17,7 +17,7 @@
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
-* 
+*
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -37,7 +37,7 @@
 #ifndef URDF_INTERFACE_POSE_H
 #define URDF_INTERFACE_POSE_H
 
-//For using the M_PI macro in visual studio it 
+//For using the M_PI macro in visual studio it
 //is necessary to define _USE_MATH_DEFINES
 #ifdef _MSC_VER
 #ifndef _USE_MATH_DEFINES
@@ -67,7 +67,7 @@ class Vector3
 
   void clear() {this->x=this->y=this->z=0.0;};
   void init(const std::string &vector_str)
-  { 
+  {
     this->clear();
     std::vector<std::string> pieces;
     std::vector<double> xyz;
@@ -82,15 +82,15 @@ class Vector3
         }
       }
     }
-    
+
     if (xyz.size() != 3)
       throw ParseError("Parser found " + boost::lexical_cast<std::string>(xyz.size())  + " elements but 3 expected while parsing vector [" + vector_str + "]");
-    
+
     this->x = xyz[0];
     this->y = xyz[1];
     this->z = xyz[2];
   }
-  
+
   Vector3 operator+(Vector3 vec)
   {
     return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
@@ -126,11 +126,11 @@ class Rotation
     if (sarg <= -0.99999) {
       pitch = -0.5*M_PI;
       roll  = 0;
-      yaw   = -2 * atan2(this->y, this->x);
+      yaw   = 2 * atan2(this->x, -this->y);
     } else if (sarg >= 0.99999) {
       pitch = 0.5*M_PI;
       roll  = 0;
-      yaw   = 2 * atan2(this->y, this->x);
+      yaw   = 2 * atan2(-this->x, this->y);
     } else {
       pitch = asin(sarg);
       roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
@@ -165,13 +165,13 @@ class Rotation
   double x,y,z,w;
 
   void init(const std::string &rotation_str)
-  { 
+  {
     this->clear();
     Vector3 rpy;
     rpy.init(rotation_str);
     setFromRPY(rpy.x, rpy.y, rpy.z);
   }
-  
+
   void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
 
   void normalize()
@@ -228,7 +228,7 @@ class Rotation
     return result;
   };
   // Get the inverse of this quaternion
-  Rotation GetInverse() const 
+  Rotation GetInverse() const
   {
     Rotation q;