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

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(){
+	NNWalker(int index,
-		//initialize random weights
+		btDynamicsWorld* ownerWorld,
-		for(int i = 0;i < BODYPART_COUNT;i++){
+		NNWalkerBrain* core,
-			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,
 		const btVector3& startingPosition,
 		const btScalar& rootBodyRadius,
 		const btScalar& rootBodyHeight,
@@ -360,19 +385,20 @@ public:
 		const btScalar& foreLegLength,
 		bool fixedBodyPosition)
 		: 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++)
 		{
@@ -381,15 +407,15 @@ public:
 		}
 
 		// Setup rigid bodies
-		btScalar rootAboveGroundHeight = foreLegLength;
+		btScalar torsoAboveGroundHeight = foreLegLength;
 		btTransform bodyOffset; bodyOffset.setIdentity();
 		bodyOffset.setOrigin(startingPosition);		
 
-		// root body
+		// torso
-		btVector3 localRootBodyPosition = btVector3(btScalar(0.), rootAboveGroundHeight, btScalar(0.)); // root body position in local reference frame
+		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){
+	void randomizeSensoryMotorWeights(){
-		for(int i = 0;i < BODYPART_COUNT;i++){
+		m_brain->randomizeSensoryMotorWeights();
-			for(int j = 0;j < JOINT_COUNT;j++){
-				m_sensoryMotorWeights[i+j*BODYPART_COUNT] = sensoryMotorWeights[i+j*BODYPART_COUNT];
-			}
-		}
 	}
 
 	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,99 +833,12 @@ 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
 
-	// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
+	m_walkersInPopulation.resize(POPULATION_SIZE, NULL);
-	// should be (numberOfsolverIterations * oldLimits)
+	m_walkerBrains.resize(POPULATION_SIZE, NULL);
-	gWalkerMotorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations;
 
-	{ // create a slider to change the motor update frequency
+	recreateWorld(); // setup world and add ground
-		SliderParams slider("Motor update frequency", &gWalkerLegTargetFrequency);
-		slider.m_minVal = 0;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	{ // create a slider to change the motor torque
-		SliderParams slider("Motor force", &gWalkerMotorStrength);
-		slider.m_minVal = 1;
-		slider.m_maxVal = 50;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-	
-	{ // create a slider to change the root body radius
-		SliderParams slider("Root body radius", &gRootBodyRadius);
-		slider.m_minVal = 0.01f;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	{ // create a slider to change the root body height
-		SliderParams slider("Root body height", &gRootBodyHeight);
-		slider.m_minVal = 0.01f;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	{ // create a slider to change the leg radius
-		SliderParams slider("Leg radius", &gLegRadius);
-		slider.m_minVal = 0.01f;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	{ // create a slider to change the leg length
-		SliderParams slider("Leg length", &gLegLength);
-		slider.m_minVal = 0.01f;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	{ // create a slider to change the fore leg radius
-		SliderParams slider("Fore Leg radius", &gForeLegRadius);
-		slider.m_minVal = 0.01f;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	{ // create a slider to change the fore leg length
-		SliderParams slider("Fore Leg length", &gForeLegLength);
-		slider.m_minVal = 0.01f;
-		slider.m_maxVal = 10;
-		slider.m_clampToNotches = false;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
-
-	if(POPULATION_SIZE > 1)
-	{ // create a slider to change the number of parallel evaluations
-		SliderParams slider("Parallel evaluations", &gParallelEvaluations);
-		slider.m_minVal = 1;
-		slider.m_maxVal = POPULATION_SIZE;
-		slider.m_clampToIntegers = true;
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
-			slider);
-	}
 
 	{ // Setup a big ground box
 		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
@@ -888,9 +851,83 @@ void NN3DWalkersExample::initPhysics()
 		m_ground->setUserPointer(GROUND_ID);
 	}
 
-	// add walker filter making the walkers never collide with each other
+	// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
-	m_filterCallback = new WalkerFilterCallback();
+	// should be (numberOfsolverIterations * oldLimits)
-	m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(m_filterCallback);
+	gWalkerMotorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations;
+
+	{ // create a slider to change the motor update frequency
+		SliderParams slider("Motor update frequency", &gWalkerLegTargetFrequency);
+		slider.m_minVal = 0;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	{ // create a slider to change the motor torque
+		SliderParams slider("Motor force", &gWalkerMotorStrength);
+		slider.m_minVal = 1;
+		slider.m_maxVal = 50;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+	
+	{ // create a slider to change the root body radius
+		SliderParams slider("Root body radius", &gRootBodyRadius);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	{ // create a slider to change the root body height
+		SliderParams slider("Root body height", &gRootBodyHeight);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	{ // create a slider to change the leg radius
+		SliderParams slider("Leg radius", &gLegRadius);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	{ // create a slider to change the leg length
+		SliderParams slider("Leg length", &gLegLength);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	{ // create a slider to change the fore leg radius
+		SliderParams slider("Fore Leg radius", &gForeLegRadius);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	{ // create a slider to change the fore leg length
+		SliderParams slider("Fore Leg length", &gForeLegLength);
+		slider.m_minVal = 0.01f;
+		slider.m_maxVal = 10;
+		slider.m_clampToNotches = false;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	if(POPULATION_SIZE > 1)
+	{ // create a slider to change the number of parallel evaluations
+		SliderParams slider("Parallel evaluations", &gParallelEvaluations);
+		slider.m_minVal = 1;
+		slider.m_maxVal = POPULATION_SIZE;
+		slider.m_clampToIntegers = true;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
+			slider);
+	}
 
 	// setup data sources for walkers in time series canvas
 	m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,400,300, "Fitness Performance");
@@ -898,61 +935,78 @@ void NN3DWalkersExample::initPhysics()
 	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_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();
-		}
 	}
 }
 
@@ -961,7 +1015,6 @@ bool NN3DWalkersExample::detectCollisions()
 	bool collisionDetected = false;
 	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
@@ -971,7 +1024,7 @@ bool NN3DWalkersExample::detectCollisions()
 			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
@@ -984,8 +1037,8 @@ bool NN3DWalkersExample::detectCollisions()
 						const btVector3& ptB = pt.getPositionWorldOnB();
 						const btVector3& normalOnB = pt.m_normalWorldOnB;
 
-					if(!DRAW_INTERPENETRATIONS){
+						if (!DRAW_INTERPENETRATIONS) {										// if no interpenetrations are drawn, break
-						return collisionDetected;
+							break;
 						}
 
 						if (m_dynamicsWorld->getDebugDrawer() && !mIsHeadless) { 				// draw self collisions
@@ -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 */
+		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_guiHelper->removeAllGraphicsInstances();
-//				m_ground->setUserIndex(-1); // reset to get a new graphics object
+				m_ground->setUserIndex(-1); 						// reset to get a new graphics object
-//				m_ground->setUserIndex2(-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_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){
+		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);
+				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);
 			}
 		}
 	}