Fixing the urdf to work better