From 2bf17a7a819a51dc0d532bbe14c18fd03c35f7a2 Mon Sep 17 00:00:00 2001 From: Benjamin Ellenberger Date: Fri, 1 Jul 2016 19:35:27 +0200 Subject: [PATCH] Initial commit. ------------ Simplifying the walker generation code and making it more understandable SQUASH THIS LATER. --- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 3 +- examples/ExampleBrowser/premake4.lua | 2 + examples/ExtendedTutorials/NN3DWalkers.cpp | 434 +++++++++++++++++++++ examples/ExtendedTutorials/NN3DWalkers.h | 23 ++ 5 files changed, 463 insertions(+), 1 deletion(-) create mode 100755 examples/ExtendedTutorials/NN3DWalkers.cpp create mode 100755 examples/ExtendedTutorials/NN3DWalkers.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 0af74fdb8..49ccf0594 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -175,6 +175,8 @@ SET(BulletExampleBrowser_SRCS ../ExtendedTutorials/InclinedPlane.h ../ExtendedTutorials/MultiPendulum.cpp ../ExtendedTutorials/MultiPendulum.h + ../ExtendedTutorials/NN3DWalkers.cpp + ../ExtendedTutorials/NN3DWalkers.h ../Collision/CollisionSdkC_Api.cpp ../Collision/CollisionSdkC_Api.h ../Collision/CollisionTutorialBullet2.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 10cfe33ee..e32d14648 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -67,6 +67,7 @@ #include "../ExtendedTutorials/InclinedPlane.h" #include "../ExtendedTutorials/NewtonsCradle.h" #include "../ExtendedTutorials/MultiPendulum.h" +#include "../ExtendedTutorials/NN3DWalkers.h" struct ExampleEntry { @@ -280,7 +281,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Inclined Plane", "Create an inclined plane to show restitution and different types of friction. Use the sliders to vary restitution and friction and press space to reset the scene.", ET_InclinedPlaneCreateFunc), ExampleEntry(1,"Newton's Cradle", "Create a Newton's Cradle using a pair of point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number of pendula in total (reset simulation), the number of displaced pendula and other options.", ET_NewtonsCradleCreateFunc), ExampleEntry(1,"Multi-Pendulum", "Create a Multi-Pendulum using point2point/slider constraints. Press 1/2 to lengthen/shorten the pendula, press 3 to displace pendula. Use the sliders to select the number of pendula in total (reset simulation), the number of displaced pendula and other options.",ET_MultiPendulumCreateFunc), - + ExampleEntry(1,"Neural Network 3D Walkers","A simple example of using evolution to make a creature walk.",NN3DWalkersCreateFunc) //todo: create a category/tutorial about advanced topics, such as optimizations, using different collision detection algorithm, different constraint solvers etc. //ExampleEntry(0,"Advanced"), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 82c63e64d..c872caa4c 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -93,6 +93,8 @@ project "App_BulletExampleBrowser" "../ExtendedTutorials/Chain.cpp", "../ExtendedTutorials/Bridge.cpp", "../ExtendedTutorials/RigidBodyFromObj.cpp", + "../ExtendedTutorials/NN3DWalkers.cpp", + "../ExtendedTutorials/NN3DWalkers.h", "../Collision/*", "../Collision/Internal/*", "../Benchmarks/*", diff --git a/examples/ExtendedTutorials/NN3DWalkers.cpp b/examples/ExtendedTutorials/NN3DWalkers.cpp new file mode 100755 index 000000000..c2a0648c7 --- /dev/null +++ b/examples/ExtendedTutorials/NN3DWalkers.cpp @@ -0,0 +1,434 @@ +/* +Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans +Motor Demo + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "NN3DWalkers.h" +#include "btBulletDynamicsCommon.h" + +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAlignedObjectArray.h" +class btBroadphaseInterface; +class btCollisionShape; +class btOverlappingPairCache; +class btCollisionDispatcher; +class btConstraintSolver; +struct btCollisionAlgorithmCreateFunc; +class btDefaultCollisionConfiguration; + +#include "../CommonInterfaces/CommonRigidBodyBase.h" + +//TODO: Maybe add pointworldToLocal and AxisWorldToLocal etc. to a helper class + +class NN3DWalkers : public CommonRigidBodyBase +{ + float m_Time; + float m_fCyclePeriod; // in milliseconds + float m_fMuscleStrength; + + btAlignedObjectArray m_walkers; + + +public: + NN3DWalkers(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper),m_fCyclePeriod(0),m_Time(0),m_fMuscleStrength(0) + { + } + + void initPhysics(); + + void exitPhysics(); + + virtual ~NN3DWalkers() + { + } + + void spawnWalker(const btVector3& startOffset, bool bFixed); + + virtual bool keyboardCallback(int key, int state); + + void setMotorTargets(btScalar deltaTime); + + void resetCamera() + { + float dist = 11; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + btVector3 getPointWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &point); + btVector3 getAxisWorldToLocal(btTransform localObjectCenterOfMassTransform, btVector3 &axis); + + btVector3 getPointLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &point); + btVector3 getAxisLocalToWorld(btTransform localObjectCenterOfMassTransform, btVector3 &axis); +}; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +#ifndef M_PI_4 +#define M_PI_4 0.785398163397448309616 +#endif + +#ifndef M_PI_8 +#define M_PI_8 0.5 * M_PI_4 +#endif + + +// /LOCAL FUNCTIONS + + + +#define NUM_LEGS 6 +#define BODYPART_COUNT 2 * NUM_LEGS + 1 +#define JOINT_COUNT BODYPART_COUNT - 1 + +class NNWalker +{ + btDynamicsWorld* m_ownerWorld; + btCollisionShape* m_shapes[BODYPART_COUNT]; + btRigidBody* m_bodies[BODYPART_COUNT]; + btTypedConstraint* m_joints[JOINT_COUNT]; + + btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape) + { + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + m_ownerWorld->addRigidBody(body); + + return body; + } + + +public: + NNWalker(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed) + : m_ownerWorld (ownerWorld) + { + btVector3 vUp(0, 1, 0); + + // + // Setup geometry + // + float rootBodyRadius = 0.25f; + float rootBodyHeight = 0.1f; + float legRadius = 0.1f; + float legLength = 0.45f; + float foreLegLength = 0.75f; + float foreLegRadius = 0.08f; + m_shapes[0] = new btCapsuleShape(btScalar(rootBodyRadius), btScalar(rootBodyHeight)); + int i; + for ( i=0; igetWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; + hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB); + hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8)); + //hingeC->setLimit(btScalar(-0.1), btScalar(0.1)); + m_joints[2*i] = hingeC; + m_ownerWorld->addConstraint(m_joints[2*i], true); + + // 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)))); + localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA; + localC = m_bodies[2+2*i]->getWorldTransform().inverse() * 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(-M_PI_8), btScalar(0.2)); + m_joints[1+2*i] = hingeC; + m_ownerWorld->addConstraint(m_joints[1+2*i], true); + } + + // Setup some damping on the m_bodies + for (i = 0; i < BODYPART_COUNT; ++i) + { + m_bodies[i]->setDamping(0.05, 0.85); + m_bodies[i]->setDeactivationTime(0.8); + //m_bodies[i]->setSleepingThresholds(1.6, 2.5); + m_bodies[i]->setSleepingThresholds(0.5f, 0.5f); + } + } + + virtual ~NNWalker () + { + int i; + + // Remove all constraints + for ( i = 0; i < JOINT_COUNT; ++i) + { + m_ownerWorld->removeConstraint(m_joints[i]); + delete m_joints[i]; m_joints[i] = 0; + } + + // Remove all bodies and shapes + for ( i = 0; i < BODYPART_COUNT; ++i) + { + m_ownerWorld->removeRigidBody(m_bodies[i]); + + delete m_bodies[i]->getMotionState(); + + delete m_bodies[i]; m_bodies[i] = 0; + delete m_shapes[i]; m_shapes[i] = 0; + } + } + + btTypedConstraint** GetJoints() {return &m_joints[0];} + +}; + + + +void motorNNPreTickCallback (btDynamicsWorld *world, btScalar timeStep) +{ + NN3DWalkers* motorDemo = (NN3DWalkers*)world->getWorldUserInfo(); + + motorDemo->setMotorTargets(timeStep); + +} + + + +void NN3DWalkers::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + // Setup the basic world + + m_Time = 0; + m_fCyclePeriod = 2000.f; // in milliseconds + +// m_fMuscleStrength = 0.05f; + // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor + // should be (numberOfsolverIterations * oldLimits) + // currently solver uses 10 iterations, so: + m_fMuscleStrength = 0.5f; + + createEmptyDynamicsWorld(); + + m_dynamicsWorld->setInternalTickCallback(motorNNPreTickCallback,this,true); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + + // Setup a big ground box + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.))); + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-10,0)); + createRigidBody(btScalar(0.),groundTransform,groundShape); + } + + // Spawn one ragdoll + btVector3 startOffset(1,0.5,0); + spawnWalker(startOffset, false); + startOffset.setValue(-2,0.5,0); + spawnWalker(startOffset, true); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +void NN3DWalkers::spawnWalker(const btVector3& startOffset, bool bFixed) +{ + NNWalker* walker = new NNWalker(m_dynamicsWorld, startOffset, bFixed); + m_walkers.push_back(walker); +} + +//void PreStep() +//{ +// +//} + + + + +void NN3DWalkers::setMotorTargets(btScalar deltaTime) +{ + + float ms = deltaTime*1000000.; + float minFPS = 1000000.f/60.f; + if (ms > minFPS) + ms = minFPS; + + m_Time += ms; + + // + // set per-frame sinusoidal position targets using angular motor (hacky?) + // + for (int r=0; r(m_walkers[r]->GetJoints()[i]); + btScalar fCurAngle = hingeC->getHingeAngle(); + + btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod; + btScalar fTargetAngle = 0.5 * (1 + sin(2 * M_PI * fTargetPercent)); + btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit()); + btScalar fAngleError = fTargetLimitAngle - fCurAngle; + btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms; + hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength); + } + } + + +} + +bool NN3DWalkers::keyboardCallback(int key, int state) +{ + switch (key) + { + case '+': case '=': + m_fCyclePeriod /= 1.1f; + if (m_fCyclePeriod < 1.f) + m_fCyclePeriod = 1.f; + return true; + break; + case '-': case '_': + m_fCyclePeriod *= 1.1f; + return true; + break; + case '[': + m_fMuscleStrength /= 1.1f; + return true; + break; + case ']': + m_fMuscleStrength *= 1.1f; + return true; + break; + default: + break; + } + + return false; +} + + + +void NN3DWalkers::exitPhysics() +{ + + int i; + + for (i=0;i