Added missing bracket
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
Merge pull request #1 from fairlight1337/master

A lot of threshold fine tuning, error checking, and more failure handling