GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName) { THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!"); // get name std::string name = processNameAttribute(graspXMLNode, true); std::string method = processStringAttribute("creation", graspXMLNode, true); float quality = processFloatAttribute(std::string("quality"), graspXMLNode, true); std::string preshapeName = processStringAttribute("preshape", graspXMLNode, true); Eigen::Matrix4f pose; pose.setIdentity(); std::vector< RobotConfig::Configuration > configDefinitions; std::string configName; rapidxml::xml_node<>* node = graspXMLNode->first_node(); while (node) { std::string nodeName = getLowerCase(node->name()); if (nodeName == "transform") { processTransformNode(graspXMLNode, name, pose); } else if (nodeName == "configuration") { THROW_VR_EXCEPTION_IF(configDefinitions.size() > 0, "Only one configuration per grasp allowed"); bool c*K = processConfigurationNode(node, configDefinitions, configName); THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in grasp tag '" << name << "'." << endl); } else { THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl); } node = node->next_sibling(); } GraspPtr grasp(new Grasp(name, robotType, eef, pose, method, quality, preshapeName)); if (configDefinitions.size() > 0) { // create & register configs std::map< std::string, float > rc; for (size_t i = 0; i < configDefinitions.size(); i++) { rc[ configDefinitions[i].name ] = configDefinitions[i].value; } grasp->setConfiguration(rc); } return grasp; }
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; }
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; }