diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp index b1e69611cdb121807c6540038ac5ca915dd54c8d..28f241c5a20ad8b13a24bcdf3721cdf6b39610aa 100644 --- a/src/multibody/model.hpp +++ b/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; } diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index 8737fe49ef479081bb163e753cf29a707fb648fe..7be8e51b5ba78dc01543633cd530bb21a3fbecac 100644 --- a/src/parsers/urdf/model.cpp +++ b/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()); + } } ///