add codes to compute grasp-point for pr2 to grasp plane