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

Tune target reached thresholds

Browse Source 
- Add QuatToEuler function to convert quaternion to roll, pitch, yaw for easy angle threshold comparison
- Tune position and rotation thresholds
This commit is contained in:
Sravan Balaji
2020-04-26 15:03:59 -04:00
parent e0d84b2adc
commit ed45f76ae0
1 changed files with 20 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -34,6 +34,7 @@
#include <kdl/chainidsolver_recursive_newton_euler.hpp> #include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <cstdlib> #include <cstdlib>
#include <math.h>
using namespace std; using namespace std;
@@ -383,8 +384,8 @@ private:
bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
{ {
// TODO: Tune threshold values // TODO: Tune threshold values
float pos_thresh = 0.01; // Meters float pos_thresh = 0.1; // Meters
float rot_thresh = 0.02; // Radians float rot_thresh = 0.0875; // Radians
float pos_err = fabs(cur.position.x - target.position.x) + float pos_err = fabs(cur.position.x - target.position.x) +
fabs(cur.position.y - target.position.y) + fabs(cur.position.y - target.position.y) +
@@ -395,33 +396,38 @@ private:
return false; return false;
} }
float qx_err = fabs(cur.orientation.x - target.orientation.x); vector<double> rpy1 = QuatToEuler(cur.orientation);
float qy_err = fabs(cur.orientation.y - target.orientation.y); vector<double> rpy2 = QuatToEuler(target.orientation);
float qz_err = fabs(cur.orientation.z - target.orientation.z);
float qw_err = fabs(cur.orientation.w - target.orientation.w);
if (qx_err > rot_thresh) 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));
if (rx_err > rot_thresh)
{ {
return false; return false;
} }
if (qy_err > rot_thresh) if (ry_err > rot_thresh)
{ {
return false; return false;
} }
if (qz_err > rot_thresh) if (rz_err > rot_thresh)
{
return false;
}
if (qw_err > rot_thresh)
{ {
return false; return false;
} }
return true; return true;
} }
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]);
return rpy;
}
}; };
int main(int argc, char **argv) int main(int argc, char **argv)