Fix for 1643, allow to instantiate multiple PyBullet Gym environments (Ant, Humanoid, Hopper, Pendula etc) in the same process (same or other thread). It uses the pybullet_utils.bullet_client to achieve this.

This commit is contained in:
Erwin Coumans
2018-05-18 16:23:54 -07:00
parent d17d496f97
commit 0abe4151e5
24 changed files with 163 additions and 403 deletions

View File

@@ -4,10 +4,9 @@ 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
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time
@@ -29,17 +28,12 @@ class SmallReactivePolicy:
return x
def main():
pybullet.connect(pybullet.DIRECT)
env = gym.make("AntBulletEnv-v0")
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset()
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[0].decode() == "torso"):
torsoId=i
print("found torso")
while 1:
frame = 0
@@ -57,14 +51,6 @@ def main():
frame += 1
distance=5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
still_open = env.render("human")
if still_open==False:

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time
@@ -34,15 +33,6 @@ def main():
pi = SmallReactivePolicy(env.observation_space, env.action_space)
#disable rendering during reset, makes loading much faster
env.reset()
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[1].decode() == "cheetah"):
torsoId=i
print("found torso")
print(p.getNumJoints(torsoId))
for j in range (p.getNumJoints(torsoId)):
print(p.getJointInfo(torsoId,j))#LinkState(torsoId,j))
while 1:
frame = 0
@@ -55,16 +45,6 @@ def main():
obs, r, done, _ = env.step(a)
score += r
frame += 1
distance=5
yaw = 0
humanPos = p.getLinkState(torsoId,4)[0]
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
still_open = env.render("human")
if still_open==False:

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time
@@ -35,14 +34,6 @@ def main():
pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset()
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[1].decode() == "hopper"):
torsoId=i
print("found torso")
print(p.getNumJoints(torsoId))
for j in range (p.getNumJoints(torsoId)):
print(p.getJointInfo(torsoId,j))#LinkState(torsoId,j))
while 1:
frame = 0
score = 0
@@ -56,16 +47,6 @@ def main():
obs, r, done, _ = env.step(a)
score += r
frame += 1
distance=5
yaw = 0
humanPos = p.getLinkState(torsoId,4)[0]
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
still_open = env.render("human")

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time
@@ -33,12 +32,6 @@ def main():
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset()
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[0].decode() == "torso"):
torsoId=i
print("found humanoid torso")
while 1:
frame = 0
score = 0
@@ -50,18 +43,7 @@ def main():
obs, r, done, _ = env.step(a)
score += r
frame += 1
distance=5
yaw = 0
time.sleep(1./60.)
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
still_open = env.render("human")
if still_open==False:

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import os.path
import time
@@ -41,28 +40,14 @@ def demo_run():
score = 0
restart_delay = 0
obs = env.reset()
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[0].decode() == "torso"):
torsoId=i
print("found humanoid torso")
while 1:
a = pi.act(obs)
obs, r, done, _ = env.step(a)
score += r
frame += 1
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
if (gui):
time.sleep(1./60)
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
still_open = env.render("human")

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time

View File

@@ -6,7 +6,6 @@ os.sys.path.insert(0,parentdir)
import gym
import numpy as np
import pybullet as p
import pybullet_envs
import time
@@ -33,15 +32,6 @@ def main():
pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset()
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[1].decode() == "walker2d"):
torsoId=i
print("found torso")
print(p.getNumJoints(torsoId))
for j in range (p.getNumJoints(torsoId)):
print(p.getJointInfo(torsoId,j))#LinkState(torsoId,j))
while 1:
frame = 0
@@ -50,21 +40,11 @@ def main():
obs = env.reset()
while 1:
time.sleep(0.01)
time.sleep(1./60.)
a = pi.act(obs)
obs, r, done, _ = env.step(a)
score += r
frame += 1
distance=5
yaw = 0
humanPos = p.getLinkState(torsoId,4)[0]
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
still_open = env.render("human")
if still_open==False:

View File

@@ -1,31 +0,0 @@
#add parent dir to find package. Only needed for source code build, pip install doesn't need it.
import os, 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)
from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv
def main():
environment = SimpleHumanoidGymEnv(renders=True)
environment._p.setGravity(0,0,0)
motorsIds=[]
for motor in environment._humanoid.motor_names:
motorsIds.append(environment._p.addUserDebugParameter(motor,-1,1,0))
while (True):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")
print(obs)
if __name__=="__main__":
main()