diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 7814bfee5..66050767e 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -78,7 +78,7 @@ typedef b3PoolBodyHandle b3PluginHandle; struct b3PluginManagerInternalData { b3ResizablePool m_plugins; - b3HashMap m_pluginMap; + b3HashMap m_pluginMap; PhysicsDirect* m_physicsDirect; b3AlignedObjectArray m_keyEvents; b3AlignedObjectArray m_vrEvents; @@ -102,8 +102,12 @@ b3PluginManager::~b3PluginManager() { while (m_data->m_pluginMap.size()) { - b3PluginHandle** plugin = m_data->m_pluginMap.getAtIndex(0); - unloadPlugin((*plugin)->m_pluginUniqueId); + int* pluginUidPtr = m_data->m_pluginMap.getAtIndex(0); + if (pluginUidPtr) + { + int pluginUid = *pluginUidPtr; + unloadPlugin(*pluginUidPtr); + } } delete m_data->m_physicsDirect; m_data->m_pluginMap.clear(); @@ -140,11 +144,11 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) { int pluginUniqueId = -1; - b3PluginHandle** pluginOrgPtr = m_data->m_pluginMap.find(pluginPath); - if (pluginOrgPtr) + int* pluginUidPtr = m_data->m_pluginMap.find(pluginPath); + if (pluginUidPtr) { //already loaded - pluginUniqueId = (*pluginOrgPtr)->m_pluginUniqueId; + pluginUniqueId = *pluginUidPtr; } else { @@ -185,7 +189,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) plugin->m_ownsPluginHandle = true; plugin->m_pluginHandle = pluginHandle; plugin->m_pluginPath = pluginPath; - m_data->m_pluginMap.insert(pluginPath, plugin); + m_data->m_pluginMap.insert(pluginPath, pluginUniqueId); } else { @@ -246,16 +250,19 @@ void b3PluginManager::tickPlugins(double timeStep, bool isPreTick) { for (int i=0;im_pluginMap.size();i++) { - b3PluginHandle** pluginPtr = m_data->m_pluginMap.getAtIndex(i); + int* pluginUidPtr = m_data->m_pluginMap.getAtIndex(i); b3PluginHandle* plugin = 0; - if (pluginPtr && *pluginPtr) + + if (pluginUidPtr) { - plugin = *pluginPtr; + int pluginUid = *pluginUidPtr; + plugin = m_data->m_plugins.getHandle(pluginUid); } else { continue; } + PFN_TICK tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc; if (tick) { @@ -319,7 +326,7 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT pluginHandle->m_userPointer = 0; - m_data->m_pluginMap.insert(pluginPath, pluginHandle); + m_data->m_pluginMap.insert(pluginPath, pluginUniqueId); { b3PluginContext context; diff --git a/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_diverse_object_grasping.py b/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_diverse_object_grasping.py index 9e4af6a08..da40d6e94 100644 --- a/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_diverse_object_grasping.py +++ b/examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_diverse_object_grasping.py @@ -47,7 +47,7 @@ def main(): print(obs) episode_rew = 0 while not done: - env.render() + env.render(mode='human') act = policy.sample_action(obs, .1) print("Action") print(act) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py index 277e52ba2..38145c9bf 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py @@ -17,7 +17,7 @@ import time import subprocess import pybullet as p import pybullet_data - +from pkg_resources import parse_version logger = logging.getLogger(__name__) @@ -100,7 +100,8 @@ class CartPoleBulletEnv(gym.Env): def _render(self, mode='human', close=False): return - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py index 8ed171be0..ed49cff59 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py @@ -14,6 +14,8 @@ import pybullet as p from . import kuka import random import pybullet_data +from pkg_resources import parse_version + maxSteps = 1000 RENDER_HEIGHT = 720 @@ -254,7 +256,8 @@ class KukaCamGymEnv(gym.Env): #print(reward) return reward - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py index 6fa57e76a..848f10a90 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py @@ -13,6 +13,7 @@ import pybullet as p from . import kuka import random import pybullet_data +from pkg_resources import parse_version largeValObservation = 100 @@ -185,6 +186,7 @@ class KukaGymEnv(gym.Env): def _render(self, mode="rgb_array", close=False): if mode != "rgb_array": return np.array([]) + base_pos,orn = self._p.getBasePositionAndOrientation(self._kuka.kukaUid) view_matrix = self._p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, @@ -199,7 +201,12 @@ class KukaGymEnv(gym.Env): (_, _, px, _, _) = self._p.getCameraImage( width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=self._p.ER_BULLET_HARDWARE_OPENGL) - rgb_array = np.array(px) + #renderer=self._p.ER_TINY_RENDERER) + + + rgb_array = np.array(px, dtype=np.uint8) + rgb_array = np.reshape(rgb_array, (RENDER_WIDTH, RENDER_HEIGHT, 4)) + rgb_array = rgb_array[:, :, :3] return rgb_array @@ -276,7 +283,8 @@ class KukaGymEnv(gym.Env): #print(reward) return reward - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py index b49e0d006..46ac77e73 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py @@ -10,7 +10,8 @@ import pybullet_data import pdb import distutils.dir_util import glob - +from pkg_resources import parse_version +import gym class KukaDiverseObjectEnv(KukaGymEnv): """Class for Kuka environment with diverse objects. @@ -323,4 +324,10 @@ class KukaDiverseObjectEnv(KukaGymEnv): selected_objects_filenames = [] for object_index in selected_objects: selected_objects_filenames += [found_object_directories[object_index]] - return selected_objects_filenames \ No newline at end of file + return selected_objects_filenames + + if parse_version(gym.__version__)>=parse_version('0.9.6'): + + reset = _reset + + step = _step \ No newline at end of file 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 158128067..d82774d14 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py @@ -20,6 +20,7 @@ from . import minitaur import os import pybullet_data from . import minitaur_env_randomizer +from pkg_resources import parse_version NUM_SUBSTEPS = 5 NUM_MOTORS = 8 @@ -377,7 +378,8 @@ class MinitaurBulletEnv(gym.Env): self.minitaur.GetObservationUpperBound()) return observation - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index bdde5d963..c0e275774 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -14,6 +14,7 @@ from . import racecar import random from . import bullet_client import pybullet_data +from pkg_resources import parse_version RENDER_HEIGHT = 720 RENDER_WIDTH = 960 @@ -175,7 +176,8 @@ class RacecarGymEnv(gym.Env): #print(reward) return reward - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py index 277453e14..9185b2373 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py @@ -14,6 +14,7 @@ from . import bullet_client from . import racecar import random import pybullet_data +from pkg_resources import parse_version RENDER_HEIGHT = 720 RENDER_WIDTH = 960 @@ -190,7 +191,8 @@ class RacecarZEDGymEnv(gym.Env): #print(reward) return reward - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py index 53b19b7a8..293e8c0c8 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoidGymEnv.py @@ -12,7 +12,7 @@ import time import pybullet as p from . import simpleHumanoid import random - +from pkg_resources import parse_version import pybullet_data @@ -110,7 +110,8 @@ class SimpleHumanoidGymEnv(gym.Env): print(reward) return reward - render = _render - reset = _reset - seed = _seed - step = _step + if parse_version(gym.__version__)>=parse_version('0.9.6'): + render = _render + reset = _reset + seed = _seed + step = _step diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index ceafb6404..c03f0e608 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -1,7 +1,7 @@ import gym, gym.spaces, gym.utils, gym.utils.seeding import numpy as np import pybullet as p - +from pkg_resources import parse_version class MJCFBaseBulletEnv(gym.Env): """ @@ -115,10 +115,11 @@ class MJCFBaseBulletEnv(gym.Env): def step(self, *args, **kwargs): return self._step(*args, **kwargs) - close = _close - render = _render - reset = _reset - seed = _seed + if parse_version(gym.__version__)>=parse_version('0.9.6'): + close = _close + render = _render + reset = _reset + seed = _seed class Camera: diff --git a/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py b/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py index 2aebfd79a..8057b2dc5 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py +++ b/examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py @@ -1,6 +1,12 @@ r"""An example to run of the minitaur gym environment with sine gaits. """ +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 math import numpy as np from pybullet_envs.bullet import minitaur_gym_env diff --git a/examples/pybullet/gym/pybullet_envs/examples/testBike.py b/examples/pybullet/gym/pybullet_envs/examples/testBike.py index 3d6e89c91..b62a8668e 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/testBike.py +++ b/examples/pybullet/gym/pybullet_envs/examples/testBike.py @@ -1,3 +1,9 @@ +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 math import time