From 466c853489c7257d4d10353b985dd20aa18c3aa9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 10 Sep 2017 10:45:38 -0700 Subject: [PATCH 01/11] add testMJCF.py script, to visualize MJCF file --- .../gym/pybullet_envs/examples/testMJCF.py | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_envs/examples/testMJCF.py diff --git a/examples/pybullet/gym/pybullet_envs/examples/testMJCF.py b/examples/pybullet/gym/pybullet_envs/examples/testMJCF.py new file mode 100644 index 000000000..ebdef3b38 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/testMJCF.py @@ -0,0 +1,28 @@ +import os +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +import pybullet as p +import pybullet_data +import time + +def test(args): + p.connect(p.GUI) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + fileName = os.path.join("mjcf", args.mjcf) + print("fileName") + print(fileName) + p.loadMJCF(fileName) + while (1): + p.stepSimulation() + p.getCameraImage(320,240) + time.sleep(0.01) + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--mjcf', help='MJCF filename', default="humanoid.xml") + args = parser.parse_args() + test(args) \ No newline at end of file From ee082396d1d772024092eb5f83d82e31a1514957 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 10 Sep 2017 16:22:45 -0700 Subject: [PATCH 02/11] fixes in libdl/DL cmake minor tweaks in pybullet OpenAI gym locomotion envs (connect to shared memory before going GUI/DIRECT) bump up pybullet version to 1.3.6 --- CMakeLists.txt | 3 +-- examples/OpenGLWindow/CMakeLists.txt | 2 +- examples/ThirdPartyLibs/Gwen/Macros.h | 6 ++++-- examples/pybullet/gym/pybullet_envs/env_bases.py | 6 ++++-- setup.py | 2 +- 5 files changed, 11 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e04fe3e7..8eb1c0b44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,7 @@ ENDIF (BULLET2_USE_THREAD_LOCKS) IF(NOT WIN32) - SET(DL dl) + SET(DL ${CMAKE_DL_LIBS}) IF(CMAKE_SYSTEM_NAME MATCHES "Linux") MESSAGE("Linux") SET(OSDEF -D_LINUX) @@ -50,7 +50,6 @@ IF(NOT WIN32) ELSE(APPLE) MESSAGE("BSD?") SET(OSDEF -D_BSD) - SET(DL "") ENDIF(APPLE) ENDIF(CMAKE_SYSTEM_NAME MATCHES "Linux") ENDIF(NOT WIN32) diff --git a/examples/OpenGLWindow/CMakeLists.txt b/examples/OpenGLWindow/CMakeLists.txt index 385b32216..78e2c2d9e 100644 --- a/examples/OpenGLWindow/CMakeLists.txt +++ b/examples/OpenGLWindow/CMakeLists.txt @@ -62,7 +62,7 @@ if (BUILD_SHARED_LIBS) else() set (CMAKE_THREAD_PREFER_PTHREAD TRUE) FIND_PACKAGE(Threads) - target_link_libraries(OpenGLWindow ${DL} ${CMAKE_THREAD_LIBS_INIT}) + target_link_libraries(OpenGLWindow ${CMAKE_THREAD_LIBS_INIT}) endif() endif() diff --git a/examples/ThirdPartyLibs/Gwen/Macros.h b/examples/ThirdPartyLibs/Gwen/Macros.h index 26585b21d..71d878c12 100644 --- a/examples/ThirdPartyLibs/Gwen/Macros.h +++ b/examples/ThirdPartyLibs/Gwen/Macros.h @@ -4,8 +4,10 @@ #define GWEN_MACROS_H #include #include -#if !defined(__APPLE__) && !defined(__OpenBSD__) -#include +#if !defined(__APPLE__) && !defined(__OpenBSD__) && !defined(__FreeBSD__) + #include +#else + #include #endif //__APPLE__ #include #include diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 8b16cd7be..1a8d5c7d6 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -40,10 +40,12 @@ class MJCFBaseBulletEnv(gym.Env): def _reset(self): if (self.physicsClientId<0): if (self.isRender): - self.physicsClientId = p.connect(p.GUI) + self.physicsClientId = p.connect(p.SHARED_MEMORY) + if (self.physicsClientId<0): + self.physicsClientId = p.connect(p.GUI) else: self.physicsClientId = p.connect(p.DIRECT) - p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + #p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() diff --git a/setup.py b/setup.py index 2ef9bd97d..3ef1c7776 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.5', + version='1.3.6', 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 a2322f7e3ee89de3be7f68bbe106fa3bdf69356b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 10 Sep 2017 21:40:33 -0700 Subject: [PATCH 03/11] trigger appveyor/travis tests --- build_visual_studio_vr_pybullet_double_cmake.bat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build_visual_studio_vr_pybullet_double_cmake.bat b/build_visual_studio_vr_pybullet_double_cmake.bat index ea53e6c6a..72158083c 100644 --- a/build_visual_studio_vr_pybullet_double_cmake.bat +++ b/build_visual_studio_vr_pybullet_double_cmake.bat @@ -1,4 +1,4 @@ mkdir build_cmake cd build_cmake cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.3.amd64\include -DPYTHON_LIBRARY=c:\python-3.5.3.amd64\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.3.amd64\libs\python35_d.lib -G "Visual Studio 14 2015 Win64" .. -start . +start . \ No newline at end of file From f38b2cf14d0f1d28e5d37cb0a25535af417e49e8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 09:23:14 -0700 Subject: [PATCH 04/11] use SHARED_MEMORY as first connection attempt, to make visualization/OpenGL rendering easier --- .../gym/pybullet_envs/bullet/bullet_client.py | 4 +++- .../pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py | 4 +++- .../gym/pybullet_envs/bullet/minitaur_gym_env.py | 3 +++ examples/pybullet/gym/pybullet_envs/env_bases.py | 12 ++++++------ setup.py | 1 + 5 files changed, 16 insertions(+), 8 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py index 6d4749eb9..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,9 @@ class BulletClient(object): def __init__(self, connection_mode=pybullet.DIRECT): """Create a simulation and connect to it.""" - self._client = pybullet.connect(connection_mode) + self._client = pybullet.connect(pybullet.SHARED_MEMORY) + if(self._client<0): + self._client = pybullet.connect(connection_mode) self._shapes = {} def __del__(self): diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index 56c9fb864..5928a7232 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -37,7 +37,9 @@ class KukaGymEnv(gym.Env): self.terminated = 0 self._p = p if self._renders: - p.connect(p.GUI) + 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]) else: p.connect(p.DIRECT) 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 0822255cb..d74906408 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -166,6 +166,9 @@ class MinitaurBulletEnv(gym.Env): def set_env_randomizer(self, env_randomizer): self._env_randomizer = env_randomizer + def configure(self, args): + self._args = args + def _reset(self): if self._hard_reset: self._pybullet_client.resetSimulation() diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 1a8d5c7d6..5e2017e42 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -39,13 +39,13 @@ class MJCFBaseBulletEnv(gym.Env): def _reset(self): if (self.physicsClientId<0): - if (self.isRender): - self.physicsClientId = p.connect(p.SHARED_MEMORY) - if (self.physicsClientId<0): + self.physicsClientId = p.connect(p.SHARED_MEMORY) + if (self.physicsClientId<0): + if (self.isRender): self.physicsClientId = p.connect(p.GUI) - else: - self.physicsClientId = p.connect(p.DIRECT) - #p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + else: + self.physicsClientId = p.connect(p.DIRECT) + p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() diff --git a/setup.py b/setup.py index 3ef1c7776..a108087c7 100644 --- a/setup.py +++ b/setup.py @@ -467,6 +467,7 @@ setup( 'Programming Language :: Python :: 3.5', 'Programming Language :: Python :: 3.6', 'Topic :: Games/Entertainment :: Simulation', + 'Topic :: Scientific/Engineering :: Artificial Intelligence', 'Framework :: Robot Framework'], package_dir = { '': 'examples/pybullet/gym'}, packages=[x for x in find_packages('examples/pybullet/gym')], From 416c29daf721ebe4fd6c95c989964e0f014b1fb7 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 09:30:39 -0700 Subject: [PATCH 05/11] fix with shared memory connection while disabling rendering --- examples/ExampleBrowser/OpenGLExampleBrowser.cpp | 9 ++++++--- examples/SharedMemory/PosixSharedMemory.cpp | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 7672e1962..0fe118331 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1204,10 +1204,13 @@ void OpenGLExampleBrowser::updateGraphics() void OpenGLExampleBrowser::update(float deltaTime) { - if (!gEnableRenderLoop) - return; + b3ChromeUtilsEnableProfiling(); - b3ChromeUtilsEnableProfiling(); + if (!gEnableRenderLoop) + { + sCurrentDemo->updateGraphics(); + return; + } B3_PROFILE("OpenGLExampleBrowser::update"); assert(glGetError()==GL_NO_ERROR); diff --git a/examples/SharedMemory/PosixSharedMemory.cpp b/examples/SharedMemory/PosixSharedMemory.cpp index 9c16b3fe7..43dcc86eb 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) { - b3Error("shmget error"); + b3Warning("shmget error"); } else { btPointerCaster result; From 105c2c948f20c408c469adc517d15762585e10e3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 09:38:55 -0700 Subject: [PATCH 06/11] add simple 'runServer.py' script, that lets you run a GUI shared memory server to connect to --- .../gym/pybullet_envs/examples/runServer.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_envs/examples/runServer.py diff --git a/examples/pybullet/gym/pybullet_envs/examples/runServer.py b/examples/pybullet/gym/pybullet_envs/examples/runServer.py new file mode 100644 index 000000000..8c37caa9d --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/runServer.py @@ -0,0 +1,18 @@ +#add parent dir to find package. Only needed for source code build, pip install doesn't need it. +import os +import inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +import pybullet_data +import pybullet as p +import time + +p.connect(p.GUI_SERVER) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) + +while(1): + time.sleep(0.01) + p.getNumBodies() + From b7e7415b46474fd073efd045df4dc3d246ee42ab Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 09:39:48 -0700 Subject: [PATCH 07/11] bump up pybullet to 1.3.7 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index a108087c7..adab003fb 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.6', + version='1.3.7', 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 3891602784a28d14fce716aa162b2a582ba86e09 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 17:34:06 -0700 Subject: [PATCH 08/11] enable continuous action space for racecarZEDGymEnv disable SHARED_MEMORY connection (it was just some debug test) --- .../gym/pybullet_envs/bullet/bullet_client.py | 2 +- .../gym/pybullet_envs/bullet/kukaGymEnv.py | 2 +- .../pybullet_envs/bullet/racecarZEDGymEnv.py | 23 +++++++++++++------ .../pybullet/gym/pybullet_envs/env_bases.py | 2 +- .../examples/racecarZEDGymEnvTest.py | 4 ++-- 5 files changed, 21 insertions(+), 12 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py b/examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py index 0eda1fb2e..fcdedf10c 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 = pybullet.connect(pybullet.SHARED_MEMORY) + self._client = -1 #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/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index 5928a7232..903727b07 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 = p.connect(p.SHARED_MEMORY) + cid = -1 #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/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index 5fcbd75f7..507f6cb1a 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -25,7 +25,7 @@ class RacecarZEDGymEnv(gym.Env): urdfRoot=pybullet_data.getDataPath(), actionRepeat=10, isEnableSelfCollision=True, - isDiscrete=True, + isDiscrete=False, renders=True): print("init") self._timeStep = 0.01 @@ -52,7 +52,13 @@ class RacecarZEDGymEnv(gym.Env): #print(observationDim) observation_high = np.array([np.finfo(np.float32).max] * observationDim) - self.action_space = spaces.Discrete(9) + if (isDiscrete): + self.action_space = spaces.Discrete(9) + else: + action_dim = 2 + self._action_bound = 1 + action_high = np.array([self._action_bound] * action_dim) + self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4)) self.viewer = None @@ -119,11 +125,14 @@ class RacecarZEDGymEnv(gym.Env): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - fwd = [-1,-1,-1,0,0,0,1,1,1] - steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] - forward = fwd[action] - steer = steerings[action] - realaction = [forward,steer] + if (self._isDiscrete): + fwd = [-1,-1,-1,0,0,0,1,1,1] + steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6] + forward = fwd[action] + steer = steerings[action] + realaction = [forward,steer] + else: + realaction = action self._racecar.applyAction(realaction) for i in range(self._actionRepeat): diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index 5e2017e42..3379ae4b4 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 = p.connect(p.SHARED_MEMORY) + self.physicsClientId = -1 #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/racecarZEDGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py index 176846651..64521b121 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py @@ -8,7 +8,7 @@ from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv def main(): - environment = RacecarZEDGymEnv(renders=True) + environment = RacecarZEDGymEnv(renders=True, isDiscrete=True) targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0) @@ -37,4 +37,4 @@ def main(): print(obs) if __name__=="__main__": - main() \ No newline at end of file + main() From de6d370b38f6ca54b5fa78446707879a0b773eb4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 17:36:04 -0700 Subject: [PATCH 09/11] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index adab003fb..395bd22b9 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.3.7', + version='1.3.8', 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 ec25f663c22339d07a4b4d6be56d9a19a98147ce Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 19:46:54 -0700 Subject: [PATCH 10/11] fix getAABB.py on mac osx fix humanoid_benchmark.py and explain how to read/interpret that file in Google Chrome --- examples/pybullet/examples/getAABB.py | 3 ++- examples/pybullet/examples/humanoid_benchmark.py | 12 ++++++++---- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/examples/pybullet/examples/getAABB.py b/examples/pybullet/examples/getAABB.py index ef1d3ee3f..a43c144e1 100644 --- a/examples/pybullet/examples/getAABB.py +++ b/examples/pybullet/examples/getAABB.py @@ -74,4 +74,5 @@ for i in range (p.getNumJoints(r2d2)): while(1): - a=0 \ No newline at end of file + a=0 + p.stepSimulation() diff --git a/examples/pybullet/examples/humanoid_benchmark.py b/examples/pybullet/examples/humanoid_benchmark.py index 30d479443..5c056cdaa 100644 --- a/examples/pybullet/examples/humanoid_benchmark.py +++ b/examples/pybullet/examples/humanoid_benchmark.py @@ -6,10 +6,10 @@ p.setPhysicsEngineParameter(numSolverIterations=5) p.setPhysicsEngineParameter(fixedTimeStep=1./240.) p.setPhysicsEngineParameter(numSubSteps=1) +p.loadURDF("plane.urdf") + objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") ob = objects[0] -p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000]) -ob = objects[1] p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923]) jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ] for jointIndex in range (p.getNumJoints(ob)): @@ -24,9 +24,13 @@ p.setRealTimeSimulation(0) #now do a benchmark print("Starting benchmark") -logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json") +fileName = "pybullet_humanoid_timings.json" + +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,fileName) for i in range(1000): p.stepSimulation() p.stopStateLogging(logId) -print("ended benchmark") \ No newline at end of file +print("ended benchmark") +print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName) + From 3d3e391b6adfc9c72d7d806c2283237f6ba3b85e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 11 Sep 2017 21:49:39 -0700 Subject: [PATCH 11/11] Support SDF capsule and cylinder. --- .../Importers/ImportURDFDemo/UrdfParser.cpp | 62 ++++++++++++++----- 1 file changed, 46 insertions(+), 16 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index c4a370c0d..e4e7f68ba 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -380,29 +380,59 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* else if (type_name == "cylinder") { geom.m_type = URDF_GEOM_CYLINDER; - if (!shape->Attribute("length") || - !shape->Attribute("radius")) - { - logger->reportError("Cylinder shape must have both length and radius attributes"); - return false; - } geom.m_hasFromTo = false; - geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); - geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(shape->Attribute("length")); + geom.m_capsuleRadius = 0.1; + geom.m_capsuleHeight = 0.1; + if (m_parseSDF) + { + if (TiXmlElement* scale = shape->FirstChildElement("radius")) + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(scale->GetText()); + } + if (TiXmlElement* scale = shape->FirstChildElement("length")) + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(scale->GetText()); + } + } else + { + if (!shape->Attribute("length") ||!shape->Attribute("radius")) + { + logger->reportError("Cylinder shape must have both length and radius attributes"); + return false; + } + geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(shape->Attribute("length")); + } } else if (type_name == "capsule") { geom.m_type = URDF_GEOM_CAPSULE; - if (!shape->Attribute("length") || - !shape->Attribute("radius")) - { - logger->reportError("Capsule shape must have both length and radius attributes"); - return false; - } geom.m_hasFromTo = false; - geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); - geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(shape->Attribute("length")); + if (m_parseSDF) + { + if (TiXmlElement* scale = shape->FirstChildElement("radius")) + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(scale->GetText()); + } + if (TiXmlElement* scale = shape->FirstChildElement("length")) + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(scale->GetText()); + } + } else + { + if (!shape->Attribute("length") || !shape->Attribute("radius")) + { + logger->reportError("Capsule shape must have both length and radius attributes"); + return false; + } + geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast(shape->Attribute("radius")); + geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast(shape->Attribute("length")); + } } else if (type_name == "mesh") {