Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix pybullet racecarGymEnv to use thread-safe pybullet

Browse Source
This commit is contained in:
Erwin Coumans
2017-08-23 18:22:20 -07:00
parent 75231d63e8
commit ff4270a517
3 changed files with 37 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/pybullet/gym/pybullet_envs/bullet/racecar.py
View File

@@ -1,24 +1,24 @@
import os import os
import pybullet as p
import numpy as np import numpy as np
import copy import copy
import math import math
class Racecar: class Racecar:
def __init__(self, urdfRootPath='', timeStep=0.01): def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01):
self.urdfRootPath = urdfRootPath self.urdfRootPath = urdfRootPath
self.timeStep = timeStep self.timeStep = timeStep
self._p = bullet_client
self.reset() self.reset()
def reset(self): def reset(self):
self.racecarUniqueId = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar.urdf"), [0,0,.2]) self.racecarUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar.urdf"), [0,0,.2])
self.maxForce = 20 self.maxForce = 20
self.nMotors = 2 self.nMotors = 2
self.motorizedwheels=[2] self.motorizedwheels=[2]
self.inactiveWheels = [3,5,7] self.inactiveWheels = [3,5,7]
for wheel in self.inactiveWheels: for wheel in self.inactiveWheels:
p.setJointMotorControl2(self.racecarUniqueId,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) self._p.setJointMotorControl2(self.racecarUniqueId,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0)
self.motorizedWheels = [2] self.motorizedWheels = [2]
self.steeringLinks=[4,6] self.steeringLinks=[4,6]
@@ -33,7 +33,7 @@ class Racecar:
def getObservation(self): def getObservation(self):
observation = [] observation = []
pos,orn=p.getBasePositionAndOrientation(self.racecarUniqueId) pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId)
observation.extend(list(pos)) observation.extend(list(pos))
observation.extend(list(orn)) observation.extend(list(orn))
@@ -52,7 +52,7 @@ class Racecar:
for motor in self.motorizedwheels: for motor in self.motorizedwheels:
p.setJointMotorControl2(self.racecarUniqueId,motor,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce) self._p.setJointMotorControl2(self.racecarUniqueId,motor,self._p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce)
for steer in self.steeringLinks: for steer in self.steeringLinks:
p.setJointMotorControl2(self.racecarUniqueId,steer,p.POSITION_CONTROL,targetPosition=steeringAngle) self._p.setJointMotorControl2(self.racecarUniqueId,steer,self._p.POSITION_CONTROL,targetPosition=steeringAngle)

55
examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
View File

@@ -5,9 +5,10 @@ from gym import spaces
from gym.utils import seeding from gym.utils import seeding
import numpy as np import numpy as np
import time import time
import pybullet as p import pybullet
from . import racecar from . import racecar
import random import random
import bullet_client
class RacecarGymEnv(gym.Env): class RacecarGymEnv(gym.Env):
metadata = { metadata = {
@@ -19,7 +20,7 @@ class RacecarGymEnv(gym.Env):
urdfRoot="", urdfRoot="",
actionRepeat=50, actionRepeat=50,
isEnableSelfCollision=True, isEnableSelfCollision=True,
renders=True): renders=False):
print("init") print("init")
self._timeStep = 0.01 self._timeStep = 0.01
self._urdfRoot = urdfRoot self._urdfRoot = urdfRoot
@@ -29,33 +30,33 @@ class RacecarGymEnv(gym.Env):
self._ballUniqueId = -1 self._ballUniqueId = -1
self._envStepCounter = 0 self._envStepCounter = 0
self._renders = renders self._renders = renders
self._p = p
if self._renders: if self._renders:
p.connect(p.GUI) self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else: else:
p.connect(p.DIRECT) self._p = bullet_client.BulletClient()
self._seed() self._seed()
self.reset() #self.reset()
observationDim = len(self.getExtendedObservation()) observationDim = 2 #len(self.getExtendedObservation())
#print("observationDim") #print("observationDim")
#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.action_space = spaces.Discrete(9)
self.observation_space = spaces.Box(-observation_high, observation_high) self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None self.viewer = None
def _reset(self): def _reset(self):
p.resetSimulation() self._p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300) #p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep) self._p.setTimeStep(self._timeStep)
#p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf")) #self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
stadiumobjects = p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf")) stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
#move the stadium objects slightly above 0 #move the stadium objects slightly above 0
for i in stadiumobjects: for i in stadiumobjects:
pos,orn = p.getBasePositionAndOrientation(i) pos,orn = self._p.getBasePositionAndOrientation(i)
newpos = [pos[0],pos[1],pos[2]+0.1] newpos = [pos[0],pos[1],pos[2]+0.1]
p.resetBasePositionAndOrientation(i,newpos,orn) self._p.resetBasePositionAndOrientation(i,newpos,orn)
dist = 5 +2.*random.random() dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random() ang = 2.*3.1415925438*random.random()
@@ -64,17 +65,17 @@ class RacecarGymEnv(gym.Env):
bally = dist * math.cos(ang) bally = dist * math.cos(ang)
ballz = 1 ballz = 1
self._ballUniqueId = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2.urdf"),[ballx,bally,ballz]) self._ballUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2.urdf"),[ballx,bally,ballz])
p.setGravity(0,0,-10) self._p.setGravity(0,0,-10)
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0 self._envStepCounter = 0
for i in range(100): for i in range(100):
p.stepSimulation() self._p.stepSimulation()
self._observation = self.getExtendedObservation() self._observation = self.getExtendedObservation()
return np.array(self._observation) return np.array(self._observation)
def __del__(self): def __del__(self):
p.disconnect() self._p = 0
def _seed(self, seed=None): def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed) self.np_random, seed = seeding.np_random(seed)
@@ -82,18 +83,18 @@ class RacecarGymEnv(gym.Env):
def getExtendedObservation(self): def getExtendedObservation(self):
self._observation = [] #self._racecar.getObservation() self._observation = [] #self._racecar.getObservation()
carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId) ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId)
invCarPos,invCarOrn = p.invertTransform(carpos,carorn) invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn)
ballPosInCar,ballOrnInCar = p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn)
self._observation.extend([ballPosInCar[0],ballPosInCar[1]]) self._observation.extend([ballPosInCar[0],ballPosInCar[1]])
return self._observation return self._observation
def _step(self, action): def _step(self, action):
if (self._renders): if (self._renders):
basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
#p.resetDebugVisualizerCamera(1, 30, -40, basePos) #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
fwd = [-5,-5,-5,0,0,0,5,5,5] fwd = [-5,-5,-5,0,0,0,5,5,5]
steerings = [-0.3,0,0.3,-0.3,0,0.3,-0.3,0,0.3] steerings = [-0.3,0,0.3,-0.3,0,0.3,-0.3,0,0.3]
@@ -102,7 +103,7 @@ class RacecarGymEnv(gym.Env):
realaction = [forward,steer] realaction = [forward,steer]
self._racecar.applyAction(realaction) self._racecar.applyAction(realaction)
for i in range(self._actionRepeat): for i in range(self._actionRepeat):
p.stepSimulation() self._p.stepSimulation()
if self._renders: if self._renders:
time.sleep(self._timeStep) time.sleep(self._timeStep)
self._observation = self.getExtendedObservation() self._observation = self.getExtendedObservation()
@@ -123,7 +124,7 @@ class RacecarGymEnv(gym.Env):
return self._envStepCounter>1000 return self._envStepCounter>1000
def _reward(self): def _reward(self):
closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000) closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
numPt = len(closestPoints) numPt = len(closestPoints)
reward=-1000 reward=-1000

2
examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py
View File

@@ -5,8 +5,8 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir) os.sys.path.insert(0,parentdir)
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
print ("hello")
environment = RacecarGymEnv(renders=True) environment = RacecarGymEnv(renders=True)
environment.reset()
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0) steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)