bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix singularity for getRPY. need to add unit test for this particular problem.

This commit is contained in:
hsu 2010-07-13 00:38:52 +00:00
parent 3438a44a14
commit cc896f2f2f
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
urdf/include/urdf/pose.h
View File

@ -114,7 +114,8 @@ public:
sqw = this->w * this->w; sqw = this->w * this->w;
roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); 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); yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
}; };