refactor pybullet/gym to allow instantiating environments directly from a pybullet install:
work-in-progress (need to add missing data files, fix paths etc)
example:
pip install pybullet
pip install gym
python
import gym
import pybullet
import pybullet_envs
env = gym.make("HumanoidBulletEnv-v0")
This commit is contained in:
Erwin Coumans
7
examples/pybullet/gym/pybullet_envs/bullet/__init__.py
Normal file
7
examples/pybullet/gym/pybullet_envs/bullet/__init__.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv
|
||||
from pybullet_envs.bullet.minitaur_bullet import MinitaurBulletEnv
|
||||
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
|
||||
from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
|
||||
from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv
|
||||
from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv
|
||||
from pybullet_envs.bullet.kukaCamGymEnv import KukaCamGymEnv
|
||||
@@ -0,0 +1,96 @@
|
||||
"""
|
||||
Classic cart-pole system implemented by Rich Sutton et al.
|
||||
Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c
|
||||
"""
|
||||
import os
|
||||
import logging
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import subprocess
|
||||
import pybullet as p
|
||||
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
class CartPoleBulletEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self, renders=True):
|
||||
# start the bullet physics server
|
||||
self._renders = renders
|
||||
if (renders):
|
||||
p.connect(p.GUI)
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
|
||||
observation_high = np.array([
|
||||
np.finfo(np.float32).max,
|
||||
np.finfo(np.float32).max,
|
||||
np.finfo(np.float32).max,
|
||||
np.finfo(np.float32).max])
|
||||
action_high = np.array([0.1])
|
||||
|
||||
self.action_space = spaces.Discrete(9)
|
||||
self.observation_space = spaces.Box(-observation_high, observation_high)
|
||||
|
||||
self.theta_threshold_radians = 1
|
||||
self.x_threshold = 2.4
|
||||
self._seed()
|
||||
# self.reset()
|
||||
self.viewer = None
|
||||
self._configure()
|
||||
|
||||
def _configure(self, display=None):
|
||||
self.display = display
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def _step(self, action):
|
||||
p.stepSimulation()
|
||||
# time.sleep(self.timeStep)
|
||||
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
|
||||
theta, theta_dot, x, x_dot = self.state
|
||||
|
||||
dv = 0.1
|
||||
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
|
||||
|
||||
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
|
||||
|
||||
done = x < -self.x_threshold \
|
||||
or x > self.x_threshold \
|
||||
or theta < -self.theta_threshold_radians \
|
||||
or theta > self.theta_threshold_radians
|
||||
reward = 1.0
|
||||
|
||||
return np.array(self.state), reward, done, {}
|
||||
|
||||
def _reset(self):
|
||||
# print("-----------reset simulation---------------")
|
||||
p.resetSimulation()
|
||||
self.cartpole = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","cartpole.urdf"),[0,0,0])
|
||||
self.timeStep = 0.01
|
||||
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
|
||||
p.setGravity(0,0, -10)
|
||||
p.setTimeStep(self.timeStep)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
|
||||
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
|
||||
p.resetJointState(self.cartpole, 1, initialAngle)
|
||||
p.resetJointState(self.cartpole, 0, initialCartPos)
|
||||
|
||||
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
|
||||
|
||||
return np.array(self.state)
|
||||
|
||||
def _render(self, mode='human', close=False):
|
||||
return
|
||||
162
examples/pybullet/gym/pybullet_envs/bullet/kuka.py
Normal file
162
examples/pybullet/gym/pybullet_envs/bullet/kuka.py
Normal file
@@ -0,0 +1,162 @@
|
||||
import os
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
import copy
|
||||
import math
|
||||
|
||||
class Kuka:
|
||||
|
||||
def __init__(self, urdfRootPath='', timeStep=0.01):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.timeStep = timeStep
|
||||
|
||||
self.maxForce = 200.
|
||||
self.fingerAForce = 6
|
||||
self.fingerBForce = 5.5
|
||||
self.fingerTipForce = 6
|
||||
self.useInverseKinematics = 1
|
||||
self.useSimulation = 1
|
||||
self.useNullSpace = 1
|
||||
self.useOrientation = 1
|
||||
self.kukaEndEffectorIndex = 6
|
||||
#lower limits for null space
|
||||
self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
|
||||
#upper limits for null space
|
||||
self.ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
|
||||
#joint ranges for null space
|
||||
self.jr=[5.8,4,5.8,4,5.8,4,6]
|
||||
#restposes for null space
|
||||
self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
|
||||
#joint damping coefficents
|
||||
self.jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
objects = p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","kuka_iiwa/kuka_with_gripper2.sdf"))
|
||||
self.kukaUid = objects[0]
|
||||
#for i in range (p.getNumJoints(self.kukaUid)):
|
||||
# print(p.getJointInfo(self.kukaUid,i))
|
||||
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
|
||||
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
|
||||
self.numJoints = p.getNumJoints(self.kukaUid)
|
||||
for jointIndex in range (self.numJoints):
|
||||
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
|
||||
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
|
||||
|
||||
self.trayUid = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
|
||||
self.endEffectorPos = [0.537,0.0,0.5]
|
||||
self.endEffectorAngle = 0
|
||||
|
||||
|
||||
self.motorNames = []
|
||||
self.motorIndices = []
|
||||
|
||||
for i in range (self.numJoints):
|
||||
jointInfo = p.getJointInfo(self.kukaUid,i)
|
||||
qIndex = jointInfo[3]
|
||||
if qIndex > -1:
|
||||
#print("motorname")
|
||||
#print(jointInfo[1])
|
||||
self.motorNames.append(str(jointInfo[1]))
|
||||
self.motorIndices.append(i)
|
||||
|
||||
def getActionDimension(self):
|
||||
if (self.useInverseKinematics):
|
||||
return len(self.motorIndices)
|
||||
return 6 #position x,y,z and roll/pitch/yaw euler angles of end effector
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
|
||||
def getObservation(self):
|
||||
observation = []
|
||||
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
|
||||
pos = state[0]
|
||||
orn = state[1]
|
||||
euler = p.getEulerFromQuaternion(orn)
|
||||
|
||||
observation.extend(list(pos))
|
||||
observation.extend(list(euler))
|
||||
|
||||
return observation
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
|
||||
#print ("self.numJoints")
|
||||
#print (self.numJoints)
|
||||
if (self.useInverseKinematics):
|
||||
|
||||
dx = motorCommands[0]
|
||||
dy = motorCommands[1]
|
||||
dz = motorCommands[2]
|
||||
da = motorCommands[3]
|
||||
fingerAngle = motorCommands[4]
|
||||
|
||||
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
|
||||
actualEndEffectorPos = state[0]
|
||||
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
|
||||
#print(actualEndEffectorPos[2])
|
||||
|
||||
|
||||
|
||||
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
|
||||
if (self.endEffectorPos[0]>0.75):
|
||||
self.endEffectorPos[0]=0.75
|
||||
if (self.endEffectorPos[0]<0.45):
|
||||
self.endEffectorPos[0]=0.45
|
||||
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
|
||||
if (self.endEffectorPos[1]<-0.22):
|
||||
self.endEffectorPos[1]=-0.22
|
||||
if (self.endEffectorPos[1]>0.22):
|
||||
self.endEffectorPos[1]=0.22
|
||||
|
||||
#print ("self.endEffectorPos[2]")
|
||||
#print (self.endEffectorPos[2])
|
||||
#print("actualEndEffectorPos[2]")
|
||||
#print(actualEndEffectorPos[2])
|
||||
if (dz>0 or actualEndEffectorPos[2]>0.10):
|
||||
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
|
||||
if (actualEndEffectorPos[2]<0.10):
|
||||
self.endEffectorPos[2] = self.endEffectorPos[2]+0.0001
|
||||
|
||||
|
||||
self.endEffectorAngle = self.endEffectorAngle + da
|
||||
pos = self.endEffectorPos
|
||||
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
|
||||
if (self.useNullSpace==1):
|
||||
if (self.useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
|
||||
else:
|
||||
if (self.useOrientation==1):
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
|
||||
else:
|
||||
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
|
||||
|
||||
#print("jointPoses")
|
||||
#print(jointPoses)
|
||||
#print("self.kukaEndEffectorIndex")
|
||||
#print(self.kukaEndEffectorIndex)
|
||||
if (self.useSimulation):
|
||||
for i in range (self.kukaEndEffectorIndex+1):
|
||||
#print(i)
|
||||
p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.03,velocityGain=1)
|
||||
else:
|
||||
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
|
||||
for i in range (self.numJoints):
|
||||
p.resetJointState(self.kukaUid,i,jointPoses[i])
|
||||
#fingers
|
||||
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
|
||||
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
|
||||
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
|
||||
|
||||
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
|
||||
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
|
||||
|
||||
|
||||
else:
|
||||
for action in range (len(motorCommands)):
|
||||
motor = self.motorIndices[action]
|
||||
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
|
||||
|
||||
192
examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py
Normal file
192
examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py
Normal file
@@ -0,0 +1,192 @@
|
||||
import os
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import pybullet as p
|
||||
from . import kuka
|
||||
import random
|
||||
|
||||
class KukaCamGymEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self,
|
||||
urdfRoot="",
|
||||
actionRepeat=1,
|
||||
isEnableSelfCollision=True,
|
||||
renders=True):
|
||||
print("init")
|
||||
self._timeStep = 1./240.
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
self._observation = []
|
||||
self._envStepCounter = 0
|
||||
self._renders = renders
|
||||
self._width = 341
|
||||
self._height = 256
|
||||
self.terminated = 0
|
||||
self._p = p
|
||||
if self._renders:
|
||||
p.connect(p.GUI)
|
||||
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
|
||||
self._seed()
|
||||
self.reset()
|
||||
observationDim = len(self.getExtendedObservation())
|
||||
#print("observationDim")
|
||||
#print(observationDim)
|
||||
|
||||
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
|
||||
self.action_space = spaces.Discrete(7)
|
||||
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
|
||||
self.viewer = None
|
||||
|
||||
def _reset(self):
|
||||
print("reset")
|
||||
self.terminated = 0
|
||||
p.resetSimulation()
|
||||
p.setPhysicsEngineParameter(numSolverIterations=150)
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"),[0,0,-1])
|
||||
|
||||
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(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)
|
||||
self._envStepCounter = 0
|
||||
p.stepSimulation()
|
||||
self._observation = self.getExtendedObservation()
|
||||
return np.array(self._observation)
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def getExtendedObservation(self):
|
||||
|
||||
#camEyePos = [0.03,0.236,0.54]
|
||||
#distance = 1.06
|
||||
#pitch=-56
|
||||
#yaw = 258
|
||||
#roll=0
|
||||
#upAxisIndex = 2
|
||||
#camInfo = p.getDebugVisualizerCamera()
|
||||
#print("width,height")
|
||||
#print(camInfo[0])
|
||||
#print(camInfo[1])
|
||||
#print("viewMatrix")
|
||||
#print(camInfo[2])
|
||||
#print("projectionMatrix")
|
||||
#print(camInfo[3])
|
||||
#viewMat = camInfo[2]
|
||||
#viewMat = p.computeViewMatrixFromYawPitchRoll(camEyePos,distance,yaw, pitch,roll,upAxisIndex)
|
||||
viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0]
|
||||
#projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
|
||||
projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
|
||||
|
||||
img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
|
||||
rgb=img_arr[2]
|
||||
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
|
||||
self._observation = np_img_arr
|
||||
return self._observation
|
||||
|
||||
def _step(self, action):
|
||||
dv = 0.01
|
||||
dx = [0,-dv,dv,0,0,0,0][action]
|
||||
dy = [0,0,0,-dv,dv,0,0][action]
|
||||
da = [0,0,0,0,0,-0.1,0.1][action]
|
||||
f = 0.3
|
||||
realAction = [dx,dy,-0.002,da,f]
|
||||
return self.step2( realAction)
|
||||
|
||||
def step2(self, action):
|
||||
self._kuka.applyAction(action)
|
||||
for i in range(self._actionRepeat):
|
||||
p.stepSimulation()
|
||||
if self._renders:
|
||||
time.sleep(self._timeStep)
|
||||
self._observation = self.getExtendedObservation()
|
||||
if self._termination():
|
||||
break
|
||||
self._envStepCounter += 1
|
||||
#print("self._envStepCounter")
|
||||
#print(self._envStepCounter)
|
||||
|
||||
done = self._termination()
|
||||
reward = self._reward()
|
||||
#print("len=%r" % len(self._observation))
|
||||
|
||||
return np.array(self._observation), reward, done, {}
|
||||
|
||||
def _render(self, mode='human', close=False):
|
||||
return
|
||||
|
||||
def _termination(self):
|
||||
#print (self._kuka.endEffectorPos[2])
|
||||
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
|
||||
actualEndEffectorPos = state[0]
|
||||
|
||||
#print("self._envStepCounter")
|
||||
#print(self._envStepCounter)
|
||||
if (self.terminated or self._envStepCounter>1000):
|
||||
self._observation = self.getExtendedObservation()
|
||||
return True
|
||||
|
||||
if (actualEndEffectorPos[2] <= 0.10):
|
||||
self.terminated = 1
|
||||
|
||||
#print("closing gripper, attempting grasp")
|
||||
#start grasp and terminate
|
||||
fingerAngle = 0.3
|
||||
|
||||
for i in range (1000):
|
||||
graspAction = [0,0,0.001,0,fingerAngle]
|
||||
self._kuka.applyAction(graspAction)
|
||||
p.stepSimulation()
|
||||
fingerAngle = fingerAngle-(0.3/100.)
|
||||
if (fingerAngle<0):
|
||||
fingerAngle=0
|
||||
|
||||
self._observation = self.getExtendedObservation()
|
||||
return True
|
||||
return False
|
||||
|
||||
def _reward(self):
|
||||
|
||||
#rewards is height of target object
|
||||
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
|
||||
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000)
|
||||
|
||||
reward = -1000
|
||||
numPt = len(closestPoints)
|
||||
#print(numPt)
|
||||
if (numPt>0):
|
||||
#print("reward:")
|
||||
reward = -closestPoints[0][8]*10
|
||||
|
||||
if (blockPos[2] >0.2):
|
||||
print("grasped a block!!!")
|
||||
print("self._envStepCounter")
|
||||
print(self._envStepCounter)
|
||||
reward = reward+1000
|
||||
|
||||
#print("reward")
|
||||
#print(reward)
|
||||
return reward
|
||||
176
examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py
Normal file
176
examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py
Normal file
@@ -0,0 +1,176 @@
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import pybullet as p
|
||||
from . import kuka
|
||||
import random
|
||||
|
||||
class KukaGymEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self,
|
||||
urdfRoot="",
|
||||
actionRepeat=1,
|
||||
isEnableSelfCollision=True,
|
||||
renders=True):
|
||||
print("init")
|
||||
self._timeStep = 1./240.
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
self._observation = []
|
||||
self._envStepCounter = 0
|
||||
self._renders = renders
|
||||
self.terminated = 0
|
||||
self._p = p
|
||||
if self._renders:
|
||||
p.connect(p.GUI)
|
||||
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
#timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json")
|
||||
self._seed()
|
||||
self.reset()
|
||||
observationDim = len(self.getExtendedObservation())
|
||||
#print("observationDim")
|
||||
#print(observationDim)
|
||||
|
||||
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
|
||||
self.action_space = spaces.Discrete(7)
|
||||
self.observation_space = spaces.Box(-observation_high, observation_high)
|
||||
self.viewer = None
|
||||
|
||||
def _reset(self):
|
||||
print("reset")
|
||||
self.terminated = 0
|
||||
p.resetSimulation()
|
||||
p.setPhysicsEngineParameter(numSolverIterations=150)
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1])
|
||||
|
||||
p.loadURDF("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])
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
|
||||
self._envStepCounter = 0
|
||||
p.stepSimulation()
|
||||
self._observation = self.getExtendedObservation()
|
||||
return np.array(self._observation)
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def getExtendedObservation(self):
|
||||
self._observation = self._kuka.getObservation()
|
||||
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
|
||||
endEffectorPos = eeState[0]
|
||||
endEffectorOrn = eeState[1]
|
||||
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
|
||||
|
||||
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
|
||||
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
|
||||
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
|
||||
self._observation.extend(list(blockPosInEE))
|
||||
self._observation.extend(list(blockEulerInEE))
|
||||
|
||||
return self._observation
|
||||
|
||||
def _step(self, action):
|
||||
dv = 0.01
|
||||
dx = [0,-dv,dv,0,0,0,0][action]
|
||||
dy = [0,0,0,-dv,dv,0,0][action]
|
||||
da = [0,0,0,0,0,-0.1,0.1][action]
|
||||
f = 0.3
|
||||
realAction = [dx,dy,-0.002,da,f]
|
||||
return self.step2( realAction)
|
||||
|
||||
def step2(self, action):
|
||||
self._kuka.applyAction(action)
|
||||
for i in range(self._actionRepeat):
|
||||
p.stepSimulation()
|
||||
if self._renders:
|
||||
time.sleep(self._timeStep)
|
||||
self._observation = self.getExtendedObservation()
|
||||
if self._termination():
|
||||
break
|
||||
self._envStepCounter += 1
|
||||
#print("self._envStepCounter")
|
||||
#print(self._envStepCounter)
|
||||
|
||||
done = self._termination()
|
||||
reward = self._reward()
|
||||
#print("len=%r" % len(self._observation))
|
||||
|
||||
return np.array(self._observation), reward, done, {}
|
||||
|
||||
def _render(self, mode='human', close=False):
|
||||
return
|
||||
|
||||
def _termination(self):
|
||||
#print (self._kuka.endEffectorPos[2])
|
||||
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
|
||||
actualEndEffectorPos = state[0]
|
||||
|
||||
#print("self._envStepCounter")
|
||||
#print(self._envStepCounter)
|
||||
if (self.terminated or self._envStepCounter>1000):
|
||||
self._observation = self.getExtendedObservation()
|
||||
return True
|
||||
|
||||
if (actualEndEffectorPos[2] <= 0.10):
|
||||
self.terminated = 1
|
||||
|
||||
#print("closing gripper, attempting grasp")
|
||||
#start grasp and terminate
|
||||
fingerAngle = 0.3
|
||||
|
||||
for i in range (1000):
|
||||
graspAction = [0,0,0.001,0,fingerAngle]
|
||||
self._kuka.applyAction(graspAction)
|
||||
p.stepSimulation()
|
||||
fingerAngle = fingerAngle-(0.3/100.)
|
||||
if (fingerAngle<0):
|
||||
fingerAngle=0
|
||||
|
||||
self._observation = self.getExtendedObservation()
|
||||
return True
|
||||
return False
|
||||
|
||||
def _reward(self):
|
||||
|
||||
#rewards is height of target object
|
||||
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
|
||||
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000)
|
||||
|
||||
reward = -1000
|
||||
numPt = len(closestPoints)
|
||||
#print(numPt)
|
||||
if (numPt>0):
|
||||
#print("reward:")
|
||||
reward = -closestPoints[0][8]*10
|
||||
|
||||
if (blockPos[2] >0.2):
|
||||
print("grasped a block!!!")
|
||||
print("self._envStepCounter")
|
||||
print(self._envStepCounter)
|
||||
reward = reward+1000
|
||||
|
||||
#print("reward")
|
||||
#print(reward)
|
||||
return reward
|
||||
142
examples/pybullet/gym/pybullet_envs/bullet/minitaur.py
Normal file
142
examples/pybullet/gym/pybullet_envs/bullet/minitaur.py
Normal file
@@ -0,0 +1,142 @@
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
|
||||
class Minitaur:
|
||||
def __init__(self, urdfRootPath=''):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.reset()
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
motorCommandsWithDir = np.multiply(motorCommands, self.motorDir)
|
||||
for i in range(self.nMotors):
|
||||
self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i])
|
||||
|
||||
def getObservation(self):
|
||||
observation = []
|
||||
observation.extend(self.getMotorAngles().tolist())
|
||||
observation.extend(self.getMotorVelocities().tolist())
|
||||
observation.extend(self.getMotorTorques().tolist())
|
||||
observation.extend(list(self.getBaseOrientation()))
|
||||
observation.extend(list(self.getBasePosition()))
|
||||
return observation
|
||||
|
||||
def getActionDimension(self):
|
||||
return self.nMotors
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
|
||||
def buildJointNameToIdDict(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
self.jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(self.quadruped, i)
|
||||
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
self.resetPose()
|
||||
for i in range(100):
|
||||
p.stepSimulation()
|
||||
|
||||
def buildMotorIdList(self):
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
|
||||
|
||||
|
||||
def reset(self):
|
||||
self.quadruped = p.loadURDF("%s/quadruped/quadruped.urdf" % self.urdfRootPath,0,0,.3)
|
||||
self.kp = 1
|
||||
self.kd = 0.1
|
||||
self.maxForce = 3.5
|
||||
self.nMotors = 8
|
||||
self.motorIdList = []
|
||||
self.motorDir = [1, -1, 1, -1, -1, 1, -1, 1]
|
||||
self.buildJointNameToIdDict()
|
||||
self.buildMotorIdList()
|
||||
|
||||
|
||||
def disableAllMotors(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
for i in range(nJoints):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||
|
||||
def setMotorAngleById(self, motorId, desiredAngle):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
|
||||
|
||||
def setMotorAngleByName(self, motorName, desiredAngle):
|
||||
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
|
||||
|
||||
|
||||
def resetPose(self):
|
||||
#right front leg
|
||||
self.disableAllMotors()
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
|
||||
self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
|
||||
|
||||
def getBasePosition(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
return position
|
||||
|
||||
def getBaseOrientation(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
return orientation
|
||||
|
||||
def getMotorAngles(self):
|
||||
motorAngles = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorAngles.append(jointState[0])
|
||||
motorAngles = np.multiply(motorAngles, self.motorDir)
|
||||
return motorAngles
|
||||
|
||||
def getMotorVelocities(self):
|
||||
motorVelocities = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorVelocities.append(jointState[1])
|
||||
motorVelocities = np.multiply(motorVelocities, self.motorDir)
|
||||
return motorVelocities
|
||||
|
||||
def getMotorTorques(self):
|
||||
motorTorques = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorTorques.append(jointState[3])
|
||||
motorTorques = np.multiply(motorTorques, self.motorDir)
|
||||
return motorTorques
|
||||
113
examples/pybullet/gym/pybullet_envs/bullet/minitaurGymEnv.py
Normal file
113
examples/pybullet/gym/pybullet_envs/bullet/minitaurGymEnv.py
Normal file
@@ -0,0 +1,113 @@
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import pybullet as p
|
||||
import minitaur_new
|
||||
|
||||
class MinitaurGymEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self,
|
||||
urdfRoot="",
|
||||
actionRepeat=1,
|
||||
isEnableSelfCollision=True,
|
||||
motorVelocityLimit=10.0,
|
||||
render=False):
|
||||
self._timeStep = 0.01
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._motorVelocityLimit = motorVelocityLimit
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
self._observation = []
|
||||
self._envStepCounter = 0
|
||||
self._render = render
|
||||
self._lastBasePosition = [0, 0, 0]
|
||||
if self._render:
|
||||
p.connect(p.GUI)
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
self._seed()
|
||||
self.reset()
|
||||
observationDim = self._minitaur.getObservationDimension()
|
||||
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
|
||||
actionDim = 8
|
||||
action_high = np.array([1] * actionDim)
|
||||
self.action_space = spaces.Box(-action_high, action_high)
|
||||
self.observation_space = spaces.Box(-observation_high, observation_high)
|
||||
self.viewer = None
|
||||
|
||||
def _reset(self):
|
||||
p.resetSimulation()
|
||||
p.setPhysicsEngineParameter(numSolverIterations=300)
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("%splane.urdf" % self._urdfRoot)
|
||||
p.setGravity(0,0,-10)
|
||||
self._minitaur = minitaur_new.Minitaur(urdfRootPath=self._urdfRoot, timeStep=self._timeStep, isEnableSelfCollision=self._isEnableSelfCollision, motorVelocityLimit=self._motorVelocityLimit)
|
||||
self._envStepCounter = 0
|
||||
self._lastBasePosition = [0, 0, 0]
|
||||
for i in range(100):
|
||||
p.stepSimulation()
|
||||
self._observation = self._minitaur.getObservation()
|
||||
return self._observation
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def _step(self, action):
|
||||
if (self._render):
|
||||
basePos = self._minitaur.getBasePosition()
|
||||
p.resetDebugVisualizerCamera(1, 30, 40, basePos)
|
||||
|
||||
if len(action) != self._minitaur.getActionDimension():
|
||||
raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(action)))
|
||||
|
||||
for i in range(len(action)):
|
||||
if not -1.01 <= action[i] <= 1.01:
|
||||
raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i]))
|
||||
|
||||
realAction = self._minitaur.convertFromLegModel(action)
|
||||
self._minitaur.applyAction(realAction)
|
||||
for i in range(self._actionRepeat):
|
||||
p.stepSimulation()
|
||||
if self._render:
|
||||
time.sleep(self._timeStep)
|
||||
self._observation = self._minitaur.getObservation()
|
||||
if self._termination():
|
||||
break
|
||||
self._envStepCounter += 1
|
||||
reward = self._reward()
|
||||
done = self._termination()
|
||||
return np.array(self._observation), reward, done, {}
|
||||
|
||||
def _render(self, mode='human', close=False):
|
||||
return
|
||||
|
||||
def is_fallen(self):
|
||||
orientation = self._minitaur.getBaseOrientation()
|
||||
rotMat = p.getMatrixFromQuaternion(orientation)
|
||||
localUp = rotMat[6:]
|
||||
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 or self._observation[-1] < 0.1
|
||||
|
||||
def _termination(self):
|
||||
return self.is_fallen()
|
||||
|
||||
def _reward(self):
|
||||
currentBasePosition = self._minitaur.getBasePosition()
|
||||
forward_reward = currentBasePosition[0] - self._lastBasePosition[0]
|
||||
self._lastBasePosition = currentBasePosition
|
||||
|
||||
energyWeight = 0.001
|
||||
energy = np.abs(np.dot(self._minitaur.getMotorTorques(), self._minitaur.getMotorVelocities())) * self._timeStep
|
||||
energy_reward = energyWeight * energy
|
||||
reward = forward_reward - energy_reward
|
||||
return reward
|
||||
@@ -0,0 +1,90 @@
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
import pybullet as p
|
||||
from pybullet_envs.bullet.minitaur import Minitaur
|
||||
|
||||
class MinitaurBulletEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self):
|
||||
self._timeStep = 0.01
|
||||
self._observation = []
|
||||
self._envStepCounter = 0
|
||||
self._lastBasePosition = [0, 0, 0]
|
||||
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.resetSimulation()
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("plane.urdf")
|
||||
p.setGravity(0,0,-10)
|
||||
self._minitaur = Minitaur()
|
||||
|
||||
observationDim = self._minitaur.getObservationDimension()
|
||||
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
|
||||
actionDim = 8
|
||||
action_high = np.array([math.pi / 2.0] * actionDim)
|
||||
self.action_space = spaces.Box(-action_high, action_high)
|
||||
self.observation_space = spaces.Box(-observation_high, observation_high)
|
||||
|
||||
self._seed()
|
||||
self.reset()
|
||||
self.viewer = None
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def _step(self, action):
|
||||
if len(action) != self._minitaur.getActionDimension():
|
||||
raise ValueError("We expect {} continuous action not {}.".format(self._minitaur.getActionDimension(), len(actions.continuous_actions)))
|
||||
|
||||
for i in range(len(action)):
|
||||
if not -math.pi/2 <= action[i] <= math.pi/2:
|
||||
raise ValueError("{}th action should be between -1 and 1 not {}.".format(i, action[i]))
|
||||
action[i] += math.pi / 2
|
||||
|
||||
self._minitaur.applyAction(action)
|
||||
p.stepSimulation()
|
||||
self._observation = self._minitaur.getObservation()
|
||||
self._envStepCounter += 1
|
||||
reward = self._reward()
|
||||
done = self._termination()
|
||||
return np.array(self._observation), reward, done, {}
|
||||
|
||||
def _reset(self):
|
||||
p.resetSimulation()
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("plane.urdf")
|
||||
p.setGravity(0,0,-10)
|
||||
self._minitaur = Minitaur()
|
||||
self._observation = self._minitaur.getObservation()
|
||||
|
||||
def _render(self, mode='human', close=False):
|
||||
return
|
||||
|
||||
def is_fallen(self):
|
||||
orientation = self._minitaur.getBaseOrientation()
|
||||
rotMat = p.getMatrixFromQuaternion(orientation)
|
||||
localUp = rotMat[6:]
|
||||
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
|
||||
|
||||
def _termination(self):
|
||||
return self.is_fallen()
|
||||
|
||||
def _reward(self):
|
||||
currentBasePosition = self._minitaur.getBasePosition()
|
||||
reward = np.dot(np.asarray([-1, 0, 0]), np.asarray(currentBasePosition) - np.asarray(self._lastBasePosition))
|
||||
self._lastBasePosition = currentBasePosition
|
||||
return reward
|
||||
176
examples/pybullet/gym/pybullet_envs/bullet/minitaur_new.py
Normal file
176
examples/pybullet/gym/pybullet_envs/bullet/minitaur_new.py
Normal file
@@ -0,0 +1,176 @@
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
import copy
|
||||
import math
|
||||
|
||||
class Minitaur:
|
||||
|
||||
def __init__(self, urdfRootPath='', timeStep=0.01, isEnableSelfCollision=True, motorVelocityLimit=10.0):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.isEnableSelfCollision = isEnableSelfCollision
|
||||
self.motorVelocityLimit = motorVelocityLimit
|
||||
self.timeStep = timeStep
|
||||
self.reset()
|
||||
|
||||
def buildJointNameToIdDict(self):
|
||||
nJoints = p.getNumJoints(self.quadruped)
|
||||
self.jointNameToId = {}
|
||||
for i in range(nJoints):
|
||||
jointInfo = p.getJointInfo(self.quadruped, i)
|
||||
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
|
||||
self.resetPose()
|
||||
|
||||
def buildMotorIdList(self):
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_leftR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_front_rightR_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
|
||||
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
|
||||
|
||||
def reset(self):
|
||||
if self.isEnableSelfCollision:
|
||||
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2], flags=p.URDF_USE_SELF_COLLISION)
|
||||
else:
|
||||
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, [0,0,.2])
|
||||
self.kp = 1
|
||||
self.kd = 1
|
||||
self.maxForce = 3.5
|
||||
self.nMotors = 8
|
||||
self.motorIdList = []
|
||||
self.motorDir = [-1, -1, -1, -1, 1, 1, 1, 1]
|
||||
self.buildJointNameToIdDict()
|
||||
self.buildMotorIdList()
|
||||
|
||||
|
||||
def setMotorAngleById(self, motorId, desiredAngle):
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
|
||||
|
||||
def setMotorAngleByName(self, motorName, desiredAngle):
|
||||
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
|
||||
|
||||
def resetPose(self):
|
||||
kneeFrictionForce = 0
|
||||
halfpi = 1.57079632679
|
||||
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
|
||||
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
|
||||
self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
#right front leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
|
||||
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
|
||||
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
|
||||
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
|
||||
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
|
||||
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
|
||||
|
||||
|
||||
def getBasePosition(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
return position
|
||||
|
||||
def getBaseOrientation(self):
|
||||
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
|
||||
return orientation
|
||||
|
||||
def getActionDimension(self):
|
||||
return self.nMotors
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
|
||||
def getObservation(self):
|
||||
observation = []
|
||||
observation.extend(self.getMotorAngles().tolist())
|
||||
observation.extend(self.getMotorVelocities().tolist())
|
||||
observation.extend(self.getMotorTorques().tolist())
|
||||
observation.extend(list(self.getBaseOrientation()))
|
||||
return observation
|
||||
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
if self.motorVelocityLimit < np.inf:
|
||||
currentMotorAngle = self.getMotorAngles()
|
||||
motorCommandsMax = currentMotorAngle + self.timeStep * self.motorVelocityLimit
|
||||
motorCommandsMin = currentMotorAngle - self.timeStep * self.motorVelocityLimit
|
||||
motorCommands = np.clip(motorCommands, motorCommandsMin, motorCommandsMax)
|
||||
motorCommandsWithDir = np.multiply(motorCommands, self.motorDir)
|
||||
# print('action: {}'.format(motorCommands))
|
||||
# print('motor: {}'.format(motorCommandsWithDir))
|
||||
for i in range(self.nMotors):
|
||||
self.setMotorAngleById(self.motorIdList[i], motorCommandsWithDir[i])
|
||||
|
||||
def getMotorAngles(self):
|
||||
motorAngles = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorAngles.append(jointState[0])
|
||||
motorAngles = np.multiply(motorAngles, self.motorDir)
|
||||
return motorAngles
|
||||
|
||||
def getMotorVelocities(self):
|
||||
motorVelocities = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorVelocities.append(jointState[1])
|
||||
motorVelocities = np.multiply(motorVelocities, self.motorDir)
|
||||
return motorVelocities
|
||||
|
||||
def getMotorTorques(self):
|
||||
motorTorques = []
|
||||
for i in range(self.nMotors):
|
||||
jointState = p.getJointState(self.quadruped, self.motorIdList[i])
|
||||
motorTorques.append(jointState[3])
|
||||
motorTorques = np.multiply(motorTorques, self.motorDir)
|
||||
return motorTorques
|
||||
|
||||
def convertFromLegModel(self, actions):
|
||||
motorAngle = copy.deepcopy(actions)
|
||||
scaleForSingularity = 1
|
||||
offsetForSingularity = 0.5
|
||||
motorAngle[0] = math.pi + math.pi / 4 * actions[0] - scaleForSingularity * math.pi / 4 * (actions[4] + 1 + offsetForSingularity)
|
||||
motorAngle[1] = math.pi - math.pi / 4 * actions[0] - scaleForSingularity * math.pi / 4 * (actions[4] + 1 + offsetForSingularity)
|
||||
motorAngle[2] = math.pi + math.pi / 4 * actions[1] - scaleForSingularity * math.pi / 4 * (actions[5] + 1 + offsetForSingularity)
|
||||
motorAngle[3] = math.pi - math.pi / 4 * actions[1] - scaleForSingularity * math.pi / 4 * (actions[5] + 1 + offsetForSingularity)
|
||||
motorAngle[4] = math.pi - math.pi / 4 * actions[2] - scaleForSingularity * math.pi / 4 * (actions[6] + 1 + offsetForSingularity)
|
||||
motorAngle[5] = math.pi + math.pi / 4 * actions[2] - scaleForSingularity * math.pi / 4 * (actions[6] + 1 + offsetForSingularity)
|
||||
motorAngle[6] = math.pi - math.pi / 4 * actions[3] - scaleForSingularity * math.pi / 4 * (actions[7] + 1 + offsetForSingularity)
|
||||
motorAngle[7] = math.pi + math.pi / 4 * actions[3] - scaleForSingularity * math.pi / 4 * (actions[7] + 1 + offsetForSingularity)
|
||||
return motorAngle
|
||||
57
examples/pybullet/gym/pybullet_envs/bullet/racecar.py
Normal file
57
examples/pybullet/gym/pybullet_envs/bullet/racecar.py
Normal file
@@ -0,0 +1,57 @@
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
import copy
|
||||
import math
|
||||
|
||||
class Racecar:
|
||||
|
||||
def __init__(self, urdfRootPath='', timeStep=0.01):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.timeStep = timeStep
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.racecarUniqueId = p.loadURDF("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.motorizedWheels = [2]
|
||||
self.steeringLinks=[4,6]
|
||||
self.speedMultiplier = 4.
|
||||
|
||||
|
||||
def getActionDimension(self):
|
||||
return self.nMotors
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
|
||||
def getObservation(self):
|
||||
observation = []
|
||||
pos,orn=p.getBasePositionAndOrientation(self.racecarUniqueId)
|
||||
|
||||
observation.extend(list(pos))
|
||||
observation.extend(list(orn))
|
||||
|
||||
return observation
|
||||
|
||||
def applyAction(self, motorCommands):
|
||||
targetVelocity=motorCommands[0]*self.speedMultiplier
|
||||
#print("targetVelocity")
|
||||
#print(targetVelocity)
|
||||
steeringAngle = motorCommands[1]
|
||||
#print("steeringAngle")
|
||||
#print(steeringAngle)
|
||||
#print("maxForce")
|
||||
#print(self.maxForce)
|
||||
|
||||
|
||||
for motor in self.motorizedwheels:
|
||||
p.setJointMotorControl2(self.racecarUniqueId,motor,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce)
|
||||
for steer in self.steeringLinks:
|
||||
p.setJointMotorControl2(self.racecarUniqueId,steer,p.POSITION_CONTROL,targetPosition=steeringAngle)
|
||||
|
||||
134
examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
Normal file
134
examples/pybullet/gym/pybullet_envs/bullet/racecarGymEnv.py
Normal file
@@ -0,0 +1,134 @@
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import pybullet as p
|
||||
from . import racecar
|
||||
import random
|
||||
|
||||
class RacecarGymEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self,
|
||||
urdfRoot="",
|
||||
actionRepeat=50,
|
||||
isEnableSelfCollision=True,
|
||||
renders=True):
|
||||
print("init")
|
||||
self._timeStep = 0.01
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
self._observation = []
|
||||
self._ballUniqueId = -1
|
||||
self._envStepCounter = 0
|
||||
self._renders = renders
|
||||
self._p = p
|
||||
if self._renders:
|
||||
p.connect(p.GUI)
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
self._seed()
|
||||
self.reset()
|
||||
observationDim = len(self.getExtendedObservation())
|
||||
#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()
|
||||
#p.setPhysicsEngineParameter(numSolverIterations=300)
|
||||
p.setTimeStep(self._timeStep)
|
||||
#p.loadURDF("%splane.urdf" % self._urdfRoot)
|
||||
stadiumobjects = p.loadSDF("%sstadium.sdf" % self._urdfRoot)
|
||||
#move the stadium objects slightly above 0
|
||||
for i in stadiumobjects:
|
||||
pos,orn = p.getBasePositionAndOrientation(i)
|
||||
newpos = [pos[0],pos[1],pos[2]+0.1]
|
||||
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 = p.loadURDF("sphere2.urdf",[ballx,bally,ballz])
|
||||
p.setGravity(0,0,-10)
|
||||
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
|
||||
self._envStepCounter = 0
|
||||
for i in range(100):
|
||||
p.stepSimulation()
|
||||
self._observation = self.getExtendedObservation()
|
||||
return np.array(self._observation)
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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]
|
||||
forward = fwd[action]
|
||||
steer = steerings[action]
|
||||
realaction = [forward,steer]
|
||||
self._racecar.applyAction(realaction)
|
||||
for i in range(self._actionRepeat):
|
||||
p.stepSimulation()
|
||||
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):
|
||||
return
|
||||
|
||||
def _termination(self):
|
||||
return self._envStepCounter>1000
|
||||
|
||||
def _reward(self):
|
||||
closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
|
||||
|
||||
numPt = len(closestPoints)
|
||||
reward=-1000
|
||||
#print(numPt)
|
||||
if (numPt>0):
|
||||
#print("reward:")
|
||||
reward = -closestPoints[0][8]
|
||||
#print(reward)
|
||||
return reward
|
||||
148
examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
Normal file
148
examples/pybullet/gym/pybullet_envs/bullet/racecarZEDGymEnv.py
Normal file
@@ -0,0 +1,148 @@
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import pybullet as p
|
||||
from . import racecar
|
||||
import random
|
||||
|
||||
class RacecarZEDGymEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self,
|
||||
urdfRoot="",
|
||||
actionRepeat=100,
|
||||
isEnableSelfCollision=True,
|
||||
renders=True):
|
||||
print("init")
|
||||
self._timeStep = 0.01
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
self._ballUniqueId = -1
|
||||
self._envStepCounter = 0
|
||||
self._renders = renders
|
||||
self._width = 100
|
||||
self._height = 10
|
||||
self._p = p
|
||||
if self._renders:
|
||||
p.connect(p.GUI)
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
self._seed()
|
||||
self.reset()
|
||||
observationDim = len(self.getExtendedObservation())
|
||||
#print("observationDim")
|
||||
#print(observationDim)
|
||||
|
||||
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
|
||||
self.action_space = spaces.Discrete(6)
|
||||
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
|
||||
|
||||
self.viewer = None
|
||||
|
||||
def _reset(self):
|
||||
p.resetSimulation()
|
||||
#p.setPhysicsEngineParameter(numSolverIterations=300)
|
||||
p.setTimeStep(self._timeStep)
|
||||
#p.loadURDF("%splane.urdf" % self._urdfRoot)
|
||||
stadiumobjects = p.loadSDF("%sstadium.sdf" % self._urdfRoot)
|
||||
#move the stadium objects slightly above 0
|
||||
for i in stadiumobjects:
|
||||
pos,orn = p.getBasePositionAndOrientation(i)
|
||||
newpos = [pos[0],pos[1],pos[2]+0.1]
|
||||
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 = p.loadURDF("sphere2red.urdf",[ballx,bally,ballz])
|
||||
p.setGravity(0,0,-10)
|
||||
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
|
||||
self._envStepCounter = 0
|
||||
for i in range(100):
|
||||
p.stepSimulation()
|
||||
self._observation = self.getExtendedObservation()
|
||||
return np.array(self._observation)
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def getExtendedObservation(self):
|
||||
carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
|
||||
carmat = p.getMatrixFromQuaternion(carorn)
|
||||
ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId)
|
||||
invCarPos,invCarOrn = p.invertTransform(carpos,carorn)
|
||||
ballPosInCar,ballOrnInCar = 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]
|
||||
up = [carmat[2],carmat[5],carmat[8]]
|
||||
viewMat = p.computeViewMatrix(eyePos,targetPos,up)
|
||||
#viewMat = p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
|
||||
#print("projectionMatrix:")
|
||||
#print(p.getDebugVisualizerCamera()[3])
|
||||
projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
|
||||
img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
|
||||
rgb=img_arr[2]
|
||||
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 = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
|
||||
#p.resetDebugVisualizerCamera(1, 30, -40, basePos)
|
||||
|
||||
fwd = [5,0,5,10,10,10]
|
||||
steerings = [-0.5,0,0.5,-0.3,0,0.3]
|
||||
forward = fwd[action]
|
||||
steer = steerings[action]
|
||||
realaction = [forward,steer]
|
||||
self._racecar.applyAction(realaction)
|
||||
for i in range(self._actionRepeat):
|
||||
p.stepSimulation()
|
||||
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):
|
||||
return
|
||||
|
||||
def _termination(self):
|
||||
return self._envStepCounter>1000
|
||||
|
||||
def _reward(self):
|
||||
closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
|
||||
|
||||
numPt = len(closestPoints)
|
||||
reward=-1000
|
||||
#print(numPt)
|
||||
if (numPt>0):
|
||||
#print("reward:")
|
||||
reward = -closestPoints[0][8]
|
||||
#print(reward)
|
||||
return reward
|
||||
124
examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoid.py
Normal file
124
examples/pybullet/gym/pybullet_envs/bullet/simpleHumanoid.py
Normal file
@@ -0,0 +1,124 @@
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
import copy
|
||||
import math
|
||||
import time
|
||||
|
||||
|
||||
|
||||
class SimpleHumanoid:
|
||||
|
||||
def __init__(self, urdfRootPath='', timeStep=0.01):
|
||||
self.urdfRootPath = urdfRootPath
|
||||
self.timeStep = timeStep
|
||||
self.reset()
|
||||
|
||||
|
||||
|
||||
def reset(self):
|
||||
self.initial_z = None
|
||||
|
||||
objs = p.loadMJCF("mjcf/humanoid_symmetric_no_ground.xml",flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
|
||||
self.human = objs[0]
|
||||
self.jdict = {}
|
||||
self.ordered_joints = []
|
||||
self.ordered_joint_indices = []
|
||||
|
||||
for j in range( p.getNumJoints(self.human) ):
|
||||
info = p.getJointInfo(self.human, j)
|
||||
link_name = info[12].decode("ascii")
|
||||
if link_name=="left_foot": self.left_foot = j
|
||||
if link_name=="right_foot": self.right_foot = j
|
||||
self.ordered_joint_indices.append(j)
|
||||
if info[2] != p.JOINT_REVOLUTE: continue
|
||||
jname = info[1].decode("ascii")
|
||||
self.jdict[jname] = j
|
||||
lower, upper = (info[8], info[9])
|
||||
self.ordered_joints.append( (j, lower, upper) )
|
||||
p.setJointMotorControl2(self.human, j, controlMode=p.VELOCITY_CONTROL, force=0)
|
||||
|
||||
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
|
||||
self.motor_power = [100, 100, 100]
|
||||
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
|
||||
self.motor_power += [100, 100, 300, 200]
|
||||
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
|
||||
self.motor_power += [100, 100, 300, 200]
|
||||
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
|
||||
self.motor_power += [75, 75, 75]
|
||||
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
|
||||
self.motor_power += [75, 75, 75]
|
||||
self.motors = [self.jdict[n] for n in self.motor_names]
|
||||
print("self.motors")
|
||||
print(self.motors)
|
||||
print("num motors")
|
||||
print(len(self.motors))
|
||||
|
||||
|
||||
|
||||
def current_relative_position(self, jointStates, human, j, lower, upper):
|
||||
#print("j")
|
||||
#print(j)
|
||||
#print (len(jointStates))
|
||||
#print(j)
|
||||
temp = jointStates[j]
|
||||
pos = temp[0]
|
||||
vel = temp[1]
|
||||
#print("pos")
|
||||
#print(pos)
|
||||
#print("vel")
|
||||
#print(vel)
|
||||
pos_mid = 0.5 * (lower + upper);
|
||||
return (
|
||||
2 * (pos - pos_mid) / (upper - lower),
|
||||
0.1 * vel
|
||||
)
|
||||
|
||||
def collect_observations(self, human):
|
||||
#print("ordered_joint_indices")
|
||||
#print(ordered_joint_indices)
|
||||
|
||||
|
||||
jointStates = p.getJointStates(human,self.ordered_joint_indices)
|
||||
j = np.array([self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints]).flatten()
|
||||
#print("j")
|
||||
#print(j)
|
||||
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)
|
||||
#print("body_xyz")
|
||||
#print(body_xyz, qx,qy,qz,qw)
|
||||
z = body_xyz[2]
|
||||
self.distance = body_xyz[0]
|
||||
if self.initial_z==None:
|
||||
self.initial_z = z
|
||||
(vx, vy, vz), _ = p.getBaseVelocity(human)
|
||||
more = np.array([z-self.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw])
|
||||
rcont = p.getContactPoints(human, -1, self.right_foot, -1)
|
||||
#print("rcont")
|
||||
#print(rcont)
|
||||
lcont = p.getContactPoints(human, -1, self.left_foot, -1)
|
||||
#print("lcont")
|
||||
#print(lcont)
|
||||
feet_contact = np.array([len(rcont)>0, len(lcont)>0])
|
||||
return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5)
|
||||
|
||||
def getActionDimension(self):
|
||||
return len(self.motors)
|
||||
|
||||
def getObservationDimension(self):
|
||||
return len(self.getObservation())
|
||||
|
||||
def getObservation(self):
|
||||
observation = self.collect_observations(self.human)
|
||||
return observation
|
||||
|
||||
def applyAction(self, actions):
|
||||
forces = [0.] * len(self.motors)
|
||||
for m in range(len(self.motors)):
|
||||
forces[m] = self.motor_power[m]*actions[m]*0.082
|
||||
p.setJointMotorControlArray(self.human, self.motors,controlMode=p.TORQUE_CONTROL, forces=forces)
|
||||
|
||||
p.stepSimulation()
|
||||
time.sleep(0.01)
|
||||
distance=5
|
||||
yaw = 0
|
||||
#humanPos, humanOrn = p.getBasePositionAndOrientation(self.human)
|
||||
#p.resetDebugVisualizerCamera(distance,yaw,-20,humanPos);
|
||||
@@ -0,0 +1,102 @@
|
||||
import math
|
||||
import gym
|
||||
from gym import spaces
|
||||
from gym.utils import seeding
|
||||
import numpy as np
|
||||
import time
|
||||
import pybullet as p
|
||||
from . import simpleHumanoid
|
||||
import random
|
||||
|
||||
class SimpleHumanoidGymEnv(gym.Env):
|
||||
metadata = {
|
||||
'render.modes': ['human', 'rgb_array'],
|
||||
'video.frames_per_second' : 50
|
||||
}
|
||||
|
||||
def __init__(self,
|
||||
urdfRoot="",
|
||||
actionRepeat=50,
|
||||
isEnableSelfCollision=True,
|
||||
renders=True):
|
||||
print("init")
|
||||
self._timeStep = 0.01
|
||||
self._urdfRoot = urdfRoot
|
||||
self._actionRepeat = actionRepeat
|
||||
self._isEnableSelfCollision = isEnableSelfCollision
|
||||
self._observation = []
|
||||
self._envStepCounter = 0
|
||||
self._renders = renders
|
||||
self._p = p
|
||||
if self._renders:
|
||||
p.connect(p.GUI)
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
self._seed()
|
||||
self.reset()
|
||||
observationDim = len(self.getExtendedObservation())
|
||||
#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()
|
||||
#p.setPhysicsEngineParameter(numSolverIterations=300)
|
||||
p.setTimeStep(self._timeStep)
|
||||
p.loadURDF("%splane.urdf" % self._urdfRoot)
|
||||
|
||||
dist = 5 +2.*random.random()
|
||||
ang = 2.*3.1415925438*random.random()
|
||||
|
||||
ballx = dist * math.sin(ang)
|
||||
bally = dist * math.cos(ang)
|
||||
ballz = 1
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
|
||||
self._envStepCounter = 0
|
||||
p.stepSimulation()
|
||||
self._observation = self.getExtendedObservation()
|
||||
return np.array(self._observation)
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def _seed(self, seed=None):
|
||||
self.np_random, seed = seeding.np_random(seed)
|
||||
return [seed]
|
||||
|
||||
def getExtendedObservation(self):
|
||||
self._observation = self._humanoid.getObservation()
|
||||
return self._observation
|
||||
|
||||
def _step(self, action):
|
||||
self._humanoid.applyAction(action)
|
||||
for i in range(self._actionRepeat):
|
||||
p.stepSimulation()
|
||||
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):
|
||||
return
|
||||
|
||||
def _termination(self):
|
||||
return self._envStepCounter>1000
|
||||
|
||||
def _reward(self):
|
||||
reward=self._humanoid.distance
|
||||
print(reward)
|
||||
return reward
|
||||
Reference in New Issue
Block a user