diff --git a/VirtualRobot/CollisionDetection/CollisionChecker.cpp b/VirtualRobot/CollisionDetection/CollisionChecker.cpp
index dea04a85ceee318d484e9a0ec24c7828047de527..7a1d6672bfc1d3522c1ce458828f57023d46c4a8 100644
--- a/VirtualRobot/CollisionDetection/CollisionChecker.cpp
+++ b/VirtualRobot/CollisionDetection/CollisionChecker.cpp
@@ -117,13 +117,10 @@ float CollisionChecker::calculateDistance (SceneObjectPtr model1, SceneObjectPtr
 
 float CollisionChecker::calculateDistance (SceneObjectSetPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2)
 {
-	THROW_VR_EXCEPTION_IF((!model1 || !model2), "NULL data");
-
-	if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
-	{
-		THROW_VR_EXCEPTION ("Collision models are linked to different Collision Checker instances");
-	}
-	THROW_VR_EXCEPTION_IF(!isInitialized(), "Not initialized");
+	VR_ASSERT(model1 && model2);
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT(isInitialized());
 
 	std::vector< CollisionModelPtr > colModels1 = model1->getCollisionModels();
 	std::vector< CollisionModelPtr > colModels2 = model2->getCollisionModels();
@@ -163,13 +160,10 @@ float CollisionChecker::calculateDistance (SceneObjectSetPtr model1, SceneObject
 
 float CollisionChecker::calculateDistance (CollisionModelPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2)
 {
-	THROW_VR_EXCEPTION_IF((!model1 || !model2), "NULL data");
-
-	if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
-	{
-		THROW_VR_EXCEPTION ("Collision models are linked to different Collision Checker instances");
-	}
-	THROW_VR_EXCEPTION_IF(!isInitialized(), "Not initialized");
+	VR_ASSERT(model1 && model2);
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT(isInitialized());
 	
 	std::vector< CollisionModelPtr > colModels = model2->getCollisionModels();
 	if (colModels.size()==0)
@@ -203,13 +197,10 @@ float CollisionChecker::calculateDistance (CollisionModelPtr model1, SceneObject
 
 float CollisionChecker::calculateDistance (CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int* trID1, int* trID2)
 {
-	THROW_VR_EXCEPTION_IF((!model1 || !model2), "NULL data");
-
-	if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
-	{
-		THROW_VR_EXCEPTION ("Collision models are linked to different Collision Checker instances");
-	}
-	THROW_VR_EXCEPTION_IF(!isInitialized(), "Not initialized");
+	VR_ASSERT(model1 && model2);
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT(isInitialized());
 
 	return collisionCheckerImplementation->calculateDistance(model1,model2,P1,P2,trID1,trID2);
 }
@@ -218,13 +209,10 @@ float CollisionChecker::calculateDistance (CollisionModelPtr model1, CollisionMo
 
 bool CollisionChecker::checkCollision (SceneObjectSetPtr model1, SceneObjectSetPtr model2)
 {
-	THROW_VR_EXCEPTION_IF((!model1 || !model2), "NULL data");
-
-	if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
-	{
-		THROW_VR_EXCEPTION ("Collision models are linked to different Collision Checker instances");
-	}
-	THROW_VR_EXCEPTION_IF(!isInitialized(), "Not initialized");
+	VR_ASSERT(model1 && model2);
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT(isInitialized());
 
 	std::vector< CollisionModelPtr > vColModels1 = model1->getCollisionModels();
 	std::vector< CollisionModelPtr > vColModels2 = model2->getCollisionModels();
@@ -257,13 +245,10 @@ bool CollisionChecker::checkCollision (SceneObjectPtr model1, SceneObjectSetPtr
 
 bool CollisionChecker::checkCollision (CollisionModelPtr model1, SceneObjectSetPtr model2)
 {
-	THROW_VR_EXCEPTION_IF((!model1 || !model2), "NULL data");
-
-	if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
-	{
-		THROW_VR_EXCEPTION ("Collision models are linked to different Collision Checker instances");
-	}
-	THROW_VR_EXCEPTION_IF(!isInitialized(), "Not initialized");
+	VR_ASSERT(model1 && model2);
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT(isInitialized());
 
 	std::vector< CollisionModelPtr > vColModels = model2->getCollisionModels();
 	if (vColModels.size()==0)
@@ -289,21 +274,11 @@ bool CollisionChecker::checkCollision (SceneObjectPtr model1, SceneObjectPtr mod
 
 bool CollisionChecker::checkCollision (CollisionModelPtr model1, CollisionModelPtr model2)
 {
-	if (!model1 || !model2)
-	{
-		VR_WARNING << "NULL data." << endl;
-		return false;
-	}
-	if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
-	{
-		VR_WARNING << "Could not go on, collision models are linked to different Collision Checker instances." << endl;
-		return false;
-	}
-	if (!isInitialized())
-	{
-		VR_WARNING << "not initialized." << endl;
-		return false;
-	}
+	VR_ASSERT(model1 && model2);
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
+	VR_ASSERT(isInitialized());
+
 	return collisionCheckerImplementation->checkCollision(model1, model2);//, storeContact);
 }
 
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.cpp b/VirtualRobot/CollisionDetection/CollisionModel.cpp
index 9b9596dce76d0427e013e23e50e1243bb959cb66..6e4eadf50cc65a51de5ea0a2948b6f3244d3cd53 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.cpp
+++ b/VirtualRobot/CollisionDetection/CollisionModel.cpp
@@ -194,6 +194,25 @@ std::string CollisionModel::getXMLString(const std::string &basePath, int tabs)
 	return ss.str();
 }
 
+VirtualRobot::CollisionModelPtr CollisionModel::CreateUnitedCollisionModel( const std::vector<CollisionModelPtr> &colModels )
+{
+	VR_ASSERT(colModels.size()>0);
+	CollisionCheckerPtr colChecker = colModels[0]->getCollisionChecker();
+	std::vector<VisualizationNodePtr> visus;
+	for (size_t i=0;i<colModels.size();i++)
+	{
+		VisualizationNodePtr v = colModels[i]->getVisualization();
+		if (v)
+			visus.push_back(v);
+		VR_ASSERT(colModels[i]->getCollisionChecker() == colChecker);
+	}
+	if (visus.size()==0)
+		return CollisionModelPtr();
+
+	VisualizationNodePtr vc = VisualizationNode::CreateUnitedVisualization(visus);
+	return CollisionModelPtr(new CollisionModel(vc,"",colChecker));
+}
+
 /*
 void CollisionModel::GetAABB( SbBox3f& store_aabb )
 {
@@ -216,34 +235,7 @@ void CollisionModel::GetOOBB(SbXfBox3f& store_oobb)
 	store_oobb = bboxAction.getXfBoundingBox();
 }
 */
-/*
-int CollisionModel::BuildColModel(std::map<SoNode*,int> &mIVIDMapping)
-{
-	if (!colChecker)
-		return 0;
-	if (m_pIVModel)
-	{
-		m_pIVModel->unref();
-	}
-	m_pIVModel = new SoSeparator();
-	m_pIVModel->ref();
-	SoMatrixTransform *pMaTr = new SoMatrixTransform();
-	m_pIVModel->addChild(pMaTr);
-
-	SoSeparator *pIVModel = new SoSeparator();
-	int nodeCount = 0;
-	//int idNr = s_nID;
-	m_vCollisionModelIds.clear();
-	int trNr = collisionModelImplementation->BuildColModel(mIVIDMapping,m_vCollisionModelIds,pIVModel);
-	m_pIVModel->addChild(pIVModel);
 
-	SbMatrix mat;
-	mat.makeIdentity();
-	SetGlobalPose(mat);
-
-	return trNr;
-}
-*/
 
 } // namespace VirtualRobot
 
diff --git a/VirtualRobot/CollisionDetection/CollisionModel.h b/VirtualRobot/CollisionDetection/CollisionModel.h
index c6ecc3728db1e05f1aba3a3b34d683c0a8cfd1e7..e47b520c746a5ded774f16b58bf93c184aab9082 100644
--- a/VirtualRobot/CollisionDetection/CollisionModel.h
+++ b/VirtualRobot/CollisionDetection/CollisionModel.h
@@ -123,6 +123,14 @@ public:
 
 	std::string getXMLString(const std::string &basePath, int tabs);
 
+	/*!
+		Create a united collision model. 
+		All triangle data is copied to one model which usually improves the collision detection performance.
+	*/
+	static CollisionModelPtr CreateUnitedCollisionModel(const std::vector<CollisionModelPtr> &colModels);
+
+
+
 protected:
 
 	//! delete all data
diff --git a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
index 1606ffad225400313114497594b13575f87677a4..ef8a11ffe849b0a9f3c62a9786c7c80c84d27093 100644
--- a/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
+++ b/VirtualRobot/CollisionDetection/PQP/CollisionCheckerPQP.cpp
@@ -34,11 +34,7 @@ float CollisionCheckerPQP::calculateDistance (CollisionModelPtr model1, Collisio
 {
 	boost::shared_ptr<PQP::PQP_Model> m1 = model1->getCollisionModelImplementation()->getPQPModel();
 	boost::shared_ptr<PQP::PQP_Model> m2 = model2->getCollisionModelImplementation()->getPQPModel();
-	if (!m1 || !m2)
-	{
-		VR_WARNING << "no internal data..." << endl;
-		return -1.0f;
-	}
+	VR_ASSERT_MESSAGE(m1&&m2,"NULL data in ColChecker!");
 
 	float res = getMinDistance(m1,m2,model1->getCollisionModelImplementation()->getGlobalPose(),model2->getCollisionModelImplementation()->getGlobalPose(),P1,P2,trID1,trID2);
 	/*if (model1->hasCollisionId(*trID2) && !model1->hasCollisionId(*trID1))
@@ -75,11 +71,7 @@ bool CollisionCheckerPQP::checkCollision (CollisionModelPtr model1, CollisionMod
 {
 	boost::shared_ptr<PQP::PQP_Model> m1 = model1->getCollisionModelImplementation()->getPQPModel();
 	boost::shared_ptr<PQP::PQP_Model> m2 = model2->getCollisionModelImplementation()->getPQPModel();
-	if (!m1 || !m2 || !pqpChecker)
-	{
-		VR_WARNING << "no internal data..." << endl;
-		return false;
-	}
+	VR_ASSERT_MESSAGE(m1&&m2,"NULL data in ColChecker!");
 
 	PQP::PQP_CollideResult result;
 	PQP::PQP_REAL R1[3][3];
@@ -216,7 +208,7 @@ float CollisionCheckerPQP::getMinDistance(boost::shared_ptr<PQP::PQP_Model> m1,
 // returns min distance between the objects
 float CollisionCheckerPQP::getMinDistance(boost::shared_ptr<PQP::PQP_Model> m1, boost::shared_ptr<PQP::PQP_Model> m2, const Eigen::Matrix4f &mat1, const Eigen::Matrix4f &mat2, Eigen::Vector3f &storeP1, Eigen::Vector3f &storeP2, int *storeID1, int *storeID2)
 {
-	THROW_VR_EXCEPTION_IF((!m1 || !m2), "NULL data..");
+	VR_ASSERT_MESSAGE(m1&&m2,"NULL data in ColChecker!");
 
 	PQP::PQP_DistanceResult result;
 
@@ -240,7 +232,8 @@ float CollisionCheckerPQP::getMinDistance(boost::shared_ptr<PQP::PQP_Model> m1,
 
 void CollisionCheckerPQP::GetPQPDistance(const boost::shared_ptr<PQP::PQP_Model>& model1, const boost::shared_ptr<PQP::PQP_Model>& model2, const Eigen::Matrix4f& matrix1, const Eigen::Matrix4f& matrix2, PQP::PQP_DistanceResult& pqpResult)
 {
-	THROW_VR_EXCEPTION_IF(!pqpChecker, "no internal data\n");
+	VR_ASSERT_MESSAGE(pqpChecker,"NULL data in ColChecker!");
+
 
 	PQP::PQP_REAL Rotation1[3][3];
 	PQP::PQP_REAL Translation1[3];
diff --git a/VirtualRobot/IK/PoseQualityManipulability.cpp b/VirtualRobot/IK/PoseQualityManipulability.cpp
index 1b02008d4a1f6fed66e811e5d0d280f4b99ca022..1ff10e129c0e823b22b37264c29c11b7e805fccd 100644
--- a/VirtualRobot/IK/PoseQualityManipulability.cpp
+++ b/VirtualRobot/IK/PoseQualityManipulability.cpp
@@ -58,11 +58,48 @@ Eigen::MatrixXf PoseQualityManipulability::getSingularVectorCartesian()
 }
 
 
+
 float PoseQualityManipulability::getManipulability(ManipulabilityIndexType i)
 {
 	return getManipulability(i,-1);
 }
 
+float PoseQualityManipulability::getManipulability( const Eigen::VectorXf &direction, int considerFirstSV )
+{
+	VR_ASSERT(direction.rows()==3 || direction.rows()==6);
+	Eigen::VectorXf d(6);
+	if ( direction.rows()==6)
+		d = direction;
+	else
+	{
+		d.setZero();
+		d.segment(0,3) = direction;
+	}
+	float result = 0;
+
+	// jacobian (in global coord system!)
+	VirtualRobot::DifferentialIKPtr jacobianGlobal(new VirtualRobot::DifferentialIK(rns));
+	jacobianGlobal->convertModelScalingtoM(convertMMtoM);
+
+	Eigen::MatrixXf jac = jacobianGlobal->getJacobianMatrix(rns->getTCP());
+	// penalize rotation
+	jac.block(3,0,3,jac.cols()) *= penalizeRotationFactor;
+
+	// compute gradient
+	Eigen::VectorXf gradient = (jac.transpose() * d).transpose();
+
+	result = gradient.norm();
+
+
+	if (penJointLimits)
+	{
+		result *= getJointLimitPenalizationFactor();
+	}
+
+	return result;
+
+}
+
 float PoseQualityManipulability::getManipulability( ManipulabilityIndexType i, int considerFirstSV )
 {
 	Eigen::VectorXf sv = getSingularValues();
@@ -110,6 +147,11 @@ float PoseQualityManipulability::getPoseQuality()
 	return getManipulability(manipulabilityType);
 }
 
+float PoseQualityManipulability::getPoseQuality( const Eigen::VectorXf &direction )
+{
+	return getManipulability(direction);
+}
+
 Eigen::VectorXf PoseQualityManipulability::getSingularValues()
 {
 	Eigen::MatrixXf jac = jacobian->getJacobianMatrix(rns->getTCP());
diff --git a/VirtualRobot/IK/PoseQualityManipulability.h b/VirtualRobot/IK/PoseQualityManipulability.h
index b817b2d5e447b8f9d20fdcee68ae264d8ab89d8c..e0f137bcfd48ca445286bed0080124bb2ed2ba7d 100644
--- a/VirtualRobot/IK/PoseQualityManipulability.h
+++ b/VirtualRobot/IK/PoseQualityManipulability.h
@@ -57,6 +57,16 @@ public:
 	*/
 	virtual float getPoseQuality();
 	virtual float getManipulability(ManipulabilityIndexType i);
+
+	/*!
+		The quality is determined for a given Cartesian direction.  
+		The current configuration of the corresponding RNS is analyzed and the quality is returned.
+		See derived classes for details.
+		\param direction A 3d or 6d vector with the Cartesian direction to investigate.
+	*/
+	virtual float getPoseQuality(const Eigen::VectorXf &direction);
+	virtual float getManipulability(const Eigen::VectorXf &direction, int considerFirstSV = -1);
+
 	/*!
 		Considers the first considerFirstSV singular values and ignores any remaining entries in the sv vector.
 		This can be useful, when constrained motions should be analyzed (e.g. 2d motions in 3d)
diff --git a/VirtualRobot/IK/PoseQualityMeasurement.cpp b/VirtualRobot/IK/PoseQualityMeasurement.cpp
index 70b92b87619094b3c5c6101226ac9efaf8b72972..0d1039969c532e19cfbc674b586a82a91bb4bf86 100644
--- a/VirtualRobot/IK/PoseQualityMeasurement.cpp
+++ b/VirtualRobot/IK/PoseQualityMeasurement.cpp
@@ -30,6 +30,13 @@ float PoseQualityMeasurement::getPoseQuality()
 	return 0.0f;
 }
 
+float PoseQualityMeasurement::getPoseQuality( const Eigen::VectorXf &direction )
+{
+	VR_ASSERT(direction.rows()==3 || direction.rows()==6);
+	VR_WARNING << "Please use derived classes..." << endl;
+	return 0.0f;
+}
+
 void PoseQualityMeasurement::setVerbose( bool v )
 {
 	verbose = v;
diff --git a/VirtualRobot/IK/PoseQualityMeasurement.h b/VirtualRobot/IK/PoseQualityMeasurement.h
index b90fb6f848caa5dd83c95b4ac9f3f1b7ec0b9b83..032ee6972009a9f408df6473417de88d01259da0 100644
--- a/VirtualRobot/IK/PoseQualityMeasurement.h
+++ b/VirtualRobot/IK/PoseQualityMeasurement.h
@@ -55,6 +55,16 @@ public:
 		See derived classes for details.
 	*/
 	virtual float getPoseQuality();
+
+	/*!
+		The quality is determined for a given Cartesian direction.  
+		The current configuration of the corresponding RNS is analyzed and the quality is returned.
+		See derived classes for details.
+		\param direction A 3d or 6d vector with the Cartesian direction to investigate.
+	*/
+	virtual float getPoseQuality(const Eigen::VectorXf &direction);
+
+
 	void setVerbose(bool v);
 
 	/*!
diff --git a/VirtualRobot/ManipulationObject.cpp b/VirtualRobot/ManipulationObject.cpp
index ea51fa1982d10e59b82beadb0354988933c0f504..5517bbe077f315aeef026ec056eccd9ef07e0fae 100644
--- a/VirtualRobot/ManipulationObject.cpp
+++ b/VirtualRobot/ManipulationObject.cpp
@@ -118,7 +118,7 @@ std::string ManipulationObject::getXMLString(const std::string &basePath, int ta
 	return ss.str();
 }
 
-ManipulationObjectPtr ManipulationObject::clone( const std::string &name, CollisionCheckerPtr colChecker )
+ManipulationObject* ManipulationObject::_clone( const std::string &name, CollisionCheckerPtr colChecker ) const
 {
 	VisualizationNodePtr clonedVisualizationNode;
 	if (visualizationModel)
@@ -127,13 +127,16 @@ ManipulationObjectPtr ManipulationObject::clone( const std::string &name, Collis
 	if (collisionModel)
 		clonedCollisionModel = collisionModel->clone(colChecker);
 
-	ManipulationObjectPtr result(new ManipulationObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
+	ManipulationObject* result = new ManipulationObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker);
 
 	if (!result)
 	{
 		VR_ERROR << "Cloning failed.." << endl;
 		return result;
 	}
+
+	result->setGlobalPose(getGlobalPose());
+
 	for (size_t i=0;i<graspSets.size();i++)
 	{
 		result->addGraspSet(graspSets[i]->clone());
diff --git a/VirtualRobot/ManipulationObject.h b/VirtualRobot/ManipulationObject.h
index 4afd1adf0d18b0d53242b2fc2f3f71fe2bcdc988..ff1a1f1393813d9a773d6e5787ba50f5f2d8dc25 100644
--- a/VirtualRobot/ManipulationObject.h
+++ b/VirtualRobot/ManipulationObject.h
@@ -79,13 +79,16 @@ public:
 	/*!
 		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() );
+	ManipulationObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {return ManipulationObjectPtr(_clone(name,colChecker));}
 
 	void setFilename(const std::string &filename);
 	std::string getFilename();
 
 protected:
 
+	virtual ManipulationObject* _clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const;
+
+
 	std::string filename;
 		
 	std::vector< GraspSetPtr > graspSets;
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 56b73b604f5e36516be5d13bffce720fd3350325..41f7091d8ff4ad93235041b8786807facfdbd7e8 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -24,6 +24,7 @@
 #define _VirtualRobot_RobotNode_h_
 
 #include "../VirtualRobotImportExport.h"
+#include "../VirtualRobotException.h"
 
 #include "../SceneObject.h"
 #include "../RobotFactory.h"
@@ -271,6 +272,11 @@ public:
 
 	void setThreadsafe(bool);
 
+
+	//! Forbid cloning method from SceneObject. We need to know the new robot for cloning
+	SceneObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {THROW_VR_EXCEPTION("Cloning not allowed this way...");}
+
+
 private: // Use the private setters and getters instead
 	float jointValue;							//< The joint value
 	std::vector<std::string> childrenNames;
@@ -281,7 +287,6 @@ private: // Use the private setters and getters instead
 
 protected:
 	///////////////////////// SETUP ////////////////////////////////////
-	//mutable boost::shared_mutex mutex; 
 	mutable boost::recursive_mutex mutex; 
 	bool use_mutex;
 
@@ -289,7 +294,6 @@ protected:
 	virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
 	virtual void setPreJointTransformation(const Eigen::Matrix4f &trafo);
 
-	//Stefan
 	virtual std::vector<std::string> getChildrenNames() const {return childrenNames;};
 	virtual std::string getParentName() const {RobotNodePtr p = parent.lock();if (p) return p->getName(); else return std::string();};
 
@@ -312,6 +316,9 @@ protected:
 	Derived classes must implement their clone method here.
 	*/
 	virtual RobotNodePtr _clone(const RobotPtr newRobot, const std::vector<std::string> newChildren, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker) = 0;
+
+	virtual SceneObject* _clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {THROW_VR_EXCEPTION("Cloning not allowed this way...");}
+
 };
 
 } // namespace VirtualRobot
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index 66066a5ac0e3e2365cc2fb494dff46fdf07fb88e..9d7fc8eece8913955ad9f60672dd3397e1101010 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -130,7 +130,7 @@ void Obstacle::print( bool printDecoration /*= true*/ )
 		cout << endl;
 }
 
-ObstaclePtr Obstacle::clone( const std::string &name, CollisionCheckerPtr colChecker )
+Obstacle* Obstacle::_clone( const std::string &name, CollisionCheckerPtr colChecker ) const
 {
 	VisualizationNodePtr clonedVisualizationNode;
 	if (visualizationModel)
@@ -139,7 +139,7 @@ ObstaclePtr Obstacle::clone( const std::string &name, CollisionCheckerPtr colChe
 	if (collisionModel)
 		clonedCollisionModel = collisionModel->clone(colChecker);
 
-	ObstaclePtr result(new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
+	Obstacle* result = new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker);
 
 	if (!result)
 	{
@@ -147,6 +147,8 @@ ObstaclePtr Obstacle::clone( const std::string &name, CollisionCheckerPtr colChe
 		return result;
 	}
 
+	result->setGlobalPose(getGlobalPose());
+
 	return result;
 }
 
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index ebf6199f411ec2794cdf49f20cc3a906fe3917cd..034040037309dd9c6fde13785067c3d0715e841b 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -54,7 +54,7 @@ public:
 	/*!
 		Clones this object. If no col checker is given, the one of the original object is used.
 	*/
-	ObstaclePtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() );
+	ObstaclePtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() )  const {return ObstaclePtr(_clone(name,colChecker));}
 
 	int getID();
 
@@ -79,6 +79,8 @@ public:
 	std::string getXMLString(const std::string &basePath, int tabs=0);
 protected:
 
+	virtual Obstacle* _clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const;
+
 	// a counter for internal ids
 	static int idCounter;
 	// my id
diff --git a/VirtualRobot/RuntimeEnvironment.cpp b/VirtualRobot/RuntimeEnvironment.cpp
index 3297423bb772362a63667a8c1b2ce41dc77a935a..3f87fa7a365bb9c38177ce08516066fb665cff64 100644
--- a/VirtualRobot/RuntimeEnvironment.cpp
+++ b/VirtualRobot/RuntimeEnvironment.cpp
@@ -44,13 +44,25 @@ namespace VirtualRobot
 					std::string sd(simox_data_path);
 					sd += std::string("/data");
 					pathFound = addDataPath(sd,true);
+					if (!pathFound)
+					{
+						std::string sd(simox_data_path);
+						sd += std::string("/VirtualRobot/data");
+						pathFound = addDataPath(sd,true);
+					}
+					if (!pathFound)
+					{
+						std::string sd(simox_data_path);
+						sd += std::string("../VirtualRobot/data");
+						pathFound = addDataPath(sd,true);
+					}
 				}
 			}
 #ifdef SIMOX_DATA_PATH
-			addDataPath(std::string(SIMOX_DATA_PATH),true);
+			pathFound = pathFound | addDataPath(std::string(SIMOX_DATA_PATH),true);
 #endif
 #ifdef VIRTUAL_ROBOT_DATA_PATH
-			addDataPath(std::string(VIRTUAL_ROBOT_DATA_PATH),true);
+			pathFound = pathFound | addDataPath(std::string(VIRTUAL_ROBOT_DATA_PATH),true);
 #endif
 		}
 	}
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 19677c28b61e48a99d35c8663bb8cab0c73e1c50..07a52f72fe2cd96e5ecb6e76113c85e2cdbe242a 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -431,7 +431,7 @@ void SceneObject::setName( const std::string &name )
 	this->name = name;
 }
 
-SceneObjectPtr SceneObject::clone( const std::string &name, CollisionCheckerPtr colChecker )
+SceneObject* SceneObject::_clone( const std::string &name, CollisionCheckerPtr colChecker ) const
 {
 	VisualizationNodePtr clonedVisualizationNode;
 	if (visualizationModel)
@@ -440,7 +440,7 @@ SceneObjectPtr SceneObject::clone( const std::string &name, CollisionCheckerPtr
 	if (collisionModel)
 		clonedCollisionModel = collisionModel->clone(colChecker);
 
-	SceneObjectPtr result(new SceneObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
+	SceneObject* result = new SceneObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker);
 
 	if (!result)
 	{
@@ -448,6 +448,8 @@ SceneObjectPtr SceneObject::clone( const std::string &name, CollisionCheckerPtr
 		return result;
 	}
 
+	result->setGlobalPose(getGlobalPose());
+
 	return result;
 }
 
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 7997134acef88acd3a7cf9491fe90856d4e687c3..0079fb74052989f644a435cfe147ffcb74b1f429 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -277,11 +277,13 @@ public:
 	/*!
 		Clones this object. If no col checker is given, the one of the original object is used.
 	*/
-	SceneObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker );
+	SceneObjectPtr clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const {return SceneObjectPtr(_clone(name,colChecker));}
 
 
 
 protected:
+	virtual SceneObject* _clone( const std::string &name, CollisionCheckerPtr colChecker = CollisionCheckerPtr() ) const;
+
 	SceneObject(){};
 
 	//! basic data, used by Obstacle and ManipulationObject
diff --git a/VirtualRobot/SceneObjectSet.cpp b/VirtualRobot/SceneObjectSet.cpp
index f95f09ded449b9bcb8d193d83971ebb647e1d250..cf281dee9b70c18e8a2de63119b2d1db0fc7e669 100644
--- a/VirtualRobot/SceneObjectSet.cpp
+++ b/VirtualRobot/SceneObjectSet.cpp
@@ -2,7 +2,9 @@
 #include "SceneObjectSet.h"
 #include "CollisionDetection/CollisionModel.h"
 #include "CollisionDetection/CollisionChecker.h"
+#include "Visualization//VisualizationNode.h"
 #include "RobotNodeSet.h"
+#include "Obstacle.h"
 #include <boost/pointer_cast.hpp>
 
 namespace VirtualRobot {
@@ -284,5 +286,23 @@ VirtualRobot::SceneObjectSetPtr SceneObjectSet::clone( const std::string &newNam
 	return result;
 }
 
+VirtualRobot::ObstaclePtr SceneObjectSet::createStaticObstacle( const std::string &name )
+{
+	//VisualizationNodePtr visus(new VisualizationNode());
+	std::vector<VisualizationNodePtr> visus;
+	std::vector<CollisionModelPtr> cols;
+	for (size_t i=0;i<sceneObjects.size();i++)
+	{
+		if (sceneObjects[i]->getVisualization())
+			visus.push_back(sceneObjects[i]->getVisualization());
+		if (sceneObjects[i]->getCollisionModel())
+			cols.push_back(sceneObjects[i]->getCollisionModel());
+	}
+	VisualizationNodePtr unitedVisu = VisualizationNode::CreateUnitedVisualization(visus);
+	CollisionModelPtr unitedColModel = CollisionModel::CreateUnitedCollisionModel(cols);
+	SceneObject::Physics phys;
+	return ObstaclePtr(new Obstacle(name,unitedVisu,unitedColModel,phys,colChecker));
+}
+
 } // namespace VirtualRobot
 
diff --git a/VirtualRobot/SceneObjectSet.h b/VirtualRobot/SceneObjectSet.h
index b47058ac30ac86b65d3c858446cd7ff4624aa98d..b7c44000005d6d246abec6fa541553b2c8b5d61b 100644
--- a/VirtualRobot/SceneObjectSet.h
+++ b/VirtualRobot/SceneObjectSet.h
@@ -113,6 +113,14 @@ public:
 	 */
 	SceneObjectSetPtr clone(const std::string &newName, CollisionCheckerPtr newColChecker);
 
+	/*!
+		This method creates a new obstacle from all added SceneObjects. 
+		Note, that the resulting object is a rigid body that can be placed in the scene but the internal structure is fixed.
+		This object can be useful when the SceneObjectSet consists of a large number of objects that do not change their relation to each other.
+		When building a static obstacle collision detection can be performed much more efficient.
+	*/
+	ObstaclePtr createStaticObstacle(const std::string &name);
+
 protected:
 
 	//! delete all data
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 6d06955c87124c64e2731e642a3fb8a74964f224..e6fb09e829995ca4bd80f609f8ab73ca10a65753 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -46,6 +46,7 @@
 #include <Inventor/nodes/SoFaceSet.h>
 #include <Inventor/nodes/SoCylinder.h>
 #include <Inventor/nodes/SoLightModel.h>
+#include <Inventor/VRMLnodes/SoVRMLBillboard.h>
 #include <iostream>
 #include <algorithm>
 #include <boost/pointer_cast.hpp>
@@ -471,6 +472,23 @@ SoSeparator* CoinVisualizationFactory::CreateText(const std::string &s)
 	return textSep;
 }
 
+SoSeparator* CoinVisualizationFactory::CreateBillboardText(const std::string &s)
+{
+	SoSeparator *textSep = new SoSeparator();
+	SoVRMLBillboard *bb = new SoVRMLBillboard();
+	textSep->addChild(bb);
+	SoTranslation *moveT = new SoTranslation();
+	moveT->translation.setValue(2.0f,2.0f,0.0f);
+	textSep->addChild(moveT);
+	SoAsciiText *textNode = new SoAsciiText();
+	/*std::string text2(*text);
+	text2.replace( ' ', "_" );*/
+	SbString text2(s.c_str());
+	text2.apply( &IVToolsHelper_ReplaceSpaceWithUnderscore );
+	textNode->string.set(text2.getString());
+	bb->addChild(textNode);
+	return textSep;
+}
 SoSeparator* CoinVisualizationFactory::CreateVertexVisualization( const Eigen::Vector3f &position, float radius, float transparency, float colorR /*= 0.5f*/, float colorG /*= 0.5f*/, float colorB /*= 0.5f*/ )
 {
 	SoSeparator *res = new SoSeparator;
@@ -1680,6 +1698,43 @@ bool CoinVisualizationFactory::renderOffscreen( SoOffscreenRenderer* renderer, S
 	return true;
 }
 
+VirtualRobot::VisualizationNodePtr CoinVisualizationFactory::createUnitedVisualization( const std::vector<VisualizationNodePtr> &visualizations ) const
+{
+	if (visualizations.size()==0)
+		return VisualizationNodePtr();
+
+	SoSeparator *s = new SoSeparator;
+	s->ref();
+	for (size_t i=0;i<visualizations.size();i++)
+	{
+		if (visualizations[i]->getType() == VisualizationFactory::getName())
+		{
+			//skip empty visus
+			continue;
+		}
+
+		if (visualizations[i]->getType() != getName())
+		{
+			VR_ERROR << "Skipping Visualization " << i << ": Is type " << visualizations[i]->getType() << ", but factory is of type " << getName() << endl;
+			continue;
+		}
+		CoinVisualizationNode* cvn = dynamic_cast<CoinVisualizationNode*>(visualizations[i].get());
+		if (cvn)
+		{
+			SoNode* n = cvn->getCoinVisualization();
+			if (n)
+				s->addChild(n->copy(FALSE));
+		} else
+		{
+			VR_WARNING << "Invalid type casting to CoinVisualizationNode?!" << endl;
+		}
+	}
+
+	VisualizationNodePtr result(new CoinVisualizationNode(s));
+	s->unrefNoDelete();
+	return result;
+}
+
 
 
 } // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
index 91bc8c394c2aa0f2c26f8247e0bbf8d7760d3258..8c05b235445dff55d6b25bbce1884885e0788d43 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h
@@ -75,6 +75,12 @@ public:
 	*/
 	virtual VisualizationNodePtr createVisualization();
 
+	/*!
+		Create a united visualization. Internally all visualizations are copied and added to one SoSeparator.
+		All visualizations have to be of type CoinVisualizationNode.
+	*/
+	virtual VisualizationNodePtr createUnitedVisualization(const std::vector<VisualizationNodePtr> &visualizations) const;
+
 
 	static SoSeparator* CreateConvexHull2DVisualization(const MathTools::ConvexHull2DPtr ch, MathTools::Plane &p, VisualizationFactory::Color colorInner = VisualizationFactory::Color::Blue(), VisualizationFactory::Color colorLine = VisualizationFactory::Color::Black(), float lineSize = 5.0f, const Eigen::Vector3f &offset =Eigen::Vector3f::Zero() );
 	static SoSeparator* CreatePolygonVisualization(const std::vector<Eigen::Vector3f> &points, VisualizationFactory::Color colorInner = VisualizationFactory::Color::Blue(), VisualizationFactory::Color colorLine = VisualizationFactory::Color::Black(), float lineSize = 5.0f);
@@ -113,6 +119,7 @@ public:
 	static SoSeparator* CreateEndEffectorVisualization(EndEffectorPtr eef, SceneObject::VisualizationType = SceneObject::Full);
 
 	static SoSeparator* CreateText(const std::string &s);
+	static SoSeparator* CreateBillboardText(const std::string &s);
 
 	/*!
 		Convenient method to retrieve a coin visualization for a robot
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index 088ead9b89f5a28ab1736396913fb2db784fc3d0..bbdd687dac25e3e433a71d7dd498c8f58a9e0dd1 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -77,6 +77,11 @@ public:
 	*/
 	virtual VisualizationNodePtr createVisualization(){return VisualizationNodePtr();}
 
+	/*!
+		Create a united visualization.
+	*/
+	virtual VisualizationNodePtr createUnitedVisualization(const std::vector<VisualizationNodePtr> &visualizations) const {return VisualizationNodePtr();}
+
 };
 typedef boost::shared_ptr<VisualizationFactory::Color> ColorPtr;
 
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index 0f3f4dbe148358a8b8ef36be7be6dcf522371e12..be326a90ddfb585acca2d94e6f3d0408cdfeb1bf 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -147,4 +147,30 @@ std::string VisualizationNode::getXMLString(const std::string &basePath, int tab
 	return ss.str();
 }
 
+VirtualRobot::VisualizationNodePtr VisualizationNode::CreateUnitedVisualization( const std::vector<VisualizationNodePtr> &visualizations )
+{
+	if (visualizations.size()==0)
+		return VisualizationNodePtr();
+	VisualizationFactoryPtr f;
+	std::vector<VisualizationNodePtr>::const_iterator i = visualizations.begin();
+	while (!f && i!=visualizations.end())
+	{
+		if ((*i)->getType() != VisualizationFactory::getName())
+		{
+			f = VisualizationFactory::fromName((*i)->getType(),NULL);
+			break;
+		}
+		i++;
+	}
+	if (i==visualizations.end())
+	{
+		VR_ERROR << "Could not find visualization factory. Aborting..." << endl;
+		return VisualizationNodePtr();
+	}
+
+	THROW_VR_EXCEPTION_IF(!f,"No VisualizationFactory");
+
+	return f->createUnitedVisualization(visualizations);
+}
+
 } // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/VisualizationNode.h b/VirtualRobot/Visualization/VisualizationNode.h
index 96e3745a46bd4be8c05824e6a5aec625844a2579..91ca3667eae3b9bb130af6e3d1f9d2d324317abc 100644
--- a/VirtualRobot/Visualization/VisualizationNode.h
+++ b/VirtualRobot/Visualization/VisualizationNode.h
@@ -119,6 +119,14 @@ public:
 	virtual std::string getType(){return VisualizationFactory::getName();}
 
 	std::string getXMLString(const std::string &basePath, int tabs);
+
+
+	/*!
+		Create a united visualization. Behavior depends on the derived implementation, 
+		but usually the visualizations are copied and united to one object.
+	*/
+	static VisualizationNodePtr CreateUnitedVisualization(const std::vector<VisualizationNodePtr> &visualizations);
+
 protected:
 	bool boundingBox; //!< Indicates, if the bounding box model was used
 	std::string filename; //!< if the visualization was build from a file, the filename is stored here