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:
@@ -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:
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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")
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user