[C++] Inertia of the world is initialized with NaN.

src/multibody/model.hpp

@@ -115,7 +115,7 @@ namespace se3
       , njoint(1)
       , nbody(1)
       , nFrames(0)
-      , inertias(1, Inertia::Zero())
+      , inertias(1)
       , jointPlacements(1, SE3::Identity())
       , joints(1)
       , parents(1, 0)
@@ -125,6 +125,10 @@ namespace se3
     {
       names[0]     = "universe";     // Should be "universe joint (trivial)"
       addFrame(Frame("universe", 0, 0, SE3::Identity(), FIXED_JOINT));
+      // Inertia of universe has no sense.
+      inertias[0].mass() = std::numeric_limits<double>::quiet_NaN();
+      inertias[0].lever().fill (std::numeric_limits<double>::quiet_NaN());
+      inertias[0].inertia().fill (std::numeric_limits<double>::quiet_NaN());
     }
     ~Model() {} // std::cout << "Destroy model" << std::endl; }
     

src/parsers/urdf/model.cpp

@@ -64,14 +64,18 @@ namespace se3
       {
         const Frame& frame = model.frames[fid];
         const SE3& p = frame.placement * placement;
-        if (Y != NULL && Y->mass > Eigen::NumTraits<double>::epsilon()) {
+        if (frame.parent > 0
+            && Y != NULL
+            && Y->mass > Eigen::NumTraits<double>::epsilon()) {
           model.appendBodyToJoint(frame.parent, convertFromUrdf(*Y), p);
         }
         model.addBodyFrame(body_name, frame.parent, p, fid);
         // Reference to model.frames[fid] can has changed because the vector
         // may have been reallocated.
-        assert (!model.inertias[model.frames[fid].parent].lever().hasNaN()
-            &&  !model.inertias[model.frames[fid].parent].inertia().data().hasNaN());
+        if (model.frames[fid].parent > 0) {
+          assert (!model.inertias[model.frames[fid].parent].lever().hasNaN()
+              &&  !model.inertias[model.frames[fid].parent].inertia().data().hasNaN());
+        }
       }
 
       ///