dangerous bug: returned state may differ from input pose and doesn't respect success criteria

[buckleytoby]

In kinematics_plugin.cpp::searchPositionIK first the IK solution is found, then the angles may be wrapped (depending on mimic joints), then the position bounds are enforced (line 616), and then the success criteria is checked on line 638

The issue: line 638 is using the success as calculated with the PREVIOUS, non-bounded IK solution, WHICH MAY DIFFER from the state that gets returned to the user's code.

The impact: the user expects the arm to rotate clockwise a maximum of 45 degrees. Instead the combination of wrap + bound leads the robot to rotate -90 degrees and smash expensive end-effectors into a table.

This is related to #21 but this bug can still occur even if the angle wrapping is deactivated, depending on the position bounds.

The fix: success criteria must be rechecked on the final state which would be returned to the user, else dangerous behavior can occur.

[v4hn]

Thank you for the analysis. Could you provide a pull-request and ideally a test that works?