From e5aea04e23e29ab3c123fe14483a7bce057069b5 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Dec 2016 13:23:50 -0800 Subject: [PATCH 1/6] add back the 'swapBuffers' in VR demo (slightly lower performance, but easier to use demo. fix issue related to TinyRenderer shadowbuffer API change --- examples/StandaloneMain/hellovr_opengl_main.cpp | 2 +- examples/TinyRenderer/TinyRenderer.h | 2 +- examples/TinyRenderer/main.cpp | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index d196c8ee8..89aa74204 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -855,7 +855,7 @@ void CMainApplication::RenderFrame() // SwapWindow { B3_PROFILE("m_app->swapBuffer"); -// m_app->swapBuffer(); + m_app->swapBuffer(); //SDL_GL_SwapWindow( m_pWindow ); } diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 4b5fc7c5f..7bc97aa1d 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -30,7 +30,7 @@ struct TinyRenderObjectData TGAImage& m_rgbColorBuffer; b3AlignedObjectArray& m_depthBuffer;//required, hence a reference - b3AlignedObjectArray* m_shadowBuffer; + b3AlignedObjectArray* m_shadowBuffer;//optional, hence a pointer b3AlignedObjectArray* m_segmentationMaskBufferPtr;//optional, hence a pointer TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer); diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp index 614b12d3a..32f5e7edd 100644 --- a/examples/TinyRenderer/main.cpp +++ b/examples/TinyRenderer/main.cpp @@ -147,7 +147,6 @@ int main(int argc, char* argv[]) renderData.m_rgbColorBuffer.set(x,y,color); renderData.m_depthBuffer[x+y*textureWidth] = -1e30f; - renderData.m_shadowBuffer[x+y*textureWidth] = -1e30f; } } From 383b30a4e40a7fe4da6802732b6fabfb3f6807d4 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Dec 2016 14:10:26 -0800 Subject: [PATCH 2/6] reset also needs to reset iterations etc move from 100 to 50 iterations for VR demo --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2872da72b..ed8d95856 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -667,8 +667,6 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() m_data = new PhysicsServerCommandProcessorInternalData(); createEmptyDynamicsWorld(); - m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; - m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100; } @@ -717,6 +715,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; + } void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies() From 61cfa18923f6f1b88bf87703fd2309bd90868a52 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 2 Dec 2016 17:44:00 -0800 Subject: [PATCH 3/6] save default VR camera tuning, requires MIDI controller tweak some values in VR demo --- .../wsg50_one_motor_gripper_free_base.sdf | 8 +++ .../wsg50_one_motor_gripper_new_free_base.sdf | 8 +++ .../PhysicsServerCommandProcessor.cpp | 40 +++++++++++--- .../SharedMemory/PhysicsServerExample.cpp | 54 ++++++++++++++++--- 4 files changed, 97 insertions(+), 13 deletions(-) diff --git a/data/gripper/wsg50_one_motor_gripper_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_free_base.sdf index 7fc3ed2b2..552c36d28 100644 --- a/data/gripper/wsg50_one_motor_gripper_free_base.sdf +++ b/data/gripper/wsg50_one_motor_gripper_free_base.sdf @@ -303,6 +303,10 @@ + + + + 0.042 0 0.145 0 0 1.5708 0.2 @@ -343,6 +347,10 @@ + + + + -0.042 0 0.145 0 0 4.71239 0.2 diff --git a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf index 68e2a0a6e..0358f7a6a 100644 --- a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf +++ b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf @@ -300,6 +300,10 @@ + + 1.0 + 1.5 + 0.062 0 0.145 0 0 1.5708 0.2 @@ -340,6 +344,10 @@ + + 1.0 + 1.5 + -0.062 0 0.145 0 0 4.71239 0.2 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ed8d95856..2b62a92c7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -48,7 +48,7 @@ int gHuskyId = -1; btTransform huskyTr = btTransform::getIdentity(); int gCreateObjectSimVR = -1; -int gEnableKukaControl = 1; +int gEnableKukaControl = 0; btScalar simTimeScalingFactor = 1; btScalar gRhsClamp = 1.f; @@ -4016,7 +4016,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) } -btVector3 gVRGripperPos(0,0,0.2); +btVector3 gVRGripperPos(0.6, 0.4, 0.7); btQuaternion gVRGripperOrn(0,0,0,1); btVector3 gVRController2Pos(0,0,0.2); btQuaternion gVRController2Orn(0,0,0,1); @@ -4156,7 +4156,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() { int bodyId = 0; - if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) + if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) { InteralBodyData* parentBody = m_data->getHandle(bodyId); if (parentBody->m_multiBody) @@ -4186,6 +4186,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_KukaId = bodyId; + InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); + if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7) + { + btScalar q[7]; + q[0] = 0;// -SIMD_HALF_PI; + q[1] = 0; + q[2] = 0; + q[3] = SIMD_HALF_PI; + q[4] = 0; + q[5] = -SIMD_HALF_PI*0.66; + q[6] = 0; + + for (int i = 0; i < 7; i++) + { + kukaBody->m_multiBody->setJointPos(i, q[i]); + } + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m); + int nLinks = kukaBody->m_multiBody->getNumLinks(); + scratch_q.resize(nLinks + 1); + scratch_m.resize(nLinks + 1); + kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m); + } loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); @@ -4194,7 +4218,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() // Load one motor gripper for kuka loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); m_data->m_gripperId = bodyId + 1; - InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId); + InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId); // Reset the default gripper motor maximum torque for damping to 0 @@ -4236,6 +4260,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1); m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2); + kukaBody = m_data->getHandle(m_data->m_KukaId); if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7) { gripperBody->m_multiBody->setHasSelfCollision(0); @@ -4289,7 +4314,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() // Table area loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + //loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); //loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); //loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); @@ -4330,9 +4355,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() if (motor) { btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75; - motor->setPositionTarget(posTarget, .2); + motor->setPositionTarget(posTarget, .8); motor->setVelocityTarget(0.0, .5); - motor->setMaxAppliedImpulse(5.0); + motor->setMaxAppliedImpulse(1.0); + } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index e5b4cc3e4..0a909e938 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -27,6 +27,7 @@ //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! extern btVector3 gLastPickPos; btVector3 gVRTeleportPos1(0,0,0); +btScalar gVRTeleportRotZ = 0; btQuaternion gVRTeleportOrn(0, 0, 0,1); extern btVector3 gVRGripperPos; extern btQuaternion gVRGripperOrn; @@ -46,10 +47,47 @@ extern btScalar simTimeScalingFactor; extern bool gVRGripperClosed; -#if B3_USE_MIDI +const char* startFileNameVR = "0_VRDemoSettings.txt"; #include +//remember the settings (you don't want to re-tune again and again...) +static void saveCurrentSettingsVR() +{ + FILE* f = fopen(startFileNameVR, "w"); + if (f) + { + fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]); + fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]); + fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]); + fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ); + fclose(f); + } +}; + +static void loadCurrentSettingsVR(b3CommandLineArgs& args) +{ + int currentEntry = 0; + FILE* f = fopen(startFileNameVR, "r"); + if (f) + { + char oneline[1024]; + char* argv[] = { 0,&oneline[0] }; + + while (fgets(oneline, 1024, f) != NULL) + { + char *pos; + if ((pos = strchr(oneline, '\n')) != NULL) + *pos = '\0'; + args.addArgs(2, argv); + } + fclose(f); + } + +}; +#if B3_USE_MIDI + + static float getParamf(float rangeMin, float rangeMax, int midiVal) { float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.)); @@ -70,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void { if (message->at(1) == 16) { - float rotZ = getParamf(-3.1415, 3.1415, message->at(2)); - gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ); - b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ); + gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2)); + gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ); + saveCurrentSettingsVR(); + b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ); } if (message->at(1) == 32) @@ -85,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void if (message->at(1) == i) { gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2)); + saveCurrentSettingsVR(); b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]); } @@ -927,7 +967,7 @@ public: virtual void processCommandLineArgs(int argc, char* argv[]) { b3CommandLineArgs args(argc,argv); - + loadCurrentSettingsVR(args); if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0])) { printf("camPosX=%f\n", gVRTeleportPos1[0]); @@ -1665,8 +1705,10 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt return; if (gGraspingController < 0) + { gGraspingController = controllerId; - + gEnableKukaControl = true; + } if (controllerId != gGraspingController) { if (button == 1 && state == 0) From 024ab6725be20887e34e73cf8d7358e29207ee58 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 5 Dec 2016 11:54:56 -0800 Subject: [PATCH 4/6] expose pybullet.setPhysicsEngineParameter(numSubSteps=int) --- examples/pybullet/pybullet.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a3da3e942..21a4b7051 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -425,15 +425,16 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int numSolverIterations = -1; int useSplitImpulse = -1; double splitImpulsePenetrationThreshold = -1; + int numSubSteps = -1; if (0 == sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", NULL }; + static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diid", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps)) { return NULL; } @@ -445,6 +446,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); } + if (numSubSteps >= 0) + { + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + } if (fixedTimeStep >= 0) { b3PhysicsParamSetTimeStep(command, fixedTimeStep); From 710ac09b565b7c5759c09456b3861b42ed833e24 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 11 Dec 2016 09:16:18 -0800 Subject: [PATCH 5/6] fix clearVelocities (should use 6+dofCount, not 6+numLinks fixes issue 878 --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index b85656875..3c6b6154c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -570,7 +570,7 @@ void btMultiBody::clearForcesAndTorques() void btMultiBody::clearVelocities() { - for (int i = 0; i < 6 + getNumLinks(); ++i) + for (int i = 0; i < 6 + getNumDofs(); ++i) { m_realBuf[i] = 0.f; } From 17570c47002d82806b41fa4019f2acd35e040e2d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 11 Dec 2016 09:28:36 -0800 Subject: [PATCH 6/6] tweak color of quadruped robot URDF, tweak quadruped.py script to make it more compatible with VR demo allow VR physics server to run with or without 'realTimeSimulation' --- data/quadruped/quadruped.urdf | 100 ++++++++---------- .../PhysicsServerCommandProcessor.cpp | 8 +- .../PhysicsServerCommandProcessor.h | 1 + .../SharedMemory/PhysicsServerExample.cpp | 8 +- .../PhysicsServerSharedMemory.cpp | 6 ++ .../SharedMemory/PhysicsServerSharedMemory.h | 2 + examples/pybullet/quadruped.py | 12 ++- 7 files changed, 70 insertions(+), 67 deletions(-) diff --git a/data/quadruped/quadruped.urdf b/data/quadruped/quadruped.urdf index 843371c32..32e9c131c 100644 --- a/data/quadruped/quadruped.urdf +++ b/data/quadruped/quadruped.urdf @@ -5,8 +5,8 @@ - - + + @@ -26,9 +26,7 @@ - - - + @@ -55,9 +53,7 @@ - - - + @@ -84,9 +80,7 @@ - - - + @@ -113,9 +107,7 @@ - - - + @@ -142,9 +134,7 @@ - - - + @@ -171,9 +161,7 @@ - - - + @@ -200,9 +188,7 @@ - - - + @@ -229,9 +215,7 @@ - - - + @@ -258,8 +242,8 @@ - - + + @@ -286,8 +270,8 @@ - - + + @@ -315,8 +299,8 @@ - - + + @@ -345,8 +329,8 @@ - - + + @@ -374,8 +358,8 @@ - - + + @@ -402,8 +386,8 @@ - - + + @@ -431,8 +415,8 @@ - - + + @@ -461,8 +445,8 @@ - - + + @@ -493,8 +477,8 @@ - - + + @@ -521,8 +505,8 @@ - - + + @@ -550,8 +534,8 @@ - - + + @@ -580,8 +564,8 @@ - - + + @@ -609,8 +593,8 @@ - - + + @@ -637,8 +621,8 @@ - - + + @@ -666,8 +650,8 @@ - - + + @@ -696,8 +680,8 @@ - - + + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2b62a92c7..167f38c0a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4030,6 +4030,12 @@ int gDroppedSimulationSteps = 0; int gNumSteps = 0; double gDtInSec = 0.f; double gSubStep = 0.f; + +void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim) +{ + m_data->m_allowRealTimeSimulation = enableRealTimeSim; +} + void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { if (gResetSimulation) @@ -4038,7 +4044,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) gResetSimulation = false; } - if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) + if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) { ///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later... diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index dc7992980..c8a060069 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -85,6 +85,7 @@ public: void replayFromLogFile(const char* fileName); void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); void stepSimulationRealTime(double dtInSec); + void enableRealTimeSimulation(bool enableRealTimeSim); void applyJointDamping(int bodyUniqueId); }; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 0a909e938..2bc823d3b 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1584,7 +1584,11 @@ void PhysicsServerExample::renderScene() if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { - gEnableRealTimeSimVR = true; + if (!gEnableRealTimeSimVR) + { + gEnableRealTimeSimVR = true; + m_physicsServer.enableRealTimeSimulation(1); + } } @@ -1819,8 +1823,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) { - gEnableRealTimeSimVR = true; - if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) { printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp index 111ac07f5..158bdac6b 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp @@ -241,6 +241,12 @@ void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec) m_data->m_commandProcessor->stepSimulationRealTime(dtInSec); } +void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim) +{ + m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim); +} + + void PhysicsServerSharedMemory::processClientCommands() { diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h index c330817b4..f29e844f2 100644 --- a/examples/SharedMemory/PhysicsServerSharedMemory.h +++ b/examples/SharedMemory/PhysicsServerSharedMemory.h @@ -28,6 +28,8 @@ public: virtual void stepSimulationRealTime(double dtInSec); + virtual void enableRealTimeSimulation(bool enableRealTimeSim); + //bool supportsJointMotor(class btMultiBody* body, int linkIndex); diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py index 6205191f1..31cffbce4 100644 --- a/examples/pybullet/quadruped.py +++ b/examples/pybullet/quadruped.py @@ -4,9 +4,10 @@ import math p.connect(p.SHARED_MEMORY) p.loadURDF("plane.urdf") -quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3) +p.setGravity(0,0,-1) +p.setRealTimeSimulation(0) +quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2) #p.getNumJoints(1) - #right front leg p.resetJointState(quadruped,0,1.57) p.resetJointState(quadruped,2,-2.2) @@ -64,16 +65,17 @@ p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1) p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0) p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0) -p.setGravity(0,0,-10) + p_gain = 2 speed = 10 amplitude = 1.3 #stand still -t_end = time.time() + 5 +t_end = time.time() + 2 while time.time() < t_end: p.stepSimulation() +p.setGravity(0,0,-10) jump_amp = 0.5 @@ -125,4 +127,4 @@ while time.time() < t_end: p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1) p.stepSimulation() - +p.setRealTimeSimulation(1)