Fixed SpatialRBInertia problem