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

add yapf style and apply yapf to format all Python files

Browse Source 
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
2019-04-27 07:31:15 -07:00
parent c591735042
commit ef9570c315
347 changed files with 70304 additions and 22752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/pybullet/gym/pybullet_envs/bullet/bullet_client.py
View File

@@ -9,9 +9,9 @@ class BulletClient(object):
def __init__(self, connection_mode=pybullet.DIRECT, options=""):
"""Create a simulation and connect to it."""
self._client = pybullet.connect(pybullet.SHARED_MEMORY)
if(self._client<0):
print("options=",options)
self._client = pybullet.connect(connection_mode,options=options)
if (self._client < 0):
print("options=", options)
self._client = pybullet.connect(connection_mode, options=options)
self._shapes = {}
def __del__(self):
@@ -25,5 +25,5 @@ class BulletClient(object):
"""Inject the client id into Bullet functions."""
attribute = getattr(pybullet, name)
if inspect.isbuiltin(attribute):
attribute = functools.partial(attribute, physicsClientId=self._client)
attribute = functools.partial(attribute, physicsClientId=self._client)
return attribute

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

@@ -2,10 +2,10 @@
Classic cart-pole system implemented by Rich Sutton et al.
Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c
"""
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import logging
import math
@@ -21,26 +21,24 @@ from pkg_resources import parse_version
logger = logging.getLogger(__name__)
class CartPoleBulletEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
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)
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.connect(p.DIRECT)
self.theta_threshold_radians = 12 * 2 * math.pi / 360
self.x_threshold = 0.4 #2.4
self.x_threshold = 0.4 #2.4
high = np.array([
self.x_threshold * 2,
np.finfo(np.float32).max,
self.theta_threshold_radians * 2,
np.finfo(np.float32).max])
self.x_threshold * 2,
np.finfo(np.float32).max, self.theta_threshold_radians * 2,
np.finfo(np.float32).max
])
self.force_mag = 10
@@ -60,7 +58,7 @@ class CartPoleBulletEnv(gym.Env):
return [seed]
def step(self, action):
force = self.force_mag if action==1 else -self.force_mag
force = self.force_mag if action == 1 else -self.force_mag
p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
p.stepSimulation()
@@ -78,16 +76,17 @@ class CartPoleBulletEnv(gym.Env):
return np.array(self.state), reward, done, {}
def reset(self):
# print("-----------reset simulation---------------")
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"),
[0, 0, 0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setGravity(0, 0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
@@ -100,4 +99,4 @@ class CartPoleBulletEnv(gym.Env):
return np.array(self.state)
def render(self, mode='human', close=False):
return
return

1
examples/pybullet/gym/pybullet_envs/bullet/env_randomizer_base.py
View File

@@ -22,4 +22,3 @@ class EnvRandomizerBase(object):
env: The environment to be randomized.
"""
pass

195
examples/pybullet/gym/pybullet_envs/bullet/kuka.py
View File

@@ -1,7 +1,7 @@
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import pybullet as p
import numpy as np
@@ -17,49 +17,60 @@ class Kuka:
self.timeStep = timeStep
self.maxVelocity = .35
self.maxForce = 200.
self.fingerAForce = 2
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useInverseKinematics = 1
self.useSimulation = 1
self.useNullSpace =21
self.useNullSpace = 21
self.useOrientation = 1
self.kukaEndEffectorIndex = 6
self.kukaGripperIndex = 7
#lower limits for null space
self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
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]
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]
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]
self.rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
self.jd=[0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001]
self.jd = [
0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
0.00001, 0.00001, 0.00001, 0.00001
]
self.reset()
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
objects = p.loadSDF(os.path.join(self.urdfRootPath, "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 ]
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(self.urdfRootPath,"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]
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(self.urdfRootPath, "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)
for i in range(self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid, i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
@@ -70,98 +81,136 @@ class Kuka:
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
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.kukaGripperIndex)
state = p.getLinkState(self.kukaUid, self.kukaGripperIndex)
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)
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.65):
self.endEffectorPos[0]=0.65
if (self.endEffectorPos[0]<0.50):
self.endEffectorPos[0]=0.50
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
self.endEffectorPos[0] = self.endEffectorPos[0] + dx
if (self.endEffectorPos[0] > 0.65):
self.endEffectorPos[0] = 0.65
if (self.endEffectorPos[0] < 0.50):
self.endEffectorPos[0] = 0.50
self.endEffectorPos[1] = self.endEffectorPos[1] + dy
if (self.endEffectorPos[1] < -0.17):
self.endEffectorPos[1] = -0.17
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.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorPos[2] = self.endEffectorPos[2] + dz
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)
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)
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)
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)
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):
for i in range(self.kukaEndEffectorIndex + 1):
#print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=self.maxForce,
maxVelocity=self.maxVelocity,
positionGain=0.3,
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])
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)
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)):
for action in range(len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,
motor,
p.POSITION_CONTROL,
targetPosition=motorCommands[action],
force=self.maxForce)

188
examples/pybullet/gym/pybullet_envs/bullet/kukaCamGymEnv.py
View File

@@ -1,8 +1,7 @@
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import math
import gym
@@ -21,11 +20,9 @@ maxSteps = 1000
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class KukaCamGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50}
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
@@ -33,7 +30,7 @@ class KukaCamGymEnv(gym.Env):
isEnableSelfCollision=True,
renders=False,
isDiscrete=False):
self._timeStep = 1./240.
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
@@ -42,14 +39,14 @@ class KukaCamGymEnv(gym.Env):
self._renders = renders
self._width = 341
self._height = 256
self._isDiscrete=isDiscrete
self._isDiscrete = isDiscrete
self.terminated = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
if (cid < 0):
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")
@@ -67,7 +64,10 @@ class KukaCamGymEnv(gym.Env):
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4), dtype=np.uint8)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height, self._width, 4),
dtype=np.uint8)
self.viewer = None
def reset(self):
@@ -75,17 +75,19 @@ class KukaCamGymEnv(gym.Env):
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000,
0.000000, 0.000000, 0.0, 1.0)
xpos = 0.5 +0.2*random.random()
ypos = 0 +0.25*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
xpos = 0.5 + 0.2 * random.random()
ypos = 0 + 0.25 * random.random()
ang = 3.1415925438 * random.random()
orn = p.getQuaternionFromEuler([0, 0, ang])
self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), xpos, ypos, -0.1,
orn[0], orn[1], orn[2], orn[3])
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
@@ -101,49 +103,59 @@ class KukaCamGymEnv(gym.Env):
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]
#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
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._isDiscrete):
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]
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]
realAction = [dx, dy, -0.002, da, f]
else:
dv = 0.01
dx = action[0] * dv
dy = action[1] * dv
da = action[2] * 0.1
f = 0.3
realAction = [dx,dy,-0.002,da,f]
realAction = [dx, dy, -0.002, da, f]
return self.step2( realAction)
return self.step2(realAction)
def step2(self, action):
for i in range(self._actionRepeat):
@@ -156,7 +168,7 @@ class KukaCamGymEnv(gym.Env):
self._observation = self.getExtendedObservation()
if self._renders:
time.sleep(self._timeStep)
time.sleep(self._timeStep)
#print("self._envStepCounter")
#print(self._envStepCounter)
@@ -170,66 +182,67 @@ class KukaCamGymEnv(gym.Env):
def render(self, mode='human', close=False):
if mode != "rgb_array":
return np.array([])
base_pos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(
fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT,
nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(
width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
base_pos, orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(fov=60,
aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>maxSteps):
if (self.terminated or self._envStepCounter > maxSteps):
self._observation = self.getExtendedObservation()
return True
maxDist = 0.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid, maxDist)
if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
if (len(closestPoints)): #(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (100):
graspAction = [0,0,0.0001,0,fingerAngle]
for i in range(100):
graspAction = [0, 0, 0.0001, 0, fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
fingerAngle = fingerAngle - (0.3 / 100.)
if (fingerAngle < 0):
fingerAngle = 0
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
for i in range(1000):
graspAction = [0, 0, 0.001, 0, fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
if (blockPos[2] > 0.23):
#print("BLOCKPOS!")
#print(blockPos[2])
break
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
if (actualEndEffectorPos[2] > 0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
@@ -237,20 +250,21 @@ class KukaCamGymEnv(gym.Env):
def _reward(self):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex)
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid, 1000, -1,
self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
if (numPt > 0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
reward = -closestPoints[0][8] * 10
if (blockPos[2] > 0.2):
#print("grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
reward = reward+1000
reward = reward + 1000
#print("reward")
#print(reward)

199
examples/pybullet/gym/pybullet_envs/bullet/kukaGymEnv.py
View File

@@ -1,7 +1,7 @@
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir)
os.sys.path.insert(0,currentdir)
print("current_dir=" + currentdir)
os.sys.path.insert(0, currentdir)
import math
import gym
@@ -20,11 +20,9 @@ largeValObservation = 100
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class KukaGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50}
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
@@ -32,10 +30,10 @@ class KukaGymEnv(gym.Env):
isEnableSelfCollision=True,
renders=False,
isDiscrete=False,
maxSteps = 1000):
maxSteps=1000):
#print("KukaGymEnv __init__")
self._isDiscrete = isDiscrete
self._timeStep = 1./240.
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
@@ -51,9 +49,9 @@ class KukaGymEnv(gym.Env):
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
if (cid < 0):
cid = 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")
@@ -67,10 +65,10 @@ class KukaGymEnv(gym.Env):
if (self._isDiscrete):
self.action_space = spaces.Discrete(7)
else:
action_dim = 3
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
action_dim = 3
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
@@ -80,17 +78,19 @@ class KukaGymEnv(gym.Env):
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000,
0.000000, 0.000000, 0.0, 1.0)
xpos = 0.55 +0.12*random.random()
ypos = 0 +0.2*random.random()
ang = 3.14*0.5+3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.15,orn[0],orn[1],orn[2],orn[3])
xpos = 0.55 + 0.12 * random.random()
ypos = 0 + 0.2 * random.random()
ang = 3.14 * 0.5 + 3.1415925438 * random.random()
orn = p.getQuaternionFromEuler([0, 0, ang])
self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), xpos, ypos, -0.15,
orn[0], orn[1], orn[2], orn[3])
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
@@ -105,47 +105,48 @@ class KukaGymEnv(gym.Env):
return [seed]
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
self._observation = self._kuka.getObservation()
gripperState = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]]
dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]]
dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]]
invGripperPos, invGripperOrn = p.invertTransform(gripperPos, gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0], gripperMat[3], gripperMat[6]]
dir1 = [gripperMat[1], gripperMat[4], gripperMat[7]]
dir2 = [gripperMat[2], gripperMat[5], gripperMat[8]]
gripperEul = p.getEulerFromQuaternion(gripperOrn)
#print("gripperEul")
#print(gripperEul)
blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn)
projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
#print("projectedBlockPos2D")
#print(projectedBlockPos2D)
#print("blockEulerInGripper")
#print(blockEulerInGripper)
gripperEul = p.getEulerFromQuaternion(gripperOrn)
#print("gripperEul")
#print(gripperEul)
blockPosInGripper, blockOrnInGripper = p.multiplyTransforms(invGripperPos, invGripperOrn,
blockPos, blockOrn)
projectedBlockPos2D = [blockPosInGripper[0], blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
#print("projectedBlockPos2D")
#print(projectedBlockPos2D)
#print("blockEulerInGripper")
#print(blockEulerInGripper)
#we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]]
#we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ = [blockPosInGripper[0], blockPosInGripper[1], blockEulerInGripper[2]]
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def step(self, action):
if (self._isDiscrete):
dv = 0.005
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.05,0.05][action]
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.05, 0.05][action]
f = 0.3
realAction = [dx,dy,-0.002,da,f]
realAction = [dx, dy, -0.002, da, f]
else:
#print("action[0]=", str(action[0]))
dv = 0.005
@@ -153,8 +154,8 @@ class KukaGymEnv(gym.Env):
dy = action[1] * dv
da = action[2] * 0.05
f = 0.3
realAction = [dx,dy,-0.002,da,f]
return self.step2( realAction)
realAction = [dx, dy, -0.002, da, f]
return self.step2(realAction)
def step2(self, action):
for i in range(self._actionRepeat):
@@ -171,11 +172,13 @@ class KukaGymEnv(gym.Env):
#print(self._envStepCounter)
done = self._termination()
npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]])
actionCost = np.linalg.norm(npaction)*10.
npaction = np.array([
action[3]
]) #only penalize rotation until learning works well [action[0],action[1],action[3]])
actionCost = np.linalg.norm(npaction) * 10.
#print("actionCost")
#print(actionCost)
reward = self._reward()-actionCost
reward = self._reward() - actionCost
#print("reward")
#print(reward)
@@ -187,22 +190,23 @@ class KukaGymEnv(gym.Env):
if mode != "rgb_array":
return np.array([])
base_pos,orn = self._p.getBasePositionAndOrientation(self._kuka.kukaUid)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(
fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT,
nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(
width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, renderer=self._p.ER_BULLET_HARDWARE_OPENGL)
#renderer=self._p.ER_TINY_RENDERER)
base_pos, orn = self._p.getBasePositionAndOrientation(self._kuka.kukaUid)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(fov=60,
aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=self._p.ER_BULLET_HARDWARE_OPENGL)
#renderer=self._p.ER_TINY_RENDERER)
rgb_array = np.array(px, dtype=np.uint8)
rgb_array = np.reshape(rgb_array, (RENDER_HEIGHT, RENDER_WIDTH, 4))
@@ -210,49 +214,47 @@ class KukaGymEnv(gym.Env):
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>self._maxSteps):
if (self.terminated or self._envStepCounter > self._maxSteps):
self._observation = self.getExtendedObservation()
return True
maxDist = 0.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid, maxDist)
if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
if (len(closestPoints)): #(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("terminating, closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (100):
graspAction = [0,0,0.0001,0,fingerAngle]
for i in range(100):
graspAction = [0, 0, 0.0001, 0, fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
fingerAngle = fingerAngle - (0.3 / 100.)
if (fingerAngle < 0):
fingerAngle = 0
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
for i in range(1000):
graspAction = [0, 0, 0.001, 0, fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
if (blockPos[2] > 0.23):
#print("BLOCKPOS!")
#print(blockPos[2])
break
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
if (actualEndEffectorPos[2] > 0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
@@ -260,18 +262,19 @@ class KukaGymEnv(gym.Env):
def _reward(self):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex)
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid, 1000, -1,
self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
if (numPt > 0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
reward = reward+10000
reward = -closestPoints[0][8] * 10
if (blockPos[2] > 0.2):
reward = reward + 10000
print("successfully grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
@@ -279,7 +282,7 @@ class KukaGymEnv(gym.Env):
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print("reward")
#print(reward)
return reward

68
examples/pybullet/gym/pybullet_envs/bullet/kuka_diverse_object_gym_env.py
View File

@@ -13,6 +13,7 @@ import glob
from pkg_resources import parse_version
import gym
class KukaDiverseObjectEnv(KukaGymEnv):
"""Class for Kuka environment with diverse objects.
@@ -61,7 +62,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
"""
self._isDiscrete = isDiscrete
self._timeStep = 1./240.
self._timeStep = 1. / 240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
@@ -85,9 +86,9 @@ class KukaDiverseObjectEnv(KukaGymEnv):
if self._renders:
self.cid = p.connect(p.SHARED_MEMORY)
if (self.cid<0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
if (self.cid < 0):
self.cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
else:
self.cid = p.connect(p.DIRECT)
self.seed()
@@ -100,9 +101,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
else:
self.action_space = spaces.Box(low=-1, high=1, shape=(3,)) # dx, dy, da
if self._removeHeightHack:
self.action_space = spaces.Box(low=-1,
high=1,
shape=(4,)) # dx, dy, dz, da
self.action_space = spaces.Box(low=-1, high=1, shape=(4,)) # dx, dy, dz, da
self.viewer = None
def reset(self):
@@ -111,17 +110,15 @@ class KukaDiverseObjectEnv(KukaGymEnv):
# Set the camera settings.
look = [0.23, 0.2, 0.54]
distance = 1.
pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3)
yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3)
pitch = -56 + self._cameraRandom * np.random.uniform(-3, 3)
yaw = 245 + self._cameraRandom * np.random.uniform(-3, 3)
roll = 0
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
look, distance, yaw, pitch, roll, 2)
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(look, distance, yaw, pitch, roll, 2)
fov = 20. + self._cameraRandom * np.random.uniform(-2, 2)
aspect = self._width / self._height
near = 0.01
far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
self._proj_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
self._attempted_grasp = False
self._env_step = 0
@@ -130,18 +127,18 @@ class KukaDiverseObjectEnv(KukaGymEnv):
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000,
0.000000, 0.000000, 0.0, 1.0)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# Choose the objects in the bin.
urdfList = self._get_random_object(
self._numObjects, self._isTest)
urdfList = self._get_random_object(self._numObjects, self._isTest)
self._objectUids = self._randomly_place_objects(urdfList)
self._observation = self._get_observation()
return np.array(self._observation)
@@ -156,17 +153,15 @@ class KukaDiverseObjectEnv(KukaGymEnv):
The list of object unique ID's.
"""
# Randomize positions of each object urdf.
objectUids = []
for urdf_name in urdfList:
xpos = 0.4 +self._blockRandom*random.random()
ypos = self._blockRandom*(random.random()-.5)
angle = np.pi/2 + self._blockRandom * np.pi * random.random()
xpos = 0.4 + self._blockRandom * random.random()
ypos = self._blockRandom * (random.random() - .5)
angle = np.pi / 2 + self._blockRandom * np.pi * random.random()
orn = p.getQuaternionFromEuler([0, 0, angle])
urdf_path = os.path.join(self._urdfRoot, urdf_name)
uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
[orn[0], orn[1], orn[2], orn[3]])
uid = p.loadURDF(urdf_path, [xpos, ypos, .15], [orn[0], orn[1], orn[2], orn[3]])
objectUids.append(uid)
# Let each object fall to the tray individual, to prevent object
# intersection.
@@ -178,9 +173,9 @@ class KukaDiverseObjectEnv(KukaGymEnv):
"""Return the observation as an image.
"""
img_arr = p.getCameraImage(width=self._width,
height=self._height,
viewMatrix=self._view_matrix,
projectionMatrix=self._proj_matrix)
height=self._height,
viewMatrix=self._view_matrix,
projectionMatrix=self._proj_matrix)
rgb = img_arr[2]
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
return np_img_arr[:, :, :3]
@@ -246,8 +241,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
break
# If we are close to the bin, attempt grasp.
state = p.getLinkState(self._kuka.kukaUid,
self._kuka.kukaEndEffectorIndex)
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
end_effector_pos = state[0]
if end_effector_pos[2] <= 0.1:
finger_angle = 0.3
@@ -257,7 +251,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
p.stepSimulation()
#if self._renders:
# time.sleep(self._timeStep)
finger_angle -= 0.3/100.
finger_angle -= 0.3 / 100.
if finger_angle < 0:
finger_angle = 0
for _ in range(500):
@@ -266,7 +260,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
finger_angle -= 0.3/100.
finger_angle -= 0.3 / 100.
if finger_angle < 0:
finger_angle = 0
self._attempted_grasp = True
@@ -274,9 +268,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
done = self._termination()
reward = self._reward()
debug = {
'grasp_success': self._graspSuccess
}
debug = {'grasp_success': self._graspSuccess}
return observation, reward, done, debug
def _reward(self):
@@ -288,8 +280,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
reward = 0
self._graspSuccess = 0
for uid in self._objectUids:
pos, _ = p.getBasePositionAndOrientation(
uid)
pos, _ = p.getBasePositionAndOrientation(uid)
# If any block is above height, provide reward.
if pos[2] > 0.2:
self._graspSuccess += 1
@@ -319,8 +310,7 @@ class KukaDiverseObjectEnv(KukaGymEnv):
urdf_pattern = os.path.join(self._urdfRoot, 'random_urdfs/*[1-9]/*.urdf')
found_object_directories = glob.glob(urdf_pattern)
total_num_objects = len(found_object_directories)
selected_objects = np.random.choice(np.arange(total_num_objects),
num_objects)
selected_objects = np.random.choice(np.arange(total_num_objects), num_objects)
selected_objects_filenames = []
for object_index in selected_objects:
selected_objects_filenames += [found_object_directories[object_index]]

195
examples/pybullet/gym/pybullet_envs/bullet/minitaur.py
View File

@@ -15,9 +15,8 @@ OVERHEAT_SHUTDOWN_TORQUE = 2.45
OVERHEAT_SHUTDOWN_TIME = 1.0
LEG_POSITION = ["front_left", "back_left", "front_right", "back_right"]
MOTOR_NAMES = [
"motor_front_leftL_joint", "motor_front_leftR_joint",
"motor_back_leftL_joint", "motor_back_leftR_joint",
"motor_front_rightL_joint", "motor_front_rightR_joint",
"motor_front_leftL_joint", "motor_front_leftR_joint", "motor_back_leftL_joint",
"motor_back_leftR_joint", "motor_front_rightL_joint", "motor_front_rightR_joint",
"motor_back_rightL_joint", "motor_back_rightR_joint"
]
LEG_LINK_ID = [2, 3, 5, 6, 8, 9, 11, 12, 15, 16, 18, 19, 21, 22, 24, 25]
@@ -33,7 +32,7 @@ class Minitaur(object):
def __init__(self,
pybullet_client,
urdf_root= os.path.join(os.path.dirname(__file__),"../data"),
urdf_root=os.path.join(os.path.dirname(__file__), "../data"),
time_step=0.01,
self_collision_enabled=False,
motor_velocity_limit=np.inf,
@@ -87,10 +86,9 @@ class Minitaur(object):
if self._accurate_motor_model_enabled:
self._kp = motor_kp
self._kd = motor_kd
self._motor_model = motor.MotorModel(
torque_control_enabled=self._torque_control_enabled,
kp=self._kp,
kd=self._kd)
self._motor_model = motor.MotorModel(torque_control_enabled=self._torque_control_enabled,
kp=self._kp,
kd=self._kd)
elif self._pd_control_enabled:
self._kp = 8
self._kd = kd_for_pd_controllers
@@ -101,15 +99,12 @@ class Minitaur(object):
self.Reset()
def _RecordMassInfoFromURDF(self):
self._base_mass_urdf = self._pybullet_client.getDynamicsInfo(
self.quadruped, BASE_LINK_ID)[0]
self._base_mass_urdf = self._pybullet_client.getDynamicsInfo(self.quadruped, BASE_LINK_ID)[0]
self._leg_masses_urdf = []
self._leg_masses_urdf.append(
self._pybullet_client.getDynamicsInfo(self.quadruped, LEG_LINK_ID[0])[
0])
self._pybullet_client.getDynamicsInfo(self.quadruped, LEG_LINK_ID[0])[0])
self._leg_masses_urdf.append(
self._pybullet_client.getDynamicsInfo(self.quadruped, MOTOR_LINK_ID[0])[
0])
self._pybullet_client.getDynamicsInfo(self.quadruped, MOTOR_LINK_ID[0])[0])
def _BuildJointNameToIdDict(self):
num_joints = self._pybullet_client.getNumJoints(self.quadruped)
@@ -119,9 +114,7 @@ class Minitaur(object):
self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
def _BuildMotorIdList(self):
self._motor_id_list = [
self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES
]
self._motor_id_list = [self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES]
def Reset(self, reload_urdf=True):
"""Reset the minitaur to its initial states.
@@ -144,39 +137,35 @@ class Minitaur(object):
self._RecordMassInfoFromURDF()
self.ResetPose(add_constraint=True)
if self._on_rack:
self._pybullet_client.createConstraint(
self.quadruped, -1, -1, -1, self._pybullet_client.JOINT_FIXED,
[0, 0, 0], [0, 0, 0], [0, 0, 1])
self._pybullet_client.createConstraint(self.quadruped, -1, -1, -1,
self._pybullet_client.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0, 0, 1])
else:
self._pybullet_client.resetBasePositionAndOrientation(
self.quadruped, INIT_POSITION, INIT_ORIENTATION)
self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0],
[0, 0, 0])
self._pybullet_client.resetBasePositionAndOrientation(self.quadruped, INIT_POSITION,
INIT_ORIENTATION)
self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0])
self.ResetPose(add_constraint=False)
self._overheat_counter = np.zeros(self.num_motors)
self._motor_enabled_list = [True] * self.num_motors
def _SetMotorTorqueById(self, motor_id, torque):
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.TORQUE_CONTROL,
force=torque)
self._pybullet_client.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.TORQUE_CONTROL,
force=torque)
def _SetDesiredMotorAngleById(self, motor_id, desired_angle):
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.POSITION_CONTROL,
targetPosition=desired_angle,
positionGain=self._kp,
velocityGain=self._kd,
force=self._max_force)
self._pybullet_client.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=motor_id,
controlMode=self._pybullet_client.POSITION_CONTROL,
targetPosition=desired_angle,
positionGain=self._kp,
velocityGain=self._kd,
force=self._max_force)
def _SetDesiredMotorAngleByName(self, motor_name, desired_angle):
self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name],
desired_angle)
self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name], desired_angle)
def ResetPose(self, add_constraint):
"""Reset the pose of the minitaur.
@@ -200,59 +189,53 @@ class Minitaur(object):
knee_angle = -2.1834
leg_position = LEG_POSITION[leg_id]
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "L_joint"],
self._motor_direction[2 * leg_id] * half_pi,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["knee_" + leg_position + "L_link"],
self._motor_direction[2 * leg_id] * knee_angle,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["motor_" + leg_position + "R_joint"],
self._motor_direction[2 * leg_id + 1] * half_pi,
targetVelocity=0)
self._pybullet_client.resetJointState(
self.quadruped,
self._joint_name_to_id["knee_" + leg_position + "R_link"],
self._motor_direction[2 * leg_id + 1] * knee_angle,
targetVelocity=0)
self._pybullet_client.resetJointState(self.quadruped,
self._joint_name_to_id["motor_" + leg_position +
"L_joint"],
self._motor_direction[2 * leg_id] * half_pi,
targetVelocity=0)
self._pybullet_client.resetJointState(self.quadruped,
self._joint_name_to_id["knee_" + leg_position +
"L_link"],
self._motor_direction[2 * leg_id] * knee_angle,
targetVelocity=0)
self._pybullet_client.resetJointState(self.quadruped,
self._joint_name_to_id["motor_" + leg_position +
"R_joint"],
self._motor_direction[2 * leg_id + 1] * half_pi,
targetVelocity=0)
self._pybullet_client.resetJointState(self.quadruped,
self._joint_name_to_id["knee_" + leg_position +
"R_link"],
self._motor_direction[2 * leg_id + 1] * knee_angle,
targetVelocity=0)
if add_constraint:
self._pybullet_client.createConstraint(
self.quadruped, self._joint_name_to_id["knee_"
+ leg_position + "R_link"],
self.quadruped, self._joint_name_to_id["knee_"
+ leg_position + "L_link"],
self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT)
self.quadruped, self._joint_name_to_id["knee_" + leg_position + "R_link"],
self.quadruped, self._joint_name_to_id["knee_" + leg_position + "L_link"],
self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0], KNEE_CONSTRAINT_POINT_RIGHT,
KNEE_CONSTRAINT_POINT_LEFT)
if self._accurate_motor_model_enabled or self._pd_control_enabled:
# Disable the default motor in pybullet.
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(self._joint_name_to_id["motor_"
+ leg_position + "L_joint"]),
jointIndex=(self._joint_name_to_id["motor_" + leg_position + "L_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=knee_friction_force)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=(self._joint_name_to_id["motor_"
+ leg_position + "R_joint"]),
jointIndex=(self._joint_name_to_id["motor_" + leg_position + "R_joint"]),
controlMode=self._pybullet_client.VELOCITY_CONTROL,
targetVelocity=0,
force=knee_friction_force)
else:
self._SetDesiredMotorAngleByName(
"motor_" + leg_position + "L_joint",
self._motor_direction[2 * leg_id] * half_pi)
self._SetDesiredMotorAngleByName("motor_" + leg_position + "L_joint",
self._motor_direction[2 * leg_id] * half_pi)
self._SetDesiredMotorAngleByName("motor_" + leg_position + "R_joint",
self._motor_direction[2 * leg_id
+ 1] * half_pi)
self._motor_direction[2 * leg_id + 1] * half_pi)
self._pybullet_client.setJointMotorControl2(
bodyIndex=self.quadruped,
@@ -273,8 +256,7 @@ class Minitaur(object):
Returns:
The position of minitaur's base.
"""
position, _ = (
self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
position, _ = (self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
return position
def GetBaseOrientation(self):
@@ -283,8 +265,7 @@ class Minitaur(object):
Returns:
The orientation of minitaur's base.
"""
_, orientation = (
self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
_, orientation = (self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
return orientation
def GetActionDimension(self):
@@ -304,10 +285,9 @@ class Minitaur(object):
"""
upper_bound = np.array([0.0] * self.GetObservationDimension())
upper_bound[0:self.num_motors] = math.pi # Joint angle.
upper_bound[self.num_motors:2 * self.num_motors] = (
motor.MOTOR_SPEED_LIMIT) # Joint velocity.
upper_bound[2 * self.num_motors:3 * self.num_motors] = (
motor.OBSERVED_TORQUE_LIMIT) # Joint torque.
upper_bound[self.num_motors:2 * self.num_motors] = (motor.MOTOR_SPEED_LIMIT) # Joint velocity.
upper_bound[2 * self.num_motors:3 * self.num_motors] = (motor.OBSERVED_TORQUE_LIMIT
) # Joint torque.
upper_bound[3 * self.num_motors:] = 1.0 # Quaternion of base orientation.
return upper_bound
@@ -354,12 +334,9 @@ class Minitaur(object):
"""
if self._motor_velocity_limit < np.inf:
current_motor_angle = self.GetMotorAngles()
motor_commands_max = (
current_motor_angle + self.time_step * self._motor_velocity_limit)
motor_commands_min = (
current_motor_angle - self.time_step * self._motor_velocity_limit)
motor_commands = np.clip(motor_commands, motor_commands_min,
motor_commands_max)
motor_commands_max = (current_motor_angle + self.time_step * self._motor_velocity_limit)
motor_commands_min = (current_motor_angle - self.time_step * self._motor_velocity_limit)
motor_commands = np.clip(motor_commands, motor_commands_min, motor_commands_max)
if self._accurate_motor_model_enabled or self._pd_control_enabled:
q = self.GetMotorAngles()
@@ -373,8 +350,7 @@ class Minitaur(object):
self._overheat_counter[i] += 1
else:
self._overheat_counter[i] = 0
if (self._overheat_counter[i] >
OVERHEAT_SHUTDOWN_TIME / self.time_step):
if (self._overheat_counter[i] > OVERHEAT_SHUTDOWN_TIME / self.time_step):
self._motor_enabled_list[i] = False
# The torque is already in the observation space because we use
@@ -382,12 +358,11 @@ class Minitaur(object):
self._observed_motor_torques = observed_torque
# Transform into the motor space when applying the torque.
self._applied_motor_torque = np.multiply(actual_torque,
self._motor_direction)
self._applied_motor_torque = np.multiply(actual_torque, self._motor_direction)
for motor_id, motor_torque, motor_enabled in zip(
self._motor_id_list, self._applied_motor_torque,
self._motor_enabled_list):
for motor_id, motor_torque, motor_enabled in zip(self._motor_id_list,
self._applied_motor_torque,
self._motor_enabled_list):
if motor_enabled:
self._SetMotorTorqueById(motor_id, motor_torque)
else:
@@ -403,14 +378,12 @@ class Minitaur(object):
self._applied_motor_torques = np.multiply(self._observed_motor_torques,
self._motor_direction)
for motor_id, motor_torque in zip(self._motor_id_list,
self._applied_motor_torques):
for motor_id, motor_torque in zip(self._motor_id_list, self._applied_motor_torques):
self._SetMotorTorqueById(motor_id, motor_torque)
else:
motor_commands_with_direction = np.multiply(motor_commands,
self._motor_direction)
for motor_id, motor_command_with_direction in zip(
self._motor_id_list, motor_commands_with_direction):
motor_commands_with_direction = np.multiply(motor_commands, self._motor_direction)
for motor_id, motor_command_with_direction in zip(self._motor_id_list,
motor_commands_with_direction):
self._SetDesiredMotorAngleById(motor_id, motor_command_with_direction)
def GetMotorAngles(self):
@@ -471,13 +444,13 @@ class Minitaur(object):
quater_pi = math.pi / 4
for i in range(self.num_motors):
action_idx = i // 2
forward_backward_component = (-scale_for_singularity * quater_pi * (
actions[action_idx + half_num_motors] + offset_for_singularity))
forward_backward_component = (
-scale_for_singularity * quater_pi *
(actions[action_idx + half_num_motors] + offset_for_singularity))
extension_component = (-1)**i * quater_pi * actions[action_idx]
if i >= half_num_motors:
extension_component = -extension_component
motor_angle[i] = (
math.pi + forward_backward_component + extension_component)
motor_angle[i] = (math.pi + forward_backward_component + extension_component)
return motor_angle
def GetBaseMassFromURDF(self):
@@ -489,8 +462,7 @@ class Minitaur(object):
return self._leg_masses_urdf
def SetBaseMass(self, base_mass):
self._pybullet_client.changeDynamics(
self.quadruped, BASE_LINK_ID, mass=base_mass)
self._pybullet_client.changeDynamics(self.quadruped, BASE_LINK_ID, mass=base_mass)
def SetLegMasses(self, leg_masses):
"""Set the mass of the legs.
@@ -504,11 +476,9 @@ class Minitaur(object):
leg_masses[1] is the mass of the motor.
"""
for link_id in LEG_LINK_ID:
self._pybullet_client.changeDynamics(
self.quadruped, link_id, mass=leg_masses[0])
self._pybullet_client.changeDynamics(self.quadruped, link_id, mass=leg_masses[0])
for link_id in MOTOR_LINK_ID:
self._pybullet_client.changeDynamics(
self.quadruped, link_id, mass=leg_masses[1])
self._pybullet_client.changeDynamics(self.quadruped, link_id, mass=leg_masses[1])
def SetFootFriction(self, foot_friction):
"""Set the lateral friction of the feet.
@@ -518,8 +488,7 @@ class Minitaur(object):
shared by all four feet.
"""
for link_id in FOOT_LINK_ID:
self._pybullet_client.changeDynamics(
self.quadruped, link_id, lateralFriction=foot_friction)
self._pybullet_client.changeDynamics(self.quadruped, link_id, lateralFriction=foot_friction)
def SetBatteryVoltage(self, voltage):
if self._accurate_motor_model_enabled:

165
examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py
View File

@@ -6,8 +6,7 @@ import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import math
import time
@@ -34,8 +33,9 @@ OBSERVATION_EPS = 0.01
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
duckStartPos = [0,0,0.25]
duckStartOrn = [0.5,0.5,0.5,0.5]
duckStartPos = [0, 0, 0.25]
duckStartOrn = [0.5, 0.5, 0.5, 0.5]
class MinitaurBulletDuckEnv(gym.Env):
"""The gym environment for the minitaur.
@@ -47,34 +47,32 @@ class MinitaurBulletDuckEnv(gym.Env):
expenditure.
"""
metadata = {
"render.modes": ["human", "rgb_array"],
"video.frames_per_second": 50
}
metadata = {"render.modes": ["human", "rgb_array"], "video.frames_per_second": 50}
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=1,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,#not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=True,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
def __init__(
self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=1,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False, #not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=True,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
"""Initialize the minitaur gym environment.
Args:
@@ -152,17 +150,14 @@ class MinitaurBulletDuckEnv(gym.Env):
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
self.seed()
self.reset()
observation_high = (
self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS)
observation_low = (
self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS)
observation_high = (self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS)
observation_low = (self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS)
action_dim = 8
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
@@ -183,35 +178,36 @@ class MinitaurBulletDuckEnv(gym.Env):
numSolverIterations=int(self._num_bullet_solver_iterations))
self._pybullet_client.setTimeStep(self._time_step)
self._groundId = self._pybullet_client.loadURDF("%s/plane.urdf" % self._urdf_root)
self._duckId = self._pybullet_client.loadURDF("%s/duck_vhacd.urdf" % self._urdf_root,duckStartPos,duckStartOrn)
self._duckId = self._pybullet_client.loadURDF("%s/duck_vhacd.urdf" % self._urdf_root,
duckStartPos, duckStartOrn)
self._pybullet_client.setGravity(0, 0, -10)
acc_motor = self._accurate_motor_model_enabled
motor_protect = self._motor_overheat_protection
self.minitaur = (minitaur.Minitaur(
pybullet_client=self._pybullet_client,
urdf_root=self._urdf_root,
time_step=self._time_step,
self_collision_enabled=self._self_collision_enabled,
motor_velocity_limit=self._motor_velocity_limit,
pd_control_enabled=self._pd_control_enabled,
accurate_motor_model_enabled=acc_motor,
motor_kp=self._motor_kp,
motor_kd=self._motor_kd,
torque_control_enabled=self._torque_control_enabled,
motor_overheat_protection=motor_protect,
on_rack=self._on_rack,
kd_for_pd_controllers=self._kd_for_pd_controllers))
self.minitaur = (minitaur.Minitaur(pybullet_client=self._pybullet_client,
urdf_root=self._urdf_root,
time_step=self._time_step,
self_collision_enabled=self._self_collision_enabled,
motor_velocity_limit=self._motor_velocity_limit,
pd_control_enabled=self._pd_control_enabled,
accurate_motor_model_enabled=acc_motor,
motor_kp=self._motor_kp,
motor_kd=self._motor_kd,
torque_control_enabled=self._torque_control_enabled,
motor_overheat_protection=motor_protect,
on_rack=self._on_rack,
kd_for_pd_controllers=self._kd_for_pd_controllers))
else:
self.minitaur.Reset(reload_urdf=False)
self._pybullet_client.resetBasePositionAndOrientation(self._duckId,duckStartPos,duckStartOrn)
self._pybullet_client.resetBasePositionAndOrientation(self._duckId, duckStartPos,
duckStartOrn)
if self._env_randomizer is not None:
self._env_randomizer.randomize_env(self)
self._env_step_counter = 0
self._last_base_position = [0, 0, 0]
self._objectives = []
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
self._pybullet_client.resetDebugVisualizerCamera(self._cam_dist, self._cam_yaw,
self._cam_pitch, [0, 0, 0])
if not self._torque_control_enabled:
for _ in range(100):
if self._pd_control_enabled or self._accurate_motor_model_enabled:
@@ -228,8 +224,7 @@ class MinitaurBulletDuckEnv(gym.Env):
for i, action_component in enumerate(action):
if not (-self._action_bound - ACTION_EPS <= action_component <=
self._action_bound + ACTION_EPS):
raise ValueError(
"{}th action {} out of bounds.".format(i, action_component))
raise ValueError("{}th action {} out of bounds.".format(i, action_component))
action = self.minitaur.ConvertFromLegModel(action)
return action
@@ -258,8 +253,8 @@ class MinitaurBulletDuckEnv(gym.Env):
if time_to_sleep > 0:
time.sleep(time_to_sleep)
base_pos = self.minitaur.GetBasePosition()
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos)
self._pybullet_client.resetDebugVisualizerCamera(self._cam_dist, self._cam_yaw,
self._cam_pitch, base_pos)
action = self._transform_action_to_motor_command(action)
for _ in range(self._action_repeat):
self.minitaur.ApplyAction(action)
@@ -281,12 +276,17 @@ class MinitaurBulletDuckEnv(gym.Env):
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT,
nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = self._pybullet_client.getCameraImage(
width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(fov=60,
aspect=float(RENDER_WIDTH) /
RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _,
_) = self._pybullet_client.getCameraImage(width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
@@ -297,9 +297,8 @@ class MinitaurBulletDuckEnv(gym.Env):
Returns:
A numpy array of motor angles.
"""
return np.array(
self._observation[MOTOR_ANGLE_OBSERVATION_INDEX:
MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS])
return np.array(self._observation[MOTOR_ANGLE_OBSERVATION_INDEX:MOTOR_ANGLE_OBSERVATION_INDEX +
NUM_MOTORS])
def get_minitaur_motor_velocities(self):
"""Get the minitaur's motor velocities.
@@ -308,8 +307,8 @@ class MinitaurBulletDuckEnv(gym.Env):
A numpy array of motor velocities.
"""
return np.array(
self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX:
MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS])
self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX:MOTOR_VELOCITY_OBSERVATION_INDEX +
NUM_MOTORS])
def get_minitaur_motor_torques(self):
"""Get the minitaur's motor torques.
@@ -318,8 +317,8 @@ class MinitaurBulletDuckEnv(gym.Env):
A numpy array of motor torques.
"""
return np.array(
self._observation[MOTOR_TORQUE_OBSERVATION_INDEX:
MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS])
self._observation[MOTOR_TORQUE_OBSERVATION_INDEX:MOTOR_TORQUE_OBSERVATION_INDEX +
NUM_MOTORS])
def get_minitaur_base_orientation(self):
"""Get the minitaur's base orientation, represented by a quaternion.
@@ -330,8 +329,8 @@ class MinitaurBulletDuckEnv(gym.Env):
return np.array(self._observation[BASE_ORIENTATION_OBSERVATION_INDEX:])
def lost_duck(self):
points = self._pybullet_client.getContactPoints(self._duckId, self._groundId);
return len(points)>0
points = self._pybullet_client.getContactPoints(self._duckId, self._groundId)
return len(points) > 0
def is_fallen(self):
"""Decide whether the minitaur has fallen.
@@ -347,8 +346,7 @@ class MinitaurBulletDuckEnv(gym.Env):
rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation)
local_up = rot_mat[6:]
pos = self.minitaur.GetBasePosition()
return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or
pos[2] < 0.13)
return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or pos[2] < 0.13)
def _termination(self):
position = self.minitaur.GetBasePosition()
@@ -364,12 +362,9 @@ class MinitaurBulletDuckEnv(gym.Env):
energy_reward = np.abs(
np.dot(self.minitaur.GetMotorTorques(),
self.minitaur.GetMotorVelocities())) * self._time_step
reward = (
self._distance_weight * forward_reward -
self._energy_weight * energy_reward + self._drift_weight * drift_reward
+ self._shake_weight * shake_reward)
self._objectives.append(
[forward_reward, energy_reward, drift_reward, shake_reward])
reward = (self._distance_weight * forward_reward - self._energy_weight * energy_reward +
self._drift_weight * drift_reward + self._shake_weight * shake_reward)
self._objectives.append([forward_reward, energy_reward, drift_reward, shake_reward])
return reward
def get_objectives(self):
@@ -383,9 +378,9 @@ class MinitaurBulletDuckEnv(gym.Env):
self._get_observation()
observation = np.array(self._observation)
if self._observation_noise_stdev > 0:
observation += (np.random.normal(
scale=self._observation_noise_stdev, size=observation.shape) *
self.minitaur.GetObservationUpperBound())
observation += (
np.random.normal(scale=self._observation_noise_stdev, size=observation.shape) *
self.minitaur.GetObservationUpperBound())
return observation
if parse_version(gym.__version__) < parse_version('0.9.6'):

12
examples/pybullet/gym/pybullet_envs/bullet/minitaur_env_randomizer.py
View File

@@ -45,24 +45,20 @@ class MinitaurEnvRandomizer(env_randomizer_base.EnvRandomizerBase):
minitaur.SetBaseMass(randomized_base_mass)
leg_masses = minitaur.GetLegMassesFromURDF()
leg_masses_lower_bound = np.array(leg_masses) * (
1.0 + self._minitaur_leg_mass_err_range[0])
leg_masses_upper_bound = np.array(leg_masses) * (
1.0 + self._minitaur_leg_mass_err_range[1])
leg_masses_lower_bound = np.array(leg_masses) * (1.0 + self._minitaur_leg_mass_err_range[0])
leg_masses_upper_bound = np.array(leg_masses) * (1.0 + self._minitaur_leg_mass_err_range[1])
randomized_leg_masses = [
np.random.uniform(leg_masses_lower_bound[i], leg_masses_upper_bound[i])
for i in range(len(leg_masses))
]
minitaur.SetLegMasses(randomized_leg_masses)
randomized_battery_voltage = random.uniform(BATTERY_VOLTAGE_RANGE[0],
BATTERY_VOLTAGE_RANGE[1])
randomized_battery_voltage = random.uniform(BATTERY_VOLTAGE_RANGE[0], BATTERY_VOLTAGE_RANGE[1])
minitaur.SetBatteryVoltage(randomized_battery_voltage)
randomized_motor_damping = random.uniform(MOTOR_VISCOUS_DAMPING_RANGE[0],
MOTOR_VISCOUS_DAMPING_RANGE[1])
minitaur.SetMotorViscousDamping(randomized_motor_damping)
randomized_foot_friction = random.uniform(MINITAUR_LEG_FRICTION[0],
MINITAUR_LEG_FRICTION[1])
randomized_foot_friction = random.uniform(MINITAUR_LEG_FRICTION[0], MINITAUR_LEG_FRICTION[1])
minitaur.SetFootFriction(randomized_foot_friction)

165
examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
View File

@@ -5,8 +5,7 @@
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import math
import time
@@ -33,6 +32,7 @@ OBSERVATION_EPS = 0.01
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class MinitaurBulletEnv(gym.Env):
"""The gym environment for the minitaur.
@@ -43,34 +43,32 @@ class MinitaurBulletEnv(gym.Env):
expenditure.
"""
metadata = {
"render.modes": ["human", "rgb_array"],
"video.frames_per_second": 50
}
metadata = {"render.modes": ["human", "rgb_array"], "video.frames_per_second": 50}
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=1,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False,#not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=True,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
def __init__(
self,
urdf_root=pybullet_data.getDataPath(),
action_repeat=1,
distance_weight=1.0,
energy_weight=0.005,
shake_weight=0.0,
drift_weight=0.0,
distance_limit=float("inf"),
observation_noise_stdev=0.0,
self_collision_enabled=True,
motor_velocity_limit=np.inf,
pd_control_enabled=False, #not needed to be true if accurate motor model is enabled (has its own better PD)
leg_model_enabled=True,
accurate_motor_model_enabled=True,
motor_kp=1.0,
motor_kd=0.02,
torque_control_enabled=False,
motor_overheat_protection=True,
hard_reset=True,
on_rack=False,
render=False,
kd_for_pd_controllers=0.3,
env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()):
"""Initialize the minitaur gym environment.
Args:
@@ -147,17 +145,14 @@ class MinitaurBulletEnv(gym.Env):
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
self.seed()
self.reset()
observation_high = (
self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS)
observation_low = (
self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS)
observation_high = (self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS)
observation_low = (self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS)
action_dim = 8
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
@@ -178,25 +173,25 @@ class MinitaurBulletEnv(gym.Env):
numSolverIterations=int(self._num_bullet_solver_iterations))
self._pybullet_client.setTimeStep(self._time_step)
plane = self._pybullet_client.loadURDF("%s/plane.urdf" % self._urdf_root)
self._pybullet_client.changeVisualShape(plane,-1,rgbaColor=[1,1,1,0.9])
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,0)
self._pybullet_client.changeVisualShape(plane, -1, rgbaColor=[1, 1, 1, 0.9])
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION, 0)
self._pybullet_client.setGravity(0, 0, -10)
acc_motor = self._accurate_motor_model_enabled
motor_protect = self._motor_overheat_protection
self.minitaur = (minitaur.Minitaur(
pybullet_client=self._pybullet_client,
urdf_root=self._urdf_root,
time_step=self._time_step,
self_collision_enabled=self._self_collision_enabled,
motor_velocity_limit=self._motor_velocity_limit,
pd_control_enabled=self._pd_control_enabled,
accurate_motor_model_enabled=acc_motor,
motor_kp=self._motor_kp,
motor_kd=self._motor_kd,
torque_control_enabled=self._torque_control_enabled,
motor_overheat_protection=motor_protect,
on_rack=self._on_rack,
kd_for_pd_controllers=self._kd_for_pd_controllers))
self.minitaur = (minitaur.Minitaur(pybullet_client=self._pybullet_client,
urdf_root=self._urdf_root,
time_step=self._time_step,
self_collision_enabled=self._self_collision_enabled,
motor_velocity_limit=self._motor_velocity_limit,
pd_control_enabled=self._pd_control_enabled,
accurate_motor_model_enabled=acc_motor,
motor_kp=self._motor_kp,
motor_kd=self._motor_kd,
torque_control_enabled=self._torque_control_enabled,
motor_overheat_protection=motor_protect,
on_rack=self._on_rack,
kd_for_pd_controllers=self._kd_for_pd_controllers))
else:
self.minitaur.Reset(reload_urdf=False)
@@ -206,8 +201,8 @@ class MinitaurBulletEnv(gym.Env):
self._env_step_counter = 0
self._last_base_position = [0, 0, 0]
self._objectives = []
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
self._pybullet_client.resetDebugVisualizerCamera(self._cam_dist, self._cam_yaw,
self._cam_pitch, [0, 0, 0])
if not self._torque_control_enabled:
for _ in range(100):
if self._pd_control_enabled or self._accurate_motor_model_enabled:
@@ -224,8 +219,7 @@ class MinitaurBulletEnv(gym.Env):
for i, action_component in enumerate(action):
if not (-self._action_bound - ACTION_EPS <= action_component <=
self._action_bound + ACTION_EPS):
raise ValueError(
"{}th action {} out of bounds.".format(i, action_component))
raise ValueError("{}th action {} out of bounds.".format(i, action_component))
action = self.minitaur.ConvertFromLegModel(action)
return action
@@ -256,14 +250,15 @@ class MinitaurBulletEnv(gym.Env):
base_pos = self.minitaur.GetBasePosition()
camInfo = self._pybullet_client.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
distance = camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*base_pos[0],0.95*curTargetPos[1]+0.05*base_pos[1],curTargetPos[2]]
pitch = camInfo[9]
targetPos = [
0.95 * curTargetPos[0] + 0.05 * base_pos[0], 0.95 * curTargetPos[1] + 0.05 * base_pos[1],
curTargetPos[2]
]
self._pybullet_client.resetDebugVisualizerCamera(
distance, yaw, pitch, base_pos)
self._pybullet_client.resetDebugVisualizerCamera(distance, yaw, pitch, base_pos)
action = self._transform_action_to_motor_command(action)
for _ in range(self._action_repeat):
self.minitaur.ApplyAction(action)
@@ -285,12 +280,17 @@ class MinitaurBulletEnv(gym.Env):
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT,
nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = self._pybullet_client.getCameraImage(
width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(fov=60,
aspect=float(RENDER_WIDTH) /
RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _,
_) = self._pybullet_client.getCameraImage(width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
@@ -301,9 +301,8 @@ class MinitaurBulletEnv(gym.Env):
Returns:
A numpy array of motor angles.
"""
return np.array(
self._observation[MOTOR_ANGLE_OBSERVATION_INDEX:
MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS])
return np.array(self._observation[MOTOR_ANGLE_OBSERVATION_INDEX:MOTOR_ANGLE_OBSERVATION_INDEX +
NUM_MOTORS])
def get_minitaur_motor_velocities(self):
"""Get the minitaur's motor velocities.
@@ -312,8 +311,8 @@ class MinitaurBulletEnv(gym.Env):
A numpy array of motor velocities.
"""
return np.array(
self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX:
MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS])
self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX:MOTOR_VELOCITY_OBSERVATION_INDEX +
NUM_MOTORS])
def get_minitaur_motor_torques(self):
"""Get the minitaur's motor torques.
@@ -322,8 +321,8 @@ class MinitaurBulletEnv(gym.Env):
A numpy array of motor torques.
"""
return np.array(
self._observation[MOTOR_TORQUE_OBSERVATION_INDEX:
MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS])
self._observation[MOTOR_TORQUE_OBSERVATION_INDEX:MOTOR_TORQUE_OBSERVATION_INDEX +
NUM_MOTORS])
def get_minitaur_base_orientation(self):
"""Get the minitaur's base orientation, represented by a quaternion.
@@ -347,8 +346,7 @@ class MinitaurBulletEnv(gym.Env):
rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation)
local_up = rot_mat[6:]
pos = self.minitaur.GetBasePosition()
return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or
pos[2] < 0.13)
return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or pos[2] < 0.13)
def _termination(self):
position = self.minitaur.GetBasePosition()
@@ -364,12 +362,9 @@ class MinitaurBulletEnv(gym.Env):
energy_reward = np.abs(
np.dot(self.minitaur.GetMotorTorques(),
self.minitaur.GetMotorVelocities())) * self._time_step
reward = (
self._distance_weight * forward_reward -
self._energy_weight * energy_reward + self._drift_weight * drift_reward
+ self._shake_weight * shake_reward)
self._objectives.append(
[forward_reward, energy_reward, drift_reward, shake_reward])
reward = (self._distance_weight * forward_reward - self._energy_weight * energy_reward +
self._drift_weight * drift_reward + self._shake_weight * shake_reward)
self._objectives.append([forward_reward, energy_reward, drift_reward, shake_reward])
return reward
def get_objectives(self):
@@ -383,9 +378,9 @@ class MinitaurBulletEnv(gym.Env):
self._get_observation()
observation = np.array(self._observation)
if self._observation_noise_stdev > 0:
observation += (np.random.normal(
scale=self._observation_noise_stdev, size=observation.shape) *
self.minitaur.GetObservationUpperBound())
observation += (
np.random.normal(scale=self._observation_noise_stdev, size=observation.shape) *
self.minitaur.GetObservationUpperBound())
return observation
if parse_version(gym.__version__) < parse_version('0.9.6'):

31
examples/pybullet/gym/pybullet_envs/bullet/motor.py
View File

@@ -7,8 +7,7 @@ MOTOR_VOLTAGE = 16.0
MOTOR_RESISTANCE = 0.186
MOTOR_TORQUE_CONSTANT = 0.0954
MOTOR_VISCOUS_DAMPING = 0
MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (MOTOR_VISCOUS_DAMPING
+ MOTOR_TORQUE_CONSTANT)
MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (MOTOR_VISCOUS_DAMPING + MOTOR_TORQUE_CONSTANT)
class MotorModel(object):
@@ -24,10 +23,7 @@ class MotorModel(object):
pd gains, viscous friction, back-EMF voltage and current-torque profile.
"""
def __init__(self,
torque_control_enabled=False,
kp=1.2,
kd=0):
def __init__(self, torque_control_enabled=False, kp=1.2, kd=0):
self._torque_control_enabled = torque_control_enabled
self._kp = kp
self._kd = kd
@@ -50,8 +46,7 @@ class MotorModel(object):
def get_viscous_dampling(self):
return self._viscous_damping
def convert_to_torque(self, motor_commands, current_motor_angle,
current_motor_velocity):
def convert_to_torque(self, motor_commands, current_motor_angle, current_motor_velocity):
"""Convert the commands (position control or torque control) to torque.
Args:
@@ -66,8 +61,8 @@ class MotorModel(object):
if self._torque_control_enabled:
pwm = motor_commands
else:
pwm = (-self._kp * (current_motor_angle - motor_commands)
- self._kd * current_motor_velocity)
pwm = (-self._kp * (current_motor_angle - motor_commands) -
self._kd * current_motor_velocity)
pwm = np.clip(pwm, -1.0, 1.0)
return self._convert_to_torque_from_pwm(pwm, current_motor_velocity)
@@ -81,21 +76,19 @@ class MotorModel(object):
actual_torque: The torque that needs to be applied to the motor.
observed_torque: The torque observed by the sensor.
"""
observed_torque = np.clip(
self._torque_constant * (pwm * self._voltage / self._resistance),
-OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT)
observed_torque = np.clip(self._torque_constant * (pwm * self._voltage / self._resistance),
-OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT)
# Net voltage is clipped at 50V by diodes on the motor controller.
voltage_net = np.clip(pwm * self._voltage -
(self._torque_constant + self._viscous_damping)
* current_motor_velocity,
-VOLTAGE_CLIPPING, VOLTAGE_CLIPPING)
voltage_net = np.clip(
pwm * self._voltage -
(self._torque_constant + self._viscous_damping) * current_motor_velocity,
-VOLTAGE_CLIPPING, VOLTAGE_CLIPPING)
current = voltage_net / self._resistance
current_sign = np.sign(current)
current_magnitude = np.absolute(current)
# Saturate torque based on empirical current relation.
actual_torque = np.interp(current_magnitude, self._current_table,
self._torque_table)
actual_torque = np.interp(current_magnitude, self._current_table, self._torque_table)
actual_torque = np.multiply(current_sign, actual_torque)
return actual_torque, observed_torque

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

@@ -4,80 +4,150 @@ import math
import numpy as np
class Racecar:
def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self._p = bullet_client
self.reset()
def __init__(self, bullet_client, urdfRootPath='', timeStep=0.01):
self.urdfRootPath = urdfRootPath
self.timeStep = timeStep
self._p = bullet_client
self.reset()
def reset(self):
car = self._p.loadURDF(os.path.join(self.urdfRootPath,"racecar/racecar_differential.urdf"), [0,0,.2],useFixedBase=False)
self.racecarUniqueId = car
#for i in range (self._p.getNumJoints(car)):
# print (self._p.getJointInfo(car,i))
for wheel in range(self._p.getNumJoints(car)):
self._p.setJointMotorControl2(car,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0)
self._p.getJointInfo(car,wheel)
def reset(self):
car = self._p.loadURDF(os.path.join(self.urdfRootPath, "racecar/racecar_differential.urdf"),
[0, 0, .2],
useFixedBase=False)
self.racecarUniqueId = car
#for i in range (self._p.getNumJoints(car)):
# print (self._p.getJointInfo(car,i))
for wheel in range(self._p.getNumJoints(car)):
self._p.setJointMotorControl2(car,
wheel,
self._p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
self._p.getJointInfo(car, wheel)
#self._p.setJointMotorControl2(car,10,self._p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = self._p.createConstraint(car,9,car,11,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=1, maxForce=10000)
#self._p.setJointMotorControl2(car,10,self._p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = self._p.createConstraint(car,
9,
car,
11,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = self._p.createConstraint(car,10,car,13,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,
10,
car,
13,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,9,car,13,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,
9,
car,
13,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,16,car,18,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = self._p.createConstraint(car,
16,
car,
18,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = self._p.createConstraint(car,16,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,
16,
car,
19,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,17,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,
17,
car,
19,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = self._p.createConstraint(car,1,car,18,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
c = self._p.createConstraint(car,3,car,19,jointType=self._p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
self._p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
c = self._p.createConstraint(car,
1,
car,
18,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = self._p.createConstraint(car,
3,
car,
19,
jointType=self._p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
self._p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
self.steeringLinks = [0,2]
self.maxForce = 20
self.nMotors = 2
self.motorizedwheels=[8,15]
self.speedMultiplier = 20.
self.steeringMultiplier = 0.5
self.steeringLinks = [0, 2]
self.maxForce = 20
self.nMotors = 2
self.motorizedwheels = [8, 15]
self.speedMultiplier = 20.
self.steeringMultiplier = 0.5
def getActionDimension(self):
return self.nMotors
def getActionDimension(self):
return self.nMotors
def getObservationDimension(self):
return len(self.getObservation())
def getObservationDimension(self):
return len(self.getObservation())
def getObservation(self):
observation = []
pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId)
def getObservation(self):
observation = []
pos, orn = self._p.getBasePositionAndOrientation(self.racecarUniqueId)
observation.extend(list(pos))
observation.extend(list(orn))
observation.extend(list(pos))
observation.extend(list(orn))
return observation
return observation
def applyAction(self, motorCommands):
targetVelocity=motorCommands[0]*self.speedMultiplier
#print("targetVelocity")
#print(targetVelocity)
steeringAngle = motorCommands[1]*self.steeringMultiplier
#print("steeringAngle")
#print(steeringAngle)
#print("maxForce")
#print(self.maxForce)
def applyAction(self, motorCommands):
targetVelocity = motorCommands[0] * self.speedMultiplier
#print("targetVelocity")
#print(targetVelocity)
steeringAngle = motorCommands[1] * self.steeringMultiplier
#print("steeringAngle")
#print(steeringAngle)
#print("maxForce")
#print(self.maxForce)
for motor in self.motorizedwheels:
self._p.setJointMotorControl2(self.racecarUniqueId,motor,self._p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=self.maxForce)
for steer in self.steeringLinks:
self._p.setJointMotorControl2(self.racecarUniqueId,steer,self._p.POSITION_CONTROL,targetPosition=steeringAngle)
for motor in self.motorizedwheels:
self._p.setJointMotorControl2(self.racecarUniqueId,
motor,
self._p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=self.maxForce)
for steer in self.steeringLinks:
self._p.setJointMotorControl2(self.racecarUniqueId,
steer,
self._p.POSITION_CONTROL,
targetPosition=steeringAngle)

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

@@ -1,7 +1,7 @@
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import math
import gym
@@ -19,11 +19,9 @@ from pkg_resources import parse_version
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class RacecarGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50}
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
@@ -42,25 +40,24 @@ class RacecarGymEnv(gym.Env):
self._renders = renders
self._isDiscrete = isDiscrete
if self._renders:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
self.seed()
#self.reset()
observationDim = 2 #len(self.getExtendedObservation())
observationDim = 2 #len(self.getExtendedObservation())
#print("observationDim")
#print(observationDim)
# observation_high = np.array([np.finfo(np.float32).max] * observationDim)
observation_high = np.ones(observationDim) * 1000 #np.inf
observation_high = np.ones(observationDim) * 1000 #np.inf
if (isDiscrete):
self.action_space = spaces.Discrete(9)
else:
action_dim = 2
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
action_dim = 2
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(-observation_high, observation_high, dtype=np.float32)
self.viewer = None
@@ -69,23 +66,24 @@ class RacecarGymEnv(gym.Env):
#p.setPhysicsEngineParameter(numSolverIterations=300)
self._p.setTimeStep(self._timeStep)
#self._p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
stadiumobjects = self._p.loadSDF(os.path.join(self._urdfRoot,"stadium.sdf"))
stadiumobjects = self._p.loadSDF(os.path.join(self._urdfRoot, "stadium.sdf"))
#move the stadium objects slightly above 0
#for i in stadiumobjects:
# 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()
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,"sphere2.urdf"),[ballx,bally,ballz])
self._p.setGravity(0,0,-10)
self._racecar = racecar.Racecar(self._p,urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._ballUniqueId = self._p.loadURDF(os.path.join(self._urdfRoot, "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):
self._p.stepSimulation()
@@ -100,26 +98,26 @@ class RacecarGymEnv(gym.Env):
return [seed]
def getExtendedObservation(self):
self._observation = [] #self._racecar.getObservation()
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 = [] #self._racecar.getObservation()
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
self._observation.extend([ballPosInCar[0], ballPosInCar[1]])
return self._observation
def step(self, action):
if (self._renders):
basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
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]
forward = fwd[action]
steer = steerings[action]
realaction = [forward,steer]
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]
forward = fwd[action]
steer = steerings[action]
realaction = [forward, steer]
else:
realaction = action
@@ -142,35 +140,37 @@ class RacecarGymEnv(gym.Env):
def render(self, mode='human', close=False):
if mode != "rgb_array":
return np.array([])
base_pos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(
fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT,
nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(
width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
base_pos, orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(fov=60,
aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
return self._envStepCounter>1000
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
reward = -1000
#print(numPt)
if (numPt>0):
if (numPt > 0):
#print("reward:")
reward = -closestPoints[0][8]
#print(reward)

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

@@ -1,7 +1,7 @@
import os, inspect
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
os.sys.path.insert(0, parentdir)
import math
import gym
@@ -19,11 +19,9 @@ from pkg_resources import parse_version
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class RacecarZEDGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second' : 50
}
metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50}
def __init__(self,
urdfRoot=pybullet_data.getDataPath(),
@@ -44,8 +42,7 @@ class RacecarZEDGymEnv(gym.Env):
self._isDiscrete = isDiscrete
if self._renders:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI)
else:
self._p = bullet_client.BulletClient()
@@ -59,11 +56,14 @@ class RacecarZEDGymEnv(gym.Env):
if (isDiscrete):
self.action_space = spaces.Discrete(9)
else:
action_dim = 2
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4), dtype=np.uint8)
action_dim = 2
self._action_bound = 1
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(low=0,
high=255,
shape=(self._height, self._width, 4),
dtype=np.uint8)
self.viewer = None
@@ -72,23 +72,24 @@ class RacecarZEDGymEnv(gym.Env):
#p.setPhysicsEngineParameter(numSolverIterations=300)
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(self._urdfRoot,"stadium.sdf"))
stadiumobjects = self._p.loadSDF(os.path.join(self._urdfRoot, "stadium.sdf"))
#move the stadium objects slightly above 0
for i in stadiumobjects:
pos,orn = self._p.getBasePositionAndOrientation(i)
newpos = [pos[0],pos[1],pos[2]+0.1]
self._p.resetBasePositionAndOrientation(i,newpos,orn)
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()
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)
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)
self._envStepCounter = 0
for i in range(100):
self._p.stepSimulation()
@@ -103,38 +104,50 @@ class RacecarZEDGymEnv(gym.Env):
return [seed]
def getExtendedObservation(self):
carpos,carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
carmat = self._p.getMatrixFromQuaternion(carorn)
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]
up = [carmat[2],carmat[5],carmat[8]]
viewMat = self._p.computeViewMatrix(eyePos,targetPos,up)
#viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
#print("projectionMatrix:")
#print(self._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 = self._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
carpos, carorn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
carmat = self._p.getMatrixFromQuaternion(carorn)
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
]
up = [carmat[2], carmat[5], carmat[8]]
viewMat = self._p.computeViewMatrix(eyePos, targetPos, up)
#viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
#print("projectionMatrix:")
#print(self._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 = self._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 = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
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]
forward = fwd[action]
steer = steerings[action]
realaction = [forward,steer]
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]
forward = fwd[action]
steer = steerings[action]
realaction = [forward, steer]
else:
realaction = action
@@ -157,35 +170,37 @@ class RacecarZEDGymEnv(gym.Env):
def render(self, mode='human', close=False):
if mode != "rgb_array":
return np.array([])
base_pos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(
fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT,
nearVal=0.1, farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(
width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix,
projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
base_pos, orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
view_matrix = self._p.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._p.computeProjectionMatrixFOV(fov=60,
aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._p.getCameraImage(width=RENDER_WIDTH,
height=RENDER_HEIGHT,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
return self._envStepCounter>1000
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
reward = -1000
#print(numPt)
if (numPt>0):
if (numPt > 0):
#print("reward:")
reward = -closestPoints[0][8]
#print(reward)