Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
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;
}