sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-21 07:52:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Angle Threshold Fix

Browse Source 
- Wrap roll, pitch, yaw to [-pi, pi] instead of [0, 2pi]
- Fix getRPY to GetRPY
This commit is contained in:
Sravan Balaji
2020-04-26 15:16:59 -04:00
parent ed45f76ae0
commit bcac9feeec
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
View File

@@ -399,9 +399,9 @@ private:
vector<double> rpy1 = QuatToEuler(cur.orientation);
vector<double> rpy2 = QuatToEuler(target.orientation);
float rx_err = fabs(fmod(rpy1[0] - rpy2[0], 2*M_PI));
float ry_err = fabs(fmod(rpy1[1] - rpy2[1], 2*M_PI));
float rz_err = fabs(fmod(rpy1[2] - rpy2[2], 2*M_PI));
float rx_err = fabs(fmod(rpy1[0] - rpy2[0] + M_PI, M_PI));
float ry_err = fabs(fmod(rpy1[1] - rpy2[1] + M_PI, M_PI));
float rz_err = fabs(fmod(rpy1[2] - rpy2[2] + M_PI, M_PI));
if (rx_err > rot_thresh)
{
@@ -424,7 +424,7 @@ private:
vector<double> QuatToEuler(geometry_msgs::Quaternion q) {
KDL::Rotation r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w);
vector<double> rpy = {0, 0, 0};
r.getRPY(rpy[0], rpy[1], rpy[2]);
r.GetRPY(rpy[0], rpy[1], rpy[2]);
return rpy;
}