RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< std::string > &robotNodeNames,
const std::string &kinematicRootName,
const std::string &tcpName,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
std::vector< RobotNodePtr > robotNodes;
if (robotNodeNames.empty())
{
VR_WARNING << " No robot nodes in set..." << endl;
}
else
{
for (unsigned int i=0;i<robotNodeNames.size();i++)
{
RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]);
THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found...");
robotNodes.push_back(node);
}
}
RobotNodePtr kinematicRoot;
if (!kinematicRootName.empty())
{
RobotNodePtr node = robot->getRobotNode(kinematicRootName);
THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found...");
kinematicRoot = node;
}
else
{
if (!robotNodes.empty())
kinematicRoot = robotNodes[0];
}
RobotNodePtr tcp;
if (!tcpName.empty())
{
RobotNodePtr node = robot->getRobotNode(tcpName);
THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found...");
tcp = node;
}
else
{
if (!robotNodes.empty())
tcp = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot);
return rns;
}
void RobotNode::updatePose(bool updateChildren)
{
THROW_VR_EXCEPTION_IF(!initialized, "Not initialized");
updateTransformationMatrices();
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
// apply propagated joint values
if (propagatedJointValues.size()>0)
{
RobotPtr r = robot.lock();
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
while (it!=propagatedJointValues.end())
{
RobotNodePtr rn = r->getRobotNode(it->first);
if (!rn)
{
VR_WARNING << "Could not propagate joint value from " << name << " to " << it->first << " because dependent joint does not exist...";
} else
{
rn->setJointValue(jointValue*it->second);
}
it++;
}
}
}
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;
}
bool RobotConfig::setConfig( const std::string &node, float value )
{
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted, skipping update..." << endl;
return false;
}
RobotNodePtr rn = r->getRobotNode(node);
if (!rn)
{
VR_WARNING << "Did not find robot node with name " << node << endl;
return false;
}
configs[rn] = value;
return true;
}
float RobotConfig::getConfig( const std::string & name ) const
{
if (!hasConfig(name))
return 0.0f;
RobotPtr r = robot.lock();
if (!r)
{
VR_WARNING << "Robot is already deleted..." << endl;
return 0.0f;
}
RobotNodePtr rn = r->getRobotNode(name);
THROW_VR_EXCEPTION_IF(!rn,"Did not find robot node with name " << name);
std::map< RobotNodePtr, float >::const_iterator i = configs.find(rn);
if (i==configs.end())
{
VR_ERROR << "Internal error..." << endl;
return 0.0f;
}
return i->second;
}
RobotConfigPtr RobotConfig::clone( RobotPtr newRobot )
{
if (!newRobot)
newRobot = robot.lock();
VR_ASSERT (newRobot);
std::map< RobotNodePtr, float > newConfigs;
std::map< RobotNodePtr, float >::iterator i = configs.begin();
while (i!=configs.end())
{
RobotNodePtr rn = newRobot->getRobotNode(i->first->getName());
if (!rn)
{
VR_WARNING << "Could not completely clone RobotConfig " << name << " because new robot does not know a RobotNode with name " << i->first->getName() << endl;
} else
{
newConfigs[rn] = i->second;
}
i++;
}
RobotConfigPtr result(new RobotConfig(newRobot,name,newConfigs));
return result;
}
