functioning code for grasp planning.