Fix bug in URDF loader