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

pybullet support for gym.Env, including v0.9.x

Browse Source
This commit is contained in:
Sam Wenke
2018-02-03 12:51:43 -05:00
parent d4b834b9f0
commit ad3c236bfd
9 changed files with 140 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
View File

@@ -40,7 +40,7 @@ class RacecarZEDGymEnv(gym.Env):
self._renders = renders
self._width = 100
self._height = 10
self._isDiscrete = isDiscrete
if self._renders:
self._p = bullet_client.BulletClient(
@@ -53,8 +53,8 @@ class RacecarZEDGymEnv(gym.Env):
observationDim = len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
if (isDiscrete):
self.action_space = spaces.Discrete(9)
else:
@@ -77,14 +77,14 @@ class RacecarZEDGymEnv(gym.Env):
pos,orn = self._p.getBasePositionAndOrientation(i)
newpos = [pos[0],pos[1],pos[2]+0.1]
self._p.resetBasePositionAndOrientation(i,newpos,orn)
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot,"sphere2red.urdf"),[ballx,bally,ballz])
self._p.setGravity(0,0,-10)
self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
@@ -104,13 +104,13 @@ class RacecarZEDGymEnv(gym.Env):
def getExtendedObservation(self):
carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
carmat = self._p.getMatrixFromQuaternion(carorn)
ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId)
ballpos,ballorn = self._p.getBasePositionAndOrientation(self._ballUniqueId)
invCarPos,invCarOrn = self._p.invertTransform(carpos,carorn)
ballPosInCar,ballOrnInCar = self._p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn)
dist0 = 0.3
dist1 = 1.
eyePos = [carpos[0]+dist0*carmat[0],carpos[1]+dist0*carmat[3],carpos[2]+dist0*carmat[6]+0.3]
targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3]
targetPos = [carpos[0]+dist1*carmat[0],carpos[1]+dist1*carmat[3],carpos[2]+dist1*carmat[6]+0.3]
up = [carmat[2],carmat[5],carmat[8]]
viewMat = self._p.computeViewMatrix(eyePos,targetPos,up)
#viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
@@ -122,12 +122,12 @@ class RacecarZEDGymEnv(gym.Env):
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
self._observation = np_img_arr
return self._observation
def _step(self, action):
if (self._renders):
basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
#self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
if (self._isDiscrete):
fwd = [-1,-1,-1,0,0,0,1,1,1]
steerings = [-0.6,0,0.6,-0.6,0,0.6,-0.6,0,0.6]
@@ -143,14 +143,14 @@ class RacecarZEDGymEnv(gym.Env):
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
if self._termination():
break
self._envStepCounter += 1
reward = self._reward()
done = self._termination()
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
@@ -177,10 +177,10 @@ class RacecarZEDGymEnv(gym.Env):
def _termination(self):
return self._envStepCounter>1000
def _reward(self):
closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
numPt = len(closestPoints)
reward=-1000
#print(numPt)
@@ -189,3 +189,8 @@ class RacecarZEDGymEnv(gym.Env):
reward = -closestPoints[0][8]
#print(reward)
return reward
render = _render
reset = _reset
seed = _seed
step = _step