Cleaned up segmented grasp planner