RobotNodeSet::RobotNodeSet(const std::string &name,
RobotWeakPtr robot,
const std::vector< RobotNodePtr > &robotNodes,
const RobotNodePtr kinematicRoot /*= RobotNodePtr()*/,
const RobotNodePtr tcp /*= RobotNodePtr()*/)
:SceneObjectSet(name,robotNodes.size()>0?robotNodes[0]->getCollisionChecker():CollisionCheckerPtr())
{
this->robotNodes = robotNodes;
this->kinematicRoot = kinematicRoot;
this->tcp = tcp;
RobotPtr rob = robot.lock();
if (!kinematicRoot && robotNodes.size()>0)
this->kinematicRoot = rob->getRootNode();
if (!tcp && robotNodes.size()>0)
this->tcp = robotNodes[robotNodes.size()-1];
this->robot = robot;
// now, the objects are stored in the parent's (SceneObjectSet) data structure, so that the methods of SceneObjectSet do work
for(size_t i = 0; i < robotNodes.size(); i++)
{
SceneObjectPtr cm = boost::dynamic_pointer_cast<SceneObject>(robotNodes[i]);
if (cm)
{
if (colChecker != cm->getCollisionChecker())
{
VR_WARNING << "col model of " << robotNodes[i]->getName() << " belongs to different instance of collision checker, in: " << getName().c_str() << endl;
} else
{
sceneObjects.push_back(cm);
}
}
}
}
void RobotNode::updateTransformationMatrices()
{
if (this->getParent())
updateTransformationMatrices(this->getParent()->getGlobalPose());
else
{
// check for root
RobotPtr r = getRobot();
if (r && r->getRootNode() == shared_from_this())
updateTransformationMatrices(r->getGlobalPose());
else
updateTransformationMatrices(Eigen::Matrix4f::Identity());
}
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< RobotNodePtr > &robotNodes,
const RobotNodePtr kinematicRoot,
const RobotNodePtr tcp,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
if (robotNodes.empty() || !robotNodes[0])
{
VR_WARNING << " No robot nodes in set..." << endl;
} else
{
CollisionCheckerPtr cc = robotNodes[0]->getCollisionChecker();
for (unsigned int i=1;i<robotNodes.size();i++)
{
if (robotNodes[0]->getRobot() != robotNodes[i]->getRobot())
{
THROW_VR_EXCEPTION("Robot nodes belong to different robots");
}
if (cc != robotNodes[i]->getCollisionChecker())
{
THROW_VR_EXCEPTION("Robot nodes belong to different collision checkers");
}
}
}
RobotNodePtr kinematicRootNode = kinematicRoot;
if (!kinematicRootNode)
{
kinematicRootNode = robot->getRootNode();
}
RobotNodePtr tcpNode = tcp;
if (!tcpNode)
{
THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the tcp node need for creating a RobotNodeSet");
tcpNode = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns(new RobotNodeSet(name, robot, robotNodes, kinematicRootNode, tcpNode));
if (registerToRobot)
{
THROW_VR_EXCEPTION_IF(robot->hasRobotNodeSet(rns), "RobotNodeSet with name " << rns->getName() << " already present in the robot");
robot->registerRobotNodeSet(rns);
}
return rns;
}
void RobotNode::updateVisualizationPose( const Eigen::Matrix4f &globalPose, bool updateChildren )
{
// check if we are a root node
SceneObjectPtr parent = getParent();
RobotPtr rob = getRobot();
if (!parent || parent == rob)
{
if (rob && rob->getRootNode() == static_pointer_cast<RobotNode>(shared_from_this()))
{
Eigen::Matrix4f gpPre = getLocalTransformation().inverse() * globalPose;
rob->setGlobalPose(gpPre, false);
} else
VR_WARNING << "INTERNAL ERROR: getParent==robot but getRoot!=this ?! " << endl;
}
this->globalPose = globalPose;
// update collision and visualization model and children
SceneObject::updatePose(updateChildren);
}
bool SceneIO::processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath )
{
THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneRobot");
// get name
std::string robotName = processNameAttribute(sceneXMLNode,true);
if (robotName.empty())
{
THROW_VR_EXCEPTION("Please specify the name of the robot...");
return false;
}
std::string initStr("initconfig");
std::string initConfigName = processStringAttribute(initStr,sceneXMLNode,true);
std::vector< RobotConfigPtr > configs;
std::vector< std::vector< RobotConfig::Configuration > > configDefinitions;
std::vector< std::string > configNames;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
std::string fileName;
rapidxml::xml_node<>* node = sceneXMLNode->first_node();
std::vector< rapidxml::xml_node<>* > rnsNodes;
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "file")
{
THROW_VR_EXCEPTION_IF(!fileName.empty(), "Multiple files defined in scene's robot tag '" << robotName << "'." << endl);
fileName = processFileNode(node,basePath);
} else if (nodeName == "configuration")
{
bool c*K = processConfigurationNodeList(node, configDefinitions, configNames);
THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in scene's robot tag '" << robotName << "'." << endl);
} else if (nodeName == "globalpose")
{
processTransformNode(node, robotName, globalPose);
} else if (nodeName == "robotnodeset")
{
rnsNodes.push_back(node);
} else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's Robot definition <" << robotName << ">." << endl);
}
node = node->next_sibling();
}
// create & register robot
THROW_VR_EXCEPTION_IF(fileName.empty(), "Missing file definition in scene's robot tag '" << robotName << "'." << endl);
RobotPtr robot = RobotIO::loadRobot(fileName);
THROW_VR_EXCEPTION_IF(!robot, "Invalid robot file in scene's robot tag '" << robotName << "'." << endl);
robot->setGlobalPose(globalPose);
scene->registerRobot(robot);
// create & register node sets
int rnsNr = 0;
for (size_t i=0;i<rnsNodes.size();i++)
{
// registers rns to robot
RobotNodeSetPtr r = processRobotNodeSet(rnsNodes[i], robot, robot->getRootNode()->getName(), rnsNr);
THROW_VR_EXCEPTION_IF(!r, "Invalid RobotNodeSet definition " << endl);
}
// create & register configs
THROW_VR_EXCEPTION_IF(configDefinitions.size()!=configNames.size(), "Invalid RobotConfig definitions " << endl);
for (size_t i=0;i<configDefinitions.size();i++)
{
RobotConfigPtr rc(new RobotConfig(robot,configNames[i],configDefinitions[i]));
scene->registerRobotConfig(robot,rc);
}
// process init config
if (!initConfigName.empty())
{
THROW_VR_EXCEPTION_IF(!scene->hasRobotConfig(robot,initConfigName), "Scene's robot tag '" << robotName << "' does not have the initConfig '" << initConfigName << "'." << endl);
robot->setJointValues(scene->getRobotConfig(robot,initConfigName));
}
return true;
}
