Adding support for sceneIO. Therefore several classes are now able to generate...

Adding support for sceneIO. Therefore several classes are now able to generate XML definition strings.

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@215 042f3d55-54a8-47e9-b7fb-15903f145c44

MotionPlanning/examples/RrtGui/RrtGuiWindow.cpp

@@ -235,6 +235,7 @@ void RrtGuiWindow::loadScene()
 		VR_ERROR << " no scene ..." << endl;
 		return;
 	}
+	//SceneIO::saveScene(scene,"testSaveScene.xml");
 	std::vector< RobotPtr > robots = scene->getRobots();
 	if (robots.size()!=1)
 	{

VirtualRobot/CollisionDetection/CollisionModel.cpp

@@ -3,6 +3,7 @@
 #include "CollisionChecker.h"
 #include "../Visualization/TriMeshModel.h"
 #include "../Visualization/VisualizationNode.h"
+#include "../XML/BaseIO.h"
 #include <algorithm>
 
 namespace VirtualRobot {
@@ -168,7 +169,30 @@ VirtualRobot::VisualizationNodePtr CollisionModel::getModelDataVisualization()
 	return modelVisualization;
 }
 
+std::string CollisionModel::getXMLString(const std::string &basePath, int tabs)
+{
+	std::stringstream ss;
+	std::string t = "\t";
+	std::string pre = "";
+	for (int i=0;i<tabs;i++)
+		pre += "\t";
 
+	ss << pre << "<CollisionModel";
+	if (getVisualization()->usedBoundingBoxVisu())
+	{
+		ss << " BoundingBox='true'";
+	}
+	ss << ">\n";
+	std::string fnC = getVisualization()->getFilename();
+	if (!fnC.empty())
+	{
+		if (!basePath.empty())
+			BaseIO::makeRelativePath(basePath,fnC);
+		ss << pre << t << "<File type='" << getVisualization()->getType() << "'>" << fnC << "</File>\n";
+	}
+	ss << pre << "</CollisionModel>\n";
+	return ss.str();
+}
 
 /*
 void CollisionModel::GetAABB( SbBox3f& store_aabb )

VirtualRobot/CollisionDetection/CollisionModel.h

@@ -118,11 +118,12 @@ public:
 
 	TriMeshModelPtr getTriMeshModel(){return collisionModelImplementation->getTriMeshModel();}
 
+	std::string getXMLString(const std::string &basePath, int tabs);
+
 protected:
 
 	//! delete all data
 	void destroyData();  
-
 	VisualizationNodePtr visualization;			// this is the original visualization
 	VisualizationNodePtr modelVisualization;	// this is the visualization of the trimeshmodel
 	bool updateVisualization;

VirtualRobot/Grasp.cpp

@@ -82,20 +82,23 @@ void Grasp::setName( const std::string &name )
 	this->name = name;
 }
 
-std::string Grasp::getXMLString()
+std::string Grasp::getXMLString(int tabs)
 {
 	std::stringstream ss;
-	std::string t = "\t";
-	std::string tt = "\t\t";
-	std::string ttt = "\t\t\t";
-	std::string tttt = "\t\t\t\t";
-
-	ss << tt << "<Grasp name='" << name << "' quality='" << quality << "' Creation='" << creation << "'>\n";
-	ss << ttt<< "<Transform>\n";
-	ss << MathTools::getTransformXMLString(poseTcp, tttt);
-	ss << ttt << "</Transform>\n";
-
-	ss << tt << "</Grasp>\n";
+	std::string t;
+	for (int i=0;i<tabs;i++)
+		t += "\t";
+	std::string tt = t;
+	tt += "\t";
+	std::string ttt = tt;
+	ttt += "\t";
+	
+	ss << t << "<Grasp name='" << name << "' quality='" << quality << "' Creation='" << creation << "'>\n";
+	ss << tt<< "<Transform>\n";
+	ss << MathTools::getTransformXMLString(poseTcp, ttt);
+	ss << tt << "</Transform>\n";
+
+	ss << t << "</Grasp>\n";
 
 	return ss.str();
 }

VirtualRobot/Grasp.h

@@ -97,7 +97,7 @@ public:
 	*/
 	Eigen::Matrix4f getTransformation();
 
-	std::string getXMLString();
+	std::string getXMLString(int tabs=2);
 
 
 	GraspPtr clone();

VirtualRobot/GraspSet.cpp

@@ -120,14 +120,16 @@ std::string GraspSet::getEndEffector()
 	return eef;
 }
 
-std::string GraspSet::getXMLString()
+std::string GraspSet::getXMLString(int tabs)
 {
 	std::stringstream ss;
-	std::string t = "\t";
+	std::string t;
+	for (int i=0;i<tabs;i++)
+		t += "\t";
 
 	ss << t << "<GraspSet name='" << name << "' RobotType='" << robotType << "' EndEffector='" << eef << "'>\n";
 	for (size_t i=0;i<grasps.size();i++)
-		ss << grasps[i]->getXMLString();
+		ss << grasps[i]->getXMLString(tabs+1);
 	ss << t << "</GraspSet>\n";
 
 	return ss.str();

VirtualRobot/GraspSet.h

@@ -73,7 +73,7 @@ public:
 
 	void print();
 
-	std::string getXMLString();
+	std::string getXMLString(int tabs=1);
 
 	GraspSetPtr clone();
 

VirtualRobot/ManipulationObject.cpp

@@ -76,54 +76,44 @@ VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet( const std::string &ro
 	return GraspSetPtr();
 }
 
-std::string ManipulationObject::getXMLString(const std::string &basePath)
+std::string ManipulationObject::getXMLString(const std::string &basePath, int tabs, bool storeLinkToFile)
 {
 	std::stringstream ss;
 	std::string t = "\t";
+	std::string pre = "";
+	for (int i=0;i<tabs;i++)
+		pre += "\t";
 
-	ss << "<ManipulationObject name='" << name << "'>\n";
-	if (visualizationModel)
+	ss << pre << "<ManipulationObject name='" << name << "'>\n";
+	
+	if (storeLinkToFile)
 	{
-		ss << t << "<Visualization";
-		if (visualizationModel->usedBoundingBoxVisu())
+		std::string relFile = filename;
+		if (!basePath.empty())
 		{
-			ss << " BoundingBox='true'";
+			BaseIO::makeRelativePath(basePath,relFile);
 		}
-		ss << ">\n";
-		std::string fnV = visualizationModel->getFilename();
-		if (!fnV.empty())
+		ss << pre << t << "<File>" << relFile << "</File>\n";
+		Eigen::Matrix4f gp = getGlobalPose();
+		if (!gp.isIdentity())
 		{
-			if (!basePath.empty())
-				BaseIO::makeRelativePath(basePath,fnV);
-			ss << t << t << "<File type='" << visualizationModel->getType() << "'>" << fnV << "</File>\n";
+			ss << pre << t << "<GlobalPose>\n";
+			ss << pre << t  << t  << "<Transform>\n";
+			ss << MathTools::getTransformXMLString(gp,tabs+3);
+			ss << pre << t  << t  << "</Transform>\n";
+			ss << pre << t << "</GlobalPose>\n";
 		}
-		ss << t << "</Visualization>\n";
-	}
-
-	if (collisionModel && collisionModel->getVisualization())
+	} else
 	{
 
-		ss << t << "<CollisionModel";
-		if (collisionModel->getVisualization()->usedBoundingBoxVisu())
+		ss << getSceneObjectXMLString(basePath,tabs+1);
+	
+		for (size_t i=0;i<graspSets.size();i++)
 		{
-			ss << " BoundingBox='true'";
+			ss << graspSets[i]->getXMLString(tabs+1) << "\n";
 		}
-		ss << ">\n";
-		std::string fnC = collisionModel->getVisualization()->getFilename();
-		if (!fnC.empty())
-		{
-			if (!basePath.empty())
-				BaseIO::makeRelativePath(basePath,fnC);
-			ss << t << t << "<File type='" << collisionModel->getVisualization()->getType() << "'>" << fnC << "</File>\n";
-		}
-		ss << t << "</CollisionModel>\n";
-	}
-	ss << "\n";
-	for (size_t i=0;i<graspSets.size();i++)
-	{
-		ss << graspSets[i]->getXMLString() << "\n";
 	}
-	ss << "</ManipulationObject>\n";
+	ss << pre << "</ManipulationObject>\n";
 
 	return ss.str();
 }
@@ -152,6 +142,16 @@ ManipulationObjectPtr ManipulationObject::clone( const std::string &name, Collis
 	return result;
 }
 
+void ManipulationObject::setFilename( const std::string &filename )
+{
+	this->filename = filename;
+}
+
+std::string ManipulationObject::getFilename()
+{
+	return filename;
+}
+
 
 
 } //  namespace

VirtualRobot/ManipulationObject.h

@@ -67,14 +67,26 @@ public:
 	*/
 	GraspSetPtr getGraspSet(const std::string &robotType,const std::string &eefName);
 
-	std::string getXMLString(const std::string &basePath = std::string());
+	/*!
+		Creates an XML representation of this object.
+		\param basePath If set, all visualization and collision model files are made relative to this path.
+		\param tabs Create indention at the beginning of each line.
+		\param storeLinkToFile If set, the data (e.g. grasps) are not explicitly listed, but an xml tag directing to the XML file, 
+				from which this instance was loaded, is set. If not set a deep description is created.
+	*/
+	std::string getXMLString(const std::string &basePath = std::string(), int tabs = 0, bool storeLinkToFile = false);
 
 	/*!
 		Clones this object. If no col checker is given, the one of the original object is used.
 	*/
 	ManipulationObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() );
 
+	void setFilename(const std::string &filename);
+	std::string getFilename();
+
 protected:
+
+	std::string filename;
 		
 	std::vector< GraspSetPtr > graspSets;
 };

VirtualRobot/MathTools.cpp

@@ -620,15 +620,46 @@ Eigen::Matrix4f MathTools::quat2eigen4f( float x, float y, float z, float w )
 	//m(3,3) = w*w + x*x + y*y + z*z;*/
 }
 
-std::string MathTools::getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs)
+std::string MathTools::getTransformXMLString(const Eigen::Matrix3f &m, int tabs, bool skipMatrixTag)
+{
+	std::string t;
+	for (int i=0;i<tabs;i++)
+		t += "\t";
+	return getTransformXMLString(m,t,skipMatrixTag);
+}
+
+std::string MathTools::getTransformXMLString(const Eigen::Matrix4f &m, int tabs, bool skipMatrixTag)
+{
+	std::string t;
+	for (int i=0;i<tabs;i++)
+		t += "\t";
+	return getTransformXMLString(m,t,skipMatrixTag);
+}
+
+std::string MathTools::getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs, bool skipMatrixTag)
 {
 	std::stringstream ss;
-	ss << tabs << "<Matrix4x4>\n";
+	if (!skipMatrixTag)
+		ss << tabs << "<Matrix4x4>\n";
 	ss << tabs << "\t<row1 c1='" << m(0,0) << "' c2='" << m(0,1) << "' c3='" << m(0,2) << "' c4='" << m(0,3) << "'/>\n";
 	ss << tabs << "\t<row2 c1='" << m(1,0) << "' c2='" << m(1,1) << "' c3='" << m(1,2) << "' c4='" << m(1,3) << "'/>\n";
 	ss << tabs << "\t<row3 c1='" << m(2,0) << "' c2='" << m(2,1) << "' c3='" << m(2,2) << "' c4='" << m(2,3) << "'/>\n";
 	ss << tabs << "\t<row4 c1='" << m(3,0) << "' c2='" << m(3,1) << "' c3='" << m(3,2) << "' c4='" << m(3,3) << "'/>\n";
-	ss << tabs << "</Matrix4x4>\n";
+	if (!skipMatrixTag)
+		ss << tabs << "</Matrix4x4>\n";
+	return ss.str();
+}
+
+std::string MathTools::getTransformXMLString(const Eigen::Matrix3f &m, const std::string &tabs, bool skipMatrixTag)
+{
+	std::stringstream ss;
+	if (!skipMatrixTag)
+		ss << tabs << "<Matrix3x3>\n";
+	ss << tabs << "\t<row1 c1='" << m(0,0) << "' c2='" << m(0,1) << "' c3='" << m(0,2) << "'/>\n";
+	ss << tabs << "\t<row2 c1='" << m(1,0) << "' c2='" << m(1,1) << "' c3='" << m(1,2) << "'/>\n";
+	ss << tabs << "\t<row3 c1='" << m(2,0) << "' c2='" << m(2,1) << "' c3='" << m(2,2) << "'/>\n";
+	if (!skipMatrixTag)
+		ss << tabs << "</Matrix3x3>\n";
 	return ss.str();
 }
 

VirtualRobot/MathTools.h

@@ -349,7 +349,10 @@ namespace MathTools
 	void VIRTUAL_ROBOT_IMPORT_EXPORT print (const std::vector<ContactPoint> &points);
 	void VIRTUAL_ROBOT_IMPORT_EXPORT print( const Eigen::Vector3f &v, bool endline = true );
 	void VIRTUAL_ROBOT_IMPORT_EXPORT print( const std::vector<float> &v, bool endline = true);
-	std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs);
+	std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix4f &m, int tabs, bool skipMatrixTag = false);
+	std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix4f &m, const std::string &tabs, bool skipMatrixTag = false);
+	std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix3f &m, int tabs, bool skipMatrixTag = false);
+	std::string VIRTUAL_ROBOT_IMPORT_EXPORT getTransformXMLString(const Eigen::Matrix3f &m, const std::string &tabs, bool skipMatrixTag = false);
 	void VIRTUAL_ROBOT_IMPORT_EXPORT convertMM2M( const std::vector<ContactPoint> points, std::vector<ContactPoint> &storeResult );
 
 

VirtualRobot/Obstacle.cpp

@@ -113,6 +113,23 @@ ObstaclePtr Obstacle::clone( const std::string &name, CollisionCheckerPtr colChe
 	return result;
 }
 
+std::string Obstacle::getXMLString(const std::string &basePath, int tabs)
+{
+	std::stringstream ss;
+	std::string t = "\t";
+	std::string pre = "";
+	for (int i=0;i<tabs;i++)
+		pre += "\t";
+
+	ss << pre << "<Obstacle name='" << name << "'>\n";
+
+	ss << getSceneObjectXMLString(basePath,tabs);
+	
+	ss << pre << "</Obstacle>\n";
+
+	return ss.str();
+}
+
 } //  namespace
 
 

VirtualRobot/Obstacle.h

@@ -60,7 +60,7 @@ public:
 
 	*/
 	static ObstaclePtr createBox(float width, float height, float depth, float colorR = 1.0f, float colorG = 0.0f, float colorB=0.0f, std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
-
+	std::string getXMLString(const std::string &basePath, int tabs=0);
 protected:
 
 	// a counter for internal ids

VirtualRobot/Robot.cpp

@@ -666,6 +666,16 @@ bool Robot::setConfig( RobotConfigPtr c )
 	return false;
 }
 
+void Robot::setFilename( const std::string &filename )
+{
+	this->filename = filename;
+}
+
+std::string Robot::getFilename()
+{
+	return filename;
+}
+
 
 } // namespace VirtualRobot
 

VirtualRobot/Robot.h

@@ -227,6 +227,9 @@ public:
 	*/
 	RobotPtr clone(const std::string &name, CollisionCheckerPtr collisionChecker = CollisionCheckerPtr());
 
+	void setFilename(const std::string &filename);
+	std::string getFilename();
+
 protected:
 	Robot();
 	/*!
@@ -236,7 +239,7 @@ protected:
 	*/
 	void createVisualizationFromCollisionModels();
 
-
+	std::string filename; // RobotIO stores the filename here
 	std::string name;
 	std::string type;
 

VirtualRobot/RobotConfig.cpp

@@ -216,7 +216,30 @@ bool RobotConfig::applyToRobot( RobotPtr r )
 	return true;
 }
 
+std::string RobotConfig::getXMLString(int tabs)
+{
+	std::stringstream ss;
+	std::string t;
+	for (int i=0;i<tabs;i++)
+		t += "\t";
+	std::string tt = t;
+	tt += "\t";
+	std::string ttt = tt;
+	ttt += "\t";
+
+	ss << t << "<Configuration name='" << name << "'>\n";
+
+	std::map < std::string, float > jv = getRobotNodeJointvalueMap();
+	std::map< std::string, float >::const_iterator i = jv.begin();
+	while (i != jv.end())
+	{
+		ss << tt << "<Node name='" << i->first << "' unit='radian' value='" << i->second << "'/>\n";
+		i++;
+	}
+	ss << t << "</Configuration>\n";
 
+	return ss.str();
+}
 
 
 

VirtualRobot/RobotConfig.h

@@ -101,6 +101,8 @@ public:
 		apply the joint values to a cloned robot. Therefore this method can be used.
 	*/
 	bool applyToRobot(RobotPtr r);
+
+	std::string getXMLString(int tabs=0);
 protected:
 
 

VirtualRobot/Scene.cpp

@@ -3,6 +3,7 @@
 #include "VirtualRobotException.h"
 #include "ManipulationObject.h"
 #include "SceneObjectSet.h"
+#include "XML/BaseIO.h"
 
 namespace VirtualRobot 
 {
@@ -446,6 +447,77 @@ std::vector< SceneObjectSetPtr > Scene::getSceneObjectSets()
 	return sceneObjectSets;
 }
 
+std::string Scene::getXMLString( const std::string &basePath )
+{
+	std::stringstream ss;
+	ss << "<Scene";
+	if (!name.empty())
+		ss << " name='" << name << "'";
+	ss << ">\n";
+	ss << "\n";
+
+	// process robots
+	for (size_t i=0;i<robots.size();i++)
+	{
+		std::string rob = robots[i]->getName();
+		RobotConfigPtr currentConfig = robots[i]->getConfig();
+		ss << "\t<Robot name='" << rob << "' initConfig='" << currentConfig->getName() << "'>\n";
+		std::string robFile = robots[i]->getFilename();
+		if (!basePath.empty())
+			BaseIO::makeRelativePath(basePath,robFile);
+
+		ss << "\t\t<File>" << robFile << "</File>\n";
+
+		// store global pose (if not identity)
+		Eigen::Matrix4f gp = robots[i]->getGlobalPose();
+		if (!gp.isIdentity())
+		{
+			ss << "\t\t<GlobalPose>\n";
+			ss << "\t\t\t<Transform>\n";
+			ss << MathTools::getTransformXMLString(gp,4);
+			ss << "\t\t\t</Transform>\n";
+			ss << "\t\t</GlobalPose>\n";
+		}
+		// store current config
+		ss << currentConfig->getXMLString(2);
+
+		// store all other configs for robot
+		std::vector<RobotConfigPtr> rc = getRobotConfigs(robots[i]);
+		for (size_t j=0;j<rc.size();j++)
+		{
+			ss << rc[j]->getXMLString(2);
+		}
+		ss << "\t</Robot>\n";
+		ss << "\n";
+	}
+
+	// process manipulation objects
+	for (size_t i=0;i<manipulationObjects.size();i++)
+	{
+		ss << manipulationObjects[i]->getXMLString(basePath,1,true);
+		ss << "\n";
+	}
+
+	// process obstacles
+	for (size_t i=0;i<obstacles.size();i++)
+	{
+		ss << obstacles[i]->getXMLString(basePath,1);
+		ss << "\n";
+	}
+
+
+	// process sceneObjectSets
+	for (size_t i=0;i<sceneObjectSets.size();i++)
+	{
+		ss << sceneObjectSets[i]->getXMLString(1);
+		ss << "\n";
+	}
+
+	ss << "</Scene>\n";
+
+	return ss.str();
+}
+
 
 } //  namespace
 

VirtualRobot/Scene.h

@@ -165,6 +165,13 @@ public:
 	*/
 	template <typename T> boost::shared_ptr<T> getVisualization(SceneObject::VisualizationType visuType = SceneObject::Full);
 
+
+	/*!
+		Creates an XML string that describes this scene. 
+		\param basePath All paths to robots or objects are stored relative to this path.
+		\return The xml string.
+	*/
+	std::string getXMLString(const std::string &basePath);
 protected:
 
 	std::string name;

VirtualRobot/SceneObject.cpp

@@ -457,5 +457,37 @@ Eigen::Matrix3f SceneObject::getInertiaMatrix()
 	return physics.intertiaMatrix;
 }
 
+std::string SceneObject::getSceneObjectXMLString(const std::string &basePath, int tabs)
+{
+	std::stringstream ss;
+	std::string t = "\t";
+	std::string pre = "";
+	for (int i=0;i<tabs;i++)
+		pre += "\t";
 
+	if (visualizationModel)
+	{
+		ss << visualizationModel->getXMLString(basePath,tabs);
+	}
+
+	if (collisionModel && collisionModel->getVisualization())
+	{
+		ss << collisionModel->getXMLString(basePath,tabs);
+	}
+	Eigen::Matrix4f gp = getGlobalPose();
+	if (!gp.isIdentity())
+	{
+		ss << pre << "<GlobalPose>\n";
+		ss << pre << "\t<Transform>\n";
+		ss << MathTools::getTransformXMLString(gp,tabs+2);
+		ss << pre << "\t</Transform>\n";
+		ss << pre << "</GlobalPose>\n";
+	}
+	if (physics.isSet())
+	{
+		ss << physics.getXMLString(tabs);
+	}
+
+	return ss.str();
+}
 } // namespace