diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
index 602f13fff37affd25ff664d3d8326d3d0f1beba4..2534e15ee0d51c2efe7d440d0c392f64887bbea2 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.cpp
@@ -16,14 +16,6 @@ BulletEngine::BulletEngine()
 	overlappingPairCache = NULL;
 	constraintSolver = NULL;
 	dynamicsWorld = NULL;
-	bulletRestitution = btScalar(0);
-	bulletFriction = btScalar(0.5f);
-	bulletDampingLinear = btScalar(0.05f);
-	//bulletDampingAngular = btScalar(0.85f);
-	bulletDampingAngular = btScalar(0.1f);
-
-	bulletSolverIterations = 100;
-	bulletSolverGlobalContactForceMixing = 0;
 }
 
 BulletEngine::~BulletEngine()
@@ -31,10 +23,27 @@ BulletEngine::~BulletEngine()
 	cleanup();
 }
 
-bool BulletEngine::init(const DynamicsWorldInfo &info)
+bool BulletEngine::init( DynamicsEngineConfigPtr config )
 {
-    boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
-	DynamicsEngine::init(info);
+	// first check if config is of type BulletEngineConfig
+	BulletEngineConfigPtr test = boost::dynamic_pointer_cast<BulletEngineConfig>(config);
+	if (!config || !test)
+	{
+		BulletEngineConfigPtr c(new BulletEngineConfig());
+		if (config)
+			c->gravity = config->gravity;
+		return init(c);
+	} else
+	{
+		return init(test);
+	}
+}
+
+bool BulletEngine::init( BulletEngineConfigPtr config )
+{
+	boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
+	DynamicsEngine::init(config);
+	bulletConfig = config;
 
 	// Setup the bullet world
 	collision_config = new btDefaultCollisionConfiguration();
@@ -53,14 +62,27 @@ bool BulletEngine::init(const DynamicsWorldInfo &info)
 
 	dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,constraintSolver,collision_config);
 
-	dynamicsWorld->setGravity(btVector3(btScalar(info.gravity[0]),btScalar(info.gravity[1]),btScalar(info.gravity[2])));
+	dynamicsWorld->setGravity(btVector3(btScalar(config->gravity[0]),btScalar(config->gravity[1]),btScalar(config->gravity[2])));
 
 	collisionFilterCallback = new BulletEngine::CustomCollisionCallback(this);
 	dynamicsWorld->getPairCache()->setOverlapFilterCallback(collisionFilterCallback);
 
 	btContactSolverInfo& solverInfo = dynamicsWorld->getSolverInfo();
-	solverInfo.m_numIterations = bulletSolverIterations;
-	solverInfo.m_globalCfm = bulletSolverGlobalContactForceMixing;
+	solverInfo.m_numIterations = config->bulletSolverIterations;
+	solverInfo.m_globalCfm = config->bulletSolverGlobalContactForceMixing;
+	solverInfo.m_erp = config->bulletSolverGlobalErrorReductionParameter;
+	//solverInfo.m_solverMode |= SOLVER_USE_2_FRICTION_DIRECTIONS;
+
+	/*
+	By default, Bullet solves positional constraints and velocity constraints coupled together. 
+	This works well in many cases, but the error reduction of position coupled to velocity introduces extra energy (noticeable as 'bounce'). 
+	Instead of coupled positional and velocity constraint solving, the two can be solved separately using the 'split impulse' option. 
+	This means that recovering from deep penetrations doesn't add any velocity. You can enable the option using:
+	*/
+	//solverInfo.m_splitImpulse = 1; //enable split impulse feature
+	//optionally set the m_splitImpulsePenetrationThreshold (only used when m_splitImpulse  is enabled)
+	//only enable split impulse position correction when the penetration is deeper than this m_splitImpulsePenetrationThreshold, otherwise use the regular velocity/position constraint coupling (Baumgarte).
+	//solverInfo.m_splitImpulsePenetrationThreshold = btScalar(-0.02);
 
 	return true;
 }
@@ -121,11 +143,11 @@ bool BulletEngine::addObject( DynamicsObjectPtr o )
 		break;
 	}
 	btObject->getRigidBody()->setCollisionFlags(btColFlag);
-	btObject->getRigidBody()->setRestitution(bulletRestitution);
-	btObject->getRigidBody()->setFriction(bulletFriction);
-	btObject->getRigidBody()->setDamping(bulletDampingLinear,bulletDampingAngular);
-	btObject->getRigidBody()->setDeactivationTime(5.0f);
-	btObject->getRigidBody()->setSleepingThresholds(0.05f, 0.05f);
+	btObject->getRigidBody()->setRestitution(bulletConfig->bulletObjectRestitution);
+	btObject->getRigidBody()->setFriction(bulletConfig->bulletObjectFriction);
+	btObject->getRigidBody()->setDamping(bulletConfig->bulletObjectDampingLinear,bulletConfig->bulletObjectDampingAngular);
+	btObject->getRigidBody()->setDeactivationTime(bulletConfig->bulletObjectDeactivation);//5.0f);
+	btObject->getRigidBody()->setSleepingThresholds(bulletConfig->bulletObjectSleepingThresholdLinear,bulletConfig->bulletObjectSleepingThresholdAngular);//0.05f, 0.05f);
 
 	//btScalar defaultContactProcessingThreshold = BT_LARGE_FLOAT;
 	//btObject->getRigidBody()->setContactProcessingThreshold(defaultContactProcessingThreshold);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
index fb7a9569162a7b8c4463480edf0dbd6eaddc6a03..8892286f8af8db12cde895146c17abd9b18d3dbd 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h
@@ -32,6 +32,42 @@
 namespace SimDynamics
 {
 
+	class BulletEngineConfig : public DynamicsEngineConfig 
+	{
+	public:
+		BulletEngineConfig() : DynamicsEngineConfig()
+		{
+			bulletObjectRestitution = btScalar(0.2);
+			bulletObjectFriction = btScalar(0.5f);
+			bulletObjectDampingLinear = btScalar(0.05f);
+			//bulletObjectDampingAngular = btScalar(0.85f);
+			bulletObjectDampingAngular = btScalar(0.1f);
+			bulletObjectDeactivation = btScalar(5.0);//1.0);
+			bulletObjectSleepingThresholdLinear = btScalar(0.05f);//1.5);
+			bulletObjectSleepingThresholdAngular = btScalar(0.05f);//2.5);
+
+			bulletSolverIterations = 100;
+			bulletSolverGlobalContactForceMixing = 0;
+			bulletSolverGlobalErrorReductionParameter = btScalar(0.5);//0.1);
+		}
+
+		virtual ~BulletEngineConfig(){}
+
+		// global setup values
+		btScalar bulletObjectRestitution;
+		btScalar bulletObjectFriction;
+		btScalar bulletObjectDampingLinear;
+		btScalar bulletObjectDampingAngular;
+		btScalar bulletObjectSleepingThresholdLinear;
+		btScalar bulletObjectSleepingThresholdAngular;
+		btScalar bulletObjectDeactivation;
+		int bulletSolverIterations;
+		btScalar bulletSolverGlobalContactForceMixing; // allow to violate constraints (eg joint limits). A value>0 may increase stablity. (standard:0)
+		btScalar bulletSolverGlobalErrorReductionParameter; // How hard should the solver try to correct misaligned joints/constraints/links. (standard 0.2)
+	};
+
+	typedef boost::shared_ptr<BulletEngineConfig> BulletEngineConfigPtr;
+
 /*!
 	This class encapsulates all calls to the bullet physics engine. 
 	Usually there is no need to instantiate this object by your own, it is automatically created when calling DynamicsWorld::Init().
@@ -41,6 +77,8 @@ class SIMDYNAMICS_IMPORT_EXPORT BulletEngine : public DynamicsEngine, public boo
 public:
 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+	friend class BulletObject;
+
 	/*!
 	Constructor
 	*/
@@ -56,7 +94,13 @@ public:
 	virtual bool addRobot(DynamicsRobotPtr r);
 	virtual bool removeRobot(DynamicsRobotPtr r);
 
-	virtual bool init(const DynamicsWorldInfo &info);
+	/*!
+		Initialize the engine with this configuration.
+		\param config Either a standard init (could be NULL), or if config is of type BulletEngineConfig, Bullet specific parameters will be considered.
+	*/
+	virtual bool init(DynamicsEngineConfigPtr config);
+	virtual bool init(BulletEngineConfigPtr config);
+
 	virtual bool cleanup();
 
 	/*!
@@ -127,17 +171,11 @@ protected:
 	btCollisionDispatcher* dispatcher;
 	btDefaultCollisionConfiguration * collision_config;
 
-	// global setup values
-	btScalar bulletRestitution;
-	btScalar bulletFriction;
-	btScalar bulletDampingLinear;
-	btScalar bulletDampingAngular;
-	int bulletSolverIterations;
-	btScalar bulletSolverGlobalContactForceMixing;
-
 	btOverlapFilterCallback * collisionFilterCallback;
 
 	VirtualRobot::ObstaclePtr groundObject;
+
+	BulletEngineConfigPtr bulletConfig;
 };
 
 typedef boost::shared_ptr<BulletEngine> BulletEnginePtr;
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp
index f3f901cd381786434f3dc20a7d6a3777a257d2a6..b93348a5d960dba76b98cb14f7f15e4350223b86 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.cpp
@@ -36,11 +36,11 @@ boost::shared_ptr<DynamicsEngineFactory> BulletEngineFactory::createInstance(voi
     return bulletFactory;
 }
 
-DynamicsEnginePtr BulletEngineFactory::createEngine()
+DynamicsEnginePtr BulletEngineFactory::createEngine(DynamicsEngineConfigPtr config)
 {
 	BulletEnginePtr bulletEngine(new BulletEngine());
-	DynamicsEngine::DynamicsWorldInfo i; // standard gravity
-	bulletEngine->init(i);
+	//DynamicsEngine::DynamicsWorldInfo i; // standard gravity
+	bulletEngine->init(config);
 	return bulletEngine;
 }
 
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
index 1e49604f5f6deafb11a05d04d6932b9004780ae6..f52f210f8f6fd5090af435559159d7b608bf629d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletEngineFactory.h
@@ -42,7 +42,7 @@ public:
 	BulletEngineFactory();
 	virtual ~BulletEngineFactory();
 
-	virtual DynamicsEnginePtr createEngine();
+	virtual DynamicsEnginePtr createEngine(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr());
 
 	virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType = DynamicsObject::eDynamic);
 	virtual DynamicsRobotPtr createRobot(VirtualRobot::RobotPtr robot);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 23374714ac57cac982c1be9239aa4702fa45fb4f..3de0590b5381e0e7520e0433f113a0d555c683c7 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -21,7 +21,7 @@ namespace SimDynamics {
 BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
 	: DynamicsObject(o, type)
 {
-	float interatiaFactor = 5.0f;
+	float interatiaFactor = 1.0f;
 #ifdef USE_BULLET_GENERIC_6DOF_CONSTRAINT
     interatiaFactor = 5.0f;
 #endif
@@ -84,8 +84,8 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o, SimulationType type)
 			collisionShape->calculateLocalInertia(mass,localInertia);
 //#ifndef USE_BULLET_GENERIC_6DOF_CONSTRAINT
 			// check for small values
-			if (localInertia.length()<1.0f && localInertia.length()>0)
-				localInertia /= localInertia.length(); // small inertia values result in freaking out joints ?!
+			//if (localInertia.length()<1.0f && localInertia.length()>0)
+			//	localInertia /= localInertia.length(); // small inertia values result in freaking out joints ?!
 //#endif
 		} else
 #ifndef USE_BULLET_GENERIC_6DOF_CONSTRAINT
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
index 1f20e9d230ea8377650143d24f9b4a2da03ca292..f40ec63fd618df7d6c527eb1a3172d8208ad71fd 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp
@@ -959,7 +959,7 @@ void BulletRobot::actuateJoints(float dt)
                     btScalar targ = btScalar(it->second.jointValueTarget+link.jointValueOffset);
                     //btScalar act = btScalar(it->first->getJointValue());
                     btScalar act = btScalar(getJointAngle(it->first));
-                    hinge->enableAngularMotor(true,(targ-act),10.0f);
+                    hinge->enableAngularMotor(true,(targ-act),bulletMaxMotorImulse);
                     //hinge->enableMotor(true);
 				    //hinge->setMotorTarget(it->second.jointValueTarget+link.jointValueOffset,dt);
 			    } else
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
index ab02017b5c9ed730175d0e6ee2c2efb0d8344476..43142461cb80ad5fff0146978086a04f2eff92e1 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.cpp
@@ -19,12 +19,15 @@ DynamicsEngine::~DynamicsEngine()
 Eigen::Vector3f DynamicsEngine::getGravity()
 {
     boost::recursive_mutex::scoped_lock scoped_lock(engineMutex);
-	return dynamicsInfo.gravity;
+	return dynamicsConfig->gravity;
 }
 
-bool DynamicsEngine::init( const DynamicsWorldInfo &info )
+bool DynamicsEngine::init( DynamicsEngineConfigPtr config )
 {
-	dynamicsInfo = info;
+	if (config)
+		dynamicsConfig = config;
+	else
+		dynamicsConfig.reset(new DynamicsEngineConfig());
 	return true;
 }
 
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngine.h b/SimDynamics/DynamicsEngine/DynamicsEngine.h
index ee010f2370d31c35fce62021ec7f1fbbc63590cf..32167c4e72a0d90ece1b152674d4bd67c753dab1 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngine.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngine.h
@@ -30,6 +30,25 @@
 
 namespace SimDynamics
 {
+	/*!
+		Standard configuration for dynamics engines
+	*/
+	class DynamicsEngineConfig 
+	{
+	public:
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+			DynamicsEngineConfig()
+		{
+			gravity << 0, 0, -9.81f;
+		}
+
+		virtual ~DynamicsEngineConfig(){}
+
+		Eigen::Vector3f gravity;
+	};
+
+
+	typedef boost::shared_ptr<DynamicsEngineConfig> DynamicsEngineConfigPtr;
 
 /*!
 	An interface class to encapsulates all calls to the underlying physics engine. 
@@ -40,17 +59,6 @@ class SIMDYNAMICS_IMPORT_EXPORT DynamicsEngine
 public:
 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-	struct DynamicsWorldInfo 
-	{
-		DynamicsWorldInfo()
-		{
-			gravity << 0, 0, -9.81f;
-		}
-
-		Eigen::Vector3f gravity;
-	};
-
-
 	/*!
 	Constructor
 	*/
@@ -60,7 +68,10 @@ public:
 	*/
 	virtual ~DynamicsEngine();
 
-	virtual bool init(const DynamicsWorldInfo &info);
+	/*!
+		Initialize the engine with this configuration
+	*/
+	virtual bool init(DynamicsEngineConfigPtr config);
 
 	Eigen::Vector3f getGravity();
 
@@ -131,7 +142,7 @@ public:
     virtual DynamicsRobotPtr getRobot(VirtualRobot::RobotPtr r);
 
 protected:
-	DynamicsWorldInfo dynamicsInfo;
+	DynamicsEngineConfigPtr dynamicsConfig;
 
 	std::vector<DynamicsObjectPtr> objects;
 	std::vector<DynamicsRobotPtr> robots;
diff --git a/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h b/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h
index 47af41b0556ee11e583b40f3cc8ea442a8a9f0d9..3d7491b99b1e7d93f077991ab02c9455bcbcb793 100644
--- a/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h
+++ b/SimDynamics/DynamicsEngine/DynamicsEngineFactory.h
@@ -46,7 +46,7 @@ public:
 	virtual ~DynamicsEngineFactory() {;}
 
 	//! Derived classes must return the correct engine implementation.
-	virtual DynamicsEnginePtr createEngine(){return DynamicsEnginePtr();}
+	virtual DynamicsEnginePtr createEngine(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr()){return DynamicsEnginePtr();}
 
 	virtual DynamicsObjectPtr createObject(VirtualRobot::SceneObjectPtr o, DynamicsObject::SimulationType simType = DynamicsObject::eDynamic)
 	{
diff --git a/SimDynamics/DynamicsWorld.cpp b/SimDynamics/DynamicsWorld.cpp
index ea1f622afd434fb8d2eaf162c86b4e78e7726016..3ef91fb21924431f4c04acd31285767b08430abd 100644
--- a/SimDynamics/DynamicsWorld.cpp
+++ b/SimDynamics/DynamicsWorld.cpp
@@ -40,7 +40,7 @@ DynamicsEnginePtr DynamicsWorld::getEngine()
 	return engine;
 }
 
-DynamicsWorldPtr DynamicsWorld::Init()
+DynamicsWorldPtr DynamicsWorld::Init(DynamicsEngineConfigPtr config)
 {
 	static Cleanup _Cleanup;
 
@@ -50,7 +50,10 @@ DynamicsWorldPtr DynamicsWorld::Init()
 
 		if (!world)
 		{
-			world.reset(new DynamicsWorld());
+			world.reset(new DynamicsWorld(config));
+		} else
+		{
+			VR_WARNING << "Dynamics world is already initialized..." << endl;
 		}
 	}
 	return world;
@@ -64,11 +67,11 @@ void DynamicsWorld::Close()
 }
 
 
-DynamicsWorld::DynamicsWorld()
+DynamicsWorld::DynamicsWorld(DynamicsEngineConfigPtr config)
 {
 	DynamicsEngineFactoryPtr factory = DynamicsEngineFactory::first(NULL);
 	THROW_VR_EXCEPTION_IF(!factory, "No Physics Engine Found. Re-Compile with engine support...");
-	engine = factory->createEngine();
+	engine = factory->createEngine(config);
 	THROW_VR_EXCEPTION_IF(!engine, "Could not create Physics Engine.");
 }        
 
diff --git a/SimDynamics/DynamicsWorld.h b/SimDynamics/DynamicsWorld.h
index 316d54639c6a0aeab79b6cea1fa1190e4c35d044..ecb20d8e116ac3487bf4abed08f57ccc255f1d59 100644
--- a/SimDynamics/DynamicsWorld.h
+++ b/SimDynamics/DynamicsWorld.h
@@ -41,7 +41,7 @@ public:
 		Initialize the dynamics world singleton.
 		If already created the instance is returned.
 	*/
-	static DynamicsWorldPtr Init();
+	static DynamicsWorldPtr Init(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr());
 	static DynamicsWorldPtr GetWorld();
 	
 	/*!
@@ -102,7 +102,7 @@ public:
 
 protected:
 	// create world with static Init method.
-	DynamicsWorld();
+	DynamicsWorld(DynamicsEngineConfigPtr config = DynamicsEngineConfigPtr());
 
 	// see http://en.wikipedia.org/wiki/Singleton_pattern for details about correct implementations of singletons in C++
 	friend class Cleanup;
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index a4d1a82b05b2282f484758151b073fb109fbfdc3..63bca20af11e744ef63b968385f106738ed2d291 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -4,6 +4,7 @@
 #include "VirtualRobot/Workspace/Reachability.h"
 #include <VirtualRobot/RuntimeEnvironment.h>
 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <SimDynamics/DynamicsEngine/BulletEngine/BulletEngine.h>
 
 #include <QFileDialog>
 #include <Eigen/Geometry>
@@ -22,6 +23,7 @@
 #include <sstream>
 using namespace std;
 using namespace VirtualRobot;
+using namespace SimDynamics;
 
 SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags flags)
 :QMainWindow(NULL)
@@ -45,8 +47,9 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla
 	// optional visualizations (not considered by dynamics)
 	SoSeparator *cc = CoinVisualizationFactory::CreateCoordSystemVisualization(10.0f);
 	sceneSep->addChild(cc);
-
-	dynamicsWorld = SimDynamics::DynamicsWorld::Init();
+	BulletEngineConfigPtr config(new BulletEngineConfig());
+	config->bulletSolverIterations = 200;
+	dynamicsWorld = SimDynamics::DynamicsWorld::Init(config);
 	SIMDYNAMICS_ASSERT(dynamicsWorld);
 
 	dynamicsWorld->createFloorPlane();
@@ -278,10 +281,13 @@ bool SimDynamicsWindow::loadRobot(std::string robotFilename)
 	}
     try
     {
-		//VirtualRobot::BoundingBox bbox = robot->getBoundingBox();
+		VirtualRobot::BoundingBox bbox = robot->getBoundingBox();
+		
+		
 	    //robot->print();
 	    Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
-	    gp(2,3) = 800.0f;
+		//gp(2,3) = 5.0f;
+		gp(2,3) = -bbox.getMin()(2) + 4.0f;
 	    robot->setGlobalPose(gp);
 	    dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
 	    dynamicsWorld->addRobot(dynamicsRobot);
diff --git a/VirtualRobot/BoundingBox.cpp b/VirtualRobot/BoundingBox.cpp
index dfe98692fd9ebc722adaa11f86d107360e4315c8..6102408f982a95de75a29a5bc06f7377fbeebb67 100644
--- a/VirtualRobot/BoundingBox.cpp
+++ b/VirtualRobot/BoundingBox.cpp
@@ -39,7 +39,7 @@ BoundingBox::BoundingBox(const std::vector< Eigen::Vector3f > &p)
 	}
 }
 
-std::vector <Eigen::Vector3f> BoundingBox::getPoints()
+std::vector <Eigen::Vector3f> BoundingBox::getPoints() const
 {
 	std::vector < Eigen::Vector3f > points;
 
@@ -68,12 +68,18 @@ void BoundingBox::print()
 
 void BoundingBox::addPoints( const std::vector < Eigen::Vector3f > &p )
 {
-	for (size_t i=1;i<p.size();i++)
+	for (size_t i=0;i<p.size();i++)
 	{
 		addPoint(p[i]);
 	}
 }
 
+void BoundingBox::addPoints( const BoundingBox &bbox)
+{
+	std::vector <Eigen::Vector3f> p = bbox.getPoints();
+	addPoints(p);
+}
+
 void BoundingBox::addPoint( const Eigen::Vector3f &p )
 {
 	for (int j=0;j<3;j++)
@@ -102,5 +108,41 @@ void BoundingBox::clear()
 	max.setZero();
 }
 
+void BoundingBox::transform( Eigen::Matrix4f &pose )
+{
+	Eigen::Vector3f result[8];
+	std::vector<Eigen::Vector3f> result3;
+	result[0] << min(0),min(1),min(2);
+	result[1] << max(0),min(1),min(2);
+	result[2] << min(0),max(1),min(2);
+	result[3] << max(0),max(1),min(2);
+	result[4] << min(0),min(1),max(2);
+	result[5] << max(0),min(1),max(2);
+	result[6] << min(0),max(1),max(2);
+	result[7] << max(0),max(1),max(2);
+	Eigen::Matrix4f m;
+	for (int i=0;i<8;i++)
+	{
+		m.setIdentity();
+		m.block(0,3,3,1) = result[i];
+		m = pose*m;
+		result3.push_back(m.block(0,3,3,1));
+	}
+
+	// now find min max values
+	min << FLT_MAX,FLT_MAX,FLT_MAX;
+	max << -FLT_MAX,-FLT_MAX,-FLT_MAX;
+	for (int i=0;i<8;i++)
+	{
+		for (int j=0;j<3;j++)
+		{
+			if (result3[i](j) < min(j))
+				min(j) = result3[i](j);
+			if (result3[i](j) > max(j))
+				max(j) = result3[i](j);
+		}
+	}
+}
+
 
 }
diff --git a/VirtualRobot/BoundingBox.h b/VirtualRobot/BoundingBox.h
index 22a5124b63bb0b64503fb7e235b339bf5d29439a..692a76cd4d18e57f9064aede0aa8b467f06ec457 100644
--- a/VirtualRobot/BoundingBox.h
+++ b/VirtualRobot/BoundingBox.h
@@ -33,7 +33,8 @@
 namespace VirtualRobot {
 
 /*!
-	An axis oriented bounding box
+	An axis oriented bounding box.
+	Todo: Some parts of this class are similar to MathTools::OOBB.
 */
 class VIRTUAL_ROBOT_IMPORT_EXPORT BoundingBox
 {
@@ -49,10 +50,11 @@ public:
 	*/
 	bool planeGoesThrough(const VirtualRobot::MathTools::Plane &p);
 
+
 	/*!
 		Returns 8 points that define the bounding box
 	*/
-	std::vector <Eigen::Vector3f> getPoints();
+	std::vector <Eigen::Vector3f> getPoints() const;
 
 	//! Print some info
 	void print();
@@ -60,7 +62,12 @@ public:
 	/*!
 		Consider these points for min/max calculation
 	*/
-	void addPoints( const std::vector < Eigen::Vector3f > &p );
+	void addPoints(const std::vector < Eigen::Vector3f > &p);
+
+	/*!
+		Consider these points for min/max calculation
+	*/
+	void addPoints(const BoundingBox &bbox);
 
 	/*!
 		Consider this point for min/max calculation
@@ -76,6 +83,11 @@ public:
 	//! set min/max to zero.
 	void clear();
 
+	/*!
+		Applies transformation to this bbox. Reorders min and max values according to pose.
+	*/
+	void transform(Eigen::Matrix4f &pose);
+
 protected:
 	Eigen::Vector3f min;
 	Eigen::Vector3f max;
diff --git a/VirtualRobot/CollisionDetection/CollisionChecker.cpp b/VirtualRobot/CollisionDetection/CollisionChecker.cpp
index 7a1d6672bfc1d3522c1ce458828f57023d46c4a8..690183651bf112853455ba0b5b7714d2bf900b25 100644
--- a/VirtualRobot/CollisionDetection/CollisionChecker.cpp
+++ b/VirtualRobot/CollisionDetection/CollisionChecker.cpp
@@ -538,7 +538,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());
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 41dc15e2fb1cc9fa40bf4201997c14cc13c7101e..250fa186f5664d3de351157965c613a6850ad25b 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -793,6 +793,23 @@ void Robot::setJointValues( const std::vector<RobotNodePtr> rn, const std::vecto
 	applyJointValuesNoLock();
 }
 
+VirtualRobot::BoundingBox Robot::getBoundingBox(bool collisionModel)
+{
+	VirtualRobot::BoundingBox bbox;
+	std::vector<RobotNodePtr> rn = getRobotNodes();
+	for (size_t i=0;i<rn.size();i++)
+	{
+		if (collisionModel && rn[i]->getCollisionModel())
+		{
+			bbox.addPoints(rn[i]->getCollisionModel()->getBoundingBox());
+		} else if (!collisionModel && rn[i]->getVisualization())
+		{
+			bbox.addPoints(rn[i]->getVisualization()->getBoundingBox());
+		}
+	}
+	return bbox;
+}
+
 
 } // namespace VirtualRobot
 
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index ce935752775b6bcb9fcba9bd60f6296cd239bc67..0741ed2e8d9e5c62e8be782857d669291afebf6e 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -348,6 +348,13 @@ public:
 	*/
 	void setJointValues(TrajectoryPtr trajectory, float t);
 
+	/*!
+		The (current) bounding box in global coordinate system.
+		\param collisionModel Either the collision or the visualization model is considered.
+		\return The bounding box.
+	*/
+	BoundingBox getBoundingBox(bool collisionModel = true);
+
 protected:
 	Robot();
 	/*!
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index 6b339b83bee171606d88e48a7195244e0162d324..46a63773548c469efb472f4185a89b62faf61d67 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -174,4 +174,15 @@ VirtualRobot::VisualizationNodePtr VisualizationNode::CreateUnitedVisualization(
 	return f->createUnitedVisualization(visualizations);
 }
 
+VirtualRobot::BoundingBox VisualizationNode::getBoundingBox()
+{
+	VirtualRobot::BoundingBox bbox;
+	TriMeshModelPtr tm = getTriMeshModel();
+	if (!tm)
+		return bbox;
+	bbox = tm->boundingBox;
+	bbox.transform(globalPose);
+	return bbox;
+}
+
 } // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/VisualizationNode.h b/VirtualRobot/Visualization/VisualizationNode.h
index 91ca3667eae3b9bb130af6e3d1f9d2d324317abc..4dac44a6a3db03e76034e33525546cac3787494c 100644
--- a/VirtualRobot/Visualization/VisualizationNode.h
+++ b/VirtualRobot/Visualization/VisualizationNode.h
@@ -127,6 +127,11 @@ public:
 	*/
 	static VisualizationNodePtr CreateUnitedVisualization(const std::vector<VisualizationNodePtr> &visualizations);
 
+	/*!
+		Returns (current) bounding box in global coordinate system.
+	*/
+	BoundingBox getBoundingBox();
+
 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
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
index 52d335094dbcca54c9511c507562e5b9cfafa7db..b9b1eb327cbace17758bb2ce6f1ea85590e843fe 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
@@ -33,7 +33,7 @@
 			<Limits unit="degree" lo="-94.62" hi="13.85"/>  <!-- ?! -->
 		</Joint>
 		<Physics>
-            <CoM location="Joint"/>
+            <CoM location="VisualizationBBoxCenter"/>
             <Mass value="2.5" units="kg" />
         </Physics>
         <Visualization enable="true">
@@ -73,14 +73,14 @@
 			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/elbow_l.iv</File>
 		</Visualization>
+		<CollisionModel>
+			<File type="Inventor">convexModel/elbow_l.iv</File>
+		</CollisionModel>
 		<Physics>
             <CoM location="Joint"/>
             <Mass value="1.15744000" units="kg" />
         </Physics>
-		<CollisionModel>
-			<File type="Inventor">convexModel/elbow_l.iv</File>
-		</CollisionModel>
-		<Child name="Underarm L"/>
+   		<Child name="Underarm L"/>
 	</RobotNode>
 	
 	<RobotNode name="Underarm L">
@@ -89,17 +89,16 @@
 			<!--Limits unit="degree" lo="-57.29" hi="174.48"/-->
 			<Limits unit="degree" lo="-108.25" hi="69.65"/>
 		</Joint>
-		<Physics>
-            <CoM location="VisualizationBBoxCenter"/>
-            <Mass value="2.26566087" units="kg" />
-        </Physics>
         <Visualization enable="true">
-			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/underarm_l.iv</File>
 		</Visualization>
 		<CollisionModel>
 			<File type="Inventor">convexModel/underarm_l.iv</File>
 		</CollisionModel>
+		<Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="2.26566087" units="kg" />
+        </Physics>
 		<Child name="Wrist 1 L"/>
 	</RobotNode>
 	
@@ -111,7 +110,7 @@
 		<Physics>
             <CoM location="VisualizationBBoxCenter"/>
             <Mass value="1.29945309" units="kg" />
-        </Physics-->
+        </Physics>
         <Visualization enable="true">
 			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/wrist1_l.iv</File>
@@ -127,11 +126,7 @@
 			<DH a="0" d="0" theta="-90" alpha="90" units="degree"/>
 			<Limits unit="degree" lo="-38.4" hi="44.32"/>
 		</Joint>
-		<Physics>
-            <CoM location="VisualizationBBoxCenter"/>
-            <Mass value="2.59945309" units="kg" />
-            <IgnoreCollision name="Underarm L"/>
-        </Physics>
+
         <Visualization enable="true">
 			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/wrist2_l.iv</File>
@@ -139,7 +134,13 @@
         <CollisionModel>
 			<File type="Inventor">convexModel/wrist2_l.iv</File>
 		</CollisionModel>
-		
+
+	    <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="2.59945309" units="kg" />
+            <IgnoreCollision name="Underarm L"/>
+        </Physics>
+        
 		<ChildFromRobot>
 			<File importEEF="true">ArmarIII-LeftHand.xml</File>
 		</ChildFromRobot>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftHand.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftHand.xml
index 1ff118d6c95fbfe5f4b5ab467d3b9b0625eae825..884cc7bf98a8ef26ab7f4e8d58b916140f3ec57a 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftHand.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftHand.xml
@@ -93,18 +93,12 @@
 				</Transform>
 			</PreJointTransform>
 		</Joint>
-		
 		<Child name="Thumb L J0"/>
 	</RobotNode>
 	
 	<RobotNode name="Thumb L J0">
 		<Joint type="revolute">
 			<Limits unit="degree" lo="0" hi="180"/>
-			<PreJointTransform>
-				<Transform>
-					<Translation x="0" y="0" z="0"/>
-				</Transform>
-			</PreJointTransform>
 			<Axis x="0" y="1" z="0"/>
 		</Joint>
 		
@@ -164,11 +158,6 @@
 	<RobotNode name="Pinky L J0">
 		<Joint type="revolute">
 			<Limits unit="degree" lo="0" hi="180"/>
-			<PreJointTransform>
-				<Transform>
-					<Translation x="0" y="0" z="0"/>
-				</Transform>
-			</PreJointTransform>
 			<Axis x="0" y="-1" z="0"/>
 		</Joint>
 		
@@ -200,7 +189,6 @@
 		</Joint>
 		
 		<Visualization enable="true">
-			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/pinky_l2.iv</File>
 		</Visualization>
 		
@@ -229,11 +217,6 @@
 	<RobotNode name="Ring L J0">
 		<Joint type="revolute">
 			<Limits unit="degree" lo="0" hi="180"/>
-			<PreJointTransform>
-				<Transform>
-					<Translation x="0" y="0" z="0"/>
-				</Transform>
-			</PreJointTransform>
 			<Axis x="0" y="-1" z="0"/>
 		</Joint>
 		
@@ -293,11 +276,6 @@
 	<RobotNode name="Middle L J0">
 		<Joint type="revolute">
 			<Limits unit="degree" lo="0" hi="180"/>
-			<PreJointTransform>
-				<Transform>
-					<Translation x="0" y="0" z="0"/>
-				</Transform>
-			</PreJointTransform>
 			<Axis x="0" y="-1" z="0"/>
 		</Joint>
 		
@@ -357,11 +335,6 @@
 	<RobotNode name="Index L J0">
 		<Joint type="revolute">
 			<Limits unit="degree" lo="0" hi="180"/>
-			<PreJointTransform>
-				<Transform>
-					<Translation x="0" y="0" z="0"/>
-				</Transform>
-			</PreJointTransform>
 			<Axis x="0" y="-1" z="0"/>
 		</Joint>
 		
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
index fc6ad2245a2270b9b9ac47175793875d22caf555..554abbe6557922fd416ffd7028f5e456ce204098 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
@@ -2,7 +2,7 @@
 
 <Robot Type="ArmarIII RightArm" RootNode="Right Arm Base">	
 
-<RobotNode name="Right Arm Base">
+    <RobotNode name="Right Arm Base">
 		<Joint type="fixed">
 			<DH a="0" d="0" theta="-90" alpha="90" units="degree"/>
 		</Joint>
@@ -89,18 +89,19 @@
 	<RobotNode name="Underarm R">
 		<Joint type="revolute">
 			<DH a="0" d="-240" theta="90" alpha="-90" units="degree"/>
+			
 			<Limits unit="degree" lo="-69.65" hi="108.25"/>
 		</Joint>
 		<Visualization enable="true">
 			<File type="Inventor">fullmodel/underarm_r.iv</File>
 		</Visualization>
+ 		<CollisionModel>
+			<File type="Inventor">convexModel/underarm_r.iv</File>
+		</CollisionModel>
 		<Physics>
             <CoM location="VisualizationBBoxCenter"/>
             <Mass value="2.26566087" units="kg" />
         </Physics>
- 		<CollisionModel>
-			<File type="Inventor">convexModel/underarm_r.iv</File>
-		</CollisionModel>
 		<Child name="Wrist 1 R"/>
 	</RobotNode>
 	
@@ -111,14 +112,14 @@
 		</Joint>
 		<Physics>
             <CoM location="VisualizationBBoxCenter"/>
-            <Mass value="1.13047000" units="kg" />
+            <Mass value="1.29945309" units="kg" /><!--1.13047000-->
         </Physics>
    	    <Visualization enable="true">
 			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/wrist1_r.iv</File>
 		</Visualization>
 		<CollisionModel>
-			<File type="Inventor">convexModel/wrist1_l.iv</File>
+			<File type="Inventor">convexModel/wrist1_r.iv</File><!--convexModel/wrist1_l.iv-->
 		</CollisionModel>
 		<Child name="Wrist 2 R"/>
 	</RobotNode>
@@ -134,13 +135,15 @@
 			<File type="Inventor">fullmodel/wrist2_r.iv</File>
 		</Visualization>
         <CollisionModel>
-			<File type="Inventor">convexModel/wrist2_l.iv</File>
+			<File type="Inventor">convexModel/wrist2_r.iv</File><!--convexModel/wrist2_l.iv-->
 		</CollisionModel>
+		
 		<Physics>
             <CoM location="VisualizationBBoxCenter"/>
             <Mass value="2.59945309" units="kg" />
             <IgnoreCollision name="Underarm R"/>
         </Physics>
+        
 		<ChildFromRobot>
 			<File importEEF="true">ArmarIII-RightHand.xml</File>
 		</ChildFromRobot>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightHand.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightHand.xml
index cc52685001b3eedcff1eee587db261ecbda141ad..6637f6bddf622315e9368edb63785ce477626424 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightHand.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightHand.xml
@@ -2,7 +2,6 @@
 
 <Robot Type="ArmarIII RightHand" RootNode="Hand R Base">
 
-
     <RobotNode name="Hand R Base">
 		<Child name="TCP R"/>
 		<Child name="GCP R"/>
@@ -46,10 +45,12 @@
 		<CollisionModel>
 			<File type="Inventor">convexModel/palm1_r.iv</File>
 		</CollisionModel>
+		
 	    <Physics>
 			<Mass value="800" unit="g"/>
 			<CoM location="VisualizationBBoxCenter"/>
-		</Physics>		
+		</Physics>	
+			
 		<Child name="Hand Palm 2 R"/>
 		<Child name="Thumb R"/>
 	</RobotNode>
@@ -72,10 +73,12 @@
 		<CollisionModel>
 			<File type="Inventor">convexModel/palm2_r.iv</File>
 		</CollisionModel>
+		
 	    <Physics>
 			<Mass value="800" unit="g"/>
 			<CoM location="VisualizationBBoxCenter"/>
 		</Physics>		
+		
 		<Child name="Pinky R"/>
 		<Child name="Ring R"/>
 		<Child name="Middle R"/>
@@ -100,7 +103,6 @@
 		</Joint>
 		
 		<Visualization enable="true">
-			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/thumb_r1.iv</File>
 		</Visualization>
 		
@@ -364,15 +366,14 @@
 		</Joint>
 		
 		<Visualization enable="true">
-			<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
 			<File type="Inventor">fullmodel/index_r2.iv</File>
 		</Visualization>
 		
 		<CollisionModel>
 			<File type="Inventor">convexModel/index_r2.iv</File>
 		</CollisionModel>
+		
 	    <Physics>
-	        
 			<Mass value="500" unit="g"/>
 			<CoM location="VisualizationBBoxCenter"/>
 		</Physics>