Ported to new breaking MoveIt API changes