found bug in our pickActionGoal creation, extracted common code from check_reachability and execute grasp into single file