new manip_to_joint_pose version
Beta version (semi-final) of -- manip_to_joint_pose.py -- SM