diff --git a/examples/CommonInterfaces/CommonTimeWarpBase.h b/examples/CommonInterfaces/CommonTimeWarpBase.h new file mode 100755 index 000000000..dde06b417 --- /dev/null +++ b/examples/CommonInterfaces/CommonTimeWarpBase.h @@ -0,0 +1,903 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2015 Google Inc. http://bulletphysics.org + + 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. + */ + +#ifndef COMMON_TIME_WARP_BASE_H +#define COMMON_TIME_WARP_BASE_H + +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btQuickprof.h" // Use your own timer, this timer is only used as we lack another timer + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + +//Solvers +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" + +#include "../Utils/b3ERPCFMHelper.hpp" // ERP/CFM setting utils + +static btScalar gSimulationSpeed = 1; // default simulation speed at startup + +// the current simulation speeds to choose from (the slider will snap to those using a custom form of snapping) +namespace SimulationSpeeds { +static double/*0*/PAUSE = 0; +static double/*1*/QUARTER_SPEED = 0.25; +static double/*2*/HALF_SPEED = 0.5; +static double/*3*/NORMAL_SPEED = 1; +static double/*4*/DOUBLE_SPEED = 2; +static double/*5*/QUADRUPLE_SPEED = 4; +static double/*6*/DECUPLE_SPEED = 10; +static double/*7*/CENTUPLE_SPEED = 100; +static double/*8*/QUINCENTUPLE_SPEED = 500; +static double /*9*/ MILLITUPLE_SPEED = 1000; +static double/*0*/MAX_SPEED = MILLITUPLE_SPEED; +static double /**/NUM_SPEEDS = 11; +}; + +// add speeds from the namespace here +static double speeds[] = { + SimulationSpeeds::PAUSE, + SimulationSpeeds::QUARTER_SPEED, SimulationSpeeds::HALF_SPEED, + SimulationSpeeds::NORMAL_SPEED, SimulationSpeeds::DOUBLE_SPEED, + SimulationSpeeds::QUADRUPLE_SPEED, SimulationSpeeds::DECUPLE_SPEED, + SimulationSpeeds::CENTUPLE_SPEED, SimulationSpeeds::QUINCENTUPLE_SPEED, + SimulationSpeeds::MILLITUPLE_SPEED}; + +static btScalar gSolverIterations = 10; // default number of solver iterations for the iterative solvers + +static bool gIsHeadless = false; // demo runs with graphics by default + +static bool gChangeErpCfm = false; // flag to make recalculation of ERP/CFM + +static int gMinSpeed = SimulationSpeeds::PAUSE; // the minimum simulation speed + +static int gMaxSpeed = SimulationSpeeds::MAX_SPEED; // the maximum simulation speed + +static bool gMaximumSpeed = false; // the demo does not try to achieve maximum stepping speed by default + +static bool gInterpolate = false; // the demo does not use any bullet interpolated physics substeps + +static bool useSplitImpulse = true; // split impulse fixes issues with restitution in Baumgarte stabilization +// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=7117&p=24631&hilit=Baumgarte#p24631 +// disabling continuous collision detection can also fix issues with restitution, though CCD is disabled by default an only kicks in at higher speeds +// set CCD speed threshold and testing sphere radius per rigidbody (rb->setCCDSpeedThreshold()) + +// all supported solvers by bullet +enum SolverEnumType { + SEQUENTIALIMPULSESOLVER = 0, + GAUSSSEIDELSOLVER = 1, + NNCGSOLVER = 2, + DANZIGSOLVER = 3, + LEMKESOLVER = 4, + FSSOLVER = 5, + NUM_SOLVERS = 6 +}; + + +// solvers can be changed by drop down menu +namespace SolverType { +static char SEQUENTIALIMPULSESOLVER[] = "Sequential Impulse Solver"; +static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver"; +static char NNCGSOLVER[] = "NNCG Solver"; +static char DANZIGSOLVER[] = "Danzig Solver"; +static char LEMKESOLVER[] = "Lemke Solver"; +static char FSSOLVER[] = "FeatherStone Solver"; +}; + +static const char* solverTypes[NUM_SOLVERS]; + +static SolverEnumType SOLVER_TYPE = SEQUENTIALIMPULSESOLVER; // You can switch the solver here + +//TODO:s=== +//TODO: Give specific explanations about solver values + +/** + * Step size of the bullet physics simulator (solverAccuracy). Accuracy versus speed. + */ +// Choose an appropriate number of steps per second for your needs +static btScalar gPhysicsStepsPerSecond = 60.0f; // Default number of steps +//static btScalar gPhysicsStepsPerSecond = 120.0f; // Double steps for more accuracy +//static btScalar gPhysicsStepsPerSecond = 240.0f; // For high accuracy +//static btScalar gPhysicsStepsPerSecond = 1000.0f; // Very high accuracy + +// appropriate inverses for seconds and milliseconds +static double fixedPhysicsStepSizeSec = 1.0f / gPhysicsStepsPerSecond; // steps size in seconds +static double fixedPhysicsStepSizeMilli = 1000.0f / gPhysicsStepsPerSecond; // step size in milliseconds + +static btScalar gApplicationFrequency = 60.0f; // number of internal application ticks per second +static int gApplicationTick = 1000.0f / gApplicationFrequency; //ms + +static btScalar gFramesPerSecond = 30.0f; // number of frames per second + +static btScalar gERPSpringK = 10; +static btScalar gERPDamperC = 1; + +static btScalar gCFMSpringK = 10; +static btScalar gCFMDamperC = 1; +static btScalar gCFMSingularityAvoidance = 0; + +//GUI related parameter changing helpers + +inline void floorSliderValues(float notUsed) { // floor values that should be ints + gSolverIterations = floor(gSolverIterations); +} + +inline void twxChangePhysicsStepsPerSecond(float physicsStepsPerSecond) { // function to change simulation physics steps per second + gPhysicsStepsPerSecond = physicsStepsPerSecond; +} + +inline void twxChangeFPS(float framesPerSecond) { + gFramesPerSecond = framesPerSecond; +} + +inline void twxChangeERPCFM(float notUsed) { // function to change ERP/CFM appropriately + gChangeErpCfm = true; +} + +inline void changeSolver(int comboboxId, const char* item, void* userPointer) { // function to change the solver + for(int i = 0; i < NUM_SOLVERS;i++){ + if(strcmp(solverTypes[i], item) == 0){ // if the strings are equal + SOLVER_TYPE = ((SolverEnumType)i); + b3Printf("=%s=\n Reset the simulation by double clicking it in the menu list.",item); + return; + } + } + b3Printf("No Change"); +} + + +inline void twxChangeSolverIterations(float notUsed){ // change the solver iterations + + floorSliderValues(0); // floor the values set by slider + +} + +inline void clampToCustomSpeedNotches(float speed) { // function to clamp to custom speed notches + double minSpeed = 0; + double minSpeedDist = SimulationSpeeds::MAX_SPEED; + for (int i = 0; i < SimulationSpeeds::NUM_SPEEDS; i++) { + double speedDist = (speeds[i]-speed >= 0)?speeds[i]-speed:speed-speeds[i]; // float absolute + + if (minSpeedDist > speedDist) { + minSpeedDist = speedDist; + minSpeed = speeds[i]; + } + } + gSimulationSpeed = minSpeed; +} + +inline void switchInterpolated(int buttonId, bool buttonState, void* userPointer){ // toggle if interpolation steps are taken + gInterpolate=!gInterpolate; +// b3Printf("Interpolate substeps %s", gInterpolate?"on":"off"); +} + +inline void switchHeadless(int buttonId, bool buttonState, void* userPointer){ // toggle if the demo should run headless + gIsHeadless=!gIsHeadless; +// b3Printf("Run headless %s", gIsHeadless?"on":"off"); +} + +inline void switchMaximumSpeed(int buttonId, bool buttonState, void* userPointer){ // toggle it the demo should run as fast as possible +// b3Printf("Run maximum speed %s", gMaximumSpeed?"on":"off"); +} + +inline void setApplicationTick(float frequency){ // set internal application tick + gApplicationTick = 1000.0f/frequency; +} + +/** + * @link: Gaffer on Games - Fix your timestep: http://gafferongames.com/game-physics/fix-your-timestep/ + */ +struct CommonTimeWarpBase: public CommonRigidBodyBase { + + CommonTimeWarpBase(struct GUIHelperInterface* helper): + CommonRigidBodyBase(helper), + mPhysicsStepsPerSecondUpdated(false), + mFramesPerSecondUpdated(false), + mSolverIterationsUpdated(false) { + + // main frame timer initialization + mApplicationStart = mLoopTimer.getTimeMilliseconds(); /**!< Initialize when the application started running */ + mInputClock = mApplicationStart; /**!< Initialize the last time the input was updated */ + mPreviousModelIteration = mApplicationStart; + mThisModelIteration = mApplicationStart; + mApplicationRuntime = mThisModelIteration - mApplicationStart; /**!< Initialize the application runtime */ + + // sub frame time initializations + mGraphicsStart = mApplicationStart; /** !< Initialize the last graphics start */ + mModelStart = mApplicationStart; /** !< Initialize the last model start */ + mInputStart = mApplicationStart; /** !< Initialize the last input start */ + + mPhysicsStepStart = mApplicationStart; /**!< Initialize the physics step start */ + mPhysicsStepEnd = mApplicationStart; /**!< Initialize the physics step end */ + + //durations + mLastGraphicsTick = 0; + mLastModelTick = 0; + mLastInputTick = 0; + mPhysicsTick = 0; + + mInputDt = 0; + mModelAccumulator = 0; + mFrameTime = 0; + + fpsTimeStamp = mLoopTimer.getTimeMilliseconds(); // to time the fps + fpsStep = 1000.0f/gFramesPerSecond; + + // performance measurements for this demo + performanceTimestamp = 0; + performedTime = 0; // time the physics steps consumed + speedUpPrintTimeStamp = mLoopTimer.getTimeSeconds(); // timer to print the speed up periodically + mLoopTimer.reset(); + } + + ~CommonTimeWarpBase(){ + + } + + + void initPhysics(){ // initialize the demo + + setupBasicParamInterface(); // setup adjustable sliders and buttons for parameters + + m_guiHelper->setUpAxis(1); // Set Y axis as Up axis + + createEmptyDynamicsWorld(); // create an empty dynamic world + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + } + + void setupBasicParamInterface(){ // setup the adjustable sliders and button for parameters + + { // create a slider to adjust the simulation speed + // Force increase the simulation speed to run the simulation with the same accuracy but a higher speed + SliderParams slider("Simulation speed", + &gSimulationSpeed); + slider.m_minVal = gMinSpeed; + slider.m_maxVal = gMaxSpeed; + slider.m_callback = clampToCustomSpeedNotches; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a button to switch to headless simulation + // This turns off the graphics update and therefore results in more time for the model update + ButtonParams button("Run headless",0,true); + button.m_callback = switchHeadless; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerButtonParameter( + button); + } + + + + { // create a button to switch to maximum speed simulation (fully deterministic) + // Interesting to test the maximal achievable speed on this hardware + ButtonParams button("Run maximum speed",0,true); + button.m_callback = switchMaximumSpeed; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerButtonParameter( + button); + } + + + + { // create a button to switch bullet to perform interpolated substeps to speed up simulation + // generally, interpolated steps are a good speed-up and should only be avoided if higher accuracy is needed (research purposes etc.) + ButtonParams button("Perform interpolated substeps",0,true); + button.m_callback = switchInterpolated; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerButtonParameter( + button); + } + + } + + void setupAdvancedParamInterface(){ + + solverTypes[0] = SolverType::SEQUENTIALIMPULSESOLVER; + solverTypes[1] = SolverType::GAUSSSEIDELSOLVER; + solverTypes[2] = SolverType::NNCGSOLVER; + solverTypes[3] = SolverType::DANZIGSOLVER; + solverTypes[4] = SolverType::LEMKESOLVER; + solverTypes[5] = SolverType::FSSOLVER; + + { + ComboBoxParams comboParams; + comboParams.m_comboboxId = 0; + comboParams.m_numItems = NUM_SOLVERS; + comboParams.m_startItem = SOLVER_TYPE; + comboParams.m_callback = changeSolver; + + comboParams.m_items=solverTypes; + m_guiHelper->getParameterInterface()->registerComboBox(comboParams); + } + + { // create a slider to adjust the number of internal application ticks + // The set application tick should contain enough time to perform a full cycle of model update (physics and input) + // and view update (graphics) with average application load. The graphics and input update determine the remaining time + // for the physics update + SliderParams slider("Application Ticks", + &gApplicationFrequency); + slider.m_minVal = gMinSpeed; + slider.m_maxVal = gMaxSpeed; + slider.m_callback = setApplicationTick; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust the number of physics steps per second + // The default number of steps is at 60, which is appropriate for most general simulations + // For simulations with higher complexity or if you experience undesired behavior, try increasing the number of steps per second + // Alternatively, try increasing the number of solver iterations if you experience jittering constraints due to non-converging solutions + SliderParams slider("Physics steps per second", &gPhysicsStepsPerSecond); + slider.m_minVal = 0; + slider.m_maxVal = 1000; + slider.m_callback = twxChangePhysicsStepsPerSecond; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust the number of frames per second + SliderParams slider("Frames per second", &gFramesPerSecond); + slider.m_minVal = 0; + slider.m_maxVal = 200; + slider.m_callback = twxChangeFPS; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust the number of solver iterations to converge to a solution + // more complex simulations might need a higher number of iterations to converge, it also + // depends on the type of solver. + SliderParams slider( + "Solver interations", + &gSolverIterations); + slider.m_minVal = 0; + slider.m_maxVal = 1000; + slider.m_callback = twxChangePhysicsStepsPerSecond; + slider.m_clampToNotches = false; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + // ERP/CFM sliders + // Advanced users: Check descriptions of ERP/CFM in BulletUtils.cpp + + { // create a slider to adjust ERP Spring k constant + SliderParams slider("Global ERP Spring k (F=k*x)", &gERPSpringK); + slider.m_minVal = 0; + slider.m_maxVal = 10; + slider.m_callback = twxChangeERPCFM; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust ERP damper c constant + SliderParams slider("Global ERP damper c (F=c*xdot)", &gERPDamperC); + slider.m_minVal = 0; + slider.m_maxVal = 10; + slider.m_callback = twxChangeERPCFM; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust CFM Spring k constant + SliderParams slider("Global CFM Spring k (F=k*x)", &gCFMSpringK); + slider.m_minVal = 0; + slider.m_maxVal = 10; + slider.m_callback = twxChangeERPCFM; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust CFM damper c constant + SliderParams slider("Global CFM damper c (F=c*xdot)", &gCFMDamperC); + slider.m_minVal = 0; + slider.m_maxVal = 10; + slider.m_callback = twxChangeERPCFM; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + { // create a slider to adjust CFM damper c constant + SliderParams slider("Global CFM singularity avoidance", &gCFMSingularityAvoidance); + slider.m_minVal = 0; + slider.m_maxVal = 10; + slider.m_callback = twxChangeERPCFM; + slider.m_clampToNotches = false; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + + } + + void createEmptyDynamicsWorld(){ // create an empty dynamics worlds according to the chosen settings via statics (top section of code) + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + //m_collisionConfiguration->setConvexConvexMultipointIterations(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + // default broadphase + m_broadphase = new btDbvtBroadphase(); + + // different solvers require different settings + switch (SOLVER_TYPE) { + case SEQUENTIALIMPULSESOLVER: { +// b3Printf("=%s=",SolverType::SEQUENTIALIMPULSESOLVER); + m_solver = new btSequentialImpulseConstraintSolver(); + break; + } + case NNCGSOLVER: { +// b3Printf("=%s=",SolverType::NNCGSOLVER); + m_solver = new btNNCGConstraintSolver(); + break; + } + case DANZIGSOLVER: { +// b3Printf("=%s=",SolverType::DANZIGSOLVER); + btDantzigSolver* mlcp = new btDantzigSolver(); + m_solver = new btMLCPSolver(mlcp); + break; + } + case GAUSSSEIDELSOLVER: { +// b3Printf("=%s=",SolverType::GAUSSSEIDELSOLVER); + btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel(); + m_solver = new btMLCPSolver(mlcp); + break; + } + case LEMKESOLVER: { +// b3Printf("=%s=",SolverType::LEMKESOLVER); + btLemkeSolver* mlcp = new btLemkeSolver(); + m_solver = new btMLCPSolver(mlcp); + break; + } + case FSSOLVER: { +// b3Printf("=%s=",SolverType::FSSOLVER); + //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support + m_solver = new btMultiBodyConstraintSolver; + + break; + } + default: + break; + } + + if (SOLVER_TYPE != FSSOLVER) { + //TODO: Set parameters for other solvers + + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, + m_broadphase, m_solver, m_collisionConfiguration); + + if (SOLVER_TYPE == DANZIGSOLVER || SOLVER_TYPE == GAUSSSEIDELSOLVER) { + m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 1; //for mlcp solver it is better to have a small A matrix + } else { + m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128; //for direct solver, it is better to solve multiple objects together, small batches have high overhead + } + + m_dynamicsWorld->getDispatchInfo().m_useContinuous = true; // set continuous collision + + } + else{ + //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support + m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, + m_broadphase, (btMultiBodyConstraintSolver*) m_solver, + m_collisionConfiguration); + } + + changeERPCFM(); // set appropriate ERP/CFM values according to the string and damper properties of the constraint + + if (useSplitImpulse) { // If you experience strong repulsion forces in your constraints, it might help to enable the split impulse feature + m_dynamicsWorld->getSolverInfo().m_splitImpulse = 1; //enable split impulse feature + // m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = + // -0.02; + // m_dynamicsWorld->getSolverInfo().m_erp2 = BulletUtils::getERP( + // fixedPhysicsStepSizeSec, 10, 1); + // m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp = + // BulletUtils::getERP(fixedPhysicsStepSizeSec, 10, 1); +// b3Printf("Using split impulse feature with ERP/TurnERP: (%f,%f)", +// m_dynamicsWorld->getSolverInfo().m_erp2, +// m_dynamicsWorld->getSolverInfo().m_splitImpulseTurnErp); + } + + m_dynamicsWorld->getSolverInfo().m_numIterations = gSolverIterations; // set the number of solver iterations for iteration based solvers + + m_dynamicsWorld->setGravity(btVector3(0, -9.81f, 0)); // set gravity to -9.81 + + } + + btScalar calculatePerformedSpeedup() { // calculate performed speedup + // we calculate the performed speed up + btScalar speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp)); +// b3Printf("Avg Effective speedup: %f",speedUp); + performedTime = 0; + performanceTimestamp = mLoopTimer.getTimeMilliseconds(); + return speedUp; + } + + + + void timeWarpSimulation(float deltaTime) // Override this + { + + } + + void stepSimulation(float deltaTime){ // customly step the simulation + do{ + +// // settings + if(mPhysicsStepsPerSecondUpdated){ + changePhysicsStepsPerSecond(gPhysicsStepsPerSecond); + mPhysicsStepsPerSecondUpdated = false; + } + + if(mFramesPerSecondUpdated){ + changeFPS(gFramesPerSecond); + mFramesPerSecondUpdated = false; + } + + if(gChangeErpCfm){ + changeERPCFM(); + gChangeErpCfm = false; + } + + if(mSolverIterationsUpdated){ + changeSolverIterations(gSolverIterations); + mSolverIterationsUpdated = false; + } + + + // structure according to the canonical game loop + // http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Canonical_Game_Loop + + //############## + // breaking conditions - if the loop should stop, then check it here + + //############# + // model update - here you perform updates of your model, be it the physics model, the game or simulation state or anything not related to graphics and input + + timeWarpSimulation(deltaTime); + if(mLoopTimer.getTimeSeconds() - speedUpPrintTimeStamp > 1){ + // on reset, we calculate the performed speed up + double speedUp = ((double)performedTime*1000.0)/((double)(mLoopTimer.getTimeMilliseconds()-performanceTimestamp)); +// b3Printf("Avg Effective speedup: %f",speedUp); + performedTime = 0; + performanceTimestamp = mLoopTimer.getTimeMilliseconds(); + speedUpPrintTimeStamp = mLoopTimer.getTimeSeconds(); + } + + // update timers + mThisModelIteration = mLoopTimer.getTimeMilliseconds(); + mFrameTime = mThisModelIteration - mPreviousModelIteration; /**!< Calculate the frame time (in Milliseconds) */ + mPreviousModelIteration = mThisModelIteration; + + // b3Printf("Current Frame time: % u", mFrameTime); + + mApplicationRuntime = mThisModelIteration - mApplicationStart; /**!< Update main frame timer (in Milliseconds) */ + + mModelStart = mLoopTimer.getTimeMilliseconds(); /**!< Begin with the model update (in Milliseconds)*/ + mLastGraphicsTick = mModelStart - mGraphicsStart; /**!< Update graphics timer (in Milliseconds) */ + + if (gMaximumSpeed /** If maximum speed is enabled*/) { + performMaxStep(); + } else { /**!< This mode tries to progress as much time as it is expected from the game loop*/ + performSpeedStep(); + } + + mInputStart = mLoopTimer.getTimeMilliseconds(); /**!< Start the input update */ + mLastModelTick = mInputStart - mModelStart; /**!< Calculate the time the model update took */ + + //############# + // Input update - Game Clock part of the loop + /** This runs once every gApplicationTick milliseconds on average */ + mInputDt = mThisModelIteration - mInputClock; + if (mInputDt >= gApplicationTick) { + mInputClock = mThisModelIteration; + // mInputHandler.injectInput(); /**!< Inject input into handlers */ + // mInputHandler.update(mInputClock); /**!< update elements that work on the current input state */ + } + + mGraphicsStart = mLoopTimer.getTimeMilliseconds(); /**!< Start the graphics update */ + mLastInputTick = mGraphicsStart - mInputStart; /**!< Calculate the time the input injection took */ + + //############# + // Graphics update - Here you perform the representation of your model, meaning graphics rendering according to what your game or simulation model describes + // In the example browser, there is a separate method called renderScene() for this + + // Uncomment this for some detailed output about the application ticks + // b3Printf( + // "Physics time: %u milliseconds / Graphics time: %u milliseconds / Input time: %u milliseconds / Total time passed: %u milliseconds", + // mLastModelTick, mLastGraphicsTick, mLastInputTick, mApplicationRuntime); + + }while(mLoopTimer.getTimeMilliseconds() - fpsTimeStamp < fpsStep); // escape the loop if it is time to render + // Unfortunately, the input is not included in the loop, therefore the input update frequency is equal to the fps + + fpsTimeStamp = mLoopTimer.getTimeMilliseconds(); + + } + + virtual bool keyboardCallback(int key, int state) + { + switch(key) + { + case '1':{ + gSimulationSpeed = SimulationSpeeds::QUARTER_SPEED; + gMaximumSpeed = false; + return true; + } + case '2':{ + gSimulationSpeed = SimulationSpeeds::HALF_SPEED; + gMaximumSpeed = false; + return true; + } + case '3':{ + gSimulationSpeed = SimulationSpeeds::NORMAL_SPEED; + gMaximumSpeed = false; + return true; + } + case '4':{ + gSimulationSpeed = SimulationSpeeds::DOUBLE_SPEED; + gMaximumSpeed = false; + return true; + } + case '5':{ + gSimulationSpeed = SimulationSpeeds::QUADRUPLE_SPEED; + gMaximumSpeed = false; + return true; + } + case '6':{ + gSimulationSpeed = SimulationSpeeds::DECUPLE_SPEED; + gMaximumSpeed = false; + return true; + } + case '7':{ + gSimulationSpeed = SimulationSpeeds::CENTUPLE_SPEED; + gMaximumSpeed = false; + return true; + } + case '8':{ + gSimulationSpeed = SimulationSpeeds::QUINCENTUPLE_SPEED; + gMaximumSpeed = false; + return true; + } + case '9':{ + gSimulationSpeed = SimulationSpeeds::MILLITUPLE_SPEED; + gMaximumSpeed = false; + return true; + } + case '0':{ + gSimulationSpeed = SimulationSpeeds::MAX_SPEED; + gMaximumSpeed = true; + return true; + } + } + return CommonRigidBodyBase::keyboardCallback(key,state); + } + + + void changePhysicsStepsPerSecond(float physicsStepsPerSecond){ // change the simulation accuracy + if (m_dynamicsWorld && physicsStepsPerSecond) { + fixedPhysicsStepSizeSec = 1.0f / physicsStepsPerSecond; + fixedPhysicsStepSizeMilli = 1000.0f / physicsStepsPerSecond; + + changeERPCFM(); + } + } + + void changeERPCFM(){ // Change ERP/CFM appropriately to the timestep and the ERP/CFM parameters above + if(m_dynamicsWorld){ + m_dynamicsWorld->getSolverInfo().m_erp = b3ERPCFMHelper::getERP( // set the error reduction parameter + fixedPhysicsStepSizeSec, // step size per second + gERPSpringK, // k of a spring in the equation F = k * x (x:position) + gERPDamperC); // k of a damper in the equation F = k * v (v:velocity) + + m_dynamicsWorld->getSolverInfo().m_globalCfm = b3ERPCFMHelper::getCFM( // set the constraint force mixing according to the time step + gCFMSingularityAvoidance, // singularity avoidance (if you experience unsolvable constraints, increase this value + fixedPhysicsStepSizeSec, // steps size per second + gCFMSpringK, // k of a spring in the equation F = k * x (x:position) + gCFMDamperC); // k of a damper in the equation F = k * v (v:velocity) + +// b3Printf("Bullet DynamicsWorld ERP: %f", +// m_dynamicsWorld->getSolverInfo().m_erp); + +// b3Printf("Bullet DynamicsWorld CFM: %f", +// m_dynamicsWorld->getSolverInfo().m_globalCfm); + } + } + + void changeSolverIterations(int iterations){ // change the number of iterations + m_dynamicsWorld->getSolverInfo().m_numIterations = iterations; + } + + void changeFPS(float framesPerSecond){ // change the frames per second + fpsStep = 1000.0f / gFramesPerSecond; + } + + void performTrueSteps(btScalar timeStep){ // physics stepping without interpolated substeps + int subSteps = round(timeStep / fixedPhysicsStepSizeSec); /**!< Calculate the number of full normal time steps we can take */ + + for (int i = 0; i < subSteps; i++) { /**!< Perform the number of substeps to reach the timestep*/ + if (timeStep && m_dynamicsWorld) { + // since we want to perform all proper steps, we perform no interpolated substeps + int subSteps = 1; + + m_dynamicsWorld->stepSimulation(btScalar(timeStep), + btScalar(subSteps), btScalar(fixedPhysicsStepSizeSec)); + } + } + } + + void performInterpolatedSteps(btScalar timeStep){ // physics stepping with interpolated substeps + int subSteps = 1 + round(timeStep / fixedPhysicsStepSizeSec); /**!< Calculate the number of full normal time steps we can take, plus 1 for safety of not losing time */ + if (timeStep && m_dynamicsWorld) { + + m_dynamicsWorld->stepSimulation(btScalar(timeStep), btScalar(subSteps), + btScalar(fixedPhysicsStepSizeSec)); /**!< Perform the number of substeps to reach the timestep*/ + } + } + + void performMaxStep(){ // perform as many steps as possible + if(gApplicationTick >= mLastGraphicsTick + mLastInputTick){ // if the remaining time for graphics is going to be positive + mPhysicsTick = gApplicationTick /**!< calculate the remaining time for physics (in Milliseconds) */ + - mLastGraphicsTick - mLastInputTick; + } + else{ + mPhysicsTick = 0; // no time for physics left / The internal application step is too high + } + + // b3Printf("Application tick: %u",gApplicationTick); + // b3Printf("Graphics tick: %u",mLastGraphicsTick); + // b3Printf("Input tick: %u",mLastInputTick); + // b3Printf("Physics tick: %u",mPhysicsTick); + + if (mPhysicsTick > 0) { // with positive physics tick we perform as many update steps until the time for it is used up + + mPhysicsStepStart = mLoopTimer.getTimeMilliseconds(); /**!< The physics updates start (in Milliseconds)*/ + mPhysicsStepEnd = mPhysicsStepStart; + + while (mPhysicsTick > mPhysicsStepEnd - mPhysicsStepStart) { /**!< Update the physics until we run out of time (in Milliseconds) */ + // b3Printf("Physics passed: %u", mPhysicsStepEnd - mPhysicsStepStart); + double timeStep = fixedPhysicsStepSizeSec; /**!< update the world (in Seconds) */ + + if (gInterpolate) { + performInterpolatedSteps(timeStep); + } else { + performTrueSteps(timeStep); + } + performedTime += timeStep; + mPhysicsStepEnd = mLoopTimer.getTimeMilliseconds(); /**!< Update the last physics step end to stop updating in time (in Milliseconds) */ + } + } + } + + + void performSpeedStep(){ // force-perform the number of steps needed to achieve a certain speed (safe to too high speeds, meaning the application will lose time, not the physics) + if (mFrameTime > gApplicationTick) { /** cap frametime to make the application lose time, not the physics (in Milliseconds) */ + mFrameTime = gApplicationTick; // This prevents the physics time accumulator to sum up too much time + } // The simulation therefore gets slower, but still performs all requested physics steps + + mModelAccumulator += mFrameTime; /**!< Accumulate the time the physics simulation has to perform in order to stay in real-time (in Milliseconds) */ + // b3Printf("Model time accumulator: %u", mModelAccumulator); + + int steps = floor(mModelAccumulator / fixedPhysicsStepSizeMilli); /**!< Calculate the number of time steps we can take */ + // b3Printf("Next steps: %i", steps); + + if (steps > 0) { /**!< Update if we can take at least one step */ + + double timeStep = gSimulationSpeed * steps * fixedPhysicsStepSizeSec; /**!< update the universe (in Seconds) */ + + if (gInterpolate) { + performInterpolatedSteps(timeStep); // perform interpolated steps + } else { + performTrueSteps(timeStep); // perform full steps + } + performedTime += timeStep; // sum up the performed time for measuring the speed up + mModelAccumulator -= steps * fixedPhysicsStepSizeMilli; /**!< Remove the time performed by the physics simulation from the accumulator, the remaining time carries over to the next cycle (in Milliseconds) */ + } + } + + void renderScene() { // render the scene + if(!gIsHeadless){ // while the simulation is not running headlessly, render to screen + CommonRigidBodyBase::renderScene(); + + if(m_dynamicsWorld->getDebugDrawer()){ + debugDraw(m_dynamicsWorld->getDebugDrawer()->getDebugMode()); + } + } + mIsHeadless = gIsHeadless; + } + void resetCamera() { // reset the camera to its original position + float dist = 41; + 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]); + } + + // loop timing components ################### + //# loop timestamps + btClock mLoopTimer; /**!< The loop timer to time the loop correctly */ + unsigned long int mApplicationStart; /**!< The time the application was started (absolute, in Milliseconds) */ + unsigned long int mPreviousModelIteration; /**!< The previous model iteration timestamp (absolute, in Milliseconds) */ + unsigned long int mThisModelIteration; /**!< This model iteration timestamp (absolute, in Milliseconds) */ + + //# loop durations + long int mModelAccumulator; /**!< The time to forward the model in this loop iteration (relative, in Milliseconds) */ + unsigned long int mFrameTime; /**!< The time to render a frame (relative, in Milliseconds) */ + unsigned long int mApplicationRuntime; /**!< The total application runtime (relative, in Milliseconds) */ + + long int mInputDt; /**!< The time difference of input that has to be fed in */ + unsigned long int mInputClock; + + long int mLastGraphicsTick; /*!< The time it took the graphics rendering last time (relative, in Milliseconds) */ + unsigned long int mGraphicsStart; + + long int mLastInputTick; /**!< The time it took the input to process last time (relative, in Milliseconds) */ + unsigned long int mInputStart; + + long int mLastModelTick; /**!< The time it took the model to update last time + This includes the bullet physics update */ + unsigned long int mModelStart; /**!< The timestamp the model started updating last (absolute, in Milliseconds)*/ + + long int mPhysicsTick; /**!< The time remaining in the loop to update the physics (relative, in Milliseconds)*/ + unsigned long int mPhysicsStepStart; /**!< The physics start timestamp (absolute, in Milliseconds) */ + unsigned long int mPhysicsStepEnd; /**!< The last physics step end (absolute, in Milliseconds) */ + + // to measure the performance of the demo + double performedTime; + unsigned long int performanceTimestamp; + + unsigned long int speedUpPrintTimeStamp; + + unsigned long int fpsTimeStamp; /**!< FPS timing variables */ + double fpsStep; + + //store old values + bool mPhysicsStepsPerSecondUpdated; + bool mFramesPerSecondUpdated; + bool mSolverIterationsUpdated; + bool mIsHeadless; +}; + +#endif //COMMON_TIME_WARP_BASE_H + diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp new file mode 100755 index 000000000..fb6f01cf4 --- /dev/null +++ b/examples/Evolution/NN3DWalkers.cpp @@ -0,0 +1,1060 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +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" +#include "LinearMath/btHashMap.h" +class btBroadphaseInterface; +class btCollisionShape; +class btOverlappingPairCache; +class btCollisionDispatcher; +class btConstraintSolver; +struct btCollisionAlgorithmCreateFunc; +class btDefaultCollisionConfiguration; +class NNWalker; + +#include "../CommonInterfaces/CommonTimeWarpBase.h" +#include "../CommonInterfaces/CommonParameterInterface.h" + +#include "../Utils/b3ReferenceFrameHelper.hpp" +#include "../RenderingExamples/TimeSeriesCanvas.h" + +static btScalar gRootBodyRadius = 0.25f; +static btScalar gRootBodyHeight = 0.1f; +static btScalar gLegRadius = 0.1f; +static btScalar gLegLength = 0.45f; +static btScalar gForeLegLength = 0.75f; +static btScalar gForeLegRadius = 0.08f; + +static btScalar gParallelEvaluations = 10.0f; + +#ifndef SIMD_PI_4 +#define SIMD_PI_4 0.5 * SIMD_HALF_PI +#endif + +#ifndef SIMD_PI_8 +#define SIMD_PI_8 0.25 * SIMD_HALF_PI +#endif + +#ifndef RANDOM_MOVEMENT +#define RANDOM_MOVEMENT false +#endif + +#ifndef RANDOMIZE_DIMENSIONS +#define RANDOMIZE_DIMENSIONS false +#endif + +#ifndef NUM_WALKERS +#define NUM_WALKERS 50 +#endif + +#ifndef EVALUATION_TIME +#define EVALUATION_TIME 10 // s +#endif + +#ifndef REAP_QTY +#define REAP_QTY 0.3f // number of walkers reaped based on their bad performance +#endif + +#ifndef SOW_CROSSOVER_QTY +#define SOW_CROSSOVER_QTY 0.2f // this means REAP_QTY-SOW_CROSSOVER_QTY = NEW_RANDOM_BREED_QTY +#endif + +#ifndef SOW_ELITE_QTY +#define SOW_ELITE_QTY 0.2f // number of walkers kept using an elitist strategy +#endif + +#ifndef SOW_MUTATION_QTY +#define SOW_MUTATION_QTY 0.5f // SOW_ELITE_QTY + SOW_MUTATION_QTY + REAP_QTY = 1 +#endif + +#ifndef MUTATION_RATE +#define MUTATION_RATE 0.5f // the mutation rate of for the walker with the worst performance +#endif + +#ifndef SOW_ELITE_PARTNER +#define SOW_ELITE_PARTNER 0.8f +#endif + +#define NUM_LEGS 6 +#define BODYPART_COUNT (2 * NUM_LEGS + 1) +#define JOINT_COUNT (BODYPART_COUNT - 1) +#define DRAW_INTERPENETRATIONS false + +void* GROUND_ID = (void*)1; + +class NN3DWalkersExample : public CommonTimeWarpBase +{ + btScalar m_Time; + btScalar m_SpeedupTimestamp; + btScalar m_targetAccumulator; + btScalar m_targetFrequency; + btScalar m_motorStrength; + int m_evaluationsQty; + int m_nextReaped; + + btAlignedObjectArray m_walkersInPopulation; + + TimeSeriesCanvas* m_timeSeriesCanvas; + +public: + NN3DWalkersExample(struct GUIHelperInterface* helper) + :CommonTimeWarpBase(helper), + m_Time(0), + m_SpeedupTimestamp(0), + m_motorStrength(0.5f), + m_targetFrequency(3), + m_targetAccumulator(0), + m_evaluationsQty(0), + m_nextReaped(0), + m_timeSeriesCanvas(0) + { + } + + virtual ~NN3DWalkersExample() + { + } + + void initPhysics(); + + virtual void exitPhysics(); + + void spawnWalker(int index, const btVector3& startOffset, bool bFixed); + + virtual bool keyboardCallback(int key, int state); + + bool detectCollisions(); + + 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]); + } + + // Evaluation + + void update(const btScalar timeSinceLastTick); + + void updateEvaluations(const btScalar timeSinceLastTick); + + void scheduleEvaluations(); + + void drawMarkings(); + + // Reaper + + void rateEvaluations(); + + void reap(); + + void sow(); + + void crossover(NNWalker* mother, NNWalker* father, NNWalker* offspring); + + void mutate(NNWalker* mutant, btScalar mutationRate); + + NNWalker* getRandomElite(); + + NNWalker* getRandomNonElite(); + + NNWalker* getNextReaped(); + + void printWalkerConfigs(); + +}; + +static NN3DWalkersExample* nn3DWalkers = NULL; + +class NNWalker +{ + btDynamicsWorld* m_ownerWorld; + btCollisionShape* m_shapes[BODYPART_COUNT]; + btRigidBody* m_bodies[BODYPART_COUNT]; + btTransform m_bodyRelativeTransforms[BODYPART_COUNT]; + btTypedConstraint* m_joints[JOINT_COUNT]; + btHashMap m_bodyTouchSensorIndexMap; + bool m_touchSensors[BODYPART_COUNT]; + btScalar m_sensoryMotorWeights[BODYPART_COUNT*JOINT_COUNT]; + + bool m_inEvaluation; + btScalar m_evaluationTime; + bool m_reaped; + btVector3 m_startPosition; + int m_index; + + 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* motionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionState,shape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + return body; + } + + +public: + + 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; + } + } + } + + NNWalker(int index, btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed) + : m_ownerWorld (ownerWorld), + m_inEvaluation(false), + m_evaluationTime(0), + m_reaped(false) + { + m_index = index; + btVector3 vUp(0, 1, 0); // up in local reference frame + + NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)m_ownerWorld->getWorldUserInfo(); + + randomizeSensoryMotorWeights(); + + // + // Setup geometry + m_shapes[0] = new btCapsuleShape(gRootBodyRadius, gRootBodyHeight); // root body capsule + int i; + for ( i=0; iaddRigidBody(m_bodies[0]); + m_bodyRelativeTransforms[0] = btTransform::getIdentity(); + m_bodies[0]->setUserPointer(this); + m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[0]), 0); + + btHingeConstraint* hingeC; + //btConeTwistConstraint* coneC; + + btTransform localA, localB, localC; + + // legs + for (i = 0; i < NUM_LEGS; i++) + { + float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body + float footYUnitPosition = sin(footAngle); // y position of the leg on the unit circle + float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle + + transform.setIdentity(); + btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+0.5*gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition*(gRootBodyRadius+0.5*gLegLength))); + transform.setOrigin(legCOM); + + // thigh + btVector3 legDirection = (legCOM - localRootBodyPosition).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]); + m_bodyRelativeTransforms[1+2*i] = transform; + m_bodies[1+2*i]->setUserPointer(this); + m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[1+2*i]),1+2*i); + + // shin + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(rootAboveGroundHeight-0.5*gForeLegLength), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength)))); + 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); + m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[2+2*i]),2+2*i); + + // hip joints + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,-footAngle,0); localA.setOrigin(btVector3(btScalar(footXUnitPosition*gRootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*gRootBodyRadius))); + 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*(gRootBodyRadius+gLegLength)), btScalar(0.), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength)))); + 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; + + m_ownerWorld->addRigidBody(m_bodies[1+2*i]); // add thigh bone + + m_ownerWorld->addConstraint(m_joints[2*i], true); // connect thigh bone with root + + if(nnWalkersDemo->detectCollisions()){ // if thigh bone causes collision, remove it again + m_ownerWorld->removeRigidBody(m_bodies[1+2*i]); + m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root + } + else{ + + m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone + m_ownerWorld->addConstraint(m_joints[1+2*i], true); // connect shin bone with thigh + + if(nnWalkersDemo->detectCollisions()){ // if shin bone causes collision, remove it again + m_ownerWorld->removeRigidBody(m_bodies[2+2*i]); + m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh + } + } + } + + // 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); + } + + removeFromWorld(); // it should not yet be in the world + } + + 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 setTouchSensor(void* bodyPointer){ + m_touchSensors[*m_bodyTouchSensorIndexMap.find(btHashPtr(bodyPointer))] = true; + } + + void clearTouchSensors(){ + for(int i = 0 ; i < BODYPART_COUNT;i++){ + m_touchSensors[i] = false; + } + } + + bool getTouchSensor(int i){ + return m_touchSensors[i]; + } + + btScalar* getSensoryMotorWeights() { + return m_sensoryMotorWeights; + } + + void addToWorld() { + int i; + // add all bodies and shapes + for ( i = 0; i < BODYPART_COUNT; ++i) + { + m_ownerWorld->addRigidBody(m_bodies[i]); + } + + // add all constraints + for ( i = 0; i < JOINT_COUNT; ++i) + { + m_ownerWorld->addConstraint(m_joints[i], true); // important! If you add constraints back, you must set bullet physics to disable collision between constrained bodies + } + m_startPosition = getPosition(); + } + + void removeFromWorld(){ + int i; + + // Remove all constraints + for ( i = 0; i < JOINT_COUNT; ++i) + { + m_ownerWorld->removeConstraint(m_joints[i]); + } + + // Remove all bodies + for ( i = 0; i < BODYPART_COUNT; ++i) + { + m_ownerWorld->removeRigidBody(m_bodies[i]); + } + } + + btVector3 getPosition() const { + btVector3 finalPosition(0,0,0); + + for(int i = 0; i < BODYPART_COUNT;i++) + { + finalPosition += m_bodies[i]->getCenterOfMassPosition(); + } + + finalPosition /= BODYPART_COUNT; + return finalPosition; + } + + btScalar getDistanceFitness() const + { + btScalar distance = 0; + + distance = (getPosition() - m_startPosition).length2(); + + return distance; + } + + btScalar getFitness() const + { + return getDistanceFitness(); // for now it is only distance + } + + void resetAt(const btVector3& position) { + btTransform resetPosition(btQuaternion::getIdentity(), position); + for (int i = 0; i < BODYPART_COUNT; ++i) + { + m_bodies[i]->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]); + if(m_bodies[i]->getMotionState()){ + m_bodies[i]->getMotionState()->setWorldTransform(resetPosition*m_bodyRelativeTransforms[i]); + } + m_bodies[i]->clearForces(); + m_bodies[i]->setAngularVelocity(btVector3(0,0,0)); + m_bodies[i]->setLinearVelocity(btVector3(0,0,0)); + + } + + clearTouchSensors(); + } + + btScalar getEvaluationTime() const { + return m_evaluationTime; + } + + void setEvaluationTime(btScalar evaluationTime) { + m_evaluationTime = evaluationTime; + } + + bool isInEvaluation() const { + return m_inEvaluation; + } + + void setInEvaluation(bool inEvaluation) { + m_inEvaluation = inEvaluation; + } + + bool isReaped() const { + return m_reaped; + } + + void setReaped(bool reaped) { + m_reaped = reaped; + } + + int getIndex() const { + return m_index; + } +}; + +void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep); + +bool legContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1) +{ + btCollisionObject* o1 = static_cast(body0); + btCollisionObject* o2 = static_cast(body1); + + void* ID1 = o1->getUserPointer(); + void* ID2 = o2->getUserPointer(); + + if (ID1 != GROUND_ID || ID2 != GROUND_ID) { + // Make a circle with a 0.9 radius at (0,0,0) + // with RGB color (1,0,0). + if(nn3DWalkers->m_dynamicsWorld->getDebugDrawer() != NULL){ + if(!nn3DWalkers->mIsHeadless){ + nn3DWalkers->m_dynamicsWorld->getDebugDrawer()->drawSphere(cp.getPositionWorldOnA(), 0.1, btVector3(1., 0., 0.)); + } + } + + if(ID1 != GROUND_ID && ID1){ + ((NNWalker*)ID1)->setTouchSensor(o1); + } + + if(ID2 != GROUND_ID && ID2){ + ((NNWalker*)ID2)->setTouchSensor(o2); + } + } + return false; +} + +struct WalkerFilterCallback : public btOverlapFilterCallback +{ + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const + { + btCollisionObject* obj0 = static_cast(proxy0->m_clientObject); + btCollisionObject* obj1 = static_cast(proxy1->m_clientObject); + + if (obj0->getUserPointer() == GROUND_ID || obj1->getUserPointer() == GROUND_ID) { // everything collides with ground + return true; + } + else{ + return ((NNWalker*)obj0->getUserPointer())->getIndex() == ((NNWalker*)obj1->getUserPointer())->getIndex(); + } + } +}; + +void floorNNSliderValue(float notUsed) { + gParallelEvaluations = floor(gParallelEvaluations); +} + +void NN3DWalkersExample::initPhysics() +{ + + setupBasicParamInterface(); // parameter interface to use timewarp + + gContactProcessedCallback = legContactProcessedCallback; + + m_guiHelper->setUpAxis(1); + + // Setup the basic world + + m_Time = 0; + + createEmptyDynamicsWorld(); + + m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + m_targetFrequency = 3; + + // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor + // should be (numberOfsolverIterations * oldLimits) + m_motorStrength = 0.05f * m_dynamicsWorld->getSolverInfo().m_numIterations; + + + { // create a slider to change the motor update frequency + SliderParams slider("Motor update frequency", &m_targetFrequency); + 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", &m_motorStrength); + 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); + } + + { // create a slider to change the number of parallel evaluations + SliderParams slider("Parallel evaluations", &gParallelEvaluations); + slider.m_minVal = 1; + slider.m_maxVal = NUM_WALKERS; + slider.m_clampToNotches = false; + slider.m_callback = floorNNSliderValue; // hack to get integer values + m_guiHelper->getParameterInterface()->registerSliderFloatParameter( + slider); + } + + + // 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)); + btRigidBody* ground = createRigidBody(btScalar(0.),groundTransform,groundShape); + ground->setFriction(5); + ground->setUserPointer(GROUND_ID); + } + + for(int i = 0; i < NUM_WALKERS ; i++){ + if(RANDOMIZE_DIMENSIONS){ + float maxDimension = 0.2f; + + // randomize the dimensions + gRootBodyRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f; + gRootBodyHeight = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f; + gLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f; + gLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f; + gForeLegLength = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f; + gForeLegRadius = ((double) rand() / (RAND_MAX)) * (maxDimension-0.01f) + 0.01f; + } + + // Spawn one walker + btVector3 offset(0,0,0); + spawnWalker(i, offset, false); + } + + btOverlapFilterCallback * filterCallback = new WalkerFilterCallback(); + m_dynamicsWorld->getPairCache()->setOverlapFilterCallback(filterCallback); + + m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,300,200, "Fitness Performance"); + m_timeSeriesCanvas ->setupTimeSeries(40, NUM_WALKERS*EVALUATION_TIME, 0); + for(int i = 0; i < NUM_WALKERS ; i++){ + m_timeSeriesCanvas->addDataSource(" ", 100*i/NUM_WALKERS,100*(NUM_WALKERS-i)/NUM_WALKERS,100*(i)/NUM_WALKERS); + } +} + + +void NN3DWalkersExample::spawnWalker(int index, const btVector3& startOffset, bool bFixed) +{ + NNWalker* walker = new NNWalker(index, m_dynamicsWorld, startOffset, bFixed); + m_walkersInPopulation.push_back(walker); +} + +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++) + { + btPersistentManifold* contactManifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i); + const btCollisionObject* obA = contactManifold->getBody0(); + const btCollisionObject* obB = contactManifold->getBody1(); + + if(obA->getUserPointer() != GROUND_ID && obB->getUserPointer() != GROUND_ID){ + + int numContacts = contactManifold->getNumContacts(); + for (int j=0;jgetContactPoint(j); + if (pt.getDistance()<0.f) + { + const btVector3& ptA = pt.getPositionWorldOnA(); + const btVector3& ptB = pt.getPositionWorldOnB(); + const btVector3& normalOnB = pt.m_normalWorldOnB; + + if(!DRAW_INTERPENETRATIONS){ + return collisionDetected; + } + + if(m_dynamicsWorld->getDebugDrawer()){ + m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnA(), 0.1, btVector3(0., 0., 1.)); + m_dynamicsWorld->getDebugDrawer()->drawSphere(pt.getPositionWorldOnB(), 0.1, btVector3(0., 0., 1.)); + } + } + } + } + } + + return collisionDetected; +} + +bool NN3DWalkersExample::keyboardCallback(int key, int state) +{ + switch (key) + { + case '[': + m_motorStrength /= 1.1f; + return true; + case ']': + m_motorStrength *= 1.1f; + return true; + case 'l': + printWalkerConfigs(); + return true; + default: + break; + } + + return CommonTimeWarpBase::keyboardCallback(key,state); +} + +void NN3DWalkersExample::exitPhysics() +{ + + gContactProcessedCallback = NULL; // clear contact processed callback on exiting + + int i; + + for (i = 0;i < NUM_WALKERS;i++) + { + NNWalker* walker = m_walkersInPopulation[i]; + delete walker; + } + + CommonRigidBodyBase::exitPhysics(); +} + +class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options) +{ + nn3DWalkers = new NN3DWalkersExample(options.m_guiHelper); + return nn3DWalkers; +} + +bool fitnessComparator (const NNWalker* a, const NNWalker* b) +{ + return a->getFitness() > b->getFitness(); // sort walkers descending +} + +void NN3DWalkersExample::rateEvaluations(){ + + m_walkersInPopulation.quickSort(fitnessComparator); // Sort walkers by fitness + + b3Printf("Best performing walker: %f meters", btSqrt(m_walkersInPopulation[0]->getDistanceFitness())); + + for(int i = 0; i < NUM_WALKERS;i++){ + m_timeSeriesCanvas->insertDataAtCurrentTime(btSqrt(m_walkersInPopulation[i]->getDistanceFitness()),0,true); + } + m_timeSeriesCanvas->nextTick(); + + for(int i = 0; i < NUM_WALKERS;i++){ + m_walkersInPopulation[i]->setEvaluationTime(0); + } + m_nextReaped = 0; +} + +void NN3DWalkersExample::reap() { + int reaped = 0; + for(int i = NUM_WALKERS-1;i >=(NUM_WALKERS-1)*(1-REAP_QTY); i--){ // reap a certain percentage + m_walkersInPopulation[i]->setReaped(true); + reaped++; + b3Printf("%i Walker(s) reaped.",reaped); + } +} + +NNWalker* NN3DWalkersExample::getRandomElite(){ + return m_walkersInPopulation[((NUM_WALKERS-1) * SOW_ELITE_QTY) * (rand()/RAND_MAX)]; +} + +NNWalker* NN3DWalkersExample::getRandomNonElite(){ + return m_walkersInPopulation[(NUM_WALKERS-1) * SOW_ELITE_QTY + (NUM_WALKERS-1) * (1.0f-SOW_ELITE_QTY) * (rand()/RAND_MAX)]; +} + +NNWalker* NN3DWalkersExample::getNextReaped() { + if((NUM_WALKERS-1) - m_nextReaped >= (NUM_WALKERS-1) * (1-REAP_QTY)){ + m_nextReaped++; + } + + if(m_walkersInPopulation[(NUM_WALKERS-1) - m_nextReaped+1]->isReaped()){ + return m_walkersInPopulation[(NUM_WALKERS-1) - m_nextReaped+1]; + } + else{ + return NULL; // we asked for too many + } + +} + +void NN3DWalkersExample::sow() { + int sow = 0; + for(int i = 0; i < NUM_WALKERS * (SOW_CROSSOVER_QTY);i++){ // create number of new crossover creatures + sow++; + b3Printf("%i Walker(s) sown.",sow); + NNWalker* mother = getRandomElite(); // Get elite partner (mother) + NNWalker* father = (SOW_ELITE_PARTNER < rand()/RAND_MAX)?getRandomElite():getRandomNonElite(); //Get elite or random partner (father) + NNWalker* offspring = getNextReaped(); + crossover(mother,father, offspring); + } + + for(int i = NUM_WALKERS*SOW_ELITE_QTY; i < NUM_WALKERS*(SOW_ELITE_QTY+SOW_MUTATION_QTY);i++){ // create mutants + mutate(m_walkersInPopulation[i], btScalar(MUTATION_RATE / (NUM_WALKERS * SOW_MUTATION_QTY) * (i-NUM_WALKERS*SOW_ELITE_QTY))); + } + + for(int i = 0; i < (NUM_WALKERS-1) * (REAP_QTY-SOW_CROSSOVER_QTY);i++){ + sow++; + b3Printf("%i Walker(s) sown.",sow); + NNWalker* reaped = getNextReaped(); + reaped->setReaped(false); + reaped->randomizeSensoryMotorWeights(); + } +} + +void NN3DWalkersExample::crossover(NNWalker* mother, NNWalker* father, NNWalker* child) { + for(int i = 0; i < BODYPART_COUNT*JOINT_COUNT;i++){ + btScalar random = ((double) rand() / (RAND_MAX)); + + if(random >= 0.5f){ + child->getSensoryMotorWeights()[i] = mother->getSensoryMotorWeights()[i]; + } + else + { + child->getSensoryMotorWeights()[i] = father->getSensoryMotorWeights()[i]; + } + } +} + +void NN3DWalkersExample::mutate(NNWalker* mutant, btScalar mutationRate) { + for(int i = 0; i < BODYPART_COUNT*JOINT_COUNT;i++){ + btScalar random = ((double) rand() / (RAND_MAX)); + + if(random >= mutationRate){ + mutant->getSensoryMotorWeights()[i] = ((double) rand() / (RAND_MAX))*2.0f-1.0f; + } + } +} + +void evaluationUpdatePreTickCallback(btDynamicsWorld *world, btScalar timeStep) { + NN3DWalkersExample* nnWalkersDemo = (NN3DWalkersExample*)world->getWorldUserInfo(); + + nnWalkersDemo->update(timeStep); +} + +void NN3DWalkersExample::update(const btScalar timeSinceLastTick) { + updateEvaluations(timeSinceLastTick); /**!< We update all evaluations that are in the loop */ + + scheduleEvaluations(); /**!< Start new evaluations and finish the old ones. */ + + drawMarkings(); /**!< Draw markings on the ground */ + + if(m_Time > m_SpeedupTimestamp + 2.0f){ // print effective speedup + b3Printf("Avg Effective speedup: %f real time",calculatePerformedSpeedup()); + m_SpeedupTimestamp = m_Time; + } +} + +void NN3DWalkersExample::updateEvaluations(const btScalar timeSinceLastTick) { + btScalar delta = timeSinceLastTick; + btScalar minFPS = 1.f/60.f; + if (delta > minFPS){ + delta = minFPS; + } + + m_Time += delta; + + m_targetAccumulator += delta; + + for(int i = 0; i < NUM_WALKERS;i++) // evaluation time passes + { + if(m_walkersInPopulation[i]->isInEvaluation()){ + m_walkersInPopulation[i]->setEvaluationTime(m_walkersInPopulation[i]->getEvaluationTime()+delta); // increase evaluation time + } + } + + if(m_targetAccumulator >= 1.0f /((double)m_targetFrequency)) + { + m_targetAccumulator = 0; + + for (int r=0; risInEvaluation()) + { + for (int i = 0; i < 2*NUM_LEGS; i++) + { + btScalar targetAngle = 0; + btHingeConstraint* hingeC = static_cast(m_walkersInPopulation[r]->getJoints()[i]); + + if(RANDOM_MOVEMENT){ + targetAngle = ((double) rand() / (RAND_MAX)); + } + else{ // neural network movement + + // accumulate sensor inputs with weights + for(int j = 0; j < JOINT_COUNT;j++){ + targetAngle += m_walkersInPopulation[r]->getSensoryMotorWeights()[i+j*BODYPART_COUNT] * m_walkersInPopulation[r]->getTouchSensor(i); + } + + // apply the activation function + targetAngle = (tanh(targetAngle)+1.0f)*0.5f; + } + btScalar targetLimitAngle = hingeC->getLowerLimit() + targetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit()); + btScalar currentAngle = hingeC->getHingeAngle(); + btScalar angleError = targetLimitAngle - currentAngle; + btScalar desiredAngularVel = 0; + if(delta){ + desiredAngularVel = angleError/delta; + } + else{ + desiredAngularVel = angleError/0.0001f; + } + hingeC->enableAngularMotor(true, desiredAngularVel, m_motorStrength); + } + + // clear sensor signals after usage + m_walkersInPopulation[r]->clearTouchSensors(); + } + } + } +} + +void NN3DWalkersExample::scheduleEvaluations() { + for(int i = 0; i < NUM_WALKERS;i++){ + + if(m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() >= EVALUATION_TIME){ /**!< tear down evaluations */ + b3Printf("An evaluation finished at %f s. Distance: %f m", m_Time, btSqrt(m_walkersInPopulation[i]->getDistanceFitness())); + m_walkersInPopulation[i]->setInEvaluation(false); + m_walkersInPopulation[i]->removeFromWorld(); + m_evaluationsQty--; + } + + if(m_evaluationsQty < gParallelEvaluations && !m_walkersInPopulation[i]->isInEvaluation() && m_walkersInPopulation[i]->getEvaluationTime() == 0){ /**!< Setup the new evaluations */ + b3Printf("An evaluation started at %f s.",m_Time); + m_evaluationsQty++; + m_walkersInPopulation[i]->setInEvaluation(true); + + if(m_walkersInPopulation[i]->getEvaluationTime() == 0){ // reset to origin if the evaluation did not yet run + m_walkersInPopulation[i]->resetAt(btVector3(0,0,0)); + } + + m_walkersInPopulation[i]->addToWorld(); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + } + } + + if(m_evaluationsQty == 0){ // if there are no more evaluations possible + rateEvaluations(); // rate evaluations by sorting them based on their fitness + + reap(); // reap worst performing walkers + + sow(); // crossover & mutate and sow new walkers + b3Printf("### A new generation started. ###"); + } +} + +void NN3DWalkersExample::drawMarkings() { + if(!mIsHeadless){ + for(int i = 0; i < NUM_WALKERS;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(),3); + } + } + + for(int i = 2; i < 50; i+=2){ // draw distance circles + if(m_dynamicsWorld->getDebugDrawer()){ + 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); + } + } + } +} + +void NN3DWalkersExample::printWalkerConfigs(){ + char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n + char* runner = configString; + sprintf(runner,"Population configuration:"); + runner +=25; + for(int i = 0;i < NUM_WALKERS;i++) { + runner[0] = '\n'; + runner++; + runner[0] = '['; + runner++; + for(int j = 0; j < BODYPART_COUNT*JOINT_COUNT;j++) { + sprintf(runner,"%.15f", m_walkersInPopulation[i]->getSensoryMotorWeights()[j]); + runner +=15; + if(j + 1 < BODYPART_COUNT*JOINT_COUNT){ + runner[0] = ','; + } + else{ + runner[0] = ']'; + } + runner++; + } + } + runner[0] = '\0'; + b3Printf(configString); +} diff --git a/examples/Evolution/NN3DWalkers.h b/examples/Evolution/NN3DWalkers.h new file mode 100755 index 000000000..59105f94c --- /dev/null +++ b/examples/Evolution/NN3DWalkers.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +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. +*/ + +#ifndef ET_NN_3D_WALKERS_EXAMPLE_H +#define ET_NN_3D_WALKERS_EXAMPLE_H + +class CommonExampleInterface* ET_NN3DWalkersCreateFunc(struct CommonExampleOptions& options); + + +#endif //ET_NN_3D_WALKERS_EXAMPLE_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 39b8c043f..8c18bb845 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -36,6 +36,8 @@ ADD_LIBRARY(BulletExampleBrowserLib ../Utils/b3Clock.h ../Utils/b3ResourcePath.cpp ../Utils/b3ResourcePath.h + ../Utils/b3ERPCFMHelper.hpp + ../Utils/b3ReferenceFrameHelper.hpp ${GwenGUISupport_SRCS} ${GwenGUISupport_HDRS} @@ -108,20 +110,30 @@ ENDIF(WIN32) SET(ExtendedTutorialsSources - ../ExtendedTutorials/SimpleBox.cpp - ../ExtendedTutorials/MultipleBoxes.cpp - ../ExtendedTutorials/SimpleJoint.cpp - ../ExtendedTutorials/SimpleCloth.cpp ../ExtendedTutorials/Chain.cpp - ../ExtendedTutorials/Bridge.cpp + ../ExtendedTutorials/Chain.h + ../ExtendedTutorials/Bridge.cpp + ../ExtendedTutorials/Bridge.h ../ExtendedTutorials/RigidBodyFromObj.cpp + ../ExtendedTutorials/RigidBodyFromObj.h + ../ExtendedTutorials/SimpleBox.cpp + ../ExtendedTutorials/SimpleBox.h + ../ExtendedTutorials/MultipleBoxes.cpp + ../ExtendedTutorials/MultipleBoxes.h + ../ExtendedTutorials/SimpleCloth.cpp + ../ExtendedTutorials/SimpleCloth.h + ../ExtendedTutorials/SimpleJoint.cpp + ../ExtendedTutorials/SimpleJoint.h + ../ExtendedTutorials/NewtonsCradle.cpp + ../ExtendedTutorials/NewtonsCradle.h ../ExtendedTutorials/InclinedPlane.cpp ../ExtendedTutorials/InclinedPlane.h - ../ExtendedTutorials/NewtonsCradle.cpp + ../ExtendedTutorials/MultiPendulum.cpp + ../ExtendedTutorials/MultiPendulum.h ) SET(BulletExampleBrowser_SRCS - + ../CommonInterfaces/CommonTimeWarpBase.h #TODO: This is wrong here, where should it go? ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp ../TinyRenderer/tgaimage.cpp @@ -165,22 +177,10 @@ SET(BulletExampleBrowser_SRCS ../Tutorial/Tutorial.h ../Tutorial/Dof6ConstraintTutorial.cpp ../Tutorial/Dof6ConstraintTutorial.h - ../ExtendedTutorials/SimpleBox.cpp - ../ExtendedTutorials/SimpleBox.h - ../ExtendedTutorials/MultipleBoxes.cpp - ../ExtendedTutorials/MultipleBoxes.h - ../ExtendedTutorials/SimpleCloth.cpp - ../ExtendedTutorials/SimpleCloth.h - ../ExtendedTutorials/SimpleJoint.cpp - ../ExtendedTutorials/SimpleJoint.h - ../ExtendedTutorials/NewtonsCradle.cpp - ../ExtendedTutorials/NewtonsCradle.h + ../Evolution/NN3DWalkers.cpp + ../Evolution/NN3DWalkers.h ../ExtendedTutorials/NewtonsRopeCradle.cpp ../ExtendedTutorials/NewtonsRopeCradle.h - ../ExtendedTutorials/InclinedPlane.cpp - ../ExtendedTutorials/InclinedPlane.h - ../ExtendedTutorials/MultiPendulum.cpp - ../ExtendedTutorials/MultiPendulum.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 15a83af59..87382fbc8 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -73,6 +73,7 @@ #include "../ExtendedTutorials/NewtonsCradle.h" #include "../ExtendedTutorials/NewtonsRopeCradle.h" #include "../ExtendedTutorials/MultiPendulum.h" +#include "../Evolution/NN3DWalkers.h" struct ExampleEntry { @@ -308,6 +309,8 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Newton's Rope Cradle", "Create a Newton's Cradle using ropes. Press 3 to displace pendula. Use the sliders to select the number (reset simulation), length and restitution of pendula and the number of displaced pendula and apply the displacement force.",ET_NewtonsRopeCradleCreateFunc), 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 (reset simulation), length and restitution of pendula, the number of displaced pendula and apply the displacement force.",ET_MultiPendulumCreateFunc), + ExampleEntry(9,"Evolution"), + ExampleEntry(1,"Neural Network 3D Walkers","A simple example of using evolution to make a creature walk.",ET_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 e5cc37dce..c28ae0e5a 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -89,6 +89,8 @@ project "App_BulletExampleBrowser" "../BasicDemo/BasicExample.*", "../Tutorial/*", "../ExtendedTutorials/*", + "../Evolution/NN3DWalkers.cpp", + "../Evolution/NN3DWalkers.h", "../Collision/*", "../RoboticsLearning/*", "../Collision/Internal/*", diff --git a/examples/Utils/b3ERPCFMHelper.hpp b/examples/Utils/b3ERPCFMHelper.hpp new file mode 100755 index 000000000..272616782 --- /dev/null +++ b/examples/Utils/b3ERPCFMHelper.hpp @@ -0,0 +1,79 @@ +#ifndef B3_ERPCFM_HELPER +#define B3_ERPCFM_HELPER + +#include + +/** + * @brief ERP/CFM Utils are to provide bullet specific helper functions. + * @details Details + * @date 2015-09-20 + * @author Benjamin Ellenberger + */ +class b3ERPCFMHelper { +public: + + /** + * == How To Use ERP and CFM == + * ERP and CFM can be independently set in many joints. They can be set in contact joints, in joint limits and various other places, to control the spongyness and springyness of the joint (or joint limit). + * If CFM is set to zero, the constraint will be hard. If CFM is set to a positive value, it will be possible to violate the constraint by "pushing on it" (for example, for contact constraints by forcing the two contacting objects together). In other words the constraint will be soft, and the softness will increase as CFM increases. What is actually happening here is that the constraint is allowed to be violated by an amount proportional to CFM times the restoring force that is needed to enforce the constraint. Note that setting CFM to a negative value can have undesirable bad effects, such as instability. Don't do it. + * By adjusting the values of ERP and CFM, you can achieve various effects. For example you can simulate springy constraints, where the two bodies oscillate as though connected by springs. Or you can simulate more spongy constraints, without the oscillation. In fact, ERP and CFM can be selected to have the same effect as any desired spring and damper constants. If you have a spring constant k_p and damping constant k_d, then the corresponding ODE constants are: + * + * ERP = \frac{h k_p} {h k_p + k_d} + * + * CFM = \frac{1} {h k_p + k_d} + * + * where h is the step size. These values will give the same effect as a spring-and-damper system simulated with implicit first order integration. + * Increasing CFM, especially the global CFM, can reduce the numerical errors in the simulation. If the system is near-singular, then this can markedly increase stability. In fact, if the system is misbehaving, one of the first things to try is to increase the global CFM. + * @link http://ode-wiki.org/wiki/index.php?title=Manual:_All&printable=yes#How_To_Use_ERP_and_CFM + * @return + */ + /** + * Joint error and the Error Reduction Parameter (ERP) + * + * When a joint attaches two bodies, those bodies are required to have certain positions and orientations relative to each other. However, it is possible for the bodies to be in positions where the joint constraints are not met. This "joint error" can happen in two ways: + * + * If the user sets the position/orientation of one body without correctly setting the position/orientation of the other body. + * During the simulation, errors can creep in that result in the bodies drifting away from their required positions. + * Figure 3 shows an example of error in a ball and socket joint (where the ball and socket do not line up). + * + * There is a mechanism to reduce joint error: during each simulation step each joint applies a special force to bring its bodies back into correct alignment. This force is controlled by the error reduction parameter (ERP), which has a value between 0 and 1. + * + * The ERP specifies what proportion of the joint error will be fixed during the next simulation step. If ERP = 0 then no correcting force is applied and the bodies will eventually drift apart as the simulation proceeds. If ERP=1 then the simulation will attempt to fix all joint error during the next time step. However, setting ERP=1 is not recommended, as the joint error will not be completely fixed due to various internal approximations. A value of ERP=0.1 to 0.8 is recommended (0.2 is the default). + * + * A global ERP value can be set that affects most joints in the simulation. However some joints have local ERP values that control various aspects of the joint. + * @link http://ode-wiki.org/wiki/index.php?title=Manual:_All&printable=yes#How_To_Use_ERP_and_CFM + * @return + */ + static btScalar getERP(btScalar timeStep, btScalar kSpring, + btScalar kDamper) { + return timeStep * kSpring / (timeStep * kSpring + kDamper); + } + + /** + * Most constraints are by nature "hard". This means that the constraints represent conditions that are never violated. For example, the ball must always be in the socket, and the two parts of the hinge must always be lined up. In practice constraints can be violated by unintentional introduction of errors into the system, but the error reduction parameter can be set to correct these errors. + * Not all constraints are hard. Some "soft" constraints are designed to be violated. For example, the contact constraint that prevents colliding objects from penetrating is hard by default, so it acts as though the colliding surfaces are made of steel. But it can be made into a soft constraint to simulate softer materials, thereby allowing some natural penetration of the two objects when they are forced together. + * There are two parameters that control the distinction between hard and soft constraints. The first is the error reduction parameter (ERP) that has already been introduced. The second is the constraint force mixing (CFM) value, that is described below. + * + * == Constraint Force Mixing (CFM) == + * What follows is a somewhat technical description of the meaning of CFM. If you just want to know how it is used in practice then skip to the next section. + * Traditionally the constraint equation for every joint has the form + * \mathbf{J} v = \mathbf{c} + * where v is a velocity vector for the bodies involved, \mathbf{J} is a "Jacobian" matrix with one row for every degree of freedom the joint removes from the system, and \mathbf{c} is a right hand side vector. At the next time step, a vector \lambda is calculated (of the same size as \mathbf{c}) such that the forces applied to the bodies to preserve the joint constraint are: + * F_c = \mathbf{J}^T \lambda + * ODE adds a new twist. ODE's constraint equation has the form + * + * \mathbf{J} v = \mathbf{c} + \textbf{CFM} \, \lambda + * where CFM is a square diagonal matrix. CFM mixes the resulting constraint force in with the constraint that produces it. A nonzero (positive) value of CFM allows the original constraint equation to be violated by an amount proportional to CFM times the restoring force \lambda that is needed to enforce the constraint. Solving for \lambda gives + * + * (\mathbf{J} \mathbf{M}^{-1} \mathbf{J}^T + \frac{1}{h} \textbf{CFM}) \lambda = \frac{1}{h} \mathbf{c} + * Thus CFM simply adds to the diagonal of the original system matrix. Using a positive value of CFM has the additional benefit of taking the system away from any singularity and thus improving the factorizer accuracy. + * @link http://ode-wiki.org/wiki/index.php?title=Manual:_All&printable=yes#How_To_Use_ERP_and_CFM + * @return + */ + static btScalar getCFM(btScalar avoidSingularity, btScalar timeStep, btScalar kSpring, + btScalar kDamper) { + return btScalar(avoidSingularity) / (timeStep * kSpring + kDamper); + } +}; + +#endif /* B3_ERPCFM_HELPER */ diff --git a/examples/Utils/b3ReferenceFrameHelper.hpp b/examples/Utils/b3ReferenceFrameHelper.hpp new file mode 100755 index 000000000..05e839d53 --- /dev/null +++ b/examples/Utils/b3ReferenceFrameHelper.hpp @@ -0,0 +1,56 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +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. +*/ + +#ifndef B3_REFERENCEFRAMEHELPER_H +#define B3_REFERENCEFRAMEHELPER_H + +#include "LinearMath/btTransform.h" +#include "LinearMath/btVector3.h" + +class b3ReferenceFrameHelper { +public: + static btVector3 getPointWorldToLocal(const btTransform& localObjectCenterOfMassTransform, const btVector3& point) { + return localObjectCenterOfMassTransform.inverse() * point; // transforms the point from the world frame into the local frame + } + + static btVector3 getPointLocalToWorld(const btTransform& localObjectCenterOfMassTransform,const btVector3& point) { + return localObjectCenterOfMassTransform * point; // transforms the point from the world frame into the local frame + } + + static btVector3 getAxisWorldToLocal(const btTransform& localObjectCenterOfMassTransform, const btVector3& axis) { + btTransform local1 = localObjectCenterOfMassTransform.inverse(); // transforms the axis from the local frame into the world frame + btVector3 zero(0,0,0); + local1.setOrigin(zero); + return local1 * axis; + } + + static btVector3 getAxisLocalToWorld(const btTransform& localObjectCenterOfMassTransform, const btVector3& axis) { + btTransform local1 = localObjectCenterOfMassTransform; // transforms the axis from the local frame into the world frame + btVector3 zero(0,0,0); + local1.setOrigin(zero); + return local1 * axis; + } + + static btTransform getTransformWorldToLocal(const btTransform& localObjectCenterOfMassTransform, const btTransform& transform) { + return localObjectCenterOfMassTransform.inverse() * transform; // transforms the axis from the local frame into the world frame + } + + static btTransform getTransformLocalToWorld(const btTransform& localObjectCenterOfMassTransform, const btTransform& transform) { + return localObjectCenterOfMassTransform * transform; // transforms the axis from the local frame into the world frame + } + +}; + +#endif /* B3_REFERENCEFRAMEHELPER_H */