Could we use const urdf::Vector3 & for the argument types?