ros-industrial-consortium/descartes — pull request #44 by jrgnicho (2014-12-19 23:49:30+00:00)
Title: Issue #43 closest joint pose implementation

This PR addresses issue #43 as follows:
- Implemented CartesianPt::getClosestJointPose(...) with the following approach:
1. Computes FK using the seed_pose passed to the method.
2. Computes the x,y ,z, rx, ry,rz from the pose obtained from FK. The orientation values are computed with the Eigen::eulerAngles(...) method
3. Each pose value is then checked to verify that it is within the points tolerance. If it isn't then that value is set to one of the bounds for the points pose and ik is solved for the modified pose values

- Created Unit test to validate the method described above.

Note: Since the Eigen::getEuler method may return different rpy combination for slightly different poses this approach may occasionally yield orientation values that are outside the cartesian point tolerances. Thus, the closest joint pose returned in these cases could be really far from the seed pose when in practice there could be a much closer one.

@shaun-edwards , @dpsolomon, any suggestions will be very welcomed.