Improved version of manip_to_pose.py