Remove graphic instances from previous runs. Still non-deterministic unfortunately.

2017-05-29 21:56:18 +02:00
parent f0212cc072
commit cd153eb6a5
1 changed files with 274 additions and 199 deletions
--- a/examples/Evolution/NN3DWalkers.cpp
+++ b/examples/Evolution/NN3DWalkers.cpp
@@ -29,6 +29,7 @@ class btConstraintSolver;
 struct btCollisionAlgorithmCreateFunc;
 class btDefaultCollisionConfiguration;
 class NNWalker;
+class NNWalkerBrain;

 #include "NN3DWalkersTimeWarpBase.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
@@ -102,7 +103,7 @@ static btScalar gParallelEvaluations = 10.0f;
 #endif

 #ifndef REBUILD_WALKER
-#define REBUILD_WALKER false // if the walker should be rebuilt on mutation
+#define REBUILD_WALKER true											// if the walker should be rebuilt on mutation
 #endif

 #ifndef TIMESTAMP_TIME
@@ -134,7 +135,8 @@ class NN3DWalkersExample : public NN3DWalkersTimeWarpBase
 	int 	 m_walkersInEvaluation; 								// number of walkers in evaluation
 	int		 m_nextReapedIndex; 									// index of the next reaped walker
 	
-	btAlignedObjectArray<class NNWalker*> m_walkersInPopulation;
+	btAlignedObjectArray<class NNWalker*> m_walkersInPopulation;	// walkers in the population
+	btAlignedObjectArray<class NNWalkerBrain*> m_walkerBrains;		// walker cores in the population
 	
 	bool m_rebuildWorldNeeded; 										// if the world should be rebuilt (for determinism)

@@ -155,17 +157,16 @@ public:
 	 m_nextReapedIndex(0),
 	 m_timeSeriesCanvas(NULL),
 	 m_ground(NULL),
-	 m_rebuildWorldNeeded(false),
 	 m_filterCallback(NULL)
 	{
 		b3Clock clock;
-		srand(clock.getSystemTimeMilliseconds());
+		srand(clock.getSystemTimeMilliseconds());					// Set random milliseconds based on system time
 	}

 	virtual ~NN3DWalkersExample()
 	{
 		//m_walkersInPopulation deallocates itself
-
+		//m_walkerBrains deallocates itself
 		delete m_timeSeriesCanvas;
 	}

@@ -305,6 +306,36 @@ public:

 static NN3DWalkersExample* nn3DWalkers = NULL;

+/**
+ * Neural Network Brain Weights.
+ */
+class NNWalkerBrain
+{
+	btScalar m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT]; // Neural Network connection weights
+
+public:
+
+	NNWalkerBrain(){
+		randomizeSensoryMotorWeights();
+	}
+
+	void randomizeSensoryMotorWeights(){
+		//initialize random weights
+		for(int i = 0;i < BODYPART_COUNT;i++){
+			for(int j = 0;j < JOINT_COUNT;j++){
+				m_sensoryMotorWeights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
+			}
+		}
+	}
+
+	btScalar* getSensoryMotorWeights() {
+		return m_sensoryMotorWeights;
+	}
+};
+
+/**
+ * Neural Networks Walker. A 6 legged robot moving controlled by a one-layer neural network which maps from leg ground touch sensors to leg joint positions.
+ */
 class NNWalker
 {
 	btDynamicsWorld*			m_ownerWorld;
@@ -314,7 +345,8 @@ class NNWalker
 	btTypedConstraint*			m_joints[JOINT_COUNT];
 	btHashMap<btHashPtr,int>	m_bodyTouchSensorIndexMap;
 	bool 						m_touchSensors[BODYPART_COUNT];
-	btScalar					m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT];
+	NNWalkerBrain*				m_brain;
+

 	bool						m_inEvaluation;
 	btScalar					m_evaluationTime;
@@ -341,16 +373,9 @@ class NNWalker

 public:

-	void randomizeSensoryMotorWeights(){
-		//initialize random weights
-		for(int i = 0;i < BODYPART_COUNT;i++){
-			for(int j = 0;j < JOINT_COUNT;j++){
-				m_sensoryMotorWeights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
-			}
-		}
-	}
-
-	NNWalker(int index, btDynamicsWorld* ownerWorld,
+	NNWalker(int index,
+		btDynamicsWorld* ownerWorld,
+		NNWalkerBrain* core,
 		const btVector3& startingPosition,
 		const btScalar& rootBodyRadius,
 		const btScalar& rootBodyHeight,
@@ -359,37 +384,38 @@ public:
 		const btScalar& foreLegRadius,
 		const btScalar& foreLegLength,
 		bool fixedBodyPosition)
-		: m_ownerWorld (ownerWorld), // the world the walker walks in
+		: m_ownerWorld(ownerWorld), 												// the world the walker walks in
+		  m_brain(core), 															// the evolution core
 		  m_inEvaluation(false), 													// the walker is not in evaluation
 		  m_evaluationTime(0), 														// reset evaluation time
 		  m_reaped(false), 															// the walker is not reaped
 		  m_startPosition(startingPosition), 										// the starting position of the walker
-		  m_legUpdateAccumulator(0),
+		  m_legUpdateAccumulator(0),												// variable to apply rhythmic leg position updates
 		  m_index(index)
 	{
-		btVector3 vUp(0, 1, 0); // up in local reference frame
+		btVector3 vUp(0, 1, 0); 													// up direction in local reference frame

 		NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo();

 		// Setup geometry
-		m_shapes[0] = new btCapsuleShape(rootBodyRadius, rootBodyHeight); // root body capsule
+		m_shapes[0] = new btCapsuleShape(rootBodyRadius, rootBodyHeight); 			// torso capsule
 		int i;
-		for ( i = 0; i < NUM_WALKER_LEGS; i++)
+		for (i = 0; i < NUM_WALKER_LEGS; i++)
 		{
 			m_shapes[1 + 2*i] = new btCapsuleShape(legRadius, legLength); 			// leg  capsule
 			m_shapes[2 + 2*i] = new btCapsuleShape(foreLegRadius, foreLegLength); 	// fore leg capsule
 		}

 		// Setup rigid bodies
-		btScalar rootAboveGroundHeight = foreLegLength;
+		btScalar torsoAboveGroundHeight = foreLegLength;
 		btTransform bodyOffset; bodyOffset.setIdentity();
 		bodyOffset.setOrigin(startingPosition);		

-		// root body
-		btVector3 localRootBodyPosition = btVector3(btScalar(0.), rootAboveGroundHeight, btScalar(0.)); // root body position in local reference frame
+		// torso
+		btVector3 localTorsoPosition = btVector3(btScalar(0.), torsoAboveGroundHeight, btScalar(0.)); // torso position in local reference frame
 		btTransform transform;
 		transform.setIdentity();
-		transform.setOrigin(localRootBodyPosition);
+		transform.setOrigin(localTorsoPosition);

 		btTransform originTransform = transform;

@@ -411,11 +437,13 @@ public:
 			float footXUnitPosition = cos(footAngle); 								// x position of the leg on the unit circle

 			transform.setIdentity();
-			btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(rootBodyRadius+0.5*legLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition*(rootBodyRadius+0.5*legLength)));
+			btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(rootBodyRadius+0.5*legLength)),
+										 btScalar(torsoAboveGroundHeight),
+										 btScalar(footYUnitPosition*(rootBodyRadius+0.5*legLength)));
 			transform.setOrigin(legCOM);

 			// thigh
-			btVector3 legDirection = (legCOM - localRootBodyPosition).normalize();
+			btVector3 legDirection = (legCOM - localTorsoPosition).normalize();
 			btVector3 kneeAxis = legDirection.cross(vUp);			
 			transform.setRotation(btQuaternion(kneeAxis, SIMD_HALF_PI));
 			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
@@ -425,7 +453,9 @@ public:

 			// shin
 			transform.setIdentity();
-			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(rootAboveGroundHeight-0.5*foreLegLength), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
+			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)),
+										  btScalar(torsoAboveGroundHeight-0.5*foreLegLength),
+										  btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
 			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
 			m_bodyRelativeTransforms[2+2*i] = transform;
 			m_bodies[2+2*i]->setUserPointer(this);
@@ -433,20 +463,22 @@ public:

 			// hip joints
 			localA.setIdentity(); localB.setIdentity();
-			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*rootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*rootBodyRadius)));
+			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*rootBodyRadius),
+																					   btScalar(0.),
+																					   btScalar(footYUnitPosition*rootBodyRadius)));
 			localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
 			hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
-			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
 			m_joints[2*i] = hingeC;

 			// knee joints
 			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
-			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)), btScalar(0.), btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
+			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(rootBodyRadius+legLength)),
+																					   btScalar(0.),
+																					   btScalar(footYUnitPosition*(rootBodyRadius+legLength))));
 			localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			localC = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[2+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
 			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
-			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
 			hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2));
 			m_joints[1+2*i] = hingeC;

@@ -509,7 +541,7 @@ public:
 	{
 		int i;

-		removeFromWorld();
+		removeFromWorld();							// remove walker from world

 		// Remove all constraints
 		for ( i = 0; i < JOINT_COUNT; ++i)
@@ -547,20 +579,12 @@ public:
 		}
 	}

-	bool getTouchSensor(int i){
-		return m_touchSensors[i];
-	}
-
 	btScalar* getSensoryMotorWeights() {
-		return m_sensoryMotorWeights;
+		return m_brain->getSensoryMotorWeights();
 	}

-	void copySensoryMotorWeights(btScalar* sensoryMotorWeights){
-		for(int i = 0;i < BODYPART_COUNT;i++){
-			for(int j = 0;j < JOINT_COUNT;j++){
-				m_sensoryMotorWeights[i+j*BODYPART_COUNT] = sensoryMotorWeights[i+j*BODYPART_COUNT];
-			}
-		}
+	void randomizeSensoryMotorWeights(){
+		m_brain->randomizeSensoryMotorWeights();
 	}

 	void addToWorld() {
@@ -613,6 +637,10 @@ public:
 		return getDistanceFitness(); // for now it is only distance
 	}

+	/**
+	 * Reset walker to a position. Does not work deterministically.
+	 * @param position
+	 */
 	void resetAt(const btVector3& position) {
 		removeFromWorld();
 		btTransform resetPosition(btQuaternion::getIdentity(), position);
@@ -670,6 +698,18 @@ public:
 		return m_index;
 	}

+	bool getTouchSensor(int i){
+		return m_touchSensors[i];
+	}
+
+	NNWalkerBrain* getBrain(){
+		return m_brain;
+	}
+
+	btRigidBody** getBodies(){
+		return &m_bodies[0];
+	}
+
 	btScalar getLegUpdateAccumulator() const {
 		return m_legUpdateAccumulator;
 	}
@@ -726,6 +766,13 @@ private:

 void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep);

+/**
+ * Draw leg contacts on ground.
+ * @param cp
+ * @param body0
+ * @param body1
+ * @return
+ */
 bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
 {
    btCollisionObject* o1 = static_cast<btCollisionObject*>(body0);
@@ -752,7 +799,10 @@ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
    return false;
 }

-struct WalkerFilterCallback : public btOverlapFilterCallback // avoids collisions among the walkers
+/**
+ * A filter avoiding collision between walkers.
+ */
+struct WalkerFilterCallback : public btOverlapFilterCallback // avoids collisions between walkers
 {
 	// return true when pairs need collision
 	virtual bool	needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
@@ -783,14 +833,24 @@ void NN3DWalkersExample::initPhysics()

 	m_SimulationTime = 0;

-	createEmptyDynamicsWorld();
-
-	m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true);
-	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-	m_dynamicsWorld->getDebugDrawer()->setDebugMode(0);
-
 	gWalkerLegTargetFrequency = 3; // Hz

+	m_walkersInPopulation.resize(POPULATION_SIZE, NULL);
+	m_walkerBrains.resize(POPULATION_SIZE, NULL);
+
+	recreateWorld(); // setup world and add ground
+
+	{ // Setup a big ground box
+		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
+		m_collisionShapes.push_back(groundShape);
+		btTransform groundTransform;
+		groundTransform.setIdentity();
+		groundTransform.setOrigin(btVector3(0,-10,0));
+		m_ground = createRigidBody(btScalar(0.),groundTransform,groundShape);
+		m_ground->setFriction(5);
+		m_ground->setUserPointer(GROUND_ID);
+	}
+
 	// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
 	// should be (numberOfsolverIterations * oldLimits)
 	gWalkerMotorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations;
@@ -800,8 +860,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	{ // create a slider to change the motor torque
@@ -809,8 +868,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 1;
 		slider.m_maxVal = 50;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
 	
 	{ // create a slider to change the root body radius
@@ -818,8 +876,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0.01f;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	{ // create a slider to change the root body height
@@ -827,8 +884,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0.01f;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	{ // create a slider to change the leg radius
@@ -836,8 +892,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0.01f;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	{ // create a slider to change the leg length
@@ -845,8 +900,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0.01f;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	{ // create a slider to change the fore leg radius
@@ -854,8 +908,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0.01f;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	{ // create a slider to change the fore leg length
@@ -863,8 +916,7 @@ void NN3DWalkersExample::initPhysics()
 		slider.m_minVal = 0.01f;
 		slider.m_maxVal = 10;
 		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}

 	if(POPULATION_SIZE > 1)
@@ -877,118 +929,119 @@ void NN3DWalkersExample::initPhysics()
 			slider);
 	}

-	{// Setup a big ground box
-		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
-		m_collisionShapes.push_back(groundShape);
-		btTransform groundTransform;
-		groundTransform.setIdentity();
-		groundTransform.setOrigin(btVector3(0,-10,0));
-		m_ground = createRigidBody(btScalar(0.),groundTransform,groundShape);
-		m_ground->setFriction(5);
-		m_ground->setUserPointer(GROUND_ID);
-	}
-
-	// add walker filter making the walkers never collide with each other
-	m_filterCallback = new WalkerFilterCallback();
-	m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(m_filterCallback);
-
 	// setup data sources for walkers in time series canvas
 	m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,400,300, "Fitness Performance");
 	m_timeSeriesCanvas->setupTimeSeries(TIME_SERIES_MIN_Y, TIME_SERIES_MAX_Y, 10, 0);
 	for(int i = 0; i < POPULATION_SIZE ; i++){
-		m_timeSeriesCanvas->addDataSource(" ", 100*i/POPULATION_SIZE,100*(POPULATION_SIZE-i)/POPULATION_SIZE,100*i/POPULATION_SIZE);
+		m_timeSeriesCanvas->addDataSource(" ", 100*i/POPULATION_SIZE, 100*(POPULATION_SIZE-i)/POPULATION_SIZE, 100*i/POPULATION_SIZE);
 	}
-
-	m_walkersInPopulation.resize(POPULATION_SIZE, NULL);
-
-	for(int i = 0; i < POPULATION_SIZE ; i++){ // spawn walkers
-		resetWalkerAt(i, m_resetPosition);
-	}
-
 }

 void NN3DWalkersExample::performModelUpdate(float deltaTime){
-	if(m_rebuildWorldNeeded){
+	if(m_rebuildWorldNeeded){ // rebuilding the world must be performed in sync to the update cycle of the example browser
 		recreateWorld();
 		m_rebuildWorldNeeded = false;
 	}
 }

+/**
+ * This method should really delete all remainings of the simulation except for the walker brains.
+ */
 void NN3DWalkersExample::recreateWorld(){

-	for(int i = 0; i < POPULATION_SIZE ; i++){ // remove walkers
+	for(int i = 0; i < POPULATION_SIZE ; i++){ 												// remove and delete walkers
+		if(m_walkersInPopulation[i] != NULL){
 			m_walkersInPopulation[i]->removeFromWorld();
+
+			for(int j = 0;j < BODYPART_COUNT;j++){ 											// remove all body graphics
+				btRigidBody* rb = static_cast<btRigidBody*>(m_walkersInPopulation[i]->getBodies()[j]);
+				if (rb != NULL) {
+					int graphicsUid = rb->getUserIndex();
+					m_guiHelper->removeGraphicsInstance(graphicsUid);
+				}
 			}

-	m_dynamicsWorld->removeRigidBody(m_ground); // remove ground
+			delete m_walkersInPopulation[i];
+			m_walkersInPopulation[i] = NULL;
+		}
+	}

-	// delete world
+	if(m_dynamicsWorld != NULL && m_ground != NULL){
+		m_dynamicsWorld->removeRigidBody(m_ground); 										// remove ground
+	}
+
+	// delete world etc.
+	if(m_dynamicsWorld != NULL){
 		delete m_dynamicsWorld;
 		m_dynamicsWorld = 0;
+	}

+	if(m_solver != NULL){
 		delete m_solver;
 		m_solver = 0;
+	}

+	if(m_broadphase != NULL){
 		delete m_broadphase;
 		m_broadphase = 0;
+	}

+	if(m_dispatcher != NULL){
 		delete m_dispatcher;
 		m_dispatcher = 0;
+	}

+	if(m_collisionConfiguration != NULL){
 		delete m_collisionConfiguration;
 		m_collisionConfiguration = 0;
+	}

 	createEmptyDynamicsWorld(); // create new world

 	// add all filters and callbacks
 	m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); 	// set evolution update pretick callback
+	m_filterCallback = new WalkerFilterCallback();
 	m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(m_filterCallback); 			// avoid collisions between walkers
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(0);

+	if(m_dynamicsWorld != NULL && m_ground != NULL){
 		m_dynamicsWorld->addRigidBody(m_ground); 											// readd ground
-
-	for(int i = 0; i < POPULATION_SIZE ; i++){ // readd walkers
-		m_walkersInPopulation[i]->setOwnerWorld(m_dynamicsWorld);
-		if(m_walkersInPopulation[i]->isInEvaluation()){
-			m_walkersInPopulation[i]->addToWorld();
-		}
 	}
 }

 bool NN3DWalkersExample::detectCollisions()
 {
 	bool collisionDetected = false;
-	if(m_dynamicsWorld){
+	if (m_dynamicsWorld) {
 		m_dynamicsWorld->performDiscreteCollisionDetection(); 							// let the collisions be calculated
-	}

 		int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
-	for (int i = 0;i < numManifolds;i++) // for each collision
+		for (int i = 0; i < numManifolds; i++) 												// for each collision
 		{
 			btPersistentManifold* contactManifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
 			// get collision objects of collision
 			const btCollisionObject* obA = contactManifold->getBody0();
 			const btCollisionObject* obB = contactManifold->getBody1();

-		if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){ // if one of them is not ground
+			if (obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID && obA->getUserPointer() == obB->getUserPointer()) { 	// if one of them is not ground

 				int numContacts = contactManifold->getNumContacts();
-			for (int j=0;j<numContacts;j++) // for each collision contact
+				for (int j = 0; j < numContacts; j++) 											// for each collision contact
 				{
 					collisionDetected = true;
 					btManifoldPoint& pt = contactManifold->getContactPoint(j);
-				if (pt.getDistance()<0.f) // if collision is penetrating
+					if (pt.getDistance() < 0.f) 											// if collision is penetrating
 					{
 						const btVector3& ptA = pt.getPositionWorldOnA();
 						const btVector3& ptB = pt.getPositionWorldOnB();
 						const btVector3& normalOnB = pt.m_normalWorldOnB;

-					if(!DRAW_INTERPENETRATIONS){
-						return collisionDetected;
+						if (!DRAW_INTERPENETRATIONS) {										// if no interpenetrations are drawn, break
+							break;
 						}

-					if(m_dynamicsWorld->getDebugDrawer() && !mIsHeadless){ // draw self collisions
+						if (m_dynamicsWorld->getDebugDrawer() && !mIsHeadless) { 				// draw self collisions
 							m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.));
 							m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.));
 						}
@@ -996,6 +1049,7 @@ bool NN3DWalkersExample::detectCollisions()
 				}
 			}
 		}
+	}

 	return collisionDetected;
 }
@@ -1260,21 +1314,21 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
 void NN3DWalkersExample::scheduleEvaluations() {
 	for(int i = 0; i < POPULATION_SIZE;i++){

-		if(m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_DURATION){ /**!< tear down evaluations */
+		if(m_walkersInPopulation[i] != NULL && m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_DURATION){ /**!< tear down evaluations */
 			b3Printf("An evaluation finished at %f s. Distance: %f m", m_SimulationTime, btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
 			m_walkersInPopulation[i]->removeFromWorld();
 			m_walkersInEvaluation--;
 		}

-		if(m_walkersInEvaluation < gParallelEvaluations && !m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0){ /**!< Setup the new evaluations */
-			b3Printf("An evaluation started at %f s.",m_SimulationTime);
+		if(m_walkersInEvaluation < gParallelEvaluations && (m_walkersInPopulation[i] == NULL || (!m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0))){ /**!< Setup the new evaluations */
+			b3Printf("An evaluation started at %f s.", m_SimulationTime);
 			m_walkersInEvaluation++;

 			if(REBUILD_WALKER){ 									// deletes and recreates the walker in the position
-//				m_guiHelper->removeAllGraphicsInstances();
-//				m_ground->setUserIndex(-1); // reset to get a new graphics object
-//				m_ground->setUserIndex2(-1); // reset to get a new graphics object
-//				m_ground->getCollisionShape()->setUserIndex(-1); // reset to get a new graphics object
+				m_guiHelper->removeAllGraphicsInstances();
+				m_ground->setUserIndex(-1); 						// reset to get a new graphics object
+				m_ground->setUserIndex2(-1); 						// reset to get a new graphics object
+				m_ground->getCollisionShape()->setUserIndex(-1); 	// reset to get a new graphics object

 				resetWalkerAt(i, m_resetPosition);
 			}
@@ -1290,7 +1344,7 @@ void NN3DWalkersExample::scheduleEvaluations() {
 	}

 	if(m_walkersInEvaluation == 0){ 								// if there are no more evaluations possible
-		if(!REBUILD_WALKER){
+		if(REBUILD_WALKER){
 			m_rebuildWorldNeeded = true;
 		}

@@ -1303,9 +1357,20 @@ void NN3DWalkersExample::scheduleEvaluations() {
 	}
 }

+/**
+ * Reset the walker at position.
+ * @param i
+ * @param resetPosition
+ */
 void NN3DWalkersExample::resetWalkerAt(int i, const btVector3& resetPosition){

-	NNWalker* newWalker = new NNWalker(i, m_dynamicsWorld, resetPosition,
+	// either copy core or create a new one if no previous existing
+	m_walkerBrains[i] = (m_walkerBrains[i] != NULL)?m_walkerBrains[i]:new NNWalkerBrain();
+
+	NNWalker* newWalker = new NNWalker(i,
+		m_dynamicsWorld,
+		m_walkerBrains[i],
+		resetPosition,
 		gRootBodyRadius,
 		gRootBodyHeight,
 		gLegRadius,
@@ -1315,7 +1380,6 @@ void NN3DWalkersExample::resetWalkerAt(int i, const btVector3& resetPosition){
 		false);

 	if(m_walkersInPopulation[i] != NULL){
-		newWalker->copySensoryMotorWeights(m_walkersInPopulation[i]->getSensoryMotorWeights());
 		delete m_walkersInPopulation[i];
 	}

@@ -1326,20 +1390,31 @@ void NN3DWalkersExample::resetWalkerAt(int i, const btVector3& resetPosition){
 * Draw distance markings on the ground.
 */
 void NN3DWalkersExample::drawMarkings() {
-	if(!mIsHeadless){
+	if(!mIsHeadless){												// if the system is not running headless
 		for(int i = 0; i < POPULATION_SIZE;i++) 					// draw current distance plates of moving walkers
 		{
 			if(m_walkersInPopulation[i]->isInEvaluation()){
 				btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
 				char performance[20];
 				sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
-				m_guiHelper->drawText3D(performance,walkerPosition.x(),walkerPosition.y()+1,walkerPosition.z(),1);
+				m_guiHelper->drawText3D(performance,
+					walkerPosition.x(),
+					walkerPosition.y() + 1,
+					walkerPosition.z(),
+					1);
 			}
 		}

-		if(m_dynamicsWorld->getDebugDrawer() && !mIsHeadless){
-			for(int i = 2; i < 50; i+=2){ // draw distance circles
-				m_dynamicsWorld->getDebugDrawer()->drawArc(btVector3(0,0,0),btVector3(0,1,0),btVector3(1,0,0),btScalar(i), btScalar(i),btScalar(0),btScalar(SIMD_2_PI),btVector3(10*i,0,0),false);
+		if(m_dynamicsWorld->getDebugDrawer()){
+			for(int i = 2; i < 50; i += 2){ 						// draw distance circles
+				m_dynamicsWorld->getDebugDrawer()->drawArc(
+					btVector3(0,0,0),
+					btVector3(0,1,0),
+					btVector3(1,0,0),
+					btScalar(i), btScalar(i), btScalar(0),
+					btScalar(SIMD_2_PI),
+					btVector3(10*i,0,0),
+					false);
 			}
 		}
 	}