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;
}
void RobotNode::setJointValue(float q)
{
RobotPtr r = getRobot();
VR_ASSERT(r);
WriteLockPtr lock = r->getWriteLock();
setJointValueNoUpdate(q);
updatePose();
}
void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
{
THROW_VR_EXCEPTION_IF(jointValues.rows() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
RobotPtr rob = robot.lock();
VR_ASSERT(rob);
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i=0;i<robotNodes.size();i++)
{
robotNodes[i]->setJointValueNoUpdate(jointValues[i]);
}
if (kinematicRoot)
kinematicRoot->updatePose();
else
rob->applyJointValues();
}
bool RobotConfig::setJointValues()
{
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted, skipping update..." << endl;
return false;
}
WriteLockPtr lock = r->getWriteLock();
for (std::map< RobotNodePtr, float >::const_iterator i=configs.begin(); i!=configs.end(); i++ )
{
i->first->setJointValueNoUpdate(i->second);
}
r->applyJointValues();
return true;
}
void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
{
VR_ASSERT(jointValues);
RobotPtr rob = robot.lock();
VR_ASSERT(rob);
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i=0;i<robotNodes.size();i++)
{
if (jointValues->hasConfig(robotNodes[i]->getName()))
robotNodes[i]->setJointValueNoUpdate(jointValues->getConfig(robotNodes[i]->getName()));
}
if (kinematicRoot)
kinematicRoot->updatePose();
else
rob->applyJointValues();
}
