Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[WIP] Fix some add/remove issues.

Browse Source
This commit is contained in:
Benelot
2016-12-30 01:46:36 +01:00
parent 3add00631f
commit 10eb8d63f8
1 changed files with 141 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

185
examples/Evolution/NN3DWalkers.cpp
View File

@@ -40,7 +40,7 @@ class NNWalker;
#endif #endif
#ifndef POPULATION_SIZE #ifndef POPULATION_SIZE
#define POPULATION_SIZE 50 // number of walkers in the population #define POPULATION_SIZE 10 // number of walkers in the population
#endif #endif
#ifndef EVALUATION_DURATION #ifndef EVALUATION_DURATION
@@ -48,13 +48,14 @@ class NNWalker;
#endif #endif
#ifndef TIME_SERIES_MAX_Y #ifndef TIME_SERIES_MAX_Y
#define TIME_SERIES_MAX_Y 20.0f #define TIME_SERIES_MAX_Y 20.0f // chart maximum y
#endif #endif
#ifndef TIME_SERIES_MIN_Y #ifndef TIME_SERIES_MIN_Y
#define TIME_SERIES_MIN_Y 0.0f #define TIME_SERIES_MIN_Y 0.0f // chart minimum y
#endif #endif
// walker dimensions and properties
static btScalar gWalkerMotorStrength = 0.5f; static btScalar gWalkerMotorStrength = 0.5f;
static btScalar gWalkerLegTargetFrequency = 3; static btScalar gWalkerLegTargetFrequency = 3;
static btScalar gRootBodyRadius = 0.25f; static btScalar gRootBodyRadius = 0.25f;
@@ -98,7 +99,7 @@ static btScalar gParallelEvaluations = 10.0f;
#endif #endif
#ifndef REBUILD_WALKER #ifndef REBUILD_WALKER
#define REBUILD_WALKER true // if the walker should be rebuilt on mutation #define REBUILD_WALKER false // if the walker should be rebuilt on mutation
#endif #endif
#ifndef TIMESTAMP_TIME #ifndef TIMESTAMP_TIME
@@ -132,7 +133,7 @@ class NN3DWalkersExample : public NN3DWalkersTimeWarpBase
btAlignedObjectArray<class NNWalker*> m_walkersInPopulation; btAlignedObjectArray<class NNWalker*> m_walkersInPopulation;
bool m_rebuildWorld; // if the world should be rebuilt (for determinism) bool m_rebuildWorldNeeded; // if the world should be rebuilt (for determinism)
btRigidBody* m_ground; // reference to ground to readd if world is rebuilt btRigidBody* m_ground; // reference to ground to readd if world is rebuilt
@@ -151,7 +152,7 @@ public:
m_nextReapedIndex(0), m_nextReapedIndex(0),
m_timeSeriesCanvas(NULL), m_timeSeriesCanvas(NULL),
m_ground(NULL), m_ground(NULL),
m_rebuildWorld(false), m_rebuildWorldNeeded(false),
m_filterCallback(NULL) m_filterCallback(NULL)
{ {
b3Clock clock; b3Clock clock;
@@ -346,7 +347,8 @@ public:
} }
} }
NNWalker(int index, btDynamicsWorld* ownerWorld, const btVector3& startingPosition, NNWalker(int index, btDynamicsWorld* ownerWorld,
const btVector3& startingPosition,
const btScalar& rootBodyRadius, const btScalar& rootBodyRadius,
const btScalar& rootBodyHeight, const btScalar& rootBodyHeight,
const btScalar& legRadius, const btScalar& legRadius,
@@ -359,14 +361,13 @@ public:
m_evaluationTime(0), // reset evaluation time m_evaluationTime(0), // reset evaluation time
m_reaped(false), // the walker is not reaped m_reaped(false), // the walker is not reaped
m_startPosition(startingPosition), // the starting position of the walker m_startPosition(startingPosition), // the starting position of the walker
m_legUpdateAccumulator(0) m_legUpdateAccumulator(0),
m_index(index)
{ {
m_index = index;
btVector3 vUp(0, 1, 0); // up in local reference frame btVector3 vUp(0, 1, 0); // up in local reference frame
NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo(); NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo();
//
// Setup geometry // Setup geometry
m_shapes[0] = new btCapsuleShape(rootBodyRadius, rootBodyHeight); // root body capsule m_shapes[0] = new btCapsuleShape(rootBodyRadius, rootBodyHeight); // root body capsule
int i; int i;
@@ -376,7 +377,6 @@ public:
m_shapes[2 + 2*i] = new btCapsuleShape(foreLegRadius, foreLegLength); // fore leg capsule m_shapes[2 + 2*i] = new btCapsuleShape(foreLegRadius, foreLegLength); // fore leg capsule
} }
//
// Setup rigid bodies // Setup rigid bodies
btScalar rootAboveGroundHeight = foreLegLength; btScalar rootAboveGroundHeight = foreLegLength;
btTransform bodyOffset; bodyOffset.setIdentity(); btTransform bodyOffset; bodyOffset.setIdentity();
@@ -454,7 +454,17 @@ public:
if(nnWalkersDemo->detectCollisions()){ // if thigh bone causes collision, remove it again if(nnWalkersDemo->detectCollisions()){ // if thigh bone causes collision, remove it again
m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root
delete m_joints[2*i];
m_joints[2*i] = NULL;
m_ownerWorld->removeRigidBody(m_bodies[1+2*i]); m_ownerWorld->removeRigidBody(m_bodies[1+2*i]);
delete m_bodies[1+2*i];
m_bodies[1+2*i] = NULL;
delete m_joints[1+2*i];
m_joints[1+2*i] = NULL;
delete m_bodies[2+2*i];
m_bodies[2+2*i] = NULL;
} }
else{ else{
m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone
@@ -462,7 +472,12 @@ public:
if(nnWalkersDemo->detectCollisions()){ // if shin bone causes collision, remove it again if(nnWalkersDemo->detectCollisions()){ // if shin bone causes collision, remove it again
m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh
delete m_joints[1+2*i];
m_joints[1+2*i] = NULL;
m_ownerWorld->removeRigidBody(m_bodies[2+2*i]); m_ownerWorld->removeRigidBody(m_bodies[2+2*i]);
delete m_bodies[2+2*i];
m_bodies[2+2*i] = NULL;
} }
} }
} }
@@ -470,36 +485,41 @@ public:
// Setup some damping on the m_bodies // Setup some damping on the m_bodies
for (i = 0; i < BODYPART_COUNT; ++i) for (i = 0; i < BODYPART_COUNT; ++i)
{ {
if(m_bodies[i] != NULL){
m_bodies[i]->setDamping(0.05, 0.85); m_bodies[i]->setDamping(0.05, 0.85);
m_bodies[i]->setDeactivationTime(0.8); m_bodies[i]->setDeactivationTime(0.8);
m_bodies[i]->setSleepingThresholds(0.5f, 0.5f); m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
m_bodies[i]->setActivationState(DISABLE_DEACTIVATION); m_bodies[i]->setActivationState(DISABLE_DEACTIVATION);
} }
}
removeFromWorld(); // the walker should not yet be in the world // Properly remove rigidbodies and joints
removeJoints();
removeRigidbodies();
clearTouchSensors(); // set touch sensors to zero clearTouchSensors(); // set touch sensors to zero
randomizeSensoryMotorWeights(); // set random sensory motor weights for neural network layer randomizeSensoryMotorWeights(); // set random sensory motor weights for neural network layer
} }
virtual ~NNWalker () virtual ~NNWalker ()
{ {
int i; int i;
removeFromWorld();
// Remove all constraints // Remove all constraints
for ( i = 0; i < JOINT_COUNT; ++i) for ( i = 0; i < JOINT_COUNT; ++i)
{ {
m_ownerWorld->removeConstraint(m_joints[i]); if(m_joints[i] != NULL){
delete m_joints[i]; m_joints[i] = 0; delete m_joints[i]; m_joints[i] = 0;
} }
}
// Remove all bodies and shapes // Remove all bodies and shapes
for ( i = 0; i < BODYPART_COUNT; ++i) for ( i = 0; i < BODYPART_COUNT; ++i)
{ {
m_ownerWorld->removeRigidBody(m_bodies[i]); if(m_bodies[i] != NULL){
delete m_bodies[i]->getMotionState(); delete m_bodies[i]->getMotionState();
delete m_bodies[i]; m_bodies[i] = 0; delete m_bodies[i]; m_bodies[i] = 0;
@@ -507,6 +527,9 @@ public:
} }
} }
m_ownerWorld = NULL;
}
btTypedConstraint** getJoints() { btTypedConstraint** getJoints() {
return &m_joints[0]; return &m_joints[0];
} }
@@ -538,45 +561,38 @@ public:
} }
void addToWorld() { void addToWorld() {
int i; if(!isInEvaluation()){
// add all bodies and shapes
for ( i = 0; i < BODYPART_COUNT; ++i)
{
m_ownerWorld->addRigidBody(m_bodies[i]);
}
// add all constraints addRigidbodies();
for ( i = 0; i < JOINT_COUNT; ++i) addJoints();
{
m_ownerWorld->addConstraint(m_joints[i], true); // important! If you add constraints back, you must set bullet physics to disable collision between constrained bodies setInEvaluation(true);
} }
} }
void removeFromWorld() { void removeFromWorld() {
int i; if(isInEvaluation()){
// Remove all constraints removeJoints();
for ( i = 0; i < JOINT_COUNT; ++i) removeRigidbodies();
{
m_ownerWorld->removeConstraint(m_joints[i]);
}
// Remove all bodies setInEvaluation(false);
for ( i = 0; i < BODYPART_COUNT; ++i)
{
m_ownerWorld->removeRigidBody(m_bodies[i]);
} }
} }
btVector3 getPosition() const { btVector3 getPosition() const {
btVector3 finalPosition(0,0,0); btVector3 finalPosition(0,0,0);
int j = 0;
for(int i = 0; i < BODYPART_COUNT;i++) for(int i = 0; i < BODYPART_COUNT;i++)
{ {
if(m_bodies[i] != NULL){
finalPosition += m_bodies[i]->getCenterOfMassPosition(); finalPosition += m_bodies[i]->getCenterOfMassPosition();
j++;
}
} }
finalPosition /= btScalar(BODYPART_COUNT); finalPosition /= btScalar(j);
return finalPosition; return finalPosition;
} }
@@ -662,6 +678,47 @@ public:
void setOwnerWorld(btDynamicsWorld* ownerWorld) { void setOwnerWorld(btDynamicsWorld* ownerWorld) {
m_ownerWorld = ownerWorld; m_ownerWorld = ownerWorld;
} }
private:
void addJoints(){
// add all constraints
for (int i = 0; i < JOINT_COUNT; ++i)
{
if(m_joints[i] != NULL){
m_ownerWorld->addConstraint(m_joints[i], true); // important! If you add constraints back, you must set bullet physics to disable collision between constrained bodies
}
}
}
void addRigidbodies() {
// add all bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
if(m_bodies[i] != NULL){
m_ownerWorld->addRigidBody(m_bodies[i]);
}
}
}
void removeJoints() {
// remove all constraints
for (int i = 0; i < JOINT_COUNT; ++i)
{
if(m_joints[i] != NULL){
m_ownerWorld->removeConstraint(m_joints[i]);
}
}
}
void removeRigidbodies() {
// remove all bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
if(m_bodies[i] != NULL){
m_ownerWorld->removeRigidBody(m_bodies[i]);
}
}
}
}; };
void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep); void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep);
@@ -674,18 +731,18 @@ bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
void* ID1 = o1->getUserPointer(); void* ID1 = o1->getUserPointer();
void* ID2 = o2->getUserPointer(); void* ID2 = o2->getUserPointer();
if (ID1 != GROUND_ID || ID2 != GROUND_ID) { if (ID1 != GROUND_ID || ID2 != GROUND_ID) { // if one of the contacts is not ground
// Make a circle with a 0.9 radius at (0,0,0) // Make a circle with a 0.9 radius at (0,0,0)
// with RGB color (1,0,0). // with RGB color (1,0,0).
if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL && !nn3DWalkers->mIsHeadless){ if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL && !nn3DWalkers->mIsHeadless){
nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.)); nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
} }
if(ID1 != GROUND_ID && ID1){ if(ID1 != GROUND_ID && ID1){ // if ID1 is not ground
((NNWalker*)ID1)->setTouchSensor(o1); ((NNWalker*)ID1)->setTouchSensor(o1);
} }
if(ID2 != GROUND_ID && ID2){ if(ID2 != GROUND_ID && ID2){ // if ID2 is not ground
((NNWalker*)ID2)->setTouchSensor(o2); ((NNWalker*)ID2)->setTouchSensor(o2);
} }
} }
@@ -710,10 +767,6 @@ struct WalkerFilterCallback : public btOverlapFilterCallback // avoids collision
} }
}; };
void floorNNSliderValue(float notUsed) {
gParallelEvaluations = floor(gParallelEvaluations);
}
void NN3DWalkersExample::initPhysics() void NN3DWalkersExample::initPhysics()
{ {
@@ -731,6 +784,7 @@ void NN3DWalkersExample::initPhysics()
m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(0);
gWalkerLegTargetFrequency = 3; // Hz gWalkerLegTargetFrequency = 3; // Hz
@@ -815,8 +869,7 @@ void NN3DWalkersExample::initPhysics()
SliderParams slider("Parallel evaluations", &gParallelEvaluations); SliderParams slider("Parallel evaluations", &gParallelEvaluations);
slider.m_minVal = 1; slider.m_minVal = 1;
slider.m_maxVal = POPULATION_SIZE; slider.m_maxVal = POPULATION_SIZE;
slider.m_clampToNotches = false; slider.m_clampToIntegers = true;
slider.m_callback = floorNNSliderValue; // hack to get integer values
m_guiHelper->getParameterInterface()->registerSliderFloatParameter( m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
slider); slider);
} }
@@ -845,27 +898,28 @@ void NN3DWalkersExample::initPhysics()
m_walkersInPopulation.resize(POPULATION_SIZE, NULL); m_walkersInPopulation.resize(POPULATION_SIZE, NULL);
for(int i = 0; i < POPULATION_SIZE ; i++){ for(int i = 0; i < POPULATION_SIZE ; i++){ // spawn walkers
// Spawn one walker
resetWalkerAt(i, m_resetPosition); resetWalkerAt(i, m_resetPosition);
} }
} }
void NN3DWalkersExample::performModelUpdate(float deltaTime){ void NN3DWalkersExample::performModelUpdate(float deltaTime){
if(m_rebuildWorld){ if(m_rebuildWorldNeeded){
recreateWorld(); recreateWorld();
m_rebuildWorld = false; m_rebuildWorldNeeded = false;
} }
} }
void NN3DWalkersExample::recreateWorld(){ void NN3DWalkersExample::recreateWorld(){
for(int i = 0; i < POPULATION_SIZE ; i++){ for(int i = 0; i < POPULATION_SIZE ; i++){ // remove walkers
m_walkersInPopulation[i]->removeFromWorld(); m_walkersInPopulation[i]->removeFromWorld();
} }
m_dynamicsWorld->removeRigidBody(m_ground); // remove ground
// delete world
delete m_dynamicsWorld; delete m_dynamicsWorld;
m_dynamicsWorld = 0; m_dynamicsWorld = 0;
@@ -881,15 +935,17 @@ void NN3DWalkersExample::recreateWorld(){
delete m_collisionConfiguration; delete m_collisionConfiguration;
m_collisionConfiguration = 0; m_collisionConfiguration = 0;
createEmptyDynamicsWorld(); createEmptyDynamicsWorld(); // create new world
// add all filters and callbacks
m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); // set evolution update pretick callback m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); // set evolution update pretick callback
m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(m_filterCallback); // avoid collisions between walkers m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(m_filterCallback); // avoid collisions between walkers
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(0);
m_dynamicsWorld->addRigidBody(m_ground); // readd ground m_dynamicsWorld->addRigidBody(m_ground); // readd ground
for(int i = 0; i < POPULATION_SIZE ; i++){ for(int i = 0; i < POPULATION_SIZE ; i++){ // readd walkers
m_walkersInPopulation[i]->setOwnerWorld(m_dynamicsWorld); m_walkersInPopulation[i]->setOwnerWorld(m_dynamicsWorld);
if(m_walkersInPopulation[i]->isInEvaluation()){ if(m_walkersInPopulation[i]->isInEvaluation()){
m_walkersInPopulation[i]->addToWorld(); m_walkersInPopulation[i]->addToWorld();
@@ -905,20 +961,21 @@ bool NN3DWalkersExample::detectCollisions()
} }
int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds(); int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
for (int i = 0;i < numManifolds;i++) for (int i = 0;i < numManifolds;i++) // for each collision
{ {
btPersistentManifold* contactManifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i); btPersistentManifold* contactManifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
// get collision objects of collision
const btCollisionObject* obA = contactManifold->getBody0(); const btCollisionObject* obA = contactManifold->getBody0();
const btCollisionObject* obB = contactManifold->getBody1(); const btCollisionObject* obB = contactManifold->getBody1();
if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){ if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){ // if one of them is not ground
int numContacts = contactManifold->getNumContacts(); int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++) for (int j=0;j<numContacts;j++) // for each collision contact
{ {
collisionDetected = true; collisionDetected = true;
btManifoldPoint& pt = contactManifold->getContactPoint(j); btManifoldPoint& pt = contactManifold->getContactPoint(j);
if (pt.getDistance()<0.f) if (pt.getDistance()<0.f) // if collision is penetrating
{ {
const btVector3& ptA = pt.getPositionWorldOnA(); const btVector3& ptA = pt.getPositionWorldOnA();
const btVector3& ptB = pt.getPositionWorldOnB(); const btVector3& ptB = pt.getPositionWorldOnB();
@@ -1002,7 +1059,7 @@ void NN3DWalkersExample::rateEvaluations(){
for(int i = 0; i < POPULATION_SIZE;i++){ // plot walker fitnesses for this round for(int i = 0; i < POPULATION_SIZE;i++){ // plot walker fitnesses for this round
m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),i,true); m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),i,true);
} }
m_timeSeriesCanvas->nextTick(); // move tick forward m_timeSeriesCanvas->nextTick(); // move to next tick of graph
for(int i = 0; i < POPULATION_SIZE;i++){ // reset all walkers for(int i = 0; i < POPULATION_SIZE;i++){ // reset all walkers
m_walkersInPopulation[i]->setEvaluationTime(0); m_walkersInPopulation[i]->setEvaluationTime(0);
@@ -1135,7 +1192,7 @@ void NN3DWalkersExample::update(const btScalar timeSinceLastTick) {
drawMarkings(); /**!< Draw markings on the ground */ drawMarkings(); /**!< Draw markings on the ground */
if(m_SimulationTime > m_LastSpeedupPrintTimestamp + TIMESTAMP_TIME){ // print effective speedup every 2 seconds if(m_SimulationTime > m_LastSpeedupPrintTimestamp + TIMESTAMP_TIME){ // print effective speedup every x seconds
b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup()); b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup());
m_LastSpeedupPrintTimestamp = m_SimulationTime; m_LastSpeedupPrintTimestamp = m_SimulationTime;
} }
@@ -1161,7 +1218,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
m_walkersInPopulation[r]->setLegUpdateAccumulator(m_walkersInPopulation[r]->getLegUpdateAccumulator() + delta); m_walkersInPopulation[r]->setLegUpdateAccumulator(m_walkersInPopulation[r]->getLegUpdateAccumulator() + delta);
if(m_walkersInPopulation[r]->getLegUpdateAccumulator() >= btScalar(1.0f) /gWalkerLegTargetFrequency) if(m_walkersInPopulation[r]->getLegUpdateAccumulator() >= btScalar(1.0f) /gWalkerLegTargetFrequency) // we update the leg movement with leg target frequency
{ {
m_walkersInPopulation[r]->setLegUpdateAccumulator(0); m_walkersInPopulation[r]->setLegUpdateAccumulator(0);
@@ -1171,6 +1228,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkersInPopulation[r]->getJoints()[i]); btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkersInPopulation[r]->getJoints()[i]);
if(hingeC != NULL){ // if hinge exists
for(int j = 0; j < JOINT_COUNT;j++){ // accumulate sensor inputs with weights (summate inputs) for(int j = 0; j < JOINT_COUNT;j++){ // accumulate sensor inputs with weights (summate inputs)
targetAngle += m_walkersInPopulation[r]->getSensoryMotorWeights()[i+j*BODYPART_COUNT] * m_walkersInPopulation[r]->getTouchSensor(i); targetAngle += m_walkersInPopulation[r]->getSensoryMotorWeights()[i+j*BODYPART_COUNT] * m_walkersInPopulation[r]->getTouchSensor(i);
} }
@@ -1185,6 +1243,7 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
hingeC->enableAngularMotor(true, desiredAngularVel, gWalkerMotorStrength); // set new target velocity hingeC->enableAngularMotor(true, desiredAngularVel, gWalkerMotorStrength); // set new target velocity
} }
} }
}
// clear sensor signals after usage // clear sensor signals after usage
m_walkersInPopulation[r]->clearTouchSensors(); m_walkersInPopulation[r]->clearTouchSensors();
@@ -1200,7 +1259,6 @@ void NN3DWalkersExample::scheduleEvaluations() {
if(m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_DURATION){ /**!< tear down evaluations */ if(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())); b3Printf("An evaluation finished at %f s. Distance: %f m", m_SimulationTime, btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
m_walkersInPopulation[i]->setInEvaluation(false);
m_walkersInPopulation[i]->removeFromWorld(); m_walkersInPopulation[i]->removeFromWorld();
m_walkersInEvaluation--; m_walkersInEvaluation--;
} }
@@ -1220,7 +1278,6 @@ void NN3DWalkersExample::scheduleEvaluations() {
else{ // resets the position of the walker without deletion else{ // resets the position of the walker without deletion
m_walkersInPopulation[i]->resetAt(m_resetPosition); m_walkersInPopulation[i]->resetAt(m_resetPosition);
} }
m_walkersInPopulation[i]->setInEvaluation(true);
m_walkersInPopulation[i]->addToWorld(); m_walkersInPopulation[i]->addToWorld();
} }
} }
@@ -1231,7 +1288,7 @@ void NN3DWalkersExample::scheduleEvaluations() {
if(m_walkersInEvaluation == 0){ // if there are no more evaluations possible if(m_walkersInEvaluation == 0){ // if there are no more evaluations possible
if(!REBUILD_WALKER){ if(!REBUILD_WALKER){
m_rebuildWorld = true; m_rebuildWorldNeeded = true;
} }
rateEvaluations(); // rate evaluations by sorting them based on their fitness rateEvaluations(); // rate evaluations by sorting them based on their fitness