From 528bd28e34a0959d2c48b84ac60828937003d886 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 31 Dec 2019 18:13:49 -0800 Subject: [PATCH 1/4] increase plane from 30 to 200 (to allow quadrupeds to run further, not fall off cliff) fix issue with gym.wrappers in pybullet_envs.agents.visualize_ppo.py --- examples/pybullet/gym/pybullet_data/plane.urdf | 4 ++-- examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_data/plane.urdf b/examples/pybullet/gym/pybullet_data/plane.urdf index b04f67e96..7608290cf 100644 --- a/examples/pybullet/gym/pybullet_data/plane.urdf +++ b/examples/pybullet/gym/pybullet_data/plane.urdf @@ -12,7 +12,7 @@ - + @@ -21,7 +21,7 @@ - + diff --git a/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py b/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py index 8524b6311..141f93c8f 100644 --- a/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py +++ b/examples/pybullet/gym/pybullet_envs/agents/visualize_ppo.py @@ -26,6 +26,7 @@ from __future__ import print_function import os import gym +from gym import wrappers import tensorflow as tf from . import tools From b6dea7ba643fa7e05ac93e91cd4c490118f7282e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 1 Jan 2020 18:47:46 -0800 Subject: [PATCH 2/4] fix a few pybullet Gym environments for rendering in stable_baselines if PYBULLET_EGL environment is set, try to enable EGL for faster rendering bump up pybullet to 2.6.2 --- data/multibody.bullet | Bin 16836 -> 16900 bytes .../pybullet/gym/pybullet_envs/env_bases.py | 43 +++++++++++++++--- .../gym/pybullet_envs/gym_locomotion_envs.py | 4 +- .../gym/pybullet_envs/scene_stadium.py | 2 +- .../gym/pybullet_utils/bullet_client.py | 11 ----- setup.py | 5 +- 6 files changed, 44 insertions(+), 21 deletions(-) diff --git a/data/multibody.bullet b/data/multibody.bullet index 563b8939bbaae7b8ceb5d8f8c6591cc6293fe864..0bfa1717be1f6b25a65b94a15d5750a3cb51599d 100644 GIT binary patch delta 2653 zcmai#d2~%z6o>bbm)x7-UeO>SNkb)wQt=|A3=h?&MMa2t3Zf{j8q%uaMMQ|Padj;n zbS7HWmWNhNk6Qjw9dtm=tv|YIXpPlc%j$QZ7gAHR)|Ye6KEL1I`<$HjvU|2FFIQom z3cGdd(tA{!5@&iwRaikCk*Yz>t12pjnnVqZ+F58lphjCW14F{%$6w#^;ql!YIyODA z!zxm(e8Kl8Y}+pUoUGT%MKC_*(0+F=>(csxQQ7uJ&t2i$=N}yEmDeY^tnfOM!%7BU z&RKKrRBlklVxOZy#Nxx-Y-N7M%pLZqv!SdaN3z3lWn|9Mfc7!=$VTa&@~J*Wk4cMi z$5xGSo!>hv$5-@}>x!?7@QK5Gf}M>MU7@eUdG*T8cB}C**W5=t*cnQ%`?{00F}YrE z=MrzHK`XOtfpsZhm^D&0iP`IQ+ZjqP9!U1JA34FXy>;gVS7o0pFTyO(SB|_Ob;i>m zG(f3?R->?JwaJqbwl7G_pIBT{kk`v9iiq-rMEn*QRy1+)m=ROP6-_Q4+1GO{>TtMc zYhp-ny{Mq8DnFo!%9DI~Tl$IDbHJG#pjud=sVPdgI;KV_hgF!`ELoRn;*jYu17?C7 zN@14hQU*mE^wD z8G$nbr-9SJY2c(ED>A()XTTY4J5wZ*e5;@umceRRZcR&1Ow+}~d;|S$giY`nd=8uG zEkRPHAsJUeH7tYGu-rP3o>-*e)$ZXg0pOqbmpnd4dxlrfRk*Mda{l6cGv-4 z*a=%;7kml3#juJq;xaVZN0qPX=o`tTn5r!OzSOGaiv_d^z}X{@-Um zw_CcetUo-{RvE>N`?RZ4Ir4Rzs_yt?AKzd&0ZP<5Oc_L5<#!^K^f~&994a ziQ`>{EAj-w+U=SMO}PLU;g;@6KBRf_7oMvW`y2j&YtohY6aKhPTqmv*?(#itq40C7V$A|VPIL1Tzk4vA6SsdSeUSkc)D zElmoU_bF6!N^5j>yxXLZd7na0-FBxMSEX@P8ds&&x~d+xU8#;vN2jCHYw7a2x-8!_ zjpcw3x|JqzoGL+ja?3Z~Yb*^tsC425#58GwhOaAmh?Dn_UOb=zX@)r-5}-M>fR*2js&F=`Ud<>#6EV0rVnaZzx3H zhyA|L5BkFZVot^s8Nfe-2pNce5DfNvhA<9=VNe9a(T>0~(#@n8M#0krM#C89W8oR* z<6t~I3(vs>c%Hq9FbO8BfixK`Q&g~&koX1mUxYQ>(^U3fA~ub2I?RBXkRpR<(f;o4 zFJ0YSWvS{Vvs7;>tL6BgMZ^ITye3wtYl{u@2DD9t0eh=@k2EPe_iX4E6#1;H}KmY ztyT_A-emn2yv^PU#&_Udcn{u(m2fMw)1P@LUrna0QQ`6teXWI$;S;*}lsuKb7cH`m z6O_@-`a(5~XKP@ni^0HPU@$OfHx27(w@f)@y(;28ImLr#1Iae}k_{)8L~LlS*uQy~ B`S}0< delta 2665 zcma);c~n$Y9LIl`8F&w2p5TaxKu7{YYQW&Y45FZAVWJ?Q;=ZJ2mRpPM4Jz(i`08Y) zW#w*WHC9?_o2TWREL*faD=W9uHVgHiK5v*&^vF4J&WC&N@BY5O<<6Z8b#-dbY!%&W zU|Cu3>Iq$DxICWv=*s%BY4x)fMhHLQVzyEf&62Q;OkIf_#?;2L2Y)VIubsAQx0xH3 z?NEZZ9`I7i9HZKrNnsOvca}9D%<+Z9BDUakA7cz;?znhA@E^@(g(AhUG+2RTv8qs;XC+krg`|Gqzxr64545pLxtSDdPRW2Dx00 zP?UF(#bC+ve`qg_P{;joQ8uMkm6QJ3Y6COiy}n1>4D=wiC3W47xu zQJh3^62(Cj2T>eEv8vT%xmoT>ZB!<@`DArj2kXI#y#XGDji%p|m1h?}wQh%(;AMD) z8q*~oorX?Br=io(Y3MX`PH)!u=5z&I5V!20r=8+qp3mID+$(XM->VWUud#j|cEN6V z19rlj@D{uc?~rGYXy$5fiq|fCrI2cJP`5&Dpf*q&s14LsCIhvB+CXif)=}%`X>W?p zQd|9WqZYe-1RqNg8N9L&J_&ptcB#ewDSQT>lY2iLVEzSs$^0wEucbS>9_V`ff1NKN zET*VJgoVurbrR}L66(Q(BHu^}U%&41EvdhQ@40q1DQ0c3Hdyxx?uXXb;AQ!@mV@B@ zfdYPnpWqOk{LHD2=oX)PZP3k$n^T(c^7ST#m8>z?#a=qGbom8-g~M0ic^|)Lo zEiTgP@*8_6;3Qn|wQlD15)~*qRE>tOCMSISItfSMC>(?J1do$azED<|A%R0q(a`Vk z2mDF1f5G3<2fwBb82B0Z8Tc7Z{BGduTPml~{6mg^p#i*Sq%RwNf*U#;Ivcv2kuu3@ zrhu~)aL(LPl%C1^&x08On)RGRBpg3qq(fRLheRo}p(w3@uGnk?@z53$lwI1Xek8hl z2WRQvoLSYq`9z^E4H6kvqBY&q`1^7BMEx&0QjMe1I4X^!(wZDq`6WlH@Xs|@^ zM&*$t7rLq*;>MmI*sqaxge+k00qb4#G8QVcb#Vy~OWB< ziTOmhnfWBR1#X4OFa@TvHw~u4ZE6@rM#}9fQtlw|o$Swm&2)Dc`*&lzhw)yx5AFxI z45vivH6J1maFP$ID(UM})l$ou&*Y3}slhT^4V5~A&p|L3#=$(84+~%+EP}tI^zAhivMnS|V^Da3F9X(9R3CQEshr;XH=>=0): + view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos, distance=self._cam_dist, yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, upAxisIndex=2) - proj_matrix = self._p.computeProjectionMatrixFOV(fov=60, + proj_matrix = self._p.computeProjectionMatrixFOV(fov=60, aspect=float(self._render_width) / self._render_height, nearVal=0.1, farVal=100.0) - (_, _, px, _, _) = self._p.getCameraImage(width=self._render_width, + (_, _, px, _, _) = self._p.getCameraImage(width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) - rgb_array = np.array(px) + try: + # Keep the previous orientation of the camera set by the user. + con_mode = self._p.getConnectionInfo()['connectionMethod'] + if con_mode==self._p.SHARED_MEMORY or con_mode == self._p.GUI: + [yaw, pitch, dist] = self._p.getDebugVisualizerCamera()[8:11] + self._p.resetDebugVisualizerCamera(dist, yaw, pitch, base_pos) + except: + pass + + else: + px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8) + rgb_array = np.array(px, dtype=np.uint8) rgb_array = np.reshape(np.array(px), (self._render_height, self._render_width, -1)) rgb_array = rgb_array[:, :, :3] return rgb_array diff --git a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py index d601c130b..cbf096600 100644 --- a/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py +++ b/examples/pybullet/gym/pybullet_envs/gym_locomotion_envs.py @@ -9,12 +9,12 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv): def __init__(self, robot, render=False): # print("WalkerBase::__init__ start") - MJCFBaseBulletEnv.__init__(self, robot, render) - self.camera_x = 0 self.walk_target_x = 1e3 # kilometer away self.walk_target_y = 0 self.stateId = -1 + MJCFBaseBulletEnv.__init__(self, robot, render) + def create_single_player_scene(self, bullet_client): self.stadium_scene = SinglePlayerStadiumScene(bullet_client, diff --git a/examples/pybullet/gym/pybullet_envs/scene_stadium.py b/examples/pybullet/gym/pybullet_envs/scene_stadium.py index 0ab34f202..388215f62 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_stadium.py +++ b/examples/pybullet/gym/pybullet_envs/scene_stadium.py @@ -32,7 +32,7 @@ class StadiumScene(Scene): for i in self.ground_plane_mjcf: self._p.changeDynamics(i, -1, lateralFriction=0.8, restitution=0.5) self._p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.8]) - self._p.configureDebugVisualizer(pybullet.COV_ENABLE_PLANAR_REFLECTION, 1) + self._p.configureDebugVisualizer(pybullet.COV_ENABLE_PLANAR_REFLECTION,i) # for j in range(p.getNumJoints(i)): # self._p.changeDynamics(i,j,lateralFriction=0) diff --git a/examples/pybullet/gym/pybullet_utils/bullet_client.py b/examples/pybullet/gym/pybullet_utils/bullet_client.py index 192cc1b87..832cee545 100644 --- a/examples/pybullet/gym/pybullet_utils/bullet_client.py +++ b/examples/pybullet/gym/pybullet_utils/bullet_client.py @@ -42,17 +42,6 @@ class BulletClient(object): """Inject the client id into Bullet functions.""" attribute = getattr(pybullet, name) - if inspect.isbuiltin(attribute): - if name not in [ - "invertTransform", - "multiplyTransforms", - "getMatrixFromQuaternion", - "getEulerFromQuaternion", - "computeViewMatrixFromYawPitchRoll", - "computeProjectionMatrixFOV", - "getQuaternionFromEuler", - ]: # A temporary hack for now. - attribute = functools.partial(attribute, physicsClientId=self._client) if name=="disconnect": self._client = -1 return attribute diff --git a/setup.py b/setup.py index 6d79f6988..9d17619d9 100644 --- a/setup.py +++ b/setup.py @@ -491,7 +491,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.6.1', + version='2.6.2', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= @@ -505,6 +505,9 @@ setup( 'game development', 'virtual reality', 'physics simulation', 'robotics', 'collision detection', 'opengl' ], + install_requires=[ + 'numpy', + ], ext_modules=extensions, classifiers=[ 'Development Status :: 5 - Production/Stable', From a9455ce89176f7e9a20d2f58738c5f01def80115 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 1 Jan 2020 18:53:32 -0800 Subject: [PATCH 3/4] add 'runServer.py' script in pybullet_utils, this will run a GUI server that accepts SHARED_MEMORY connections. Handy if you run a pybullet_envs Gym environment and want to visualize it. By default, bullet_client will try to connect to a shared memory connection first. --- .../pybullet/gym/pybullet_utils/runServer.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 examples/pybullet/gym/pybullet_utils/runServer.py diff --git a/examples/pybullet/gym/pybullet_utils/runServer.py b/examples/pybullet/gym/pybullet_utils/runServer.py new file mode 100644 index 000000000..054fe66a4 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/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): + #this is a no-op command, to allow GUI updates on Mac OSX (main thread) + p.setPhysicsEngineParameter() + time.sleep(0.01) From c39afa61cbf84f068173a2a23bd9af27ed50e0be Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 1 Jan 2020 20:27:27 -0800 Subject: [PATCH 4/4] revert bullet_client.py --- examples/pybullet/gym/pybullet_utils/bullet_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/pybullet/gym/pybullet_utils/bullet_client.py b/examples/pybullet/gym/pybullet_utils/bullet_client.py index 832cee545..a477f5c4d 100644 --- a/examples/pybullet/gym/pybullet_utils/bullet_client.py +++ b/examples/pybullet/gym/pybullet_utils/bullet_client.py @@ -41,7 +41,7 @@ class BulletClient(object): def __getattr__(self, name): """Inject the client id into Bullet functions.""" attribute = getattr(pybullet, name) - + attribute = functools.partial(attribute, physicsClientId=self._client) if name=="disconnect": self._client = -1 return attribute