diff --git a/GraspPlanning/ContactConeGenerator.cpp b/GraspPlanning/ContactConeGenerator.cpp
index 580bf3504864c89c649a7adc39b5a8e8032afbbc..1c66eaa27377eb6d6d07c5e0b590d01976495976 100644
--- a/GraspPlanning/ContactConeGenerator.cpp
+++ b/GraspPlanning/ContactConeGenerator.cpp
@@ -32,14 +32,15 @@ ContactConeGenerator::ContactConeGenerator(int coneSamples, float frictionCoeff,
 	//Generation of generic friction cone discretized by an 8-sided polyhedron
 	this->frictionCoeff = frictionCoeff;
 	this->frictionConeAngle = atan(frictionCoeff);
-	this->frictionConeRad = 2*unitForce*sin((unitForce*2*frictionConeAngle) / (2*unitForce));
+	this->frictionConeRad = unitForce*sin(frictionConeAngle);
+	this->frictionConeHeight = unitForce*cos(frictionConeAngle);
 	this->frictionConeSamples = coneSamples;
 	for (int i = 0; i < frictionConeSamples; i++)
 	{
 	    Eigen::Vector3f p;
 	    p(0) = unitForce*(float)(cos(frictionConeAngle)*cos(i*2.0*M_PI/frictionConeSamples));
 		p(1) = unitForce*(float)(cos(frictionConeAngle)*sin(i*2.0*M_PI/frictionConeSamples));
-		p(2) = unitForce*(float)cos(frictionConeAngle);
+		p(2) = (float)frictionConeHeight;
 		frictionConeRimPoints.push_back(p);
 	}
 }
@@ -49,61 +50,31 @@ ContactConeGenerator::~ContactConeGenerator()
     frictionConeRimPoints.clear();
 }
 
-
-
 void ContactConeGenerator::computeConePoints( const VirtualRobot::MathTools::ContactPoint &point, std::vector<VirtualRobot::MathTools::ContactPoint> &storeConePoints )
 {
 	//Rotate generic friction cone to align with object normals
-	
 	Eigen::Vector3f upRightNormal(0.0f,0.0f,1.0f);
 	MathTools::Quaternion objNormalRot =  MathTools::getRotation(upRightNormal,point.n);
     Eigen::Matrix4f objNormalTrafo = MathTools::quat2eigen4f(objNormalRot);
         
     Eigen::Vector3f conePoint;
 	
-	/*SbVec3f conePoint;
-	SbVec3f objPoint(point.p(0),point.p(1),point.p(2));
-	SbVec3f objNormal(point.n(0),point.n(1),point.n(2));
-	SbRotation objNormalRot(objNormal,SbVec3f(0.0,0.0,1.0));
-	SbMatrix objNormalTrafo;
-	objNormalTrafo.setRotate(objNormalRot);*/
-
 	float scaleFactor = 1.0f;
 	for (int i = 0; i < frictionConeSamples; i++)
 	{
 		VirtualRobot::MathTools::ContactPoint newConePoint;
 		Eigen::Vector3f conePointOrg = frictionConeRimPoints[i]*scaleFactor;
 		conePoint = MathTools::transformPosition(conePointOrg,objNormalTrafo);
-		    
-		//SbVec3f conePointOrg((float)frictionConeRimPoints[i][0]*scaleFactor,(float)frictionConeRimPoints[i][1]*scaleFactor,(float)frictionConeRimPoints[i][2]*scaleFactor);
-		//objNormalTrafo.multMatrixVec(conePointOrg, conePoint);
 		newConePoint.p = conePoint + point.p;
 		newConePoint.n = conePoint;
 		newConePoint.n.normalize();
-		
-		/*newConePoint.p(0) = conePoint[0]+objPoint[0];
-		newConePoint.p(1) = conePoint[1]+objPoint[1];
-		newConePoint.p(2) = conePoint[2]+objPoint[2];
-		newConePoint.n(0) = conePoint[0];
-		newConePoint.n(1) = conePoint[1];
-		newConePoint.n(2) = conePoint[2];*/
-		/*float l = sqrtf(conePoint[0]*conePoint[0] + conePoint[1]*conePoint[1] + conePoint[2]*conePoint[2]);
-		if (l>=1e-8)
-		{
-			l = 1.0f / l;
-			newConePoint.n(0) *= l;
-			newConePoint.n(1) *= l;
-			newConePoint.n(2) *= l;
-		}*/
 		storeConePoints.push_back(newConePoint);
 	}
 }
 
 void ContactConeGenerator::computeConePoints(const VirtualRobot::MathTools::ContactPoint &point, std::vector<Eigen::Vector3f> &storeConePoints)
 {
-    
     //Rotate generic friction cone to align with object normals
-	
 	Eigen::Vector3f upRightNormal(0.0f,0.0f,1.0f);
 	MathTools::Quaternion objNormalRot =  MathTools::getRotation(upRightNormal,point.n); // invert?!
     Eigen::Matrix4f objNormalTrafo = MathTools::quat2eigen4f(objNormalRot);
@@ -119,25 +90,21 @@ void ContactConeGenerator::computeConePoints(const VirtualRobot::MathTools::Cont
 		newConePoint = conePoint + point.p;
 		storeConePoints.push_back(newConePoint);
 	}
-    /*
-	//Rotate generic friction cone to align with object normals
-	SbVec3f conePoint;
-	SbVec3f objPoint(point.p(0),point.p(1),point.p(2));
-	SbVec3f objNormal(point.n(0),point.n(1),point.n(2));
-	SbRotation objNormalRot(objNormal,SbVec3f(0.0,0.0,1.0));
-	SbMatrix objNormalTrafo;
-	objNormalTrafo.setRotate(objNormalRot);
-	float scaleFactor = 1.0f;
-	for (int i = 0; i < frictionConeSamples; i++)
-	{
-		GraspStudio::Vec3D newConePoint;
-		SbVec3f conePointOrg((float)frictionConeRimPoints[i][0]*scaleFactor,(float)frictionConeRimPoints[i][1]*scaleFactor,(float)frictionConeRimPoints[i][2]*scaleFactor);
-		objNormalTrafo.multMatrixVec(conePointOrg, conePoint);
-		newConePoint.p(0) = conePoint[0]+objPoint[0];
-		newConePoint.p(1) = conePoint[1]+objPoint[1];
-		newConePoint.p(2) = conePoint[2]+objPoint[2];
-		storeConePoints.push_back(newConePoint);
-	}*/
+}
+
+float ContactConeGenerator::getConeAngle()
+{
+	return (float)frictionConeAngle;
+}
+
+float ContactConeGenerator::getConeRadius()
+{
+	return (float)frictionConeRad;
+}
+
+float ContactConeGenerator::getConeHeight()
+{
+	return (float)frictionConeHeight;
 }
 
 } // namespace
diff --git a/GraspPlanning/ContactConeGenerator.h b/GraspPlanning/ContactConeGenerator.h
index fb7e140a424a3f510b2752df76180341c57b2b8e..4ae0d30f7c4a9534bb266ba12118f5662e8ee95b 100644
--- a/GraspPlanning/ContactConeGenerator.h
+++ b/GraspPlanning/ContactConeGenerator.h
@@ -52,6 +52,21 @@ public:
 	//! Computes the cone points without normals. coneSamples computed points are appended to storeConePoints. 
 	void computeConePoints(const VirtualRobot::MathTools::ContactPoint &point, std::vector<Eigen::Vector3f> &storeConePoints);
 
+	/*!
+		Returns the opening angle of a friction cone. [rad]
+	*/
+	float getConeAngle();
+
+	/*!
+		Returns the radius a friction cone.
+	*/
+	float getConeRadius();
+
+	/*!
+		Returns the height a friction cone.
+	*/
+	float getConeHeight();
+
 private:
 
 	//Friction cone relevant parameters
@@ -59,6 +74,7 @@ private:
 	double frictionCoeff;
 	double frictionConeAngle;
 	double frictionConeRad;
+	double frictionConeHeight;
 	std::vector< Eigen::Vector3f > frictionConeRimPoints;
 	int frictionConeSamples;
 
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
index 9366bf98afd3bf3b241ef5be1c7e89b5fa2534b4..f84345329285f54ed2d8ec511a8c84344f9aea81 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.cpp
@@ -110,4 +110,9 @@ bool GraspQualityMeasure::isValid()
 	return isGraspForceClosure();
 }
 
+GraspStudio::ContactConeGeneratorPtr GraspQualityMeasure::getConeGenerator()
+{
+	return coneGenerator;
+}
+
 } // namespace
diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.h b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
index 51ee95be0c705db710baa6ad6a1833681b7f9a98..7aa92e168724612063243ce069d217fba99bae01 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
@@ -66,6 +66,7 @@ public:
 
 	virtual bool isValid();
 
+	virtual ContactConeGeneratorPtr getConeGenerator();
 protected:
 	
 	//Methods
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index ba8847cdcec5b4760b3ff5f6be88fdb731b179f0..001fa78c8679bcd88e217a6e8f7675bd7c863dcb 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -1,6 +1,7 @@
 
 #include "GraspPlannerWindow.h"
 #include "GraspPlanning/Visualization/CoinVisualization/CoinConvexHullVisualization.h"
+#include "GraspPlanning/ContactConeGenerator.h"
 #include "VirtualRobot/EndEffector/EndEffector.h"
 #include "VirtualRobot/Workspace/Reachability.h"
 #include "VirtualRobot/ManipulationObject.h"
@@ -33,6 +34,7 @@
 #include <sstream>
 using namespace std;
 using namespace VirtualRobot;
+using namespace GraspStudio;
 
 float TIMER_MS = 30.0f;
 
@@ -70,7 +72,7 @@ GraspPlannerWindow::GraspPlannerWindow(std::string &robFile, std::string &eefNam
 
 
 
-	m_pExViewer->viewAll();
+	viewer->viewAll();
 
 	/*SoSensorManager *sensor_mgr = SoDB::getSensorManager();
 	SoTimerSensor *timer = new SoTimerSensor(timerCB, this);
@@ -112,19 +114,19 @@ GraspPlannerWindow::~GraspPlannerWindow()
 void GraspPlannerWindow::setupUI()
 {
 	 UI.setupUi(this);
-	 m_pExViewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);
+	 viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);
 
 	// setup
-	m_pExViewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
-	m_pExViewer->setAccumulationBuffer(true);
+	viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
+	viewer->setAccumulationBuffer(true);
 #ifdef WIN32
-	m_pExViewer->setAntialiasing(true, 8);
+	viewer->setAntialiasing(true, 8);
 #endif
-	m_pExViewer->setGLRenderAction(new SoLineHighlightRenderAction);
-	m_pExViewer->setTransparencyType(SoGLRenderAction::BLEND);
-	m_pExViewer->setFeedbackVisibility(true);
-	m_pExViewer->setSceneGraph(sceneSep);
-	m_pExViewer->viewAll();
+	viewer->setGLRenderAction(new SoLineHighlightRenderAction);
+	viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
+	viewer->setFeedbackVisibility(true);
+	viewer->setSceneGraph(sceneSep);
+	viewer->viewAll();
 
 	connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll()));
 	connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan()));
@@ -200,9 +202,13 @@ void GraspPlannerWindow::buildVisu()
 	
 	frictionConeSep->removeAllChildren();
 	bool fc = (UI.checkBoxCones->isChecked());
-	if (fc && contacts.size()>0)
+	if (fc && contacts.size()>0 && qualityMeasure)
 	{
-		SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(contacts);
+		ContactConeGeneratorPtr cg = qualityMeasure->getConeGenerator();
+		float radius = cg->getConeRadius();
+		float height = cg->getConeHeight();
+		float scaling = 30.0f;
+		SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(contacts,height*scaling,radius*scaling);
 		if (visualisationNode)
 			frictionConeSep->addChild(visualisationNode);
 	}
@@ -212,7 +218,7 @@ void GraspPlannerWindow::buildVisu()
 	if (!UI.checkBoxGrasps->isChecked() && sceneSep->findChild(graspsSep)>=0)
 		sceneSep->removeChild(graspsSep);
 
-	m_pExViewer->scheduleRedraw();
+	viewer->scheduleRedraw();
 }
 
 int GraspPlannerWindow::main()
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
index 76cab3036ca559d7bb26f8debf235b4e95c77d69..1d9d11d71dc413e2f72044836be28204346a2781 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
@@ -74,7 +74,7 @@ protected:
 
 	static void timerCB(void * data, SoSensor * sensor);
 	Ui::GraspPlanner UI;
-	SoQtExaminerViewer *m_pExViewer; /*!< Viewer to display the 3D model of the robot and the environment. */
+	SoQtExaminerViewer *viewer; /*!< Viewer to display the 3D model of the robot and the environment. */
 		
 	SoSeparator *sceneSep;
 	SoSeparator *robotSep;
diff --git a/VirtualRobot/IK/PoseQualityMeasurement.cpp b/VirtualRobot/IK/PoseQualityMeasurement.cpp
index b99aa70c4a2c62507f1ab5ec3dbef77c7fb15c60..70b92b87619094b3c5c6101226ac9efaf8b72972 100644
--- a/VirtualRobot/IK/PoseQualityMeasurement.cpp
+++ b/VirtualRobot/IK/PoseQualityMeasurement.cpp
@@ -15,6 +15,7 @@ PoseQualityMeasurement::PoseQualityMeasurement(VirtualRobot::RobotNodeSetPtr rns
 {
 	THROW_VR_EXCEPTION_IF( (!rns || !rns->getTCP()), "NULL data");
 	name = "PoseQualityMeasurement";
+	considerObstacle = false;
 	verbose = false;
 }
 
@@ -49,4 +50,15 @@ bool PoseQualityMeasurement::consideringJointLimits()
 	return false;
 }
 
+void PoseQualityMeasurement::setObstacleDistanceVector( const Eigen::Vector3f &directionSurfaceToObstance )
+{
+	considerObstacle = true;
+	obstacleDir = directionSurfaceToObstance;
+}
+
+void PoseQualityMeasurement::disableObstacleDistance()
+{
+	considerObstacle = false;
+}
+
 }
diff --git a/VirtualRobot/IK/PoseQualityMeasurement.h b/VirtualRobot/IK/PoseQualityMeasurement.h
index c12c1dbc1b45ef0e60781f8ba9c64d4ad30e2879..b90fb6f848caa5dd83c95b4ac9f3f1b7ec0b9b83 100644
--- a/VirtualRobot/IK/PoseQualityMeasurement.h
+++ b/VirtualRobot/IK/PoseQualityMeasurement.h
@@ -68,10 +68,20 @@ public:
 	//! Indicates if joint limits are considered.
 	virtual bool consideringJointLimits();
 
+	/*!
+		Consider obstacles. Here, the shortest distance on the surface of the RNS to an obstacle is set (in TCP coords). 
+		This obstacle vector may be considered by any further calculations (depending on the implementation).
+	*/
+	virtual void setObstacleDistanceVector(const Eigen::Vector3f &directionSurfaceToObstance);
+	virtual void disableObstacleDistance();
 
 protected:
 	std::string name;
     VirtualRobot::RobotNodeSetPtr rns;
+
+	bool considerObstacle;
+	Eigen::Vector3f obstacleDir;
+
 	bool verbose;
 };
 
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index e15580affb7647a4704ec3b42b61de7844c37769..bbc64839cc2b6210b42a35cbc7d508aca3af87aa 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -709,7 +709,7 @@ SoNode * CoinVisualizationFactory::getCoinVisualization( VisualizationNodePtr vi
 	return coinVisu->getCoinVisualization();	 
 }
 
-SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInfo &contact )
+SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInfo &contact, float frictionConeHeight,  float frictionConeRadius )
 {
 	SoSeparator *result = new SoSeparator();
 	result->ref();
@@ -756,8 +756,8 @@ SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInf
 	tr3->matrix.setValue(m3);
 
 	// create cone
-	float fConeHeight = 30.0f;
-	float fConeRadius = 15.0f;
+	float fConeHeight = frictionConeHeight;
+	float fConeRadius = frictionConeRadius;
 	SoCone *cone3 = new SoCone();
 	cone3->bottomRadius = fConeRadius;
 	cone3->height = fConeHeight;
@@ -809,12 +809,12 @@ SoNode * CoinVisualizationFactory::getCoinVisualization( EndEffector::ContactInf
 	return result;
 }
 
-SoNode * CoinVisualizationFactory::getCoinVisualization( std::vector <EndEffector::ContactInfo> &contacts )
+SoNode * CoinVisualizationFactory::getCoinVisualization( std::vector <EndEffector::ContactInfo> &contacts, float frictionConeHeight,  float frictionConeRadius )
 {
 	SoSeparator *res = new SoSeparator;
 	res->ref();
 	for (size_t i=0;i<contacts.size();i++)
-		res->addChild(getCoinVisualization(contacts[i]));
+		res->addChild(getCoinVisualization(contacts[i],frictionConeHeight,frictionConeRadius));
 	res->unrefNoDelete();
 	return res;
 }
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 05c7fbe7304935fcfe6ebcead3c996e02d6471d8..39834f788c5f13d3f964a54218b9992d45d2dcfc 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -124,10 +124,19 @@ public:
 	static SoNode *getCoinVisualization(SceneObjectPtr object, SceneObject::VisualizationType visuType);
 
 	/*!
-		Convenient method to retrieve a coin visualization for a set of contacts
+		Convenient method to retrieve a coin visualization for a set of contacts.
+		\param contacts The contacts to be visualized
+		\param frictionConeHeight The height of the friction cone [mm].
+		\param frictionConeRadius The radius of the cone [mm].
 	*/
-	static SoNode *getCoinVisualization(std::vector <EndEffector::ContactInfo> &contacts);
-	static SoNode *getCoinVisualization(EndEffector::ContactInfo &contact);
+	static SoNode *getCoinVisualization(std::vector <EndEffector::ContactInfo> &contacts, float frictionConeHeight = 30.0f,  float frictionConeRadius = 15.0f);
+	/*!
+		Convenient method to retrieve a coin visualization for a contact.
+		\param contact The contact to be visualized
+		\param frictionConeHeight The height of the friction cone [mm].
+		\param frictionConeRadius The radius of the cone [mm].
+	*/
+	static SoNode *getCoinVisualization(EndEffector::ContactInfo &contact, float frictionConeHeight = 30.0f,  float frictionConeRadius = 15.0f);
 	
 	static SoNode *getCoinVisualization(VisualizationNodePtr visu);
 
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index ee1be154f8a8a24459dc3d1d2b0c162c43201bc4..1f3b2e064a100d4b58946c350080e5ec6143fb2f 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -26,6 +26,8 @@ WorkspaceRepresentation::WorkspaceRepresentation(RobotPtr robot)
 	THROW_VR_EXCEPTION_IF(!robot,"Need a robot ptr here");
 	this->robot = robot;
 	type = "WorkspaceRepresentation";
+	versionMajor = 2;
+	versionMinor = 3;
 	reset();
 }
 
@@ -201,12 +203,18 @@ void WorkspaceRepresentation::load(const std::string &filename)
 		// Check version
 		int version[2];
 		readArray<int>(version, 2, file);
-		THROW_VR_EXCEPTION_IF(
-			(version[0] > 2) || 
-			(version[0] == 2 && !(version[1] == 0 || version[1] == 1 || version[1] == 2)) || 
-			(version[0] == 1 && !(version[1] == 0 || version[1] == 2 || version[1] == 3)
-			),	"Wrong file format version");
-
+		// first check if the current version is used
+		if (version[0] != versionMajor || version[1] != versionMinor)
+		{
+			// now check if an older version is used
+			THROW_VR_EXCEPTION_IF(
+				(version[0] > 2) || 
+				(version[0] == 2 && !(version[1] == 0 || version[1] == 1 || version[1] == 2 || version[1] == 3)) || 
+				(version[0] == 1 && !(version[1] == 0 || version[1] == 2 || version[1] == 3)
+				),	"Wrong file format version");
+		}
+		versionMajor = version[0];
+		versionMinor = version[1];
 		// Check Robot name
 		readString(tmpString, file);
 		THROW_VR_EXCEPTION_IF(tmpString != robot->getType(), "Wrong Robot");
@@ -344,8 +352,8 @@ void WorkspaceRepresentation::save(const std::string &filename)
 		writeString(file, tmpStr);
 
 		// Version
-		write<int>(file, 2);
-		write<int>(file, 2);
+		write<int>(file, versionMajor);
+		write<int>(file, versionMinor);
 
 		// Robot type
 		writeString(file, robot->getType());
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h
index e4b6b17a459d15bd4dc03dff58ee5b8208af4fae..184a96485246d0c7049e72cab01cd2095f46d155 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.h
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h
@@ -278,6 +278,9 @@ protected:
 	
 	std::string type;
 
+	int versionMajor;
+	int versionMinor;
+
 };
 
 
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index adfbbe2d6a9b83411ebd3a4116e28bfffb25137b..7de58c2fed4d9f23af7dcb75d4bb2295f0a4eb11 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -203,6 +203,16 @@ void BaseIO::processTransformNode(rapidxml::xml_node<char> *transformXMLNode, co
 		}
 		rotation = true;
 		translation = true;
+		if (hasUnitsAttribute(matrixXMLNode))
+		{
+			Units u = getUnitsAttribute(matrixXMLNode,Units::eLength);
+			if (u.isMeter())
+			{
+				transform(0,3) *= 1000.0f;
+				transform(1,3) *= 1000.0f;
+				transform(2,3) *= 1000.0f;
+			}
+		}
 	}
 
 	// Rotation Matrix 3x3
@@ -622,14 +632,28 @@ VisualizationNodePtr BaseIO::processVisualizationTag(rapidxml::xml_node<char> *v
 	{
 		enableVisu = isTrue(attr->value());
 	}
+
+	attr = visuXMLNode->first_attribute("useascollisionmodel", 0, false);
+	if (attr)
+	{
+		useAsColModel = isTrue(attr->value());
+	}
 	if (enableVisu)
 	{
 		rapidxml::xml_node<> *visuFileXMLNode = visuXMLNode->first_node("file",0,false);
 		if (visuFileXMLNode)
 		{
 			attr = visuFileXMLNode->first_attribute("type", 0, false);
-			THROW_VR_EXCEPTION_IF(!attr, "Missing 'type' attribute in <Visualization> tag of node " << tagName << "." << endl)
+			//THROW_VR_EXCEPTION_IF(!attr, "Missing 'type' attribute in <Visualization> tag of node " << tagName << "." << endl)
+			if (!attr)
+			{
+				if (VisualizationFactory::first(NULL))
+					visuFileType = VisualizationFactory::first(NULL)->getName();
+				else
+					VR_WARNING << "No visualization present..." << endl;
+			} else
 				visuFileType = attr->value();
+
 			getLowerCase(visuFileType);
 			visuFile = processFileNode(visuFileXMLNode,basePath);
 			//visuFile = visuFileXMLNode->value();
@@ -665,8 +689,16 @@ VisualizationNodePtr BaseIO::processVisualizationTag(rapidxml::xml_node<char> *v
 					coordAxisText = tagName;
 
 				attr = coordXMLNode->first_attribute("type", 0, false);
-				THROW_VR_EXCEPTION_IF(!attr, "Missing 'type' attribute in <CoordinateAxis> tag of node " << tagName << "." << endl)
+				//THROW_VR_EXCEPTION_IF(!attr, "Missing 'type' attribute in <CoordinateAxis> tag of node " << tagName << "." << endl)
+				if (!attr)
+				{
+					if (VisualizationFactory::first(NULL))
+						visuCoordType = VisualizationFactory::first(NULL)->getName();
+					else
+						VR_WARNING << "No visualization present..." << endl;
+				} else
 					visuCoordType = attr->value();
+				
 				getLowerCase(visuCoordType);
 				VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuCoordType, NULL);
 
@@ -721,9 +753,16 @@ CollisionModelPtr BaseIO::processCollisionTag(rapidxml::xml_node<char> *colXMLNo
 		if (colFileXMLNode)
 		{
 			attr = colFileXMLNode->first_attribute("type", 0, false);
-			THROW_VR_EXCEPTION_IF(!attr, "Expecting 'type' attribute in <Collisionmodel> tag of node " << tagName << "." << endl);
-
-			collisionFileType = attr->value();
+			//THROW_VR_EXCEPTION_IF(!attr, "Expecting 'type' attribute in <Collisionmodel> tag of node " << tagName << "." << endl);
+			//collisionFileType = attr->value();
+			if (!attr)
+			{
+				if (VisualizationFactory::first(NULL))
+					collisionFileType = VisualizationFactory::first(NULL)->getName();
+				else
+					VR_WARNING << "No visualization present..." << endl;
+			} else
+				collisionFileType = attr->value();
 			getLowerCase(collisionFileType);
 			collisionFile = processFileNode(colFileXMLNode,basePath);
 		
diff --git a/VirtualRobot/data/robots/iCub/iCub.xml b/VirtualRobot/data/robots/iCub/iCub.xml
index b9ccebd5e872d4f47b0f506636a2f75f2da15c13..1e71bf75afb25a71e9b6bd743be14be5d3bac0e4 100644
--- a/VirtualRobot/data/robots/iCub/iCub.xml
+++ b/VirtualRobot/data/robots/iCub/iCub.xml
@@ -168,7 +168,7 @@
 						<row4 c1="0" c2="0"  c3="0" c4="1"/>
 					</matrix4x4>
 				</Transform>
-			</prejointtransform>
+			</PreJointTransform>
 		</Joint>
 		<ChildFromRobot>
 			<File>iCub_LeftLeg.xml</File>
@@ -286,4 +286,4 @@
 	</RobotNodeSet>
 
 
-</Robot>
\ No newline at end of file
+</Robot>
diff --git a/VirtualRobot/data/robots/iCub/iCub_LeftHand.xml b/VirtualRobot/data/robots/iCub/iCub_LeftHand.xml
index ab77e58af9b7cd5ec7a1e48318267a56bdcbb6a3..71317334fea14534f3517de6e6db571f95c0d55a 100644
--- a/VirtualRobot/data/robots/iCub/iCub_LeftHand.xml
+++ b/VirtualRobot/data/robots/iCub/iCub_LeftHand.xml
@@ -656,7 +656,6 @@
 		</Actor>
 	</Endeffector>
 
-  <!-- ROBOT NODE SETS -->
 	<RobotNodeSet name="Left Hand" kinematicRoot="Left Hand Start" tcp="Left Arm TCP">
 		<Node name="Left Hand Thumb Joint1"/>
 		<Node name="Left Hand Thumb Joint2"/>
@@ -684,4 +683,4 @@
 		<Node name="Left Hand Pinky Joint4"/>
 	</RobotNodeSet>
 	
-	</Robot>
+</Robot>
diff --git a/VirtualRobot/data/robots/iCub/iCub_RightHand.xml b/VirtualRobot/data/robots/iCub/iCub_RightHand.xml
index 6592e975e784e804c544ffa0f739a461f3517dde..41df5a335b0d62ecc010ad8c0ef6b8c8b60b4257 100644
--- a/VirtualRobot/data/robots/iCub/iCub_RightHand.xml
+++ b/VirtualRobot/data/robots/iCub/iCub_RightHand.xml
@@ -674,5 +674,33 @@
 		<Node name="Right Hand Pinky Joint3"/>
 		<Node name="Right Hand Pinky Joint4"/>
 	</RobotNodeSet>
+
+	<RobotNodeSet name="Right Hand Col Model" kinematicRoot="Right Hand Start"  tcp="Right Arm TCP">
+		<Node name="Right Hand Palm"/>
+		<Node name="Right Hand Thumb Joint1"/>
+		<Node name="Right Hand Thumb Joint2"/>
+		<Node name="Right Hand Thumb Joint3"/>
+		<Node name="Right Hand Thumb Joint4"/>
+		
+		<Node name="Right Hand Index Joint1"/>
+		<Node name="Right Hand Index Joint2"/>
+		<Node name="Right Hand Index Joint3"/>
+		<Node name="Right Hand Index Joint4"/>
+		
+		<Node name="Right Hand Middle Joint1"/>
+		<Node name="Right Hand Middle Joint2"/>
+		<Node name="Right Hand Middle Joint3"/>
+		<Node name="Right Hand Middle Joint4"/>
+		
+		<Node name="Right Hand Ring Joint1"/>
+		<Node name="Right Hand Ring Joint2"/>
+		<Node name="Right Hand Ring Joint3"/>
+		<Node name="Right Hand Ring Joint4"/>
+		
+		<Node name="Right Hand Pinky Joint1"/>
+		<Node name="Right Hand Pinky Joint2"/>
+		<Node name="Right Hand Pinky Joint3"/>
+		<Node name="Right Hand Pinky Joint4"/>
+	</RobotNodeSet>
 	
 </Robot>