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);
}
}
}
}
/**
* This method creates a VirtualRobot::RobotNodeRevolute from DH parameters.
*
* \return instance of VirtualRobot::RobotNodeRevolute.
*/
RobotNodePtr RobotNodeRevoluteFactory::createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, dhParameters.aMM(), dhParameters.dMM(), dhParameters.alphaRadian(), dhParameters.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p, CollisionCheckerPtr(), rntype));
return robotNode;
}
ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");
bool visuProcessed = false;
bool colProcessed = false;
VisualizationNodePtr visualizationNode;
CollisionModelPtr collisionModel;
bool useAsColModel = false;
SceneObject::Physics physics;
bool physicsDefined = false;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
// get name
std::string objName = processNameAttribute(objectXMLNode);
// first check if there is an xml file to load
rapidxml::xml_node<>* xmlFileNode = objectXMLNode->first_node("file", 0, false);
if (xmlFileNode)
{
std::string xmlFile = processFileNode(xmlFileNode, basePath);
ObstaclePtr result = loadObstacle(xmlFile);
if (!result)
{
return result;
}
if (!objName.empty())
{
result->setName(objName);
}
// update global pose
rapidxml::xml_node<>* poseNode = objectXMLNode->first_node("globalpose", 0, false);
if (poseNode)
{
processTransformNode(poseNode, objName, globalPose);
result->setGlobalPose(globalPose);
}
return result;
}
THROW_VR_EXCEPTION_IF(objName.empty(), "Obstacle definition expects attribute 'name'");
rapidxml::xml_node<>* node = objectXMLNode->first_node();
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "visualization")
{
THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl);
visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel)
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
std::string colModelName = objName;
colModelName += "_VISU_ColModel";
// todo: ID?
collisionModel.reset(new CollisionModel(visualizationNode, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
}
else if (nodeName == "collisionmodel")
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
else if (nodeName == "physics")
{
THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
processPhysicsTag(node, objName, physics);
physicsDefined = true;
}
else if (nodeName == "globalpose")
{
processTransformNode(node, objName, globalPose);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
}
node = node->next_sibling();
}
// build object
ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
object->setGlobalPose(globalPose);
return object;
}
/**
* This method creates a VirtualRobot::RobotNodeRevolute.
*
* \return instance of VirtualRobot::RobotNodeRevolute.
*/
RobotNodePtr RobotNodeRevoluteFactory::createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics &p, RobotNode::RobotNodeType rntype) const
{
RobotNodePtr robotNode(new RobotNodeRevolute(robot, nodeName, limitLow, limitHigh, preJointTransform, axis, visualizationModel, collisionModel, jointValueOffset, p, CollisionCheckerPtr(), rntype));
return robotNode;
}