bool RobotConfig::setJointValues( RobotPtr r )
{
if (!r)
return false;
WriteLockPtr lock = r->getWriteLock();
std::map < std::string, float > jv = getRobotNodeJointValueMap();
std::map< std::string, float >::const_iterator i = jv.begin();
// first check if all nodes are present
while (i != jv.end())
{
if (!r->hasRobotNode(i->first))
return false;
i++;
}
// apply jv
i = jv.begin();
while (i != jv.end())
{
RobotNodePtr rn = r->getRobotNode(i->first);
if (!rn)
return false;
rn->setJointValueNoUpdate(i->second);
i++;
}
r->applyJointValues();
return true;
}
