From 17570c47002d82806b41fa4019f2acd35e040e2d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 11 Dec 2016 09:28:36 -0800 Subject: [PATCH] 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)