Fixed a few thresholds and made code cleaner
Fixed more thresholds and made checking for reachability rely in IK only
Added more information upon missing to grasp an object