Improved code for choosing and setting planning group and end-effector. Briefly tested on PR2.