diff --git a/Extras/ConvexDecomposition/premake4.lua b/Extras/ConvexDecomposition/premake4.lua index 05ae0f685..9310593a7 100644 --- a/Extras/ConvexDecomposition/premake4.lua +++ b/Extras/ConvexDecomposition/premake4.lua @@ -3,6 +3,9 @@ kind "StaticLib" includedirs {".","../../src"} + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", "**.h" diff --git a/Extras/HACD/premake4.lua b/Extras/HACD/premake4.lua index 14b83929a..c2f03c173 100644 --- a/Extras/HACD/premake4.lua +++ b/Extras/HACD/premake4.lua @@ -3,6 +3,9 @@ kind "StaticLib" includedirs {"."} + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", "**.h" diff --git a/Extras/InverseDynamics/premake4.lua b/Extras/InverseDynamics/premake4.lua index 0a524d46a..c1e263bfb 100644 --- a/Extras/InverseDynamics/premake4.lua +++ b/Extras/InverseDynamics/premake4.lua @@ -6,6 +6,9 @@ "../../src" } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "*.cpp", "*.hpp" diff --git a/Extras/Serialize/BulletFileLoader/premake4.lua b/Extras/Serialize/BulletFileLoader/premake4.lua index a1a814278..9fa0cb00c 100644 --- a/Extras/Serialize/BulletFileLoader/premake4.lua +++ b/Extras/Serialize/BulletFileLoader/premake4.lua @@ -2,6 +2,10 @@ kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end + includedirs { "../../../src" } diff --git a/Extras/Serialize/BulletWorldImporter/premake4.lua b/Extras/Serialize/BulletWorldImporter/premake4.lua index 4d2d9d879..e7dbd9a77 100644 --- a/Extras/Serialize/BulletWorldImporter/premake4.lua +++ b/Extras/Serialize/BulletWorldImporter/premake4.lua @@ -6,7 +6,11 @@ "../BulletFileLoader", "../../../src" } - + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + files { "**.cpp", "**.h" diff --git a/Extras/Serialize/BulletXmlWorldImporter/premake4.lua b/Extras/Serialize/BulletXmlWorldImporter/premake4.lua index 147f3e951..d940dfd8a 100644 --- a/Extras/Serialize/BulletXmlWorldImporter/premake4.lua +++ b/Extras/Serialize/BulletXmlWorldImporter/premake4.lua @@ -1,7 +1,7 @@ project "BulletXmlWorldImporter" kind "StaticLib" - targetdir "../../lib" + --targetdir "../../lib" includedirs { "../BulletWorldImporter", "../BulletFileLoader", @@ -11,4 +11,4 @@ files { "**.cpp", "**.h" - } \ No newline at end of file + } diff --git a/Extras/VHACD/src/premake4.lua b/Extras/VHACD/src/premake4.lua index 3566943be..10083cffe 100644 --- a/Extras/VHACD/src/premake4.lua +++ b/Extras/VHACD/src/premake4.lua @@ -1,6 +1,9 @@ project "vhacd" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "../inc","../public", } diff --git a/data/multibody.bullet b/data/multibody.bullet index 9f8e7ab89..d621a77ca 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 65e38ece8..3c38d756d 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -3,7 +3,11 @@ project "App_BulletExampleBrowser" language "C++" kind "ConsoleApp" - + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + hasCL = findOpenCL("clew") if (hasCL) then @@ -209,6 +213,10 @@ project "BulletExampleBrowserLib" "../../src", "../ThirdPartyLibs", } + + if os.is("Linux") then + buildoptions{"-fPIC"} + end if _OPTIONS["lua"] then includedirs{"../ThirdPartyLibs/lua-5.2.3/src"} diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index 6edb4a69f..33e54d0fa 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -1992,15 +1992,15 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons B3_ATTRIBUTE_ALIGNED16(struct) SortableTransparentInstance { b3Scalar m_projection; - + int m_shapeIndex; int m_instanceId; }; B3_ATTRIBUTE_ALIGNED16(struct) TransparentDistanceSortPredicate { - - inline bool operator() (const SortableTransparentInstance& a, const SortableTransparentInstance& b) const + + inline bool operator() (const SortableTransparentInstance& a, const SortableTransparentInstance& b) const { return (a.m_projection > b.m_projection); @@ -2218,7 +2218,6 @@ b3Assert(glGetError() ==GL_NO_ERROR); } b3AlignedObjectArray transparentInstances; - { int curOffset = 0; //GLuint lastBindTexture = 0; @@ -2229,7 +2228,8 @@ b3Assert(glGetError() ==GL_NO_ERROR); m_data->m_activeCamera->getCameraForwardVector(fwd); b3Vector3 camForwardVec; camForwardVec.setValue(fwd[0],fwd[1],fwd[2]); - + + for (int obj=0;objm_instance_positions_ptr[inst.m_instanceId*4+0], - m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], - m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); + m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); centerPosition *= -1;//reverse sort opaque instances inst.m_projection = centerPosition.dot(camForwardVec); transparentInstances.push_back(inst); @@ -2256,10 +2256,10 @@ b3Assert(glGetError() ==GL_NO_ERROR); { inst.m_instanceId = curOffset+i; b3Vector3 centerPosition; - + centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0], - m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], - m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); + m_data->m_instance_positions_ptr[inst.m_instanceId*4+1], + m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]); inst.m_projection = centerPosition.dot(camForwardVec); transparentInstances.push_back(inst); } @@ -2270,6 +2270,7 @@ b3Assert(glGetError() ==GL_NO_ERROR); TransparentDistanceSortPredicate sorter; transparentInstances.quickSort(sorter); + } diff --git a/examples/OpenGLWindow/premake4.lua b/examples/OpenGLWindow/premake4.lua index 17dfdda09..e0b163678 100644 --- a/examples/OpenGLWindow/premake4.lua +++ b/examples/OpenGLWindow/premake4.lua @@ -12,7 +12,11 @@ "../ThirdPartyLibs", "../../src", } - + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + --links { --} diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index f22e4c0aa..3268e351e 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -1,7 +1,7 @@ #include "b3RobotSimulatorClientAPI.h" #include "../SharedMemory/PhysicsClientC_API.h" -#include "b3RobotSimulatorClientAPI_InternalData.h"" +#include "b3RobotSimulatorClientAPI_InternalData.h" #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //PHYSICS_UDP diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 4ea326d98..d1ab3af24 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -92,13 +92,14 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs ) +IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) + LINK_LIBRARIES(BulletSoftBody) +ENDIF() + LINK_LIBRARIES( Bullet3Common BulletWorldImporter BulletFileLoader BulletInverseDynamicsUtils BulletInverseDynamics BulletDynamics BulletCollision LinearMath BussIK ) -IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) - LINK_LIBRARIES(BulletSoftBody) -ENDIF() IF (WIN32) ADD_EXECUTABLE(App_PhysicsServer_SharedMemory @@ -149,7 +150,7 @@ INCLUDE_DIRECTORIES( ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE) LINK_LIBRARIES( - Bullet3Common BulletWorldImporter BulletInverseDynamicsUtils BulletInverseDynamics BulletDynamics BulletCollision LinearMath BussIK OpenGLWindow + BulletSoftBody Bullet3Common BulletWorldImporter BulletInverseDynamicsUtils BulletInverseDynamics BulletDynamics BulletCollision LinearMath BussIK OpenGLWindow ) 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/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index a81acd4b9..fd5f87543 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -12,6 +12,9 @@ includedirs {".","../../src", "../ThirdPartyLibs"} links { "BulletSoftBody", "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK" } +if os.is("Linux") then + links{"dl"} +end language "C++" diff --git a/examples/SharedMemory/tcp/premake4.lua b/examples/SharedMemory/tcp/premake4.lua index 790294228..46596cec1 100644 --- a/examples/SharedMemory/tcp/premake4.lua +++ b/examples/SharedMemory/tcp/premake4.lua @@ -73,6 +73,7 @@ links { end if os.is("Linux") then defines {"_LINUX"} + links{"dl"} end if os.is("MacOSX") then defines {"_DARWIN"} diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua index 1255c236c..dd8f64d61 100644 --- a/examples/SharedMemory/udp/premake4.lua +++ b/examples/SharedMemory/udp/premake4.lua @@ -14,6 +14,7 @@ project ("App_PhysicsServerSharedMemoryBridgeUDP") links {"Ws2_32","Winmm"} end if os.is("Linux") then + links{"dl"} end if os.is("MacOSX") then end @@ -68,6 +69,9 @@ if os.is("Windows") then defines { "WIN32" } links {"Ws2_32","Winmm"} end + if os.is("Linux") then + links{"dl"} + end language "C++" diff --git a/examples/ThirdPartyLibs/BussIK/premake4.lua b/examples/ThirdPartyLibs/BussIK/premake4.lua index 74a0630a0..a999b0e0f 100644 --- a/examples/ThirdPartyLibs/BussIK/premake4.lua +++ b/examples/ThirdPartyLibs/BussIK/premake4.lua @@ -5,6 +5,9 @@ includedirs { "." } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "*.cpp", "*.h", diff --git a/examples/ThirdPartyLibs/Gwen/premake4.lua b/examples/ThirdPartyLibs/Gwen/premake4.lua index 74d16aac9..c456a8da3 100644 --- a/examples/ThirdPartyLibs/Gwen/premake4.lua +++ b/examples/ThirdPartyLibs/Gwen/premake4.lua @@ -11,6 +11,9 @@ end defines { "GWEN_COMPILE_STATIC" } defines { "DONT_USE_GLUT"} + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { ".",".." } diff --git a/examples/ThirdPartyLibs/clsocket/premake4.lua b/examples/ThirdPartyLibs/clsocket/premake4.lua index 57447b1fa..9e3ce2d0b 100644 --- a/examples/ThirdPartyLibs/clsocket/premake4.lua +++ b/examples/ThirdPartyLibs/clsocket/premake4.lua @@ -16,6 +16,9 @@ includedirs { ".","include","src" } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "src/SimpleSocket.cpp", "src/ActiveSocket.cpp", diff --git a/examples/ThirdPartyLibs/enet/premake4.lua b/examples/ThirdPartyLibs/enet/premake4.lua index 05390e766..4eb2036b6 100644 --- a/examples/ThirdPartyLibs/enet/premake4.lua +++ b/examples/ThirdPartyLibs/enet/premake4.lua @@ -18,6 +18,9 @@ includedirs { ".","include" } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "callbacks.c", "compress.c", diff --git a/examples/ThirdPartyLibs/lua-5.2.3/premake4.lua b/examples/ThirdPartyLibs/lua-5.2.3/premake4.lua index 504ccb83c..a82fff668 100644 --- a/examples/ThirdPartyLibs/lua-5.2.3/premake4.lua +++ b/examples/ThirdPartyLibs/lua-5.2.3/premake4.lua @@ -18,6 +18,9 @@ includedirs { "src" } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "src/*.c", "src/*.h" diff --git a/examples/ThirdPartyLibs/serial/premake4.lua b/examples/ThirdPartyLibs/serial/premake4.lua index f57a6618f..d2dfeb637 100644 --- a/examples/ThirdPartyLibs/serial/premake4.lua +++ b/examples/ThirdPartyLibs/serial/premake4.lua @@ -4,6 +4,9 @@ includedirs {"include"} + if os.is("Linux") then + buildoptions{"-fPIC"} + end if os.is("Windows") then files{ "src/impl/win.cc", 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 e28a9e962..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__) @@ -64,10 +64,10 @@ class CartPoleBulletEnv(gym.Env): # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state - + dv = 0.1 deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] - + p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) done = x < -self.x_threshold \ @@ -99,3 +99,9 @@ class CartPoleBulletEnv(gym.Env): def _render(self, mode='human', close=False): return + + 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 4e7d7e70e..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 @@ -56,8 +58,8 @@ class KukaCamGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -74,15 +76,15 @@ class KukaCamGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.5 +0.2*random.random() ypos = 0 +0.25*random.random() ang = 3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -98,7 +100,7 @@ class KukaCamGymEnv(gym.Env): return [seed] def getExtendedObservation(self): - + #camEyePos = [0.03,0.236,0.54] #distance = 1.06 #pitch=-56 @@ -118,13 +120,13 @@ class KukaCamGymEnv(gym.Env): viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0] #projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] - + img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix) rgb=img_arr[2] np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.01 @@ -142,7 +144,7 @@ class KukaCamGymEnv(gym.Env): realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -158,11 +160,11 @@ class KukaCamGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() reward = self._reward() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -190,18 +192,18 @@ class KukaCamGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -212,7 +214,7 @@ class KukaCamGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -227,18 +229,18 @@ class KukaCamGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) - reward = -1000 + reward = -1000 numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -254,3 +256,8 @@ class KukaCamGymEnv(gym.Env): #print(reward) return reward + 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 b9ef16017..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 @@ -44,7 +45,7 @@ class KukaGymEnv(gym.Env): self._maxSteps = maxSteps self.terminated = 0 self._cam_dist = 1.3 - self._cam_yaw = 180 + self._cam_yaw = 180 self._cam_pitch = -40 self._p = p @@ -61,8 +62,8 @@ class KukaGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([largeValObservation] * observationDim) + + observation_high = np.array([largeValObservation] * observationDim) if (self._isDiscrete): self.action_space = spaces.Discrete(7) else: @@ -80,15 +81,15 @@ class KukaGymEnv(gym.Env): p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1]) - + p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - + xpos = 0.55 +0.12*random.random() ypos = 0 +0.2*random.random() ang = 3.14*0.5+3.1415925438*random.random() orn = p.getQuaternionFromEuler([0,0,ang]) self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3]) - + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -115,7 +116,7 @@ class KukaGymEnv(gym.Env): dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]] dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]] dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]] - + gripperEul = p.getEulerFromQuaternion(gripperOrn) #print("gripperEul") #print(gripperEul) @@ -126,17 +127,17 @@ class KukaGymEnv(gym.Env): #print(projectedBlockPos2D) #print("blockEulerInGripper") #print(blockEulerInGripper) - + #we return the relative x,y position and euler angle of block in gripper space blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]] - + #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1) #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1) - + self._observation.extend(list(blockInGripperPosXYEulZ)) return self._observation - + def _step(self, action): if (self._isDiscrete): dv = 0.005 @@ -154,7 +155,7 @@ class KukaGymEnv(gym.Env): f = 0.3 realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) - + def step2(self, action): for i in range(self._actionRepeat): self._kuka.applyAction(action) @@ -168,7 +169,7 @@ class KukaGymEnv(gym.Env): #print("self._envStepCounter") #print(self._envStepCounter) - + done = self._termination() npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]]) actionCost = np.linalg.norm(npaction)*10. @@ -177,14 +178,15 @@ class KukaGymEnv(gym.Env): reward = self._reward()-actionCost #print("reward") #print(reward) - + #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} 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 @@ -208,18 +215,18 @@ class KukaGymEnv(gym.Env): #print (self._kuka.endEffectorPos[2]) state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) actualEndEffectorPos = state[0] - + #print("self._envStepCounter") #print(self._envStepCounter) if (self.terminated or self._envStepCounter>self._maxSteps): self._observation = self.getExtendedObservation() return True - maxDist = 0.005 + maxDist = 0.005 closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist) - + if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43): self.terminated = 1 - + #print("terminating, closing gripper, attempting grasp") #start grasp and terminate fingerAngle = 0.3 @@ -230,7 +237,7 @@ class KukaGymEnv(gym.Env): fingerAngle = fingerAngle-(0.3/100.) if (fingerAngle<0): fingerAngle=0 - + for i in range (1000): graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) @@ -245,19 +252,19 @@ class KukaGymEnv(gym.Env): if (actualEndEffectorPos[2]>0.5): break - + self._observation = self.getExtendedObservation() return True return False - + def _reward(self): - + #rewards is height of target object blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) - closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex) reward = -1000 - + numPt = len(closestPoints) #print(numPt) if (numPt>0): @@ -276,10 +283,8 @@ class KukaGymEnv(gym.Env): #print(reward) return reward - def reset(self): - """Resets the state of the environment and returns an initial observation. - - Returns: observation (object): the initial observation of the - space. - """ - return self._reset() \ No newline at end of file + 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_duck_gym_env.py b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py index 44b41dd7d..b5c3375b4 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py @@ -385,3 +385,8 @@ class MinitaurBulletDuckEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + render = _render + reset = _reset + seed = _seed + step = _step 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 38ec631a5..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 @@ -376,3 +377,9 @@ class MinitaurBulletEnv(gym.Env): scale=self._observation_noise_stdev, size=observation.shape) * self.minitaur.GetObservationUpperBound()) return observation + + 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 7537d54c1..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 @@ -50,8 +51,8 @@ class RacecarGymEnv(gym.Env): #self.reset() observationDim = 2 #len(self.getExtendedObservation()) #print("observationDim") - #print(observationDim) - # observation_high = np.array([np.finfo(np.float32).max] * observationDim) + #print(observationDim) + # observation_high = np.array([np.finfo(np.float32).max] * observationDim) observation_high = np.ones(observationDim) * 1000 #np.inf if (isDiscrete): self.action_space = spaces.Discrete(9) @@ -59,7 +60,7 @@ class RacecarGymEnv(gym.Env): 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.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -74,14 +75,14 @@ class RacecarGymEnv(gym.Env): # pos,orn = self._p.getBasePositionAndOrientation(i) # newpos = [pos[0],pos[1],pos[2]-0.1] # self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -101,18 +102,18 @@ class RacecarGymEnv(gym.Env): def getExtendedObservation(self): self._observation = [] #self._racecar.getObservation() carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) - + self._observation.extend([ballPosInCar[0],ballPosInCar[1]]) return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + 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] @@ -128,14 +129,14 @@ class RacecarGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -159,13 +160,13 @@ class RacecarGymEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array - + def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -174,3 +175,9 @@ class RacecarGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + 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 d9546de8c..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 @@ -40,7 +41,7 @@ class RacecarZEDGymEnv(gym.Env): self._renders = renders self._width = 100 self._height = 10 - + self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient( @@ -53,8 +54,8 @@ class RacecarZEDGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) if (isDiscrete): self.action_space = spaces.Discrete(9) else: @@ -77,14 +78,14 @@ class RacecarZEDGymEnv(gym.Env): pos,orn = self._p.getBasePositionAndOrientation(i) newpos = [pos[0],pos[1],pos[2]+0.1] self._p.resetBasePositionAndOrientation(i,newpos,orn) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2red.urdf"),[ballx,bally,ballz]) self._p.setGravity(0,0,-10) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) @@ -104,13 +105,13 @@ class RacecarZEDGymEnv(gym.Env): def getExtendedObservation(self): carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) carmat = self._p.getMatrixFromQuaternion(carorn) - ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) dist0 = 0.3 dist1 = 1. eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3] - targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] + targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3] up = [carmat[2],carmat[5],carmat[8]] viewMat = self._p.computeViewMatrix(eyePos,targetPos,up) #viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2) @@ -122,12 +123,12 @@ class RacecarZEDGymEnv(gym.Env): np_img_arr = np.reshape(rgb, (self._height, self._width, 4)) self._observation = np_img_arr return self._observation - + def _step(self, action): if (self._renders): basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) - + 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] @@ -143,14 +144,14 @@ class RacecarZEDGymEnv(gym.Env): if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() - + if self._termination(): break self._envStepCounter += 1 reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -177,10 +178,10 @@ class RacecarZEDGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): - closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) - + closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) + numPt = len(closestPoints) reward=-1000 #print(numPt) @@ -189,3 +190,9 @@ class RacecarZEDGymEnv(gym.Env): reward = -closestPoints[0][8] #print(reward) return reward + + 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 c632e7f31..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 @@ -46,8 +46,8 @@ class SimpleHumanoidGymEnv(gym.Env): observationDim = len(self.getExtendedObservation()) #print("observationDim") #print(observationDim) - - observation_high = np.array([np.finfo(np.float32).max] * observationDim) + + observation_high = np.array([np.finfo(np.float32).max] * observationDim) self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None @@ -57,14 +57,14 @@ class SimpleHumanoidGymEnv(gym.Env): #p.setPhysicsEngineParameter(numSolverIterations=300) p.setTimeStep(self._timeStep) p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf")) - + dist = 5 +2.*random.random() ang = 2.*3.1415925438*random.random() - + ballx = dist * math.sin(ang) bally = dist * math.cos(ang) ballz = 1 - + p.setGravity(0,0,-10) self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -82,7 +82,7 @@ class SimpleHumanoidGymEnv(gym.Env): def getExtendedObservation(self): self._observation = self._humanoid.getObservation() return self._observation - + def _step(self, action): self._humanoid.applyAction(action) for i in range(self._actionRepeat): @@ -96,7 +96,7 @@ class SimpleHumanoidGymEnv(gym.Env): reward = self._reward() done = self._termination() #print("len=%r" % len(self._observation)) - + return np.array(self._observation), reward, done, {} def _render(self, mode='human', close=False): @@ -104,8 +104,14 @@ class SimpleHumanoidGymEnv(gym.Env): def _termination(self): return self._envStepCounter>1000 - + def _reward(self): reward=self._humanoid.distance print(reward) return reward + + 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 5d49c22c3..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): """ @@ -43,7 +43,7 @@ class MJCFBaseBulletEnv(gym.Env): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False - + self.physicsClientId = 0 else: self.ownsPhysicsClient = True @@ -75,12 +75,12 @@ class MJCFBaseBulletEnv(gym.Env): self.isRender = True if mode != "rgb_array": return np.array([]) - + base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz - + view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, @@ -100,6 +100,7 @@ class MJCFBaseBulletEnv(gym.Env): rgb_array = rgb_array[:, :, :3] return rgb_array + def _close(self): if (self.ownsPhysicsClient): if (self.physicsClientId>=0): @@ -109,6 +110,18 @@ class MJCFBaseBulletEnv(gym.Env): def HUD(self, state, a, done): pass + # backwards compatibility for gym >= v0.9.x + # for extension of this class. + def step(self, *args, **kwargs): + return self._step(*args, **kwargs) + + if parse_version(gym.__version__)>=parse_version('0.9.6'): + close = _close + render = _render + reset = _reset + seed = _seed + + class Camera: def __init__(self): pass 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 diff --git a/src/Bullet3Collision/premake4.lua b/src/Bullet3Collision/premake4.lua index 0b47f8ea5..bd0da541e 100644 --- a/src/Bullet3Collision/premake4.lua +++ b/src/Bullet3Collision/premake4.lua @@ -6,6 +6,9 @@ includedirs {".."} + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", diff --git a/src/Bullet3Common/premake4.lua b/src/Bullet3Common/premake4.lua index 1331c6327..f03573ec2 100644 --- a/src/Bullet3Common/premake4.lua +++ b/src/Bullet3Common/premake4.lua @@ -4,6 +4,10 @@ kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end + includedirs {".."} files { diff --git a/src/Bullet3Dynamics/premake4.lua b/src/Bullet3Dynamics/premake4.lua index 669336a6a..e05b2cd19 100644 --- a/src/Bullet3Dynamics/premake4.lua +++ b/src/Bullet3Dynamics/premake4.lua @@ -8,6 +8,9 @@ ".." } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", diff --git a/src/Bullet3Geometry/premake4.lua b/src/Bullet3Geometry/premake4.lua index 1a230f8c0..cce93f7ee 100644 --- a/src/Bullet3Geometry/premake4.lua +++ b/src/Bullet3Geometry/premake4.lua @@ -6,6 +6,9 @@ includedirs {".."} + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", diff --git a/src/Bullet3OpenCL/premake4.lua b/src/Bullet3OpenCL/premake4.lua index 55a861363..ee35fdb52 100644 --- a/src/Bullet3OpenCL/premake4.lua +++ b/src/Bullet3OpenCL/premake4.lua @@ -9,6 +9,9 @@ function createProject(vendor) kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { ".",".." diff --git a/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua b/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua index ec2f0a51a..b9ee301be 100644 --- a/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua +++ b/src/Bullet3Serialize/Bullet2FileLoader/premake4.lua @@ -5,6 +5,10 @@ includedirs { "../../../src" } + + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", diff --git a/src/BulletCollision/premake4.lua b/src/BulletCollision/premake4.lua index 70407771d..6297bf3df 100644 --- a/src/BulletCollision/premake4.lua +++ b/src/BulletCollision/premake4.lua @@ -1,6 +1,9 @@ project "BulletCollision" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "..", } diff --git a/src/BulletDynamics/premake4.lua b/src/BulletDynamics/premake4.lua index 32414dce3..fdaf0513d 100644 --- a/src/BulletDynamics/premake4.lua +++ b/src/BulletDynamics/premake4.lua @@ -3,6 +3,9 @@ includedirs { "..", } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "Dynamics/*.cpp", "Dynamics/*.h", diff --git a/src/BulletInverseDynamics/premake4.lua b/src/BulletInverseDynamics/premake4.lua index 774e037b3..fdd176b09 100644 --- a/src/BulletInverseDynamics/premake4.lua +++ b/src/BulletInverseDynamics/premake4.lua @@ -1,6 +1,9 @@ project "BulletInverseDynamics" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "..", } diff --git a/src/BulletSoftBody/premake4.lua b/src/BulletSoftBody/premake4.lua index 57a575fb1..a75e33619 100644 --- a/src/BulletSoftBody/premake4.lua +++ b/src/BulletSoftBody/premake4.lua @@ -5,6 +5,9 @@ includedirs { "..", } + if os.is("Linux") then + buildoptions{"-fPIC"} + end files { "**.cpp", "**.h" diff --git a/src/LinearMath/premake4.lua b/src/LinearMath/premake4.lua index 524e2c316..5f0fda6bf 100644 --- a/src/LinearMath/premake4.lua +++ b/src/LinearMath/premake4.lua @@ -1,6 +1,9 @@ project "LinearMath" kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end includedirs { "..", } diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index cba4b587c..e0798123f 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -11,13 +11,13 @@ INCLUDE_DIRECTORIES( #ADD_DEFINITIONS(-DGTEST_HAS_PTHREAD=1) ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VARIADIC_MAX=10) +IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) + LINK_LIBRARIES(BulletSoftBody) +ENDIF() LINK_LIBRARIES( BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK ) -IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) - LINK_LIBRARIES(BulletSoftBody) -ENDIF() IF (NOT WIN32) LINK_LIBRARIES( pthread ) diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 31f19ada2..57f2adf49 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -164,6 +164,9 @@ project ("Test_PhysicsServerLoopBack") "BussIK", "LinearMath" } + if os.is("Linux") then + links{"dl"} + end files { "test.c", @@ -250,6 +253,9 @@ end "BussIK", "LinearMath" } + if os.is("Linux") then + links{"dl"} + end files { "test.c", diff --git a/test/gtest-1.7.0/premake4.lua b/test/gtest-1.7.0/premake4.lua index 79aade120..1d96f8315 100644 --- a/test/gtest-1.7.0/premake4.lua +++ b/test/gtest-1.7.0/premake4.lua @@ -16,3 +16,7 @@ includedirs { ".","include" } + if os.is("Linux") then + buildoptions{"-fPIC"} + end + \ No newline at end of file