Commit 75edd31e authored by florent's avatar florent
Browse files

Read dof bounds of robot in kxml file and set ChppHumanoidRobot bounds

     * src/humanoid-robot.cc.
parent c89e5ade
......@@ -415,6 +415,20 @@ HumanoidRobot::buildKinematicChain(ChppHumanoidRobotShPtr inRobot,
" is neither of type FreeflyerJoint, RotationJoint nor TranslationJoint.";
throw exception(message);
}
// Set joint bounds
for (unsigned int iDof = 0; iDof < kxmlJoint->kwsJoint()->countDofs();
iDof++) {
bool isBounded = kxmlJoint->kwsJoint()->dof(iDof)->isBounded();
double vmin = kxmlJoint->kwsJoint()->dof(iDof)->vmin();
double vmax = kxmlJoint->kwsJoint()->dof(iDof)->vmax();
hppJoint->kppJoint()->kwsJoint()->dof(iDof)->isBounded(isBounded);
hppJoint->kppJoint()->kwsJoint()->dof(iDof)->vmin(vmin);
hppJoint->kppJoint()->kwsJoint()->dof(iDof)->vmax(vmax);
hppJoint->jrlJoint()->lowerBound(iDof, vmin);
hppJoint->jrlJoint()->upperBound(iDof, vmax);
}
// Register joint in map
jointMap_[inJoint->name()] = hppJoint;
......
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