From c250a5f0b993e9f89699f0649e2e8c896dbac5ba Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 09:56:39 -0700 Subject: [PATCH 1/4] re-enable shared memory connection for pybullet Gym envs (with fallback to GUI or DIRECT) suppress shared memory connection warnings add fallback from ER_BULLET_HARDWARE_OPENGL to TinyRenderer if not available --- examples/SharedMemory/PhysicsClientSharedMemory.cpp | 4 ++-- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 12 +++++++++--- examples/SharedMemory/PosixSharedMemory.cpp | 2 +- .../gym/pybullet_envs/bullet/bullet_client.py | 2 +- .../gym/pybullet_envs/bullet/kukaCamGymEnv.py | 4 +++- .../pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py | 2 +- examples/pybullet/gym/pybullet_envs/env_bases.py | 2 +- .../pybullet/gym/pybullet_envs/examples/runServer.py | 3 ++- test/SharedMemory/test.c | 6 +++++- 9 files changed, 25 insertions(+), 12 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e23c4ceee..e5cf7b58d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -78,7 +78,7 @@ struct PhysicsClientSharedMemoryInternalData { m_hasLastServerStatus(false), m_sharedMemoryKey(SHARED_MEMORY_KEY), m_verboseOutput(false), - m_timeOutInSeconds(1e30) + m_timeOutInSeconds(5) {} void processServerStatus(); @@ -311,7 +311,7 @@ bool PhysicsClientSharedMemory::connect() { m_data->m_isConnected = true; } } else { - b3Warning("Cannot connect to shared memory"); + //b3Warning("Cannot connect to shared memory"); return false; } #if 0 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a459137b3..db318f056 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3541,7 +3541,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + bool handled = false; + if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) { @@ -3551,14 +3552,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm segmentationMaskBuffer, numRequestedPixels, startPixelIndex,width,height,&numPixelsCopied); - m_data->m_guiHelper->debugDisplayCameraImageData(viewMat, + if (numPixelsCopied>0) + { + handled = true; + m_data->m_guiHelper->debugDisplayCameraImageData(viewMat, projMat,pixelRGBA,numRequestedPixels, depthBuffer,numRequestedPixels, 0, numRequestedPixels, startPixelIndex,width,height,&numPixelsCopied); + } - } else + } + if (!handled) { if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index 43dcc86eb..b2e2f1f23 100644 --- a/examples/SharedMemory/PosixSharedMemory.cpp +++ b/examples/SharedMemory/PosixSharedMemory.cpp @@ -91,7 +91,7 @@ void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCr int id = shmget((key_t) key, (size_t) size,flags); if (id < 0) { - b3Warning("shmget error"); + //b3Warning("shmget error1"); } else { btPointerCaster result; diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py index fcdedf10c..0eda1fb2e 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py @@ -8,7 +8,7 @@ class BulletClient(object): def __init__(self, connection_mode=pybullet.DIRECT): """Create a simulation and connect to it.""" - self._client = -1 #pybullet.connect(pybullet.SHARED_MEMORY) + self._client = pybullet.connect(pybullet.SHARED_MEMORY) if(self._client<0): self._client = pybullet.connect(connection_mode) self._shapes = {} diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py index 087006407..ea2b07b82 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py @@ -40,7 +40,9 @@ class KukaCamGymEnv(gym.Env): self.terminated = 0 self._p = p if self._renders: - p.connect(p.GUI) + cid = p.connect(p.SHARED_MEMORY) + if (cid<0): + p.connect(p.GUI) p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33]) else: p.connect(p.DIRECT) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index 903727b07..5928a7232 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -37,7 +37,7 @@ class KukaGymEnv(gym.Env): self.terminated = 0 self._p = p if self._renders: - cid = -1 #p.connect(p.SHARED_MEMORY) + cid = p.connect(p.SHARED_MEMORY) if (cid<0): cid = p.connect(p.GUI) p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33]) diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 3379ae4b4..5e2017e42 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -39,7 +39,7 @@ class MJCFBaseBulletEnv(gym.Env): def _reset(self): if (self.physicsClientId<0): - self.physicsClientId = -1 #p.connect(p.SHARED_MEMORY) + self.physicsClientId = p.connect(p.SHARED_MEMORY) if (self.physicsClientId<0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) diff --git a/examples/pybullet/gym/pybullet_envs/examples/runServer.py b/examples/pybullet/gym/pybullet_envs/examples/runServer.py index 8c37caa9d..bcd95f812 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/runServer.py +++ b/examples/pybullet/gym/pybullet_envs/examples/runServer.py @@ -13,6 +13,7 @@ p.connect(p.GUI_SERVER) p.setAdditionalSearchPath(pybullet_data.getDataPath()) while(1): + #this is a no-op command, to allow GUI updates on Mac OSX (main thread) + p.setPhysicsEngineParameter() time.sleep(0.01) - p.getNumBodies() diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index e475a4705..583bd86b6 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -258,6 +258,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) command = b3InitRequestCameraImage(sm); b3RequestCameraImageSetPixelResolution(command, width, height); + b3RequestCameraImageSelectRenderer(command,ER_BULLET_HARDWARE_OPENGL); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -298,7 +299,10 @@ void testSharedMemory(b3PhysicsClientHandle sm) } } - } + } else + { + b3Warning("Cannot submit commands.\n"); + } b3DisconnectSharedMemory(sm); From 2eaac410b56f500d83766e0fc779282e11552ebf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 12:07:19 -0700 Subject: [PATCH 2/4] remove memory of loadTexture. --- examples/SharedMemory/TinyRendererVisualShapeConverter.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 622788999..ee49f47d5 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -1071,7 +1071,12 @@ void TinyRendererVisualShapeConverter::resetAll() delete ptr; } } - + + for (int i=0;im_textures.size();i++) + { + free(m_data->m_textures[i].textureData); + } + m_data->m_textures.clear(); m_data->m_swRenderInstances.clear(); m_data->m_visualShapes.clear(); } From 340a8f47044b9340d3d00a5bf5d2d576967d0afa Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 13:30:16 -0700 Subject: [PATCH 3/4] add sigaction handler to Example Browser, to always shutdown shared memory make GUI_SERVER more reliable next attempt to connect to SHARED_MEMORY in Gym envs, if available, before DIRECT/GUI allow software rendering fallback, even if ER_BULLET_HARDWARE_OPENGL is chosen in getCameraImage --- examples/ExampleBrowser/main.cpp | 46 ++++++++++++++++++- .../SharedMemoryInProcessPhysicsC_API.cpp | 4 +- .../pybullet_envs/bullet/minitaur_gym_env.py | 2 +- .../pybullet/gym/pybullet_envs/env_bases.py | 2 +- examples/pybullet/pybullet.c | 11 +++-- setup.py | 2 +- 6 files changed, 57 insertions(+), 10 deletions(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 46bbd2b0b..180f94a77 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -20,8 +20,51 @@ static double gMinUpdateTimeMicroSecs = 1000.; +static bool interrupted=false; +static OpenGLExampleBrowser* sExampleBrowser=0; + +#ifndef _WIN32 +#include +#include +#include +static void cleanup(int signo) +{ +printf("SIG cleanup: %d\n", signo); + + if (interrupted) { // this is the second time, we're hanging somewhere + // if (example) { + // example->abort(); + // } + + b3Printf("Aborting and deleting SharedMemoryCommon object"); + sleep(1); + delete sExampleBrowser; + sExampleBrowser = 0; + errx(EXIT_FAILURE, "aborted example on signal %d", signo); + } else + { + b3Printf("no action"); + } + interrupted = true; + warnx("caught signal %d", signo); +} +#endif//_WIN32 + int main(int argc, char* argv[]) { + +#ifndef _WIN32 + struct sigaction action; + memset(&action, 0x0, sizeof(action)); + action.sa_handler = cleanup; + static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM }; + for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) { + if (0 != sigaction(signos[ii], &action, NULL)) { + err(EXIT_FAILURE, "signal %d", signos[ii]); + } + } +#endif + { b3CommandLineArgs args(argc, argv); b3Clock clock; @@ -31,6 +74,7 @@ int main(int argc, char* argv[]) examples.initExampleEntries(); OpenGLExampleBrowser* exampleBrowser = new OpenGLExampleBrowser(&examples); + sExampleBrowser = exampleBrowser;//for etc, cleanup shared memory bool init = exampleBrowser->init(argc, argv); exampleBrowser->registerFileImporter(".urdf", ImportURDFCreateFunc); exampleBrowser->registerFileImporter(".sdf", ImportSDFCreateFunc); @@ -57,7 +101,7 @@ int main(int argc, char* argv[]) clock.reset(); exampleBrowser->update(deltaTimeInSeconds); } - } while (!exampleBrowser->requestedExit()); + } while (!exampleBrowser->requestedExit() && !interrupted); } delete exampleBrowser; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 8f01bfb5a..e8d43c8b1 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -86,7 +86,7 @@ public: b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]) { InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 1); - cl->setSharedMemoryKey(SHARED_MEMORY_KEY); + cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1); cl->connect(); return (b3PhysicsClientHandle ) cl; } @@ -94,7 +94,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]) { InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 0); - cl->setSharedMemoryKey(SHARED_MEMORY_KEY); + cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1); cl->connect(); return (b3PhysicsClientHandle ) cl; } diff --git a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py index d74906408..6e7872a61 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -278,7 +278,7 @@ class MinitaurBulletEnv(gym.Env): nearVal=0.1, farVal=100.0) (_, _, px, _, _) = self._pybullet_client.getCameraImage( width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, - projectionMatrix=proj_matrix) + projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] return rgb_array diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 5e2017e42..f291d556b 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -82,7 +82,7 @@ class MJCFBaseBulletEnv(gym.Env): (_, _, px, _, _) = p.getCameraImage( width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, - #renderer=p.ER_BULLET_HARDWARE_OPENGL + renderer=p.ER_BULLET_HARDWARE_OPENGL ) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e1df46636..82249434a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -440,13 +440,16 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P command = b3InitSyncBodyInfoCommand(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); -#if 0 if (statusType != CMD_BODY_INFO_COMPLETED) { - PyErr_SetString(SpamError, "b3InitSyncBodyInfoCommand failed."); - return NULL; + printf("Connection terminated, couldn't get body info\n"); + b3DisconnectSharedMemory(sm); + sm = 0; + sPhysicsClients1[freeIndex] = 0; + sPhysicsClientsGUI[freeIndex] = 0; + sNumPhysicsClients++; + return -1; } -#endif } } return PyInt_FromLong(freeIndex); diff --git a/setup.py b/setup.py index 395bd22b9..5cae36c27 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.8', + version='1.4.0', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 6942a9f8ef669867d9434027015c9f775e5d167a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 13 Sep 2017 13:48:37 -0700 Subject: [PATCH 4/4] remove a debug printf --- examples/ExampleBrowser/main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 180f94a77..07c168309 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -29,7 +29,6 @@ static OpenGLExampleBrowser* sExampleBrowser=0; #include static void cleanup(int signo) { -printf("SIG cleanup: %d\n", signo); if (interrupted) { // this is the second time, we're hanging somewhere // if (example) {