Verified Commit 9519c6d8 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

model: handle the case where jmodel.nq and jmodel.nv are equal to 0

This is the case for instance for the JointMimics
parent 5a0e1015
......@@ -70,6 +70,7 @@ namespace pinocchio
assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
&&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) );
assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
assert(joint_model.nq() >= joint_model.nv());
assert(max_effort.size() == joint_model.nv()
&& max_velocity.size() == joint_model.nv()
......@@ -89,9 +90,8 @@ namespace pinocchio
nq += joint_model.nq();
nv += joint_model.nv();
// Optimal efficiency here would be using the static-dim bottomRows, while specifying the dimension in argument in the case where D::NV is Eigen::Dynamic.
// However, this option is not compiling in Travis (why?).
// As efficiency of ModelTpl::addJoint is not critical, the dynamic bottomRows is used here.
if(joint_model.nq() > 0 && joint_model.nv() > 0)
{
effortLimit.conservativeResize(nv);
jmodel.jointVelocitySelector(effortLimit) = max_effort;
velocityLimit.conservativeResize(nv);
......@@ -113,6 +113,7 @@ namespace pinocchio
jmodel.jointVelocitySelector(rotorInertia).setZero();
rotorGearRatio.conservativeResize(nv);
jmodel.jointVelocitySelector(rotorGearRatio).setZero();
}
// Init and add joint index to its parent subtrees.
subtrees.push_back(IndexVector(1));
......
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