Added safety check for inertial properties in reading URDF