Fixing issue with using std::vector and Eigen.

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

MotionPlanning/CSpace/CSpacePath.cpp

@@ -528,13 +528,13 @@ const std::vector <Eigen::VectorXf>& CSpacePath::getPathData() const
 	return path;
 }
 
-std::vector<Eigen::Matrix4f,Eigen::aligned_allocator<Eigen::Matrix4f> > CSpacePath::createWorkspacePath( VirtualRobot::RobotNodePtr r )
+std::vector<Eigen::Matrix4f > CSpacePath::createWorkspacePath( VirtualRobot::RobotNodePtr r )
 {
 	VR_ASSERT(r);
 	VR_ASSERT(cspace);
 	VirtualRobot::RobotNodeSetPtr rns = cspace->getRobotNodeSet();
 	VR_ASSERT(rns);
-	std::vector<Eigen::Matrix4f,Eigen::aligned_allocator<Eigen::Matrix4f> > result;
+	std::vector<Eigen::Matrix4f > result;
 
 	if (cspace->hasExclusiveRobotAccess())
 		CSpace::lock();

MotionPlanning/CSpace/CSpacePath.h

@@ -68,7 +68,7 @@ public:
 	unsigned int getNrOfPathPoints() const;
 
 	//! to retrieve entries of path
-	bool getPathEntries(unsigned int start, unsigned int end , std::vector<Eigen::VectorXf> &storePosList) const;
+	bool getPathEntries(unsigned int start, unsigned int end , std::vector<Eigen::VectorXf > &storePosList) const;
 
 	/*!
 	  Creates a copy of the path instance.
@@ -118,7 +118,7 @@ public:
 	  \param c configuration / valid joint values to insert as a point in solution path
 	*/
 	virtual void insertPosition(unsigned int pos, const Eigen::VectorXf &c);
-	virtual void insertPosition(unsigned int pos, std::vector<Eigen::VectorXf> &newConfigurations);
+	virtual void insertPosition(unsigned int pos, std::vector<Eigen::VectorXf > &newConfigurations);
 	virtual void insertPath(unsigned int pos, CSpacePathPtr pathToInsert);
 
 
@@ -138,7 +138,7 @@ public:
 	virtual void print() const;
 
 	//! For quick access to data.
-	const std::vector <Eigen::VectorXf>& getPathData() const;
+	const std::vector <Eigen::VectorXf >& getPathData() const;
 
 	/*!
 		Creates the corresponding path in workspace.
@@ -147,13 +147,13 @@ public:
 				(The result is a std::vector of Eigen::Matrix4f, but since Eigen alignes memory allocation in a special way, there must be 
 				aligned_allocator passed to the std::vector template.)
 	*/
-	std::vector<Eigen::Matrix4f,Eigen::aligned_allocator<Eigen::Matrix4f> > createWorkspacePath(VirtualRobot::RobotNodePtr r);
+	std::vector<Eigen::Matrix4f > createWorkspacePath(VirtualRobot::RobotNodePtr r);
 
 
 	CSpacePtr getCSpace();
 protected:
 
-	std::vector <Eigen::VectorXf> path;		//!< vector with configurations which represent the path
+	std::vector<Eigen::VectorXf > path;		//!< vector with configurations which represent the path
 	unsigned int dimension;		//!< dimension of rrt space
 	CSpacePtr cspace;
 };

MotionPlanning/CSpace/CSpaceTree.cpp

@@ -185,8 +185,8 @@ bool CSpaceTree::appendPath(CSpaceNodePtr startNode, CSpacePathPtr path, int *st
 	SABA_ASSERT (hasNode(startNode))
 	SABA_ASSERT (path)
 	CSpaceNodePtr n = startNode;
-	const std::vector <Eigen::VectorXf> data = path->getPathData();
-	std::vector <Eigen::VectorXf>::const_iterator it;
+	const std::vector<Eigen::VectorXf > data = path->getPathData();
+	std::vector<Eigen::VectorXf >::const_iterator it;
 	for (it=data.begin(); it!=data.end(); it++)
 	{
 		n = appendNode(*it, n->ID);

VirtualRobot/CollisionDetection/CDManager.h

@@ -27,7 +27,7 @@
 #ifndef _VirtualRobot_CDManager_h_
 #define _VirtualRobot_CDManager_h_
 
-#include "../VirtualRobot.h"
+#include "../VirtualRobotImportExport.h"
 #include "CollisionModel.h"
 #include "../SceneObjectSet.h"
 #include "CollisionChecker.h"

VirtualRobot/CollisionDetection/CollisionChecker.cpp

@@ -528,7 +528,7 @@ void CollisionChecker::getContacts( const MathTools::Plane &p, CollisionModelPtr
 	bbox.min -= Eigen::Vector3f(maxDist,maxDist,maxDist);
 	bbox.max += Eigen::Vector3f(maxDist,maxDist,maxDist);
 
-	std::vector <Eigen::Vector3f> ptsBB = bbox.getPoints();
+	std::vector<Eigen::Vector3f > ptsBB = bbox.getPoints();
 	for (size_t i=0;i<ptsBB.size();i++)
 	{
 		ptsBB[i] = MathTools::transformPosition(ptsBB[i],colModel->getGlobalPose());

VirtualRobot/CollisionDetection/CollisionModel.cpp

@@ -128,7 +128,7 @@ std::vector< Eigen::Vector3f > CollisionModel::getModelVeticesGlobal()
 	Eigen::Matrix4f t;
 	t.setIdentity();
 
-	for (std::vector<Eigen::Vector3f>::iterator i=model->vertices.begin(); i!=model->vertices.end(); i++)
+	for (std::vector<Eigen::Vector3f >::iterator i=model->vertices.begin(); i!=model->vertices.end(); i++)
 	{
 		t.block(0,3,3,1)=*i;
 		t = globalPose * t;
@@ -142,7 +142,7 @@ BoundingBox CollisionModel::getBoundingBox( bool global /*= true*/ )
 	if (global)
 	{
 
-		std::vector <Eigen::Vector3f> pts = bbox.getPoints();
+		std::vector<Eigen::Vector3f> pts = bbox.getPoints();
 		for (size_t i=0;i<pts.size();i++)
 			pts[i] = MathTools::transformPosition(pts[i],globalPose);
 

VirtualRobot/IK/PoseQualityManipulability.h

@@ -23,6 +23,8 @@
 #ifndef __PoseQualityManipulability_H_
 #define __PoseQualityManipulability_H_
 
+#include "../VirtualRobotImportExport.h"
+
 #include "PoseQualityMeasurement.h"
 
 namespace VirtualRobot

VirtualRobot/IK/PoseQualityMeasurement.h

@@ -23,12 +23,12 @@
 #ifndef __PoseQualityMeasurement_H_
 #define __PoseQualityMeasurement_H_
 
-#include "VirtualRobot/VirtualRobot.h"
-#include "VirtualRobot/Robot.h"
-#include "VirtualRobot/VirtualRobotException.h"
-#include "VirtualRobot/Nodes/RobotNode.h"
-#include "VirtualRobot/RobotNodeSet.h"
-#include "VirtualRobot/IK/DifferentialIK.h"
+#include "../VirtualRobotImportExport.h"
+#include "../Robot.h"
+#include "../VirtualRobotException.h"
+#include "../Nodes/RobotNode.h"
+#include "../RobotNodeSet.h"
+#include "../IK/DifferentialIK.h"
 
 #include <string.h>
 #include <vector>

VirtualRobot/SceneObject.h

@@ -23,7 +23,6 @@
 #ifndef _VirtualRobot_SceneObject_h_
 #define _VirtualRobot_SceneObject_h_
 
-#include "VirtualRobot.h"
 #include "VirtualRobotImportExport.h"
 #include "Visualization/VisualizationNode.h"
 #include <Eigen/Core>

VirtualRobot/VirtualRobot.h

@@ -99,6 +99,20 @@
 
 #endif
 
+
+// allow std vector to be used with Eigen objects
+
+#include<Eigen/StdVector>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2f)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3f)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector4f)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXf)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2f)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3f)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4f)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::MatrixXf)
+
+
 #include <boost/shared_ptr.hpp>
 #include <boost/assert.hpp>
 #include <boost/weak_ptr.hpp>

VirtualRobot/examples/SceneViewer/SceneViewer.cpp

@@ -4,6 +4,7 @@
 #include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/Visualization/VisualizationFactory.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
 
 
 
@@ -32,7 +33,11 @@ int main(int argc, char *argv[])
 	std::string filename(VR_BASE_DIR "/examples/showScene/scene1.xml");
 	//std::string filename(VR_BASE_DIR "/examples/showScene/sceneiCub.xml");
 
+	VirtualRobot::RuntimeEnvironment::considerKey("scene");
+	VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
+	VirtualRobot::RuntimeEnvironment::print();
 
+	filename = VirtualRobot::RuntimeEnvironment::checkValidFileParameter("scene",filename);
 	showSceneWindow rw(filename);
 
 	rw.main();

VirtualRobot/examples/SceneViewer/showSceneWindow.cpp

@@ -109,92 +109,9 @@ QString showSceneWindow::formatString(const char *s, float f)
 
 void showSceneWindow::resetSceneryAll()
 {
-	/*if (!m_pRobot)
-		return;
-
-	for (unsigned int i=0;i<allRobotNodes.size();i++)
-	{
-		allRobotNodes[i]->setJointValue(0);
-	}
-
-	selectJoint(UI.comboBoxJoint->currentIndex());*/
-}
 
-
-/*
-void showSceneWindow::displayTriangles()
-{
-	QString text1,text2,text3;
-	int trisAllFull, trisRNSFull, trisJointFull;
-	trisAllFull = trisRNSFull = trisJointFull = 0;
-	int trisAllCol,trisRNSCol,trisJointCol;
-	trisAllCol = trisRNSCol = trisJointCol = 0;
-	if (m_pRobot)
-	{
-		trisAllFull = m_pRobot->getNumFaces(false);
-		trisAllCol = m_pRobot->getNumFaces(true);
-		trisRNSFull = trisAllFull;
-		trisRNSCol = trisAllCol;
-	}
-	if (currentRobotNodeSet)
-	{
-		trisRNSFull = currentRobotNodeSet->getNumFaces(false);
-		trisRNSCol = currentRobotNodeSet->getNumFaces(true);
-	}
-	if (currentRobotNode)
-	{
-		trisJointFull = currentRobotNode->getNumFaces(false);
-		trisJointCol = currentRobotNode->getNumFaces(true);
-	}
-	if (UI.checkBoxColModel->checkState() == Qt::Checked)
-	{
-		text1 = tr("Total\t:") + QString::number(trisAllCol);
-		text2 = tr("RobotNodeSet:\t") + QString::number(trisRNSCol);
-		text3 = tr("Joint:\t") + QString::number(trisJointCol);
-	} else
-	{		
-		text1 = tr("Total:\t") + QString::number(trisAllFull);
-		text2 = tr("RobotNodeSet:\t") + QString::number(trisRNSFull);
-		text3 = tr("Joint:\t") + QString::number(trisJointFull);
-	}
-	UI.labelInfo1->setText(text1);
-	UI.labelInfo2->setText(text2);
-	UI.labelInfo3->setText(text3);
 }
 
-void showSceneWindow::robotFullModel()
-{
-	if (!m_pRobot)
-		return;
-
-	bool showFullModel = UI.checkBoxFullModel->checkState() == Qt::Checked;
-
-	m_pRobot->setupVisualization(showFullModel, true);
-
-}
-
-void showSceneWindow::collisionModel()
-{
-	if (!m_pRobot)
-		return;
-	robotSep->removeAllChildren();
-	//setRobotModelShape(UI.checkBoxColModel->state() == QCheckBox::On);
-	useColModel = UI.checkBoxColModel->checkState() == Qt::Checked;
-    visualization = m_pRobot->getVisualization<CoinVisualization>(useColModel);
-	SoNode* visualisationNode = NULL;
-	if (visualization)
-		visualisationNode = visualization->getCoinVisualization();
-
-	if (visualisationNode)
-		robotSep->addChild(visualisationNode);
-
-	selectJoint(UI.comboBoxJoint->currentIndex());
-
-	UI.checkBoxStructure->setEnabled(!useColModel);
-	UI.checkBoxFullModel->setEnabled(!useColModel);
-	UI.checkBoxRobotCoordSystems->setEnabled(!useColModel);
-
-}*/
 
 
 void showSceneWindow::closeEvent(QCloseEvent *event)
@@ -220,12 +137,6 @@ void showSceneWindow::buildVisu()
 	if (visualisationNode)
 		sceneVisuSep->addChild(visualisationNode);
 
-	/*selectJoint(UI.comboBoxJoint->currentIndex());
-
-	UI.checkBoxStructure->setEnabled(!useColModel);
-	UI.checkBoxFullModel->setEnabled(!useColModel);
-	UI.checkBoxRobotCoordSystems->setEnabled(!useColModel);*/
-
 }
 
 int showSceneWindow::main()
@@ -384,6 +295,33 @@ void showSceneWindow::loadScene()
 		return;
 	}
 
+	/*std::vector<VirtualRobot::ManipulationObjectPtr> mo;
+	mo = scene->getManipulationObjects();
+	cout << "Printing " << mo.size() << " objects" << endl;
+	for (size_t i=0;i<mo.size();i++)
+	{
+		mo[i]->print();
+		mo[i]->showCoordinateSystem(true);
+		Eigen::Vector3f c = mo[i]->getCoMGlobal();
+		cout << "com global: \n" << c << endl;
+		c = mo[i]->getCoMLocal();
+		cout << "com local: \n" << c << endl;
+		//mo[i]->showBoundingBox(true);
+	}*/
+	/*std::vector<VirtualRobot::ObstaclePtr> o;
+	o = scene->getObstacles();
+	cout << "Printing " << o.size() << " obstacles" << endl;
+	for (size_t i=0;i<o.size();i++)
+	{
+		o[i]->print();
+		o[i]->showCoordinateSystem(true);
+		Eigen::Vector3f c = o[i]->getCoMGlobal();
+		cout << "com global: \n" << c << endl;
+		c = o[i]->getCoMLocal();
+		cout << "com local: \n" << c << endl;
+		//mo[i]->showBoundingBox(true);
+	}*/
+
 	// get nodes
 	/*m_pRobot->getRobotNodes(allRobotNodes);
 	m_pRobot->getRobotNodeSets(robotNodeSets);