Merge pull request #1279 from erwincoumans/master

more fixes in the pybullet gym environments
This commit is contained in:
erwincoumans
2017-08-26 15:19:38 -07:00
committed by GitHub
18 changed files with 220 additions and 172 deletions

15
.gitignore vendored
View File

@@ -3,3 +3,18 @@
/build_cmake/
*.pyc
# Python
__pycache__/
*.py[cod]
# Pip
pip-selfcheck.json
*.whl
*.egg
*.egg-info
# Setuptools
/build
/dist
*.eggs

View File

@@ -1,3 +1,4 @@
import os
import math
import gym
from gym import spaces
@@ -52,15 +53,15 @@ class KukaGymEnv(gym.Env):
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"),[0,0,-1])
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.05*random.random()
ypos = 0 +0.05*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
self.blockUid =p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","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)

View File

@@ -15,11 +15,11 @@ class MJCFBaseBulletEnv(gym.Env):
'video.frames_per_second': 60
}
def __init__(self, robot):
def __init__(self, robot, render=False):
self.scene = None
self.physicsClientId=-1
self.camera = Camera()
self.isRender = render
self.robot = robot
self._seed()
@@ -33,6 +33,15 @@ class MJCFBaseBulletEnv(gym.Env):
return [seed]
def _reset(self):
print("self.isRender=")
print(self.isRender)
if (self.physicsClientId<0):
if (self.isRender):
self.physicsClientId = p.connect(p.GUI)
else:
self.physicsClientId = p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
if self.scene is None:
self.scene = self.create_single_player_scene()
if not self.scene.multiplayer:
@@ -49,7 +58,13 @@ class MJCFBaseBulletEnv(gym.Env):
return s
def _render(self, mode, close):
pass
if (mode=="human"):
self.isRender = True
def _close(self):
if (self.physicsClientId>=0):
p.disconnect(self.physicsClientId)
self.physicsClientId = -1
def HUD(self, state, a, done):
pass

View File

@@ -0,0 +1,23 @@
from . import enjoy_TF_AntBulletEnv_v0_2017may
from . import enjoy_TF_HalfCheetahBulletEnv_v0_2017may
from . import enjoy_TF_AntBulletEnv_v0_2017may
from . import enjoy_TF_HopperBulletEnv_v0_2017may
from . import enjoy_TF_HumanoidBulletEnv_v0_2017may
from . import enjoy_TF_InvertedDoublePendulumBulletEnv_v0_2017may
from . import enjoy_TF_InvertedPendulumBulletEnv_v0_2017may
from . import enjoy_TF_InvertedPendulumSwingupBulletEnv_v0_2017may
from . import enjoy_TF_Walker2DBulletEnv_v0_2017may
from . import enjoy_kuka_grasping
from . import enjoy_pybullet_cartpole
from . import enjoy_pybullet_racecar
from . import enjoy_pybullet_zed_racecar
from . import kukaGymEnvTest
from . import racecarGymEnvTest
from . import racecarZEDGymEnvTest
from . import simpleHumanoidGymEnvTest
from . import train_kuka_cam_grasping
from . import train_kuka_grasping
from . import train_pybullet_cartpole
from . import train_pybullet_racecar
from . import train_pybullet_zed_racecar

View File

@@ -28,16 +28,12 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("AntBulletEnv-v0")
cid = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
@@ -49,10 +45,8 @@ def demo_run():
frame = 0
score = 0
restart_delay = 0
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
obs = env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while 1:
time.sleep(0.001)
a = pi.act(obs)
@@ -311,4 +305,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ -0.0680, +0.1401, -0.0628, -0.1317, +0.1489, +0.1844, -0.1147, +0.0137])
if __name__=="__main__":
demo_run()
main()

View File

@@ -27,17 +27,13 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("HalfCheetahBulletEnv-v0")
cid = p.connect(p.GUI)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
#disable rendering during reset, makes loading much faster
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
@@ -52,10 +48,8 @@ def demo_run():
frame = 0
score = 0
restart_delay = 0
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
obs = env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while 1:
time.sleep(0.001)
a = pi.act(obs)
@@ -311,4 +305,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ +0.1367, +0.1107, -0.0148, +0.1158, -0.0820, +0.3047])
if __name__=="__main__":
demo_run()
main()

View File

@@ -30,15 +30,12 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("HopperBulletEnv-v0")
cid = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if (p.getBodyInfo(i)[1].decode() == "hopper"):
@@ -52,10 +49,8 @@ def demo_run():
score = 0
restart_delay = 0
#disable rendering during reset, makes loading much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
obs = env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while 1:
time.sleep(0.001)
a = pi.act(obs)
@@ -301,4 +296,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ +0.4868, -0.0987, -0.0946])
if __name__=="__main__":
demo_run()
main()

View File

@@ -28,15 +28,11 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("HumanoidBulletEnv-v0")
cid = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
@@ -47,10 +43,8 @@ def demo_run():
frame = 0
score = 0
restart_delay = 0
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
obs = env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while 1:
time.sleep(0.001)
a = pi.act(obs)
@@ -517,4 +511,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ -0.0356, +0.0776, -0.0344, +0.1375, +0.1048, +0.3648, +0.3240, +0.1319, +0.1161, +0.3373, +0.3193, +0.0120, +0.0253, -0.2434, -0.1291, +0.1042, -0.2448])
if __name__=="__main__":
demo_run()
main()

View File

@@ -27,11 +27,10 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("InvertedDoublePendulumBulletEnv-v0")
cid = p.connect(p.GUI)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
while 1:
@@ -179,4 +178,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ +0.0190])
if __name__=="__main__":
demo_run()
main()

View File

@@ -27,11 +27,10 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
print("create env")
env = gym.make("InvertedPendulumBulletEnv-v0")
print("connecting")
cid = p.connect(p.GUI)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
while 1:
@@ -174,4 +173,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ +0.0352])
if __name__=="__main__":
demo_run()
main()

View File

@@ -27,11 +27,10 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("InvertedPendulumSwingupBulletEnv-v0")
cid = p.connect(p.GUI)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
while 1:
@@ -175,4 +174,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ +0.2753])
if __name__=="__main__":
demo_run()
main()

View File

@@ -27,17 +27,12 @@ class SmallReactivePolicy:
x = np.dot(x, weights_final_w) + weights_final_b
return x
def demo_run():
def main():
env = gym.make("Walker2DBulletEnv-v0")
cid = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
env.reset()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
torsoId = -1
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
@@ -305,4 +300,4 @@ weights_final_w = np.array([
weights_final_b = np.array([ +0.0090, +0.2404, +0.1022, +0.1035, +0.0621, -0.0160])
if __name__=="__main__":
demo_run()
main()

View File

@@ -7,31 +7,34 @@ os.sys.path.insert(0,parentdir)
from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv
import time
def main():
environment = KukaGymEnv(renders=True)
motorsIds=[]
#motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537))
#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
dv = 0.001
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
done = False
while (not done):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step2(action)
obs = environment.getExtendedObservation()
environment = KukaGymEnv(renders=True)
motorsIds=[]
#motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537))
#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
dv = 0.001
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
done = False
while (not done):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step2(action)
obs = environment.getExtendedObservation()
if __name__=="__main__":
main()

View File

@@ -7,33 +7,38 @@ os.sys.path.insert(0,parentdir)
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
isDiscrete = False
environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete)
environment.reset()
def main():
environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete)
environment.reset()
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
if (isDiscrete):
discreteAction = 0
if (targetVelocity<-0.33):
discreteAction=0
else:
if (targetVelocity>0.33):
discreteAction=6
else:
discreteAction=3
if (steeringAngle>-0.17):
if (steeringAngle>0.17):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
else:
action=[targetVelocity,steeringAngle]
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")
print(obs)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
if (isDiscrete):
discreteAction = 0
if (targetVelocity<-0.33):
discreteAction=0
else:
if (targetVelocity>0.33):
discreteAction=6
else:
discreteAction=3
if (steeringAngle>-0.17):
if (steeringAngle>0.17):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
else:
action=[targetVelocity,steeringAngle]
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")
print(obs)
if __name__=="__main__":
main()

View File

@@ -5,31 +5,36 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
print ("hello")
environment = RacecarZEDGymEnv(renders=True)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
def main():
environment = RacecarZEDGymEnv(renders=True)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
discreteAction = 0
if (targetVelocity<-0.33):
discreteAction=0
else:
if (targetVelocity>0.33):
discreteAction=6
else:
discreteAction=3
if (steeringAngle>-0.17):
if (steeringAngle>0.17):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")
print(obs)
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
discreteAction = 0
if (targetVelocity<-0.33):
discreteAction=0
else:
if (targetVelocity>0.33):
discreteAction=6
else:
discreteAction=3
if (steeringAngle>-0.17):
if (steeringAngle>0.17):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")
print(obs)
if __name__=="__main__":
main()

View File

@@ -5,22 +5,27 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv
print ("hello")
environment = SimpleHumanoidGymEnv(renders=True)
environment._p.setGravity(0,0,0)
def main():
environment = SimpleHumanoidGymEnv(renders=True)
motorsIds=[]
for motor in environment._humanoid.motor_names:
motorsIds.append(environment._p.addUserDebugParameter(motor,-1,1,0))
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)
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()

View File

@@ -1,12 +1,15 @@
from .scene_stadium import SinglePlayerStadiumScene
from .env_bases import MJCFBaseBulletEnv
import numpy as np
import pybullet as p
from robot_locomotors import Hopper, Walker2D, HalfCheetah, Ant, Humanoid
class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
def __init__(self, robot):
MJCFBaseBulletEnv.__init__(self, robot)
def __init__(self, robot, render=False):
print("WalkerBase::__init__")
MJCFBaseBulletEnv.__init__(self, robot, render)
self.camera_x = 0
self.walk_target_x = 1e3 # kilometer away
self.walk_target_y = 0
@@ -16,11 +19,15 @@ class WalkerBaseBulletEnv(MJCFBaseBulletEnv):
return self.stadium_scene
def _reset(self):
r = MJCFBaseBulletEnv._reset(self)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(
self.stadium_scene.ground_plane_mjcf)
self.ground_ids = set([(self.parts[f].bodies[self.parts[f].bodyIndex], self.parts[f].bodyPartIndex) for f in
self.foot_ground_object_names])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
return r
def move_robot(self, init_x, init_y, init_z):

View File

@@ -428,7 +428,7 @@ hh = setup_py_dir + "/" + datadir
for root, dirs, files in os.walk(hh):
for fn in files:
ext = os.path.splitext(fn)[1][1:]
if ext and ext in 'png jpg urdf sdf obj mtl dae off stl STL xml '.split():
if ext and ext in 'png gif jpg urdf sdf obj mtl dae off stl STL xml '.split():
fn = root + "/" + fn
need_files.append(fn[1+len(hh):])
@@ -440,7 +440,7 @@ print("-----")
setup(
name = 'pybullet',
version='1.2.4',
version='1.2.6',
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',