From cc896f2f2f8f28333f7c3893e3d45038a0a0e794 Mon Sep 17 00:00:00 2001 From: hsu Date: Tue, 13 Jul 2010 00:38:52 +0000 Subject: [PATCH] fix singularity for getRPY. need to add unit test for this particular problem. --- urdf/include/urdf/pose.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/urdf/include/urdf/pose.h b/urdf/include/urdf/pose.h index 89e0ec9..74eaef5 100644 --- a/urdf/include/urdf/pose.h +++ b/urdf/include/urdf/pose.h @@ -114,7 +114,8 @@ public: sqw = this->w * this->w; roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); - pitch = asin(-2 * (this->x*this->z - this->w*this->y)); + 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); };