Merging set_pose service addition into jade_devel