1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2015 Google Inc. http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "NN3DWalkers.h"
17
18 #include <cmath>
19
20 #include "btBulletDynamicsCommon.h"
21
22 #include "LinearMath/btIDebugDraw.h"
23 #include "LinearMath/btAlignedObjectArray.h"
24 #include "LinearMath/btHashMap.h"
25 class btBroadphaseInterface;
26 class btCollisionShape;
27 class btOverlappingPairCache;
28 class btCollisionDispatcher;
29 class btConstraintSolver;
30 struct btCollisionAlgorithmCreateFunc;
31 class btDefaultCollisionConfiguration;
32 class NNWalker;
33
34 #include "NN3DWalkersTimeWarpBase.h"
35 #include "../CommonInterfaces/CommonParameterInterface.h"
36
37 #include "../Utils/b3ReferenceFrameHelper.hpp"
38 #include "../RenderingExamples/TimeSeriesCanvas.h"
39
40 static btScalar gRootBodyRadius = 0.25f;
41 static btScalar gRootBodyHeight = 0.1f;
42 static btScalar gLegRadius = 0.1f;
43 static btScalar gLegLength = 0.45f;
44 static btScalar gForeLegLength = 0.75f;
45 static btScalar gForeLegRadius = 0.08f;
46
47 static btScalar gParallelEvaluations = 10.0f;
48
49 #ifndef SIMD_PI_4
50 #define SIMD_PI_4 0.5 * SIMD_HALF_PI
51 #endif
52
53 #ifndef SIMD_PI_8
54 #define SIMD_PI_8 0.25 * SIMD_HALF_PI
55 #endif
56
57 #ifndef RANDOM_MOVEMENT
58 #define RANDOM_MOVEMENT false
59 #endif
60
61 #ifndef RANDOMIZE_DIMENSIONS
62 #define RANDOMIZE_DIMENSIONS false
63 #endif
64
65 #ifndef NUM_WALKERS
66 #define NUM_WALKERS 50
67 #endif
68
69 #ifndef EVALUATION_TIME
70 #define EVALUATION_TIME 10 // s
71 #endif
72
73 #ifndef REAP_QTY
74 #define REAP_QTY 0.3f // number of walkers reaped based on their bad performance
75 #endif
76
77 #ifndef SOW_CROSSOVER_QTY
78 #define SOW_CROSSOVER_QTY 0.2f // this means REAP_QTY-SOW_CROSSOVER_QTY = NEW_RANDOM_BREED_QTY
79 #endif
80
81 #ifndef SOW_ELITE_QTY
82 #define SOW_ELITE_QTY 0.2f // number of walkers kept using an elitist strategy
83 #endif
84
85 #ifndef SOW_MUTATION_QTY
86 #define SOW_MUTATION_QTY 0.5f // SOW_ELITE_QTY + SOW_MUTATION_QTY + REAP_QTY = 1
87 #endif
88
89 #ifndef MUTATION_RATE
90 #define MUTATION_RATE 0.5f // the mutation rate of for the walker with the worst performance
91 #endif
92
93 #ifndef SOW_ELITE_PARTNER
94 #define SOW_ELITE_PARTNER 0.8f
95 #endif
96
97 #define NUM_LEGS 6
98 #define BODYPART_COUNT (2 * NUM_LEGS + 1)
99 #define JOINT_COUNT (BODYPART_COUNT - 1)
100 #define DRAW_INTERPENETRATIONS false
101
102 void* GROUND_ID = (void*)1;
103
104 class NN3DWalkersExample : public NN3DWalkersTimeWarpBase
105 {
106 btScalar m_Time;
107 btScalar m_SpeedupTimestamp;
108 btScalar m_targetAccumulator;
109 btScalar m_targetFrequency;
110 btScalar m_motorStrength;
111 int m_evaluationsQty;
112 int m_nextReaped;
113
114 btAlignedObjectArray<class NNWalker*> m_walkersInPopulation;
115
116 TimeSeriesCanvas* m_timeSeriesCanvas;
117
118 public:
NN3DWalkersExample(struct GUIHelperInterface * helper)119 NN3DWalkersExample(struct GUIHelperInterface* helper)
120 : NN3DWalkersTimeWarpBase(helper),
121 m_Time(0),
122 m_SpeedupTimestamp(0),
123 m_targetAccumulator(0),
124 m_targetFrequency(3),
125 m_motorStrength(0.5f),
126 m_evaluationsQty(0),
127 m_nextReaped(0),
128 m_timeSeriesCanvas(0)
129 {
130 }
131
~NN3DWalkersExample()132 virtual ~NN3DWalkersExample()
133 {
134 delete m_timeSeriesCanvas;
135 }
136
137 void initPhysics();
138
139 virtual void exitPhysics();
140
141 void spawnWalker(int index, const btVector3& startOffset, bool bFixed);
142
143 virtual bool keyboardCallback(int key, int state);
144
145 bool detectCollisions();
146
resetCamera()147 void resetCamera()
148 {
149 float dist = 11;
150 float pitch = -35;
151 float yaw = 52;
152 float targetPos[3] = {0, 0.46, 0};
153 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
154 }
155
156 // Evaluation
157
158 void update(const btScalar timeSinceLastTick);
159
160 void updateEvaluations(const btScalar timeSinceLastTick);
161
162 void scheduleEvaluations();
163
164 void drawMarkings();
165
166 // Reaper
167
168 void rateEvaluations();
169
170 void reap();
171
172 void sow();
173
174 void crossover(NNWalker* mother, NNWalker* father, NNWalker* offspring);
175
176 void mutate(NNWalker* mutant, btScalar mutationRate);
177
178 NNWalker* getRandomElite();
179
180 NNWalker* getRandomNonElite();
181
182 NNWalker* getNextReaped();
183
184 void printWalkerConfigs();
185 };
186
187 static NN3DWalkersExample* nn3DWalkers = NULL;
188
189 class NNWalker
190 {
191 btDynamicsWorld* m_ownerWorld;
192 btCollisionShape* m_shapes[BODYPART_COUNT];
193 btRigidBody* m_bodies[BODYPART_COUNT];
194 btTransform m_bodyRelativeTransforms[BODYPART_COUNT];
195 btTypedConstraint* m_joints[JOINT_COUNT];
196 btHashMap<btHashPtr, int> m_bodyTouchSensorIndexMap;
197 bool m_touchSensors[BODYPART_COUNT];
198 btScalar m_sensoryMotorWeights[BODYPART_COUNT * JOINT_COUNT];
199
200 bool m_inEvaluation;
201 btScalar m_evaluationTime;
202 bool m_reaped;
203 btVector3 m_startPosition;
204 int m_index;
205
localCreateRigidBody(btScalar mass,const btTransform & startTransform,btCollisionShape * shape)206 btRigidBody* localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
207 {
208 bool isDynamic = (mass != 0.f);
209
210 btVector3 localInertia(0, 0, 0);
211 if (isDynamic)
212 shape->calculateLocalInertia(mass, localInertia);
213
214 btDefaultMotionState* motionState = new btDefaultMotionState(startTransform);
215 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, motionState, shape, localInertia);
216 btRigidBody* body = new btRigidBody(rbInfo);
217
218 return body;
219 }
220
221 public:
randomizeSensoryMotorWeights()222 void randomizeSensoryMotorWeights()
223 {
224 //initialize random weights
225 for (int i = 0; i < BODYPART_COUNT; i++)
226 {
227 for (int j = 0; j < JOINT_COUNT; j++)
228 {
229 m_sensoryMotorWeights[i + j * BODYPART_COUNT] = ((double)rand() / (RAND_MAX)) * 2.0f - 1.0f;
230 }
231 }
232 }
233
NNWalker(int index,btDynamicsWorld * ownerWorld,const btVector3 & positionOffset,bool bFixed)234 NNWalker(int index, btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
235 : m_ownerWorld(ownerWorld),
236 m_inEvaluation(false),
237 m_evaluationTime(0),
238 m_reaped(false)
239 {
240 m_index = index;
241 btVector3 vUp(0, 1, 0); // up in local reference frame
242
243 NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo();
244
245 randomizeSensoryMotorWeights();
246
247 //
248 // Setup geometry
249 m_shapes[0] = new btCapsuleShape(gRootBodyRadius, gRootBodyHeight); // root body capsule
250 int i;
251 for (i = 0; i < NUM_LEGS; i++)
252 {
253 m_shapes[1 + 2 * i] = new btCapsuleShape(gLegRadius, gLegLength); // leg capsule
254 m_shapes[2 + 2 * i] = new btCapsuleShape(gForeLegRadius, gForeLegLength); // fore leg capsule
255 }
256
257 //
258 // Setup rigid bodies
259 btScalar rootAboveGroundHeight = gForeLegLength;
260 btTransform bodyOffset;
261 bodyOffset.setIdentity();
262 bodyOffset.setOrigin(positionOffset);
263
264 // root body
265 btVector3 localRootBodyPosition = btVector3(btScalar(0.), rootAboveGroundHeight, btScalar(0.)); // root body position in local reference frame
266 btTransform transform;
267 transform.setIdentity();
268 transform.setOrigin(localRootBodyPosition);
269
270 btTransform originTransform = transform;
271
272 m_bodies[0] = localCreateRigidBody(btScalar(bFixed ? 0. : 1.), bodyOffset * transform, m_shapes[0]);
273 m_ownerWorld->addRigidBody(m_bodies[0]);
274 m_bodyRelativeTransforms[0] = btTransform::getIdentity();
275 m_bodies[0]->setUserPointer(this);
276 m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[0]), 0);
277
278 btHingeConstraint* hingeC;
279 //btConeTwistConstraint* coneC;
280
281 btTransform localA, localB, localC;
282
283 // legs
284 for (i = 0; i < NUM_LEGS; i++)
285 {
286 float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
287 float footYUnitPosition = std::sin(footAngle); // y position of the leg on the unit circle
288 float footXUnitPosition = std::cos(footAngle); // x position of the leg on the unit circle
289
290 transform.setIdentity();
291 btVector3 legCOM = btVector3(btScalar(footXUnitPosition * (gRootBodyRadius + 0.5 * gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition * (gRootBodyRadius + 0.5 * gLegLength)));
292 transform.setOrigin(legCOM);
293
294 // thigh
295 btVector3 legDirection = (legCOM - localRootBodyPosition).normalize();
296 btVector3 kneeAxis = legDirection.cross(vUp);
297 transform.setRotation(btQuaternion(kneeAxis, SIMD_HALF_PI));
298 m_bodies[1 + 2 * i] = localCreateRigidBody(btScalar(1.), bodyOffset * transform, m_shapes[1 + 2 * i]);
299 m_bodyRelativeTransforms[1 + 2 * i] = transform;
300 m_bodies[1 + 2 * i]->setUserPointer(this);
301 m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[1 + 2 * i]), 1 + 2 * i);
302
303 // shin
304 transform.setIdentity();
305 transform.setOrigin(btVector3(btScalar(footXUnitPosition * (gRootBodyRadius + gLegLength)), btScalar(rootAboveGroundHeight - 0.5 * gForeLegLength), btScalar(footYUnitPosition * (gRootBodyRadius + gLegLength))));
306 m_bodies[2 + 2 * i] = localCreateRigidBody(btScalar(1.), bodyOffset * transform, m_shapes[2 + 2 * i]);
307 m_bodyRelativeTransforms[2 + 2 * i] = transform;
308 m_bodies[2 + 2 * i]->setUserPointer(this);
309 m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[2 + 2 * i]), 2 + 2 * i);
310
311 // hip joints
312 localA.setIdentity();
313 localB.setIdentity();
314 localA.getBasis().setEulerZYX(0, -footAngle, 0);
315 localA.setOrigin(btVector3(btScalar(footXUnitPosition * gRootBodyRadius), btScalar(0.), btScalar(footYUnitPosition * gRootBodyRadius)));
316 localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1 + 2 * i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(), localA));
317 hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1 + 2 * i], localA, localB);
318 hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
319 //hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
320 m_joints[2 * i] = hingeC;
321
322 // knee joints
323 localA.setIdentity();
324 localB.setIdentity();
325 localC.setIdentity();
326 localA.getBasis().setEulerZYX(0, -footAngle, 0);
327 localA.setOrigin(btVector3(btScalar(footXUnitPosition * (gRootBodyRadius + gLegLength)), btScalar(0.), btScalar(footYUnitPosition * (gRootBodyRadius + gLegLength))));
328 localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1 + 2 * i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(), localA));
329 localC = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[2 + 2 * i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(), localA));
330 hingeC = new btHingeConstraint(*m_bodies[1 + 2 * i], *m_bodies[2 + 2 * i], localB, localC);
331 //hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
332 hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2));
333 m_joints[1 + 2 * i] = hingeC;
334
335 m_ownerWorld->addRigidBody(m_bodies[1 + 2 * i]); // add thigh bone
336
337 m_ownerWorld->addConstraint(m_joints[2 * i], true); // connect thigh bone with root
338
339 if (nnWalkersDemo->detectCollisions())
340 { // if thigh bone causes collision, remove it again
341 m_ownerWorld->removeRigidBody(m_bodies[1 + 2 * i]);
342 m_ownerWorld->removeConstraint(m_joints[2 * i]); // disconnect thigh bone from root
343 }
344 else
345 {
346 m_ownerWorld->addRigidBody(m_bodies[2 + 2 * i]); // add shin bone
347 m_ownerWorld->addConstraint(m_joints[1 + 2 * i], true); // connect shin bone with thigh
348
349 if (nnWalkersDemo->detectCollisions())
350 { // if shin bone causes collision, remove it again
351 m_ownerWorld->removeRigidBody(m_bodies[2 + 2 * i]);
352 m_ownerWorld->removeConstraint(m_joints[1 + 2 * i]); // disconnect shin bone from thigh
353 }
354 }
355 }
356
357 // Setup some damping on the m_bodies
358 for (i = 0; i < BODYPART_COUNT; ++i)
359 {
360 m_bodies[i]->setDamping(0.05, 0.85);
361 m_bodies[i]->setDeactivationTime(0.8);
362 //m_bodies[i]->setSleepingThresholds(1.6, 2.5);
363 m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
364 }
365
366 removeFromWorld(); // it should not yet be in the world
367 }
368
~NNWalker()369 virtual ~NNWalker()
370 {
371 int i;
372
373 // Remove all constraints
374 for (i = 0; i < JOINT_COUNT; ++i)
375 {
376 m_ownerWorld->removeConstraint(m_joints[i]);
377 delete m_joints[i];
378 m_joints[i] = 0;
379 }
380
381 // Remove all bodies and shapes
382 for (i = 0; i < BODYPART_COUNT; ++i)
383 {
384 m_ownerWorld->removeRigidBody(m_bodies[i]);
385
386 delete m_bodies[i]->getMotionState();
387
388 delete m_bodies[i];
389 m_bodies[i] = 0;
390 delete m_shapes[i];
391 m_shapes[i] = 0;
392 }
393 }
394
getJoints()395 btTypedConstraint** getJoints()
396 {
397 return &m_joints[0];
398 }
399
setTouchSensor(void * bodyPointer)400 void setTouchSensor(void* bodyPointer)
401 {
402 m_touchSensors[*m_bodyTouchSensorIndexMap.find(btHashPtr(bodyPointer))] = true;
403 }
404
clearTouchSensors()405 void clearTouchSensors()
406 {
407 for (int i = 0; i < BODYPART_COUNT; i++)
408 {
409 m_touchSensors[i] = false;
410 }
411 }
412
getTouchSensor(int i)413 bool getTouchSensor(int i)
414 {
415 return m_touchSensors[i];
416 }
417
getSensoryMotorWeights()418 btScalar* getSensoryMotorWeights()
419 {
420 return m_sensoryMotorWeights;
421 }
422
addToWorld()423 void addToWorld()
424 {
425 int i;
426 // add all bodies and shapes
427 for (i = 0; i < BODYPART_COUNT; ++i)
428 {
429 m_ownerWorld->addRigidBody(m_bodies[i]);
430 }
431
432 // add all constraints
433 for (i = 0; i < JOINT_COUNT; ++i)
434 {
435 m_ownerWorld->addConstraint(m_joints[i], true); // important! If you add constraints back, you must set bullet physics to disable collision between constrained bodies
436 }
437 m_startPosition = getPosition();
438 }
439
removeFromWorld()440 void removeFromWorld()
441 {
442 int i;
443
444 // Remove all constraints
445 for (i = 0; i < JOINT_COUNT; ++i)
446 {
447 m_ownerWorld->removeConstraint(m_joints[i]);
448 }
449
450 // Remove all bodies
451 for (i = 0; i < BODYPART_COUNT; ++i)
452 {
453 m_ownerWorld->removeRigidBody(m_bodies[i]);
454 }
455 }
456
getPosition() const457 btVector3 getPosition() const
458 {
459 btVector3 finalPosition(0, 0, 0);
460
461 for (int i = 0; i < BODYPART_COUNT; i++)
462 {
463 finalPosition += m_bodies[i]->getCenterOfMassPosition();
464 }
465
466 finalPosition /= BODYPART_COUNT;
467 return finalPosition;
468 }
469
getDistanceFitness() const470 btScalar getDistanceFitness() const
471 {
472 btScalar distance = 0;
473
474 distance = (getPosition() - m_startPosition).length2();
475
476 return distance;
477 }
478
getFitness() const479 btScalar getFitness() const
480 {
481 return getDistanceFitness(); // for now it is only distance
482 }
483
resetAt(const btVector3 & position)484 void resetAt(const btVector3& position)
485 {
486 btTransform resetPosition(btQuaternion::getIdentity(), position);
487 for (int i = 0; i < BODYPART_COUNT; ++i)
488 {
489 m_bodies[i]->setWorldTransform(resetPosition * m_bodyRelativeTransforms[i]);
490 if (m_bodies[i]->getMotionState())
491 {
492 m_bodies[i]->getMotionState()->setWorldTransform(resetPosition * m_bodyRelativeTransforms[i]);
493 }
494 m_bodies[i]->clearForces();
495 m_bodies[i]->setAngularVelocity(btVector3(0, 0, 0));
496 m_bodies[i]->setLinearVelocity(btVector3(0, 0, 0));
497 }
498
499 clearTouchSensors();
500 }
501
getEvaluationTime() const502 btScalar getEvaluationTime() const
503 {
504 return m_evaluationTime;
505 }
506
setEvaluationTime(btScalar evaluationTime)507 void setEvaluationTime(btScalar evaluationTime)
508 {
509 m_evaluationTime = evaluationTime;
510 }
511
isInEvaluation() const512 bool isInEvaluation() const
513 {
514 return m_inEvaluation;
515 }
516
setInEvaluation(bool inEvaluation)517 void setInEvaluation(bool inEvaluation)
518 {
519 m_inEvaluation = inEvaluation;
520 }
521
isReaped() const522 bool isReaped() const
523 {
524 return m_reaped;
525 }
526
setReaped(bool reaped)527 void setReaped(bool reaped)
528 {
529 m_reaped = reaped;
530 }
531
getIndex() const532 int getIndex() const
533 {
534 return m_index;
535 }
536 };
537
538 void evaluationUpdatePreTickCallback(btDynamicsWorld* world, btScalar timeStep);
539
legContactProcessedCallback(btManifoldPoint & cp,void * body0,void * body1)540 bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
541 {
542 btCollisionObject* o1 = static_cast<btCollisionObject*>(body0);
543 btCollisionObject* o2 = static_cast<btCollisionObject*>(body1);
544
545 void* ID1 = o1->getUserPointer();
546 void* ID2 = o2->getUserPointer();
547
548 if (ID1 != GROUND_ID || ID2 != GROUND_ID)
549 {
550 // Make a circle with a 0.9 radius at (0,0,0)
551 // with RGB color (1,0,0).
552 if (nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL)
553 {
554 if (!nn3DWalkers->mIsHeadless)
555 {
556 nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.));
557 }
558 }
559
560 if (ID1 != GROUND_ID && ID1)
561 {
562 ((NNWalker*)ID1)->setTouchSensor(o1);
563 }
564
565 if (ID2 != GROUND_ID && ID2)
566 {
567 ((NNWalker*)ID2)->setTouchSensor(o2);
568 }
569 }
570 return false;
571 }
572
573 struct WalkerFilterCallback : public btOverlapFilterCallback
574 {
575 // return true when pairs need collision
needBroadphaseCollisionWalkerFilterCallback576 virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
577 {
578 btCollisionObject* obj0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
579 btCollisionObject* obj1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
580
581 if (obj0->getUserPointer() == GROUND_ID || obj1->getUserPointer() == GROUND_ID)
582 { // everything collides with ground
583 return true;
584 }
585 else
586 {
587 return ((NNWalker*)obj0->getUserPointer())->getIndex() == ((NNWalker*)obj1->getUserPointer())->getIndex();
588 }
589 }
590 };
591
initPhysics()592 void NN3DWalkersExample::initPhysics()
593 {
594 setupBasicParamInterface(); // parameter interface to use timewarp
595
596 gContactProcessedCallback = legContactProcessedCallback;
597
598 m_guiHelper->setUpAxis(1);
599
600 // Setup the basic world
601
602 m_Time = 0;
603
604 createEmptyDynamicsWorld();
605
606 m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true);
607 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
608
609 m_targetFrequency = 3;
610
611 // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
612 // should be (numberOfsolverIterations * oldLimits)
613 m_motorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations;
614
615 { // create a slider to change the motor update frequency
616 SliderParams slider("Motor update frequency", &m_targetFrequency);
617 slider.m_minVal = 0;
618 slider.m_maxVal = 10;
619 slider.m_clampToNotches = false;
620 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
621 slider);
622 }
623
624 { // create a slider to change the motor torque
625 SliderParams slider("Motor force", &m_motorStrength);
626 slider.m_minVal = 1;
627 slider.m_maxVal = 50;
628 slider.m_clampToNotches = false;
629 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
630 slider);
631 }
632
633 { // create a slider to change the root body radius
634 SliderParams slider("Root body radius", &gRootBodyRadius);
635 slider.m_minVal = 0.01f;
636 slider.m_maxVal = 10;
637 slider.m_clampToNotches = false;
638 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
639 slider);
640 }
641
642 { // create a slider to change the root body height
643 SliderParams slider("Root body height", &gRootBodyHeight);
644 slider.m_minVal = 0.01f;
645 slider.m_maxVal = 10;
646 slider.m_clampToNotches = false;
647 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
648 slider);
649 }
650
651 { // create a slider to change the leg radius
652 SliderParams slider("Leg radius", &gLegRadius);
653 slider.m_minVal = 0.01f;
654 slider.m_maxVal = 10;
655 slider.m_clampToNotches = false;
656 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
657 slider);
658 }
659
660 { // create a slider to change the leg length
661 SliderParams slider("Leg length", &gLegLength);
662 slider.m_minVal = 0.01f;
663 slider.m_maxVal = 10;
664 slider.m_clampToNotches = false;
665 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
666 slider);
667 }
668
669 { // create a slider to change the fore leg radius
670 SliderParams slider("Fore Leg radius", &gForeLegRadius);
671 slider.m_minVal = 0.01f;
672 slider.m_maxVal = 10;
673 slider.m_clampToNotches = false;
674 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
675 slider);
676 }
677
678 { // create a slider to change the fore leg length
679 SliderParams slider("Fore Leg length", &gForeLegLength);
680 slider.m_minVal = 0.01f;
681 slider.m_maxVal = 10;
682 slider.m_clampToNotches = false;
683 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
684 slider);
685 }
686
687 { // create a slider to change the number of parallel evaluations
688 SliderParams slider("Parallel evaluations", &gParallelEvaluations);
689 slider.m_minVal = 1;
690 slider.m_maxVal = NUM_WALKERS;
691 slider.m_clampToIntegers = true;
692 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(
693 slider);
694 }
695
696 // Setup a big ground box
697 {
698 btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.), btScalar(10.), btScalar(200.)));
699 m_collisionShapes.push_back(groundShape);
700 btTransform groundTransform;
701 groundTransform.setIdentity();
702 groundTransform.setOrigin(btVector3(0, -10, 0));
703 btRigidBody* ground = createRigidBody(btScalar(0.), groundTransform, groundShape);
704 ground->setFriction(5);
705 ground->setUserPointer(GROUND_ID);
706 }
707
708 for (int i = 0; i < NUM_WALKERS; i++)
709 {
710 if (RANDOMIZE_DIMENSIONS)
711 {
712 float maxDimension = 0.2f;
713
714 // randomize the dimensions
715 gRootBodyRadius = ((double)rand() / (RAND_MAX)) * (maxDimension - 0.01f) + 0.01f;
716 gRootBodyHeight = ((double)rand() / (RAND_MAX)) * (maxDimension - 0.01f) + 0.01f;
717 gLegRadius = ((double)rand() / (RAND_MAX)) * (maxDimension - 0.01f) + 0.01f;
718 gLegLength = ((double)rand() / (RAND_MAX)) * (maxDimension - 0.01f) + 0.01f;
719 gForeLegLength = ((double)rand() / (RAND_MAX)) * (maxDimension - 0.01f) + 0.01f;
720 gForeLegRadius = ((double)rand() / (RAND_MAX)) * (maxDimension - 0.01f) + 0.01f;
721 }
722
723 // Spawn one walker
724 btVector3 offset(0, 0, 0);
725 spawnWalker(i, offset, false);
726 }
727
728 btOverlapFilterCallback* filterCallback = new WalkerFilterCallback();
729 m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback);
730
731 m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface, 300, 200, "Fitness Performance");
732 m_timeSeriesCanvas->setupTimeSeries(40, NUM_WALKERS * EVALUATION_TIME, 0);
733 for (int i = 0; i < NUM_WALKERS; i++)
734 {
735 m_timeSeriesCanvas->addDataSource(" ", 100 * i / NUM_WALKERS, 100 * (NUM_WALKERS - i) / NUM_WALKERS, 100 * (i) / NUM_WALKERS);
736 }
737 }
738
spawnWalker(int index,const btVector3 & startOffset,bool bFixed)739 void NN3DWalkersExample::spawnWalker(int index, const btVector3& startOffset, bool bFixed)
740 {
741 NNWalker* walker = new NNWalker(index, m_dynamicsWorld, startOffset, bFixed);
742 m_walkersInPopulation.push_back(walker);
743 }
744
detectCollisions()745 bool NN3DWalkersExample::detectCollisions()
746 {
747 bool collisionDetected = false;
748 if (m_dynamicsWorld)
749 {
750 m_dynamicsWorld->performDiscreteCollisionDetection(); // let the collisions be calculated
751 }
752
753 int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
754 for (int i = 0; i < numManifolds; i++)
755 {
756 btPersistentManifold* contactManifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
757 const btCollisionObject* obA = contactManifold->getBody0();
758 const btCollisionObject* obB = contactManifold->getBody1();
759
760 if (obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID)
761 {
762 int numContacts = contactManifold->getNumContacts();
763 for (int j = 0; j < numContacts; j++)
764 {
765 collisionDetected = true;
766 btManifoldPoint& pt = contactManifold->getContactPoint(j);
767 if (pt.getDistance() < 0.f)
768 {
769 //const btVector3& ptA = pt.getPositionWorldOnA();
770 //const btVector3& ptB = pt.getPositionWorldOnB();
771 //const btVector3& normalOnB = pt.m_normalWorldOnB;
772
773 if (!DRAW_INTERPENETRATIONS)
774 {
775 return collisionDetected;
776 }
777
778 if (m_dynamicsWorld->getDebugDrawer())
779 {
780 m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.));
781 m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.));
782 }
783 }
784 }
785 }
786 }
787
788 return collisionDetected;
789 }
790
keyboardCallback(int key,int state)791 bool NN3DWalkersExample::keyboardCallback(int key, int state)
792 {
793 switch (key)
794 {
795 case '[':
796 m_motorStrength /= 1.1f;
797 return true;
798 case ']':
799 m_motorStrength *= 1.1f;
800 return true;
801 case 'l':
802 printWalkerConfigs();
803 return true;
804 default:
805 break;
806 }
807
808 return NN3DWalkersTimeWarpBase::keyboardCallback(key, state);
809 }
810
exitPhysics()811 void NN3DWalkersExample::exitPhysics()
812 {
813 gContactProcessedCallback = NULL; // clear contact processed callback on exiting
814
815 int i;
816
817 for (i = 0; i < NUM_WALKERS; i++)
818 {
819 NNWalker* walker = m_walkersInPopulation[i];
820 delete walker;
821 }
822
823 CommonRigidBodyBase::exitPhysics();
824 }
825
ET_NN3DWalkersCreateFunc(struct CommonExampleOptions & options)826 class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options)
827 {
828 nn3DWalkers = new NN3DWalkersExample(options.m_guiHelper);
829 return nn3DWalkers;
830 }
831
fitnessComparator(const NNWalker * a,const NNWalker * b)832 bool fitnessComparator(const NNWalker* a, const NNWalker* b)
833 {
834 return a->getFitness() > b->getFitness(); // sort walkers descending
835 }
836
rateEvaluations()837 void NN3DWalkersExample::rateEvaluations()
838 {
839 m_walkersInPopulation.quickSort(fitnessComparator); // Sort walkers by fitness
840
841 b3Printf("Best performing walker: %f meters", btSqrt(m_walkersInPopulation[0]->getDistanceFitness()));
842
843 for (int i = 0; i < NUM_WALKERS; i++)
844 {
845 m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()), 0, true);
846 }
847 m_timeSeriesCanvas->nextTick();
848
849 for (int i = 0; i < NUM_WALKERS; i++)
850 {
851 m_walkersInPopulation[i]->setEvaluationTime(0);
852 }
853 m_nextReaped = 0;
854 }
855
reap()856 void NN3DWalkersExample::reap()
857 {
858 int reaped = 0;
859 for (int i = NUM_WALKERS - 1; i >= (NUM_WALKERS - 1) * (1 - REAP_QTY); i--)
860 { // reap a certain percentage
861 m_walkersInPopulation[i]->setReaped(true);
862 reaped++;
863 b3Printf("%i Walker(s) reaped.", reaped);
864 }
865 }
866
getRandomElite()867 NNWalker* NN3DWalkersExample::getRandomElite()
868 {
869 return m_walkersInPopulation[((NUM_WALKERS - 1) * SOW_ELITE_QTY) * (rand() / RAND_MAX)];
870 }
871
getRandomNonElite()872 NNWalker* NN3DWalkersExample::getRandomNonElite()
873 {
874 return m_walkersInPopulation[(NUM_WALKERS - 1) * SOW_ELITE_QTY + (NUM_WALKERS - 1) * (1.0f - SOW_ELITE_QTY) * (rand() / RAND_MAX)];
875 }
876
getNextReaped()877 NNWalker* NN3DWalkersExample::getNextReaped()
878 {
879 if ((NUM_WALKERS - 1) - m_nextReaped >= (NUM_WALKERS - 1) * (1 - REAP_QTY))
880 {
881 m_nextReaped++;
882 }
883
884 if (m_walkersInPopulation[(NUM_WALKERS - 1) - m_nextReaped + 1]->isReaped())
885 {
886 return m_walkersInPopulation[(NUM_WALKERS - 1) - m_nextReaped + 1];
887 }
888 else
889 {
890 return NULL; // we asked for too many
891 }
892 }
893
sow()894 void NN3DWalkersExample::sow()
895 {
896 int sow = 0;
897 for (int i = 0; i < NUM_WALKERS * (SOW_CROSSOVER_QTY); i++)
898 { // create number of new crossover creatures
899 sow++;
900 b3Printf("%i Walker(s) sown.", sow);
901 NNWalker* mother = getRandomElite(); // Get elite partner (mother)
902 NNWalker* father = (SOW_ELITE_PARTNER < rand() / RAND_MAX) ? getRandomElite() : getRandomNonElite(); //Get elite or random partner (father)
903 NNWalker* offspring = getNextReaped();
904 crossover(mother, father, offspring);
905 }
906
907 for (int i = NUM_WALKERS * SOW_ELITE_QTY; i < NUM_WALKERS * (SOW_ELITE_QTY + SOW_MUTATION_QTY); i++)
908 { // create mutants
909 mutate(m_walkersInPopulation[i], btScalar(MUTATION_RATE / (NUM_WALKERS * SOW_MUTATION_QTY) * (i - NUM_WALKERS * SOW_ELITE_QTY)));
910 }
911
912 for (int i = 0; i < (NUM_WALKERS - 1) * (REAP_QTY - SOW_CROSSOVER_QTY); i++)
913 {
914 sow++;
915 b3Printf("%i Walker(s) sown.", sow);
916 NNWalker* reaped = getNextReaped();
917 reaped->setReaped(false);
918 reaped->randomizeSensoryMotorWeights();
919 }
920 }
921
crossover(NNWalker * mother,NNWalker * father,NNWalker * child)922 void NN3DWalkersExample::crossover(NNWalker* mother, NNWalker* father, NNWalker* child)
923 {
924 for (int i = 0; i < BODYPART_COUNT * JOINT_COUNT; i++)
925 {
926 btScalar random = ((double)rand() / (RAND_MAX));
927
928 if (random >= 0.5f)
929 {
930 child->getSensoryMotorWeights()[i] = mother->getSensoryMotorWeights()[i];
931 }
932 else
933 {
934 child->getSensoryMotorWeights()[i] = father->getSensoryMotorWeights()[i];
935 }
936 }
937 }
938
mutate(NNWalker * mutant,btScalar mutationRate)939 void NN3DWalkersExample::mutate(NNWalker* mutant, btScalar mutationRate)
940 {
941 for (int i = 0; i < BODYPART_COUNT * JOINT_COUNT; i++)
942 {
943 btScalar random = ((double)rand() / (RAND_MAX));
944
945 if (random >= mutationRate)
946 {
947 mutant->getSensoryMotorWeights()[i] = ((double)rand() / (RAND_MAX)) * 2.0f - 1.0f;
948 }
949 }
950 }
951
evaluationUpdatePreTickCallback(btDynamicsWorld * world,btScalar timeStep)952 void evaluationUpdatePreTickCallback(btDynamicsWorld* world, btScalar timeStep)
953 {
954 NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)world->getWorldUserInfo();
955
956 nnWalkersDemo->update(timeStep);
957 }
958
update(const btScalar timeSinceLastTick)959 void NN3DWalkersExample::update(const btScalar timeSinceLastTick)
960 {
961 updateEvaluations(timeSinceLastTick); /**!< We update all evaluations that are in the loop */
962
963 scheduleEvaluations(); /**!< Start new evaluations and finish the old ones. */
964
965 drawMarkings(); /**!< Draw markings on the ground */
966
967 if (m_Time > m_SpeedupTimestamp + 2.0f)
968 { // print effective speedup
969 b3Printf("Avg Effective speedup: %f real time", calculatePerformedSpeedup());
970 m_SpeedupTimestamp = m_Time;
971 }
972 }
973
updateEvaluations(const btScalar timeSinceLastTick)974 void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick)
975 {
976 btScalar delta = timeSinceLastTick;
977 btScalar minFPS = 1.f / 60.f;
978 if (delta > minFPS)
979 {
980 delta = minFPS;
981 }
982
983 m_Time += delta;
984
985 m_targetAccumulator += delta;
986
987 for (int i = 0; i < NUM_WALKERS; i++) // evaluation time passes
988 {
989 if (m_walkersInPopulation[i]->isInEvaluation())
990 {
991 m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime() + delta); // increase evaluation time
992 }
993 }
994
995 if (m_targetAccumulator >= 1.0f / ((double)m_targetFrequency))
996 {
997 m_targetAccumulator = 0;
998
999 for (int r = 0; r < NUM_WALKERS; r++)
1000 {
1001 if (m_walkersInPopulation[r]->isInEvaluation())
1002 {
1003 for (int i = 0; i < 2 * NUM_LEGS; i++)
1004 {
1005 btScalar targetAngle = 0;
1006 btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_walkersInPopulation[r]->getJoints()[i]);
1007
1008 if (RANDOM_MOVEMENT)
1009 {
1010 targetAngle = ((double)rand() / (RAND_MAX));
1011 }
1012 else
1013 { // neural network movement
1014
1015 // accumulate sensor inputs with weights
1016 for (int j = 0; j < JOINT_COUNT; j++)
1017 {
1018 targetAngle += m_walkersInPopulation[r]->getSensoryMotorWeights()[i + j * BODYPART_COUNT] * m_walkersInPopulation[r]->getTouchSensor(i);
1019 }
1020
1021 // apply the activation function
1022 targetAngle = (std::tanh(targetAngle) + 1.0f) * 0.5f;
1023 }
1024 btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
1025 btScalar currentAngle = hingeC->getHingeAngle();
1026 btScalar angleError = targetLimitAngle - currentAngle;
1027 btScalar desiredAngularVel = 0;
1028 if (delta)
1029 {
1030 desiredAngularVel = angleError / delta;
1031 }
1032 else
1033 {
1034 desiredAngularVel = angleError / 0.0001f;
1035 }
1036 hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength);
1037 }
1038
1039 // clear sensor signals after usage
1040 m_walkersInPopulation[r]->clearTouchSensors();
1041 }
1042 }
1043 }
1044 }
1045
scheduleEvaluations()1046 void NN3DWalkersExample::scheduleEvaluations()
1047 {
1048 for (int i = 0; i < NUM_WALKERS; i++)
1049 {
1050 if (m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_TIME)
1051 { /**!< tear down evaluations */
1052 b3Printf("An evaluation finished at %f s. Distance: %f m", m_Time, btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
1053 m_walkersInPopulation[i]->setInEvaluation(false);
1054 m_walkersInPopulation[i]->removeFromWorld();
1055 m_evaluationsQty--;
1056 }
1057
1058 if (m_evaluationsQty < gParallelEvaluations && !m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0)
1059 { /**!< Setup the new evaluations */
1060 b3Printf("An evaluation started at %f s.", m_Time);
1061 m_evaluationsQty++;
1062 m_walkersInPopulation[i]->setInEvaluation(true);
1063
1064 if (m_walkersInPopulation[i]->getEvaluationTime() == 0)
1065 { // reset to origin if the evaluation did not yet run
1066 m_walkersInPopulation[i]->resetAt(btVector3(0, 0, 0));
1067 }
1068
1069 m_walkersInPopulation[i]->addToWorld();
1070 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
1071 }
1072 }
1073
1074 if (m_evaluationsQty == 0)
1075 { // if there are no more evaluations possible
1076 rateEvaluations(); // rate evaluations by sorting them based on their fitness
1077
1078 reap(); // reap worst performing walkers
1079
1080 sow(); // crossover & mutate and sow new walkers
1081 b3Printf("### A new generation started. ###");
1082 }
1083 }
1084
drawMarkings()1085 void NN3DWalkersExample::drawMarkings()
1086 {
1087 if (!mIsHeadless)
1088 {
1089 for (int i = 0; i < NUM_WALKERS; i++) // draw current distance plates of moving walkers
1090 {
1091 if (m_walkersInPopulation[i]->isInEvaluation())
1092 {
1093 btVector3 walkerPosition = m_walkersInPopulation[i]->getPosition();
1094 char performance[20];
1095 sprintf(performance, "%.2f m", btSqrt(m_walkersInPopulation[i]->getDistanceFitness()));
1096 m_guiHelper->drawText3D(performance, walkerPosition.x(), walkerPosition.y() + 1, walkerPosition.z(), 1);
1097 }
1098 }
1099
1100 for (int i = 2; i < 50; i += 2)
1101 { // draw distance circles
1102 if (m_dynamicsWorld->getDebugDrawer())
1103 {
1104 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);
1105 }
1106 }
1107 }
1108 }
1109
printWalkerConfigs()1110 void NN3DWalkersExample::printWalkerConfigs()
1111 {
1112 #if 0
1113 char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n
1114 char* runner = configString;
1115 sprintf(runner,"Population configuration:");
1116 runner +=25;
1117 for(int i = 0;i < NUM_WALKERS;i++) {
1118 runner[0] = '\n';
1119 runner++;
1120 runner[0] = '[';
1121 runner++;
1122 for(int j = 0; j < BODYPART_COUNT*JOINT_COUNT;j++) {
1123 sprintf(runner,"%.15f", m_walkersInPopulation[i]->getSensoryMotorWeights()[j]);
1124 runner +=15;
1125 if(j + 1 < BODYPART_COUNT*JOINT_COUNT){
1126 runner[0] = ',';
1127 }
1128 else{
1129 runner[0] = ']';
1130 }
1131 runner++;
1132 }
1133 }
1134 runner[0] = '\0';
1135 b3Printf(configString);
1136 #endif
1137 }
1138