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

Fix and reconfigure demo by rebuilding walkers every time.

Browse Source
This commit is contained in:
Benelot
2016-11-01 21:03:46 +01:00
parent 4559de6c11
commit e10ca70944
1 changed files with 43 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
examples/Evolution/NN3DWalkers.cpp
View File

@@ -42,7 +42,7 @@ class NNWalker;
#endif
#ifndef POPULATION_SIZE
#define POPULATION_SIZE 10 // number of walkers in the population
#define POPULATION_SIZE 100 // number of walkers in the population
#endif
#ifndef EVALUATION_DURATION
@@ -93,14 +93,6 @@ class NNWalker;
#define REBUILD_WALKER true // if the walker should be rebuilt on mutation
#endif
#ifndef RANDOM_WALKER_MOVEMENT
#define RANDOM_WALKER_MOVEMENT false // movement is chosen randomly and not via neural network
#endif
#ifndef RANDOMIZE_WALKER_DIMENSIONS
#define RANDOMIZE_WALKER_DIMENSIONS false // if the walker dimensions should be mutated or not
#endif
#ifndef TIMESTAMP_TIME
#define TIMESTAMP_TIME 2000.0f // delay between speed up timestamps
#endif
@@ -158,7 +150,7 @@ public:
m_legLength(0.45f),
m_foreLegLength(0.75f),
m_foreLegRadius(0.08f),
m_parallelEvaluations(1.0f),
m_parallelEvaluations(10.0f),
// others
m_resetPosition(0,0,0),
m_SimulationTime(0),
@@ -188,14 +180,6 @@ public:
*/
virtual void exitPhysics();
/**
* Spawn a walker at startPosition.
* @param index
* @param startPosition
* @param fixedRootBodyPosition
*/
void spawnWalker(int index, const btVector3& startPosition, bool fixedRootBodyPosition);
virtual bool keyboardCallback(int key, int state);
/**
@@ -381,9 +365,7 @@ public:
//initialize random weights
for(int i = 0;i < BODYPART_COUNT;i++){
for(int j = 0;j < JOINT_COUNT;j++){
//TODO: clean this up
m_sensoryMotorWeights[i+j*BODYPART_COUNT] = ((double) rand() / (RAND_MAX))*2.0f-1.0f;
//m_sensoryMotorWeights[i+j*BODYPART_COUNT] = 1;
}
}
}
@@ -408,10 +390,6 @@ public:
NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo();
clearTouchSensors(); // set touch sensors to zero
randomizeSensoryMotorWeights(); // set random sensory motor weights for neural network layer
//
// Setup geometry
m_shapes[0] = new btCapsuleShape(rootBodyRadius, rootBodyHeight); // root body capsule
@@ -443,7 +421,6 @@ public:
m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[0]), 0);
btHingeConstraint* hingeC;
//btConeTwistConstraint* coneC;
btTransform localA, localB, localC;
@@ -494,7 +471,7 @@ public:
hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2));
m_joints[1+2*i] = hingeC;
//test if we cause a collision with priorly inserted bodies. This prevents the walkers to have to resolve collisions on startup
//test if we cause a collision with priorly inserted bodies. This prevents the walkers from having to resolve collisions on startup
m_ownerWorld->addRigidBody(m_bodies[1+2*i]); // add thigh bone
m_ownerWorld->addConstraint(m_joints[2*i], true); // connect thigh bone with root
@@ -524,6 +501,11 @@ public:
}
removeFromWorld(); // the walker should not yet be in the world
clearTouchSensors(); // set touch sensors to zero
randomizeSensoryMotorWeights(); // set random sensory motor weights for neural network layer
}
virtual ~NNWalker ()
@@ -637,12 +619,13 @@ public:
}
void resetAt(const btVector3& position) {
removeFromWorld();
btTransform resetPosition(btQuaternion::getIdentity(), position);
for (int i = 0; i < 2*NUM_WALKER_LEGS; i++)
for (int i = 0; i < JOINT_COUNT; i++)
{
btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(getJoints()[i]);
hingeC->enableAngularMotor(false,0,0);
btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(getJoints()[i]);
hingeC->enableMotor(false);
}
for (int i = 0; i < BODYPART_COUNT; ++i)
@@ -657,7 +640,7 @@ public:
}
}
m_startPosition = getPosition();
m_startPosition = position; // the starting position of the walker
m_legUpdateAccumulator = 0;
@@ -861,9 +844,7 @@ void NN3DWalkersExample::initPhysics()
slider);
}
// Setup a big ground box
{
{// Setup a big ground box
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
@@ -874,41 +855,25 @@ void NN3DWalkersExample::initPhysics()
ground->setUserPointer(GROUND_ID);
}
for(int i = 0; i < POPULATION_SIZE ; i++){
if(RANDOMIZE_WALKER_DIMENSIONS){
float maxDimension = 0.2f;
// randomize the dimensions
m_rootBodyRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
m_rootBodyHeight = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
m_legRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
m_legLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
m_foreLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
m_foreLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f;
}
// Spawn one walker
spawnWalker(i, m_resetPosition, false);
}
// add walker filter making the walkers never collide with each other
btOverlapFilterCallback * filterCallback = new WalkerFilterCallback();
m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(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_walkersInPopulation.resize(POPULATION_SIZE, NULL);
for(int i = 0; i < POPULATION_SIZE ; i++){
// Spawn one walker
resetWalkerAt(i, m_resetPosition);
}
void NN3DWalkersExample::spawnWalker(int index, const btVector3& resetPosition, bool fixedBodyPosition)
{
NNWalker* walker = new NNWalker(index, m_dynamicsWorld, resetPosition, m_rootBodyRadius,m_rootBodyHeight,m_legRadius,m_legLength,m_foreLegRadius,m_foreLegLength, fixedBodyPosition);
m_walkersInPopulation.push_back(walker);
}
bool NN3DWalkersExample::detectCollisions()
@@ -1167,17 +1132,11 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
m_SimulationTime += delta;
for(int i = 0; i < POPULATION_SIZE;i++) // evaluation time passes
for(int r = 0; r < POPULATION_SIZE;r++) // evaluation time passes
{
if(m_walkersInPopulation[i]->isInEvaluation()){
m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime()+delta); // increase evaluation time
}
}
if(m_walkersInPopulation[r] != NULL && m_walkersInPopulation[r]->isInEvaluation()){
m_walkersInPopulation[r]->setEvaluationTime(m_walkersInPopulation[r]->getEvaluationTime()+delta); // increase evaluation time
for (int r = 0; r < POPULATION_SIZE; r++)
{
if(m_walkersInPopulation[r]->isInEvaluation())
{
m_walkersInPopulation[r]->setLegUpdateAccumulator(m_walkersInPopulation[r]->getLegUpdateAccumulator() + delta);
if(m_walkersInPopulation[r]->getLegUpdateAccumulator() >= btScalar(1.0f) /m_walkerLegTargetFrequency)
@@ -1189,17 +1148,13 @@ void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) {
btScalar targetAngle = 0; // angle in range [0,1]
btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkersInPopulation[r]->getJoints()[i]);
if(RANDOM_WALKER_MOVEMENT){
targetAngle = ((double) rand() / (RAND_MAX));
}
else{ // neural network movement
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 = (tanh(targetAngle)+1.0f)*0.5f; // apply the activation function (threshold) [0;1]
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 = (tanh(targetAngle)+1.0f)*0.5f; // apply the activation function (threshold) [0;1]
btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit()); // [lowerLimit;upperLimit]
btScalar currentAngle = hingeC->getHingeAngle();
btScalar angleError = targetLimitAngle - currentAngle; // target current delta
@@ -1257,9 +1212,20 @@ void NN3DWalkersExample::scheduleEvaluations() {
void NN3DWalkersExample::resetWalkerAt(int i, const btVector3& resetPosition){
NNWalker* newWalker = new NNWalker(i, m_dynamicsWorld, resetPosition, m_rootBodyRadius,m_rootBodyHeight,m_legRadius,m_legLength,m_foreLegRadius,m_foreLegLength, false);
newWalker->copySensoryMotorWeights(m_walkersInPopulation[i]->getSensoryMotorWeights());
delete m_walkersInPopulation[i];
NNWalker* newWalker = new NNWalker(i, m_dynamicsWorld, resetPosition,
m_rootBodyRadius,
m_rootBodyHeight,
m_legRadius,
m_legLength,
m_foreLegRadius,
m_foreLegLength,
false);
if(m_walkersInPopulation[i] != NULL){
newWalker->copySensoryMotorWeights(m_walkersInPopulation[i]->getSensoryMotorWeights());
delete m_walkersInPopulation[i];
}
m_walkersInPopulation[i] = newWalker;
}