Beta version of 'manip_to_pose'