Hydro devel

Check if way-point is in the IK Solution. If IK solution is not found the way-point changes its color to yellow.
The IK solver has 1 iteration and 0.001 seconds timeout for each attempt for solving IK.