Some clean-up in the URDF loader