rqt_moveit) Fix to Issue #25