From ff4270a517be3ec01757aa3d179a8f28e06d1d53 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 23 Aug 2017 18:22:20 -0700 Subject: [PATCH] fix pybullet racecarGymEnv to use thread-safe pybullet --- .../gym/pybullet_envs/bullet/racecar.py | 14 ++--- .../gym/pybullet_envs/bullet/racecarGymEnv.py | 57 ++++++++++--------- .../examples/racecarGymEnvTest.py | 2 +- 3 files changed, 37 insertions(+), 36 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecar.py b/examples/pybullet/gym/pybullet_envs/bullet/racecar.py index 91e21fd79..12736d94f 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecar.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecar.py @@ -1,24 +1,24 @@ import os -import pybullet as p import numpy as np import copy import math class Racecar: - def __init__(self, urdfRootPath='', timeStep=0.01): + def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01): self.urdfRootPath = urdfRootPath self.timeStep = timeStep + self._p = bullet_client self.reset() 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.nMotors = 2 self.motorizedwheels=[2] self.inactiveWheels = [3,5,7] 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.steeringLinks=[4,6] @@ -33,7 +33,7 @@ class Racecar: def getObservation(self): observation = [] - pos,orn=p.getBasePositionAndOrientation(self.racecarUniqueId) + pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId) observation.extend(list(pos)) observation.extend(list(orn)) @@ -52,7 +52,7 @@ class Racecar: 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: - p.setJointMotorControl2(self.racecarUniqueId,steer,p.POSITION_CONTROL,targetPosition=steeringAngle) + self._p.setJointMotorControl2(self.racecarUniqueId,steer,self._p.POSITION_CONTROL,targetPosition=steeringAngle) diff --git a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py index 1feb6d980..a1fe54cbb 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py @@ -5,9 +5,10 @@ from gym import spaces from gym.utils import seeding import numpy as np import time -import pybullet as p +import pybullet from . import racecar import random +import bullet_client class RacecarGymEnv(gym.Env): metadata = { @@ -19,7 +20,7 @@ class RacecarGymEnv(gym.Env): urdfRoot="", actionRepeat=50, isEnableSelfCollision=True, - renders=True): + renders=False): print("init") self._timeStep = 0.01 self._urdfRoot = urdfRoot @@ -29,33 +30,33 @@ class RacecarGymEnv(gym.Env): self._ballUniqueId = -1 self._envStepCounter = 0 self._renders = renders - self._p = p if self._renders: - p.connect(p.GUI) + self._p = bullet_client.BulletClient( + connection_mode=pybullet.GUI) else: - p.connect(p.DIRECT) + self._p = bullet_client.BulletClient() + self._seed() - self.reset() - observationDim = len(self.getExtendedObservation()) + #self.reset() + observationDim = 2 #len(self.getExtendedObservation()) #print("observationDim") - #print(observationDim) - + #print(observationDim) observation_high = np.array([np.finfo(np.float32).max] * observationDim) self.action_space = spaces.Discrete(9) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None def _reset(self): - p.resetSimulation() + self._p.resetSimulation() #p.setPhysicsEngineParameter(numSolverIterations=300) - p.setTimeStep(self._timeStep) - #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")) + self._p.setTimeStep(self._timeStep) + #self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf")) + stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf")) #move the stadium objects slightly above 0 for i in stadiumobjects: - pos,orn = p.getBasePositionAndOrientation(i) + pos,orn = self._p.getBasePositionAndOrientation(i) 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() ang = 2.*3.1415925438*random.random() @@ -64,17 +65,17 @@ class RacecarGymEnv(gym.Env): bally = dist * math.cos(ang) ballz = 1 - self._ballUniqueId = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2.urdf"),[ballx,bally,ballz]) - p.setGravity(0,0,-10) - self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) + self._ballUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2.urdf"),[ballx,bally,ballz]) + self._p.setGravity(0,0,-10) + self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 for i in range(100): - p.stepSimulation() + self._p.stepSimulation() self._observation = self.getExtendedObservation() return np.array(self._observation) def __del__(self): - p.disconnect() + self._p = 0 def _seed(self, seed=None): self.np_random, seed = seeding.np_random(seed) @@ -82,18 +83,18 @@ class RacecarGymEnv(gym.Env): def getExtendedObservation(self): self._observation = [] #self._racecar.getObservation() - carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId) - invCarPos,invCarOrn = p.invertTransform(carpos,carorn) - ballPosInCar,ballOrnInCar = p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) + carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId) + invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn) + ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn) self._observation.extend([ballPosInCar[0],ballPosInCar[1]]) return self._observation def _step(self, action): if (self._renders): - basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) - #p.resetDebugVisualizerCamera(1, 30, -40, basePos) + basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId) + #self._p.resetDebugVisualizerCamera(1, 30, -40, basePos) 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] @@ -102,7 +103,7 @@ class RacecarGymEnv(gym.Env): realaction = [forward,steer] self._racecar.applyAction(realaction) for i in range(self._actionRepeat): - p.stepSimulation() + self._p.stepSimulation() if self._renders: time.sleep(self._timeStep) self._observation = self.getExtendedObservation() @@ -123,7 +124,7 @@ class RacecarGymEnv(gym.Env): return self._envStepCounter>1000 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) reward=-1000 diff --git a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py index 6fa07a7b8..f7445625e 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py +++ b/examples/pybullet/gym/pybullet_envs/examples/racecarGymEnvTest.py @@ -5,8 +5,8 @@ parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv -print ("hello") environment = RacecarGymEnv(renders=True) +environment.reset() targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0) steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)