From 4338f41fa6525e94ce420aefa985e8e8fd09fe29 Mon Sep 17 00:00:00 2001
From: hsu <devnull@localhost>
Date: Tue, 13 Jul 2010 05:19:46 +0000
Subject: [PATCH] #4247 fix wrong coefficient in previous correction

---
 urdf/include/urdf/pose.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/urdf/include/urdf/pose.h b/urdf/include/urdf/pose.h
index dbf81ee..9013806 100644
--- a/urdf/include/urdf/pose.h
+++ b/urdf/include/urdf/pose.h
@@ -115,7 +115,7 @@ public:
 
     roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
     double sarg = -2 * (this->x*this->z - this->w*this->y);
-    pitch = sarg <= 1.0 ? -2.0*M_PI : (sarg >= 1.0 ? 2.0*M_PI : asin(sarg));
+    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);
 
   };