finalized calculating pregrasp and grasp poses
added a moveToPoint function, rearranged the structure and added some comments