Move arm inbound on init #184
Conversation
| if (m_degrees_of_freedom == 6) | ||
| { | ||
| // We add angle limitations for joints 1,2 and 4 on 6 dof | ||
| limited_joints[1] = 128.9; |
There was a problem hiding this comment.
is there a variable holding these values? instead of magic numbers?
There was a problem hiding this comment.
I don't think so. I got them from the User Guide, p.86.
| // Angles received by GetMeasuredJointAngles are in range [0,360], but waypoints are in range [-180, 180] | ||
| angle = m_math_util.toDeg(m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(angle))); | ||
|
|
||
| if(limited_joints.find(i) != limited_joints.end()) |
There was a problem hiding this comment.
I find if (limited_joints.count(i) more readable but that's personal
| { | ||
| float delta = m_math_util.findDeltaFromBoundaries(angle, limited_joints.at(i)); | ||
|
|
||
| if (delta != 0) |
There was a problem hiding this comment.
be careful for float equality, should probably check for delta > epsilon
| @@ -100,6 +100,18 @@ double KortexMathUtil::absolute_position_from_relative(double relative_position, | |||
| return relative_position * range + min_value; | |||
| } | |||
|
|
|||
| double KortexMathUtil::findDeltaFromBoundaries(double value, double limit) | |||
There was a problem hiding this comment.
could rename to something more verbose like findDistanceToBoundary
| limited_joints[2] = 147.8; | ||
| limited_joints[4] = 120.3; | ||
| } | ||
| else |
There was a problem hiding this comment.
else if (m_degrees_of_freedom == 7) and an else to handle error case of something different?
|
|
||
| if(limited_joints.find(i) != limited_joints.end()) | ||
| { | ||
| float delta = m_math_util.findDeltaFromBoundaries(angle, limited_joints.at(i)); |
There was a problem hiding this comment.
this function returns a double and you're casting to float, should be double everywhere or float everywhere
When the arm is turned off, it might fall and move "out of bounds" for certain joints and it will be impossible to move the arm using moveit or waypoints.
This function will move the arm within bounds when initializing the ROS driver using jointspeed commands (since waypoints are rejected)