Cleaned up RobustGrasp a little