示例#1
0
    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;
    }
示例#2
0
ScenePtr SceneIO::processSceneAttributes(rapidxml::xml_node<char>* sceneXMLNode)
{
	// process attributes of scene

	// get name
	std::string sceneName = processNameAttribute(sceneXMLNode);
	THROW_VR_EXCEPTION_IF (sceneName.empty(),"Scene definition expects attribute 'name'");

	// build scene
	ScenePtr scene(new Scene(sceneName));
	return scene;
}
示例#3
0
bool SceneIO::processSceneObjectSet(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene )
{
	THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneObjectSet");

	// get name
	std::string sosName = processNameAttribute(sceneXMLNode,true);
	if (sosName.empty())
	{
		THROW_VR_EXCEPTION("Please specify the name of the scene object set...");
		return false;
	}
	SceneObjectSetPtr sos(new SceneObjectSet(sosName));

	rapidxml::xml_node<>* node = sceneXMLNode->first_node();

	while (node)
	{
		std::string nodeName = getLowerCase(node->name());
		if (nodeName == "sceneobject")
		{
			std::string oname = processNameAttribute(node);
			
			THROW_VR_EXCEPTION_IF(!(scene->hasObstacle(oname) || scene->hasManipulationObject(oname)), " Object with name '" << oname << "' not known in scene '" << scene->getName() << "'." << endl);
			if (scene->hasObstacle(oname))
			{
				sos->addSceneObject(scene->getObstacle(oname));
			} else
				sos->addSceneObject(scene->getManipulationObject(oname));
		} else
		{
			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's SceneObjectSet definition <" << sosName << ">." << endl);
		}

		node = node->next_sibling();
	}

	scene->registerSceneObjectSet(sos);
	return true;
}
示例#4
0
    GraspSetPtr ObjectIO::processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName)
    {
        THROW_VR_EXCEPTION_IF(!graspSetXMLNode, "No <GraspSet> tag ?!");

        // get name
        std::string gsName = processNameAttribute(graspSetXMLNode, true);
        std::string gsRobotType = processStringAttribute(std::string("robottype"), graspSetXMLNode, true);
        std::string gsEEF = processStringAttribute(std::string("endeffector"), graspSetXMLNode, true);

        if (gsName.empty() || gsRobotType.empty() || gsEEF.empty())
        {
            THROW_VR_EXCEPTION("GraspSet tags must have valid attributes 'Name', 'RobotType' and 'EndEffector'");
        }

        GraspSetPtr result(new GraspSet(gsName, gsRobotType, gsEEF));

        rapidxml::xml_node<>* node = graspSetXMLNode->first_node();

        while (node)
        {
            std::string nodeName = getLowerCase(node->name());

            if (nodeName == "grasp")
            {
                GraspPtr grasp = processGrasp(node, gsRobotType, gsEEF, objName);
                THROW_VR_EXCEPTION_IF(!grasp, "Invalid 'Grasp' tag in '" << objName << "'." << endl);
                result->addGrasp(grasp);
            }
            else
            {
                THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in GraspSet <" << gsName << ">." << endl);
            }

            node = node->next_sibling();
        }

        return result;
    }
示例#5
0
    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;
    }
示例#6
0
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;
}
示例#7
0
ScenePtr SceneIO::processScene(rapidxml::xml_node<char>* sceneXMLNode, const std::string& basePath)
{
	THROW_VR_EXCEPTION_IF(!sceneXMLNode, "No <Scene> tag in XML definition");

	// process Attributes
	ScenePtr scene;
	scene = processSceneAttributes(sceneXMLNode);

	// process xml nodes
	std::vector<rapidxml::xml_node<char>* > sceneSetNodes;
	std::vector<rapidxml::xml_node<char>* > trajectoryNodes;

	
	rapidxml::xml_node<> *XMLNode = sceneXMLNode->first_node(NULL,0,false);
	while (XMLNode)
	{
		std::string nodeName_ = XMLNode->name();
		std::string nodeName = getLowerCase(XMLNode->name());
		if (nodeName == "robot")
		{
			bool r = processSceneRobot(XMLNode, scene, basePath);
			if (!r)
			{
				std::string failedNodeName = processNameAttribute(XMLNode);
				THROW_VR_EXCEPTION("Failed to create robot " << failedNodeName << " in scene " << scene->getName() << endl);
			} 
			
		} else if (nodeName=="obstacle")
		{
			bool r = processSceneObstacle(XMLNode, scene, basePath);
			if (!r)
			{
				std::string failedNodeName = processNameAttribute(XMLNode);
				THROW_VR_EXCEPTION("Failed to create obstacle " << failedNodeName << " in scene " << scene->getName() << endl);
			}
		} else if (nodeName=="manipulationobject")
		{
			bool r = processSceneManipulationObject(XMLNode, scene, basePath);
			if (!r)
			{
				std::string failedNodeName = processNameAttribute(XMLNode);
				THROW_VR_EXCEPTION("Failed to create ManipulationObject " << failedNodeName << " in scene " << scene->getName() << endl);
			}
		} else if (nodeName=="trajectory")
		{
			trajectoryNodes.push_back(XMLNode);
		} else if (nodeName=="sceneobjectset")
		{
			sceneSetNodes.push_back(XMLNode);
			
		} else
		{
			THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl);
		}
		XMLNode = XMLNode->next_sibling(NULL,0,false);
	}

	// process all sceneSetNodes
	for (size_t i=0;i<sceneSetNodes.size();i++)
	{
		bool r = processSceneObjectSet(sceneSetNodes[i], scene);
		if (!r)
		{
			std::string failedNodeName = processNameAttribute(XMLNode);
			THROW_VR_EXCEPTION("Failed to create SceneObjectSet " << failedNodeName << " in scene " << scene->getName() << endl);
		}
	}
	

	// process all trajectories
	for (size_t i=0;i<trajectoryNodes.size();i++)
	{
		bool r = processSceneTrajectory(trajectoryNodes[i], scene);
		if (!r)
		{
			std::string failedNodeName = processNameAttribute(XMLNode);
			THROW_VR_EXCEPTION("Failed to create trajectory " << failedNodeName << " in scene " << scene->getName() << endl);
		}
	}
	
	return scene;
}