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

Merge pull request #1407 from erwincoumans/master

Browse Source 
fix for issue 1400
This commit is contained in:
erwincoumans
2017-11-01 18:23:35 -07:00
committed by GitHub
parent d52fb7510b 4798d66f19
commit bce657a08a
22 changed files with 855 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
Extras/VHACD/test/src/main.cpp
View File

@@ -38,6 +38,14 @@
using namespace VHACD;
using namespace std;
bool replace(std::string& str, const std::string& from, const std::string& to) {
size_t start_pos = str.find(from);
if(start_pos == std::string::npos)
return false;
str.replace(start_pos, from.length(), to);
return true;
}
class MyCallback : public IVHACD::IUserCallback {
public:
MyCallback(void) {}
@@ -293,7 +301,16 @@ void ParseParameters(int argc, char* argv[], Parameters& params)
for (int i = 1; i < argc; ++i) {
if (!strcmp(argv[i], "--input")) {
if (++i < argc)
{
params.m_fileNameIn = argv[i];
//come up with some default output name, if not provided
if (params.m_fileNameOut.length()==0)
{
string tmp = argv[i];
replace(tmp,".obj",".vhacd.obj");
params.m_fileNameOut = tmp;
}
}
}
else if (!strcmp(argv[i], "--output")) {
if (++i < argc)

13
examples/pybullet/examples/integrate.py Normal file
View File

@@ -0,0 +1,13 @@
import pybullet as p
p.connect(p.GUI)
cube = p.loadURDF("cube.urdf")
frequency = 240
timeStep = 1./frequency
p.setGravity(0,0,-9.8)
p.changeDynamics(cube,-1,linearDamping=0,angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep = timeStep)
for i in range (frequency):
p.stepSimulation()
pos,orn = p.getBasePositionAndOrientation(cube)
print(pos)

8
examples/pybullet/gym/pybullet_envs/__init__.py
View File

@@ -9,6 +9,14 @@ register(
reward_threshold=5.0,
)
register(
id='MinitaurBulletDuckEnv-v0',
entry_point='pybullet_envs.bullet:MinitaurBulletDuckEnv',
timestep_limit=1000,
reward_threshold=5.0,
)
register(
id='RacecarBulletEnv-v0',
entry_point='pybullet_envs.bullet:RacecarGymEnv',

30
examples/pybullet/gym/pybullet_envs/agents/configs.py
View File

@@ -23,6 +23,7 @@ import functools
from agents import ppo
from agents.scripts import networks
from pybullet_envs.bullet import minitaur_gym_env
from pybullet_envs.bullet import minitaur_duck_gym_env
from pybullet_envs.bullet import minitaur_env_randomizer
import pybullet_envs.bullet.minitaur_gym_env as minitaur_gym_env
import pybullet_envs
@@ -33,7 +34,7 @@ def default():
# General
algorithm = ppo.PPOAlgorithm
num_agents = 10
eval_episodes = 25
eval_episodes = 20
use_gpu = False
# Network
network = networks.feed_forward_gaussian
@@ -46,7 +47,7 @@ def default():
init_mean_factor = 0.05
init_logstd = -1
# Optimization
update_every = 25
update_every = 20
policy_optimizer = 'AdamOptimizer'
value_optimizer = 'AdamOptimizer'
update_epochs_policy = 50
@@ -99,6 +100,16 @@ def pybullet_ant():
steps = 5e7 # 50M
return locals()
def pybullet_kuka_grasping():
"""Configuration for Bullet Kuka grasping task."""
locals().update(default())
# Environment
env = 'KukaBulletEnv-v0'
max_length = 1000
steps = 1e7 # 10M
return locals()
def pybullet_racecar():
"""Configuration for Bullet MIT Racecar task."""
locals().update(default())
@@ -124,3 +135,18 @@ def pybullet_minitaur():
steps = 3e7 # 30M
return locals()
def pybullet_duck_minitaur():
"""Configuration specific to minitaur_gym_env.MinitaurBulletDuckEnv class."""
locals().update(default())
randomizer = (minitaur_env_randomizer.MinitaurEnvRandomizer())
env = functools.partial(
minitaur_gym_env.MinitaurBulletDuckEnv,
accurate_motor_model_enabled=True,
motor_overheat_protection=True,
pd_control_enabled=True,
env_randomizer=randomizer,
render=False)
max_length = 1000
steps = 3e7 # 30M
return locals()

2
examples/pybullet/gym/pybullet_envs/baselines/enjoy_kuka_grasping.py
View File

@@ -12,7 +12,7 @@ from baselines import deepq
def main():
env = KukaGymEnv(renders=True)
env = KukaGymEnv(renders=True, isDiscrete=True)
act = deepq.load("kuka_model.pkl")
print(act)
while True:

2
examples/pybullet/gym/pybullet_envs/baselines/train_kuka_cam_grasping.py
View File

@@ -25,7 +25,7 @@ def callback(lcl, glb):
def main():
env = KukaCamGymEnv(renders=True)
env = KukaCamGymEnv(renders=False, isDiscrete=True)
model = deepq.models.cnn_to_mlp(
convs=[(32, 8, 4), (64, 4, 2), (64, 3, 1)],
hiddens=[256],

2
examples/pybullet/gym/pybullet_envs/baselines/train_kuka_grasping.py
View File

@@ -25,7 +25,7 @@ def callback(lcl, glb):
def main():
env = KukaGymEnv(renders=False)
env = KukaGymEnv(renders=False, isDiscrete=True)
model = deepq.models.mlp([64])
act = deepq.learn(
env,

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

@@ -1,5 +1,6 @@
from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv
from pybullet_envs.bullet.minitaur_gym_env import MinitaurBulletEnv
from pybullet_envs.bullet.minitaur_duck_gym_env import MinitaurBulletDuckEnv
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
from pybullet_envs.bullet.simpleHumanoidGymEnv import SimpleHumanoidGymEnv

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

@@ -17,14 +17,15 @@ class Kuka:
self.timeStep = timeStep
self.maxForce = 200.
self.fingerAForce = 6
self.fingerBForce = 5.5
self.fingerTipForce = 6
self.fingerAForce = 2
self.fingerBForce = 2.5
self.fingerTipForce = 2
self.useInverseKinematics = 1
self.useSimulation = 1
self.useNullSpace = 1
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]
#upper limits for null space
@@ -76,7 +77,7 @@ class Kuka:
def getObservation(self):
observation = []
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
state = p.getLinkState(self.kukaUid,self.kukaGripperIndex)
pos = state[0]
orn = state[1]
euler = p.getEulerFromQuaternion(orn)
@@ -106,13 +107,13 @@ class Kuka:
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.75):
self.endEffectorPos[0]=0.75
if (self.endEffectorPos[0]<0.45):
self.endEffectorPos[0]=0.45
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.22):
self.endEffectorPos[1]=-0.22
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
@@ -120,10 +121,8 @@ class Kuka:
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
if (dz>0 or actualEndEffectorPos[2]>0.10):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
if (actualEndEffectorPos[2]<0.10):
self.endEffectorPos[2] = self.endEffectorPos[2]+0.0001
#if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorAngle = self.endEffectorAngle + da
@@ -147,7 +146,7 @@ class Kuka:
if (self.useSimulation):
for i in range (self.kukaEndEffectorIndex+1):
#print(i)
p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.03,velocityGain=1)
p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,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):

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

@@ -14,7 +14,10 @@ import pybullet as p
from . import kuka
import random
import pybullet_data
maxSteps = 1000
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class KukaCamGymEnv(gym.Env):
metadata = {
@@ -26,8 +29,8 @@ class KukaCamGymEnv(gym.Env):
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=True):
print("init")
renders=False,
isDiscrete=False):
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
@@ -37,6 +40,7 @@ class KukaCamGymEnv(gym.Env):
self._renders = renders
self._width = 341
self._height = 256
self._isDiscrete=isDiscrete
self.terminated = 0
self._p = p
if self._renders:
@@ -54,12 +58,17 @@ class KukaCamGymEnv(gym.Env):
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(7)
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)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
self.viewer = None
def _reset(self):
print("reset")
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
@@ -68,8 +77,8 @@ class KukaCamGymEnv(gym.Env):
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.05*random.random()
ypos = 0 +0.05*random.random()
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])
@@ -117,24 +126,36 @@ class KukaCamGymEnv(gym.Env):
return self._observation
def _step(self, action):
dv = 0.01
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]
f = 0.3
realAction = [dx,dy,-0.002,da,f]
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]
f = 0.3
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]
return self.step2( realAction)
def step2(self, action):
self._kuka.applyAction(action)
for i in range(self._actionRepeat):
self._kuka.applyAction(action)
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
if self._termination():
break
#self._observation = self.getExtendedObservation()
self._envStepCounter += 1
self._observation = self.getExtendedObservation()
if self._renders:
time.sleep(self._timeStep)
#print("self._envStepCounter")
#print(self._envStepCounter)
@@ -145,7 +166,25 @@ class KukaCamGymEnv(gym.Env):
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
return
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)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
#print (self._kuka.endEffectorPos[2])
@@ -154,25 +193,41 @@ class KukaCamGymEnv(gym.Env):
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>1000):
if (self.terminated or self._envStepCounter>maxSteps):
self._observation = self.getExtendedObservation()
return True
if (actualEndEffectorPos[2] <= 0.10):
maxDist = 0.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
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 (1000):
graspAction = [0,0,0.001,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
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
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)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
@@ -181,7 +236,7 @@ class KukaCamGymEnv(gym.Env):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
@@ -189,13 +244,13 @@ class KukaCamGymEnv(gym.Env):
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
print("grasped a block!!!")
print("self._envStepCounter")
print(self._envStepCounter)
#print("grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
reward = reward+1000
#print("reward")
#print(reward)
return reward

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

@@ -14,6 +14,10 @@ from . import kuka
import random
import pybullet_data
largeValObservation = 100
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class KukaGymEnv(gym.Env):
metadata = {
@@ -25,8 +29,11 @@ class KukaGymEnv(gym.Env):
urdfRoot=pybullet_data.getDataPath(),
actionRepeat=1,
isEnableSelfCollision=True,
renders=True):
print("init")
renders=False,
isDiscrete=False,
maxSteps = 1000):
#print("KukaGymEnv __init__")
self._isDiscrete = isDiscrete
self._timeStep = 1./240.
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
@@ -34,7 +41,12 @@ class KukaGymEnv(gym.Env):
self._observation = []
self._envStepCounter = 0
self._renders = renders
self._maxSteps = maxSteps
self.terminated = 0
self._cam_dist = 1.3
self._cam_yaw = 180
self._cam_pitch = -40
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
@@ -50,13 +62,19 @@ class KukaGymEnv(gym.Env):
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(7)
observation_high = np.array([largeValObservation] * observationDim)
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)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
def _reset(self):
print("reset")
#print("KukaGymEnv _reset")
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
@@ -65,11 +83,11 @@ class KukaGymEnv(gym.Env):
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.05*random.random()
ypos = 0 +0.05*random.random()
ang = 3.1415925438*random.random()
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.1,orn[0],orn[1],orn[2],orn[3])
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)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
@@ -87,49 +105,104 @@ class KukaGymEnv(gym.Env):
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
endEffectorPos = eeState[0]
endEffectorOrn = eeState[1]
gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
self._observation.extend(list(blockPosInEE))
self._observation.extend(list(blockEulerInEE))
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)
#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)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def _step(self, action):
dv = 0.01
dx = [0,-dv,dv,0,0,0,0][action]
dy = [0,0,0,-dv,dv,0,0][action]
da = [0,0,0,0,0,-0.1,0.1][action]
f = 0.3
realAction = [dx,dy,-0.002,da,f]
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]
f = 0.3
realAction = [dx,dy,-0.002,da,f]
else:
#print("action[0]=", str(action[0]))
dv = 0.005
dx = action[0] * dv
dy = action[1] * dv
da = action[2] * 0.05
f = 0.3
realAction = [dx,dy,-0.002,da,f]
return self.step2( realAction)
def step2(self, action):
self._kuka.applyAction(action)
for i in range(self._actionRepeat):
self._kuka.applyAction(action)
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
if self._termination():
break
self._envStepCounter += 1
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
#print("self._envStepCounter")
#print(self._envStepCounter)
done = self._termination()
reward = self._reward()
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
#print("reward")
#print(reward)
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
return
def _render(self, mode="rgb_array", close=False):
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)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
#print (self._kuka.endEffectorPos[2])
@@ -138,25 +211,41 @@ class KukaGymEnv(gym.Env):
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>1000):
if (self.terminated or self._envStepCounter>self._maxSteps):
self._observation = self.getExtendedObservation()
return True
if (actualEndEffectorPos[2] <= 0.10):
maxDist = 0.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("closing gripper, attempting grasp")
#print("terminating, closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (1000):
graspAction = [0,0,0.001,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
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
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)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
@@ -165,21 +254,24 @@ class KukaGymEnv(gym.Env):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex)
reward = -1000
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
print("grasped a block!!!")
print("self._envStepCounter")
print(self._envStepCounter)
reward = reward+1000
#print("reward")
reward = reward+10000
print("successfully grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print(reward)
return reward

387
examples/pybullet/gym/pybullet_envs/bullet/minitaur_duck_gym_env.py Normal file
View File

@@ -0,0 +1,387 @@
"""This file implements the gym environment of minitaur.
"""
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)
import math
import time
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import pybullet
from . import bullet_client
from . import minitaur
import os
import pybullet_data
from . import minitaur_env_randomizer
NUM_SUBSTEPS = 5
NUM_MOTORS = 8
MOTOR_ANGLE_OBSERVATION_INDEX = 0
MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS
MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS
BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS
ACTION_EPS = 0.01
OBSERVATION_EPS = 0.01
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
duckStartPos = [0,0,0.25]
duckStartOrn = [0.5,0.5,0.5,0.5]
class MinitaurBulletDuckEnv(gym.Env):
"""The gym environment for the minitaur.
It simulates the locomotion of a minitaur, a quadruped robot. The state space
include the angles, velocities and torques for all the motors and the action
space is the desired motor angle for each motor. The reward function is based
on how far the minitaur walks in 1000 steps and penalizes the energy
expenditure.
"""
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()):
"""Initialize the minitaur gym environment.
Args:
urdf_root: The path to the urdf data folder.
action_repeat: The number of simulation steps before actions are applied.
distance_weight: The weight of the distance term in the reward.
energy_weight: The weight of the energy term in the reward.
shake_weight: The weight of the vertical shakiness term in the reward.
drift_weight: The weight of the sideways drift term in the reward.
distance_limit: The maximum distance to terminate the episode.
observation_noise_stdev: The standard deviation of observation noise.
self_collision_enabled: Whether to enable self collision in the sim.
motor_velocity_limit: The velocity limit of each motor.
pd_control_enabled: Whether to use PD controller for each motor.
leg_model_enabled: Whether to use a leg motor to reparameterize the action
space.
accurate_motor_model_enabled: Whether to use the accurate DC motor model.
motor_kp: proportional gain for the accurate motor model.
motor_kd: derivative gain for the accurate motor model.
torque_control_enabled: Whether to use the torque control, if set to
False, pose control will be used.
motor_overheat_protection: Whether to shutdown the motor that has exerted
large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
(OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
details.
hard_reset: Whether to wipe the simulation and load everything when reset
is called. If set to false, reset just place the minitaur back to start
position and set its pose to initial configuration.
on_rack: Whether to place the minitaur on rack. This is only used to debug
the walking gait. In this mode, the minitaur's base is hanged midair so
that its walking gait is clearer to visualize.
render: Whether to render the simulation.
kd_for_pd_controllers: kd value for the pd controllers of the motors
env_randomizer: An EnvRandomizer to randomize the physical properties
during reset().
"""
self._time_step = 0.01
self._action_repeat = action_repeat
self._num_bullet_solver_iterations = 300
self._urdf_root = urdf_root
self._self_collision_enabled = self_collision_enabled
self._motor_velocity_limit = motor_velocity_limit
self._observation = []
self._env_step_counter = 0
self._is_render = render
self._last_base_position = [0, 0, 0]
self._distance_weight = distance_weight
self._energy_weight = energy_weight
self._drift_weight = drift_weight
self._shake_weight = shake_weight
self._distance_limit = distance_limit
self._observation_noise_stdev = observation_noise_stdev
self._action_bound = 1
self._pd_control_enabled = pd_control_enabled
self._leg_model_enabled = leg_model_enabled
self._accurate_motor_model_enabled = accurate_motor_model_enabled
self._motor_kp = motor_kp
self._motor_kd = motor_kd
self._torque_control_enabled = torque_control_enabled
self._motor_overheat_protection = motor_overheat_protection
self._on_rack = on_rack
self._cam_dist = 1.0
self._cam_yaw = 0
self._duckId = -1
self._cam_pitch = -30
self._hard_reset = True
self._kd_for_pd_controllers = kd_for_pd_controllers
self._last_frame_time = 0.0
print("urdf_root=" + self._urdf_root)
self._env_randomizer = env_randomizer
# PD control needs smaller time step for stability.
if pd_control_enabled or accurate_motor_model_enabled:
self._time_step /= NUM_SUBSTEPS
self._num_bullet_solver_iterations /= NUM_SUBSTEPS
self._action_repeat *= NUM_SUBSTEPS
if self._is_render:
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)
action_dim = 8
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high)
self.observation_space = spaces.Box(observation_low, observation_high)
self.viewer = None
self._hard_reset = hard_reset # This assignment need to be after reset()
def set_env_randomizer(self, env_randomizer):
self._env_randomizer = env_randomizer
def configure(self, args):
self._args = args
def _reset(self):
if self._hard_reset:
self._pybullet_client.resetSimulation()
self._pybullet_client.setPhysicsEngineParameter(
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._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))
else:
self.minitaur.Reset(reload_urdf=False)
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])
if not self._torque_control_enabled:
for _ in range(100):
if self._pd_control_enabled or self._accurate_motor_model_enabled:
self.minitaur.ApplyAction([math.pi / 2] * 8)
self._pybullet_client.stepSimulation()
return self._noisy_observation()
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def _transform_action_to_motor_command(self, action):
if self._leg_model_enabled:
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))
action = self.minitaur.ConvertFromLegModel(action)
return action
def _step(self, action):
"""Step forward the simulation, given the action.
Args:
action: A list of desired motor angles for eight motors.
Returns:
observations: The angles, velocities and torques of all motors.
reward: The reward for the current state-action pair.
done: Whether the episode has ended.
info: A dictionary that stores diagnostic information.
Raises:
ValueError: The action dimension is not the same as the number of motors.
ValueError: The magnitude of actions is out of bounds.
"""
if self._is_render:
# Sleep, otherwise the computation takes less time than real time,
# which will make the visualization like a fast-forward video.
time_spent = time.time() - self._last_frame_time
self._last_frame_time = time.time()
time_to_sleep = self._action_repeat * self._time_step - time_spent
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)
action = self._transform_action_to_motor_command(action)
for _ in range(self._action_repeat):
self.minitaur.ApplyAction(action)
self._pybullet_client.stepSimulation()
self._env_step_counter += 1
reward = self._reward()
done = self._termination()
return np.array(self._noisy_observation()), reward, done, {}
def _render(self, mode="rgb_array", close=False):
if mode != "rgb_array":
return np.array([])
base_pos = self.minitaur.GetBasePosition()
view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
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)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def get_minitaur_motor_angles(self):
"""Get the minitaur's motor angles.
Returns:
A numpy array of motor angles.
"""
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.
Returns:
A numpy array of motor velocities.
"""
return np.array(
self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX:
MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS])
def get_minitaur_motor_torques(self):
"""Get the minitaur's motor torques.
Returns:
A numpy array of motor torques.
"""
return np.array(
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.
Returns:
A numpy array of minitaur's orientation.
"""
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
def is_fallen(self):
"""Decide whether the minitaur has fallen.
If the up directions between the base and the world is larger (the dot
product is smaller than 0.85) or the base is very low on the ground
(the height is smaller than 0.13 meter), the minitaur is considered fallen.
Returns:
Boolean value that indicates whether the minitaur has fallen.
"""
orientation = self.minitaur.GetBaseOrientation()
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)
def _termination(self):
position = self.minitaur.GetBasePosition()
distance = math.sqrt(position[0]**2 + position[1]**2)
return self.lost_duck() or self.is_fallen() or distance > self._distance_limit
def _reward(self):
current_base_position = self.minitaur.GetBasePosition()
forward_reward = current_base_position[0] - self._last_base_position[0]
drift_reward = -abs(current_base_position[1] - self._last_base_position[1])
shake_reward = -abs(current_base_position[2] - self._last_base_position[2])
self._last_base_position = current_base_position
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])
return reward
def get_objectives(self):
return self._objectives
def _get_observation(self):
self._observation = self.minitaur.GetObservation()
return self._observation
def _noisy_observation(self):
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())
return observation

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

@@ -15,6 +15,9 @@ import random
from . import bullet_client
import pybullet_data
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class RacecarGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
@@ -136,20 +139,27 @@ class RacecarGymEnv(gym.Env):
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
width=320
height=200
img_arr = self._p.getCameraImage(width,height)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]
dep=img_arr[3]
#print 'width = %d height = %d' % (w,h)
# reshape creates np array
np_img_arr = np.reshape(rgb, (h, w, 4))
# remove alpha channel
np_img_arr = np_img_arr[:, :, :3]
return np_img_arr
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)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
return self._envStepCounter>1000

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

@@ -15,6 +15,9 @@ from . import racecar
import random
import pybullet_data
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
class RacecarZEDGymEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
@@ -151,7 +154,26 @@ class RacecarZEDGymEnv(gym.Env):
return np.array(self._observation), reward, done, {}
def _render(self, mode='human', close=False):
return
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)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
return self._envStepCounter>1000

40
examples/pybullet/gym/pybullet_envs/examples/kukaCamGymEnvTest.py Normal file
View File

@@ -0,0 +1,40 @@
#add parent dir to find package. Only needed for source code build, pip install doesn't need it.
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)
from pybullet_envs.bullet.kukaCamGymEnv import KukaCamGymEnv
import time
def main():
environment = KukaCamGymEnv(renders=True,isDiscrete=False)
motorsIds=[]
#motorsIds.append(environment._p.addUserDebugParameter("posX",0.4,0.75,0.537))
#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
dv = 1
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
done = False
while (not done):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
if __name__=="__main__":
main()

8
examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest.py
View File

@@ -9,7 +9,7 @@ import time
def main():
environment = KukaGymEnv(renders=True)
environment = KukaGymEnv(renders=True,isDiscrete=False, maxSteps = 10000000)
motorsIds=[]
@@ -19,10 +19,10 @@ def main():
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
dv = 0.001
dv = 0.01
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,-dv))
motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
@@ -37,4 +37,4 @@ def main():
obs = environment.getExtendedObservation()
if __name__=="__main__":
main()
main()

38
examples/pybullet/gym/pybullet_envs/examples/kukaGymEnvTest2.py Normal file
View File

@@ -0,0 +1,38 @@
#add parent dir to find package. Only needed for source code build, pip install doesn't need it.
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)
from pybullet_envs.bullet.kukaGymEnv import KukaGymEnv
import time
def main():
environment = KukaGymEnv(renders=True,isDiscrete=False, maxSteps = 10000000)
motorsIds=[]
#motorsIds.append(environment._p.addUserDebugParameter("posX",-1,1,0))
#motorsIds.append(environment._p.addUserDebugParameter("posY",-.22,.3,0.0))
#motorsIds.append(environment._p.addUserDebugParameter("posZ",0.1,1,0.2))
#motorsIds.append(environment._p.addUserDebugParameter("yaw",-3.14,3.14,0))
#motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
dv = 1
motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
done = False
while (not done):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
if __name__=="__main__":
main()

14
examples/pybullet/gym/pybullet_envs/examples/minitaur_gym_env_example.py
View File

@@ -44,9 +44,9 @@ def MotorOverheatExample():
motor_kd=0.00,
on_rack=False)
action = [2.0] * 8
action = [.0] * 8
for i in range(8):
action[i] = 2.0 - 0.5 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1)
action[i] = .0 - 0.1 * (-1 if i % 2 == 0 else 1) * (-1 if i < 4 else 1)
steps = 500
actions_and_observations = []
@@ -112,9 +112,9 @@ def SinePolicyExample():
on_rack=False)
sum_reward = 0
steps = 20000
amplitude_1_bound = 0.5
amplitude_2_bound = 0.5
speed = 40
amplitude_1_bound = 0.1
amplitude_2_bound = 0.1
speed = 1
for step_counter in range(steps):
time_step = 0.01
@@ -124,9 +124,9 @@ def SinePolicyExample():
amplitude2 = amplitude_2_bound
steering_amplitude = 0
if t < 10:
steering_amplitude = 0.5
steering_amplitude = 0.1
elif t < 20:
steering_amplitude = -0.5
steering_amplitude = -0.1
else:
steering_amplitude = 0

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

@@ -41,4 +41,4 @@ def main():
print(obs)
if __name__=="__main__":
main()
main()

38
examples/pybullet/gym/pybullet_envs/examples/racecarZEDGymEnvTest.py
View File

@@ -3,12 +3,13 @@ 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)
isDiscrete = False
from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv
def main():
environment = RacecarZEDGymEnv(renders=True, isDiscrete=True)
environment = RacecarZEDGymEnv(renders=True, isDiscrete=isDiscrete)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
@@ -16,25 +17,28 @@ def main():
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = environment._p.readUserDebugParameter(steeringSlider)
discreteAction = 0
if (targetVelocity<-0.33):
discreteAction=0
if (isDiscrete):
discreteAction = 0
if (targetVelocity<-0.33):
discreteAction=0
else:
if (targetVelocity>0.33):
discreteAction=6
else:
discreteAction=3
if (steeringAngle>-0.17):
if (steeringAngle>0.17):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
else:
if (targetVelocity>0.33):
discreteAction=6
else:
discreteAction=3
if (steeringAngle>-0.17):
if (steeringAngle>0.17):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
action=[targetVelocity,steeringAngle]
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")
print(obs)
#print("obs")
#print(obs)
if __name__=="__main__":
main()

2
setup.py
View File

@@ -441,7 +441,7 @@ print("-----")
setup(
name = 'pybullet',
version='1.5.8',
version='1.6.3',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

2
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View File

@@ -615,7 +615,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep)
btScalar defaultRollingFrictionImpulse = 0.f;
btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
rollingFriction = calcRollingFriction(contactPt);
rollingFriction = calcRollingFriction(contactPt)/btScalar(getNumWheels());
}
}