model: add helper assert

src/multibody/data.hxx

@@ -166,6 +166,8 @@ namespace pinocchio
       const int nvj    = nv   (model.joints[joint]);
       const int idx_vj = idx_v(model.joints[joint]);
       
+      assert(idx_vj >= 0 && idx_vj < model.nv);
+      
       if(parent>0) parents_fromRow[(Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
       else         parents_fromRow[(Index)idx_vj] = -1;
       nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint];

src/multibody/model.hxx

@@ -83,6 +83,9 @@ namespace pinocchio
     JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
     jmodel.setIndexes(idx,nq,nv);
     
+    assert(jmodel.idx_q() >= 0);
+    assert(jmodel.idx_v() >= 0);
+
     inertias       .push_back(Inertia::Zero());
     parents        .push_back(parent);
     jointPlacements.push_back(joint_placement);