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

#4247 fix wrong coefficient in previous correction

This commit is contained in:
hsu 2010-07-13 05:16:21 +00:00
parent 489af45067
commit a6203bdf11
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -115,7 +115,7 @@ public:
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);
double sarg = -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)); pitch = sarg <= 1.0 ? -2.0*M_PI : (sarg >= 1.0 ? 2.0*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);
}; };