Verified Commit f9986693 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

model: add helper assert

parent 8f71c603
......@@ -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];
......
......@@ -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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment