Fixed bugs in URDF reader; bugs still present