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

allow continuous control for MIT racecar gym environment, use differential drive version

Browse Source
This commit is contained in:
Erwin Coumans
2017-08-23 23:12:26 -07:00
parent 8a4f51baa4
commit f0c32b84c0
26 changed files with 374 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -22,7 +22,7 @@ class BulletClient(object):
"""Inject the client id into Bullet functions."""
attribute = getattr(pybullet, name)
if inspect.isbuiltin(attribute):
if name not in ["invertTransform", "multiplyTransforms",
if name not in ["invertTransform", "computeViewMatrix","multiplyTransforms",
"getMatrixFromQuaternion"]: # A temporary hack for now.
attribute = functools.partial(attribute, physicsClientId=self._client)
return attribute

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

@@ -4,7 +4,7 @@
import copy
import math
import numpy as np
import motor
from . import motor
import os
INIT_POSITION = [0, 0, .2]

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

@@ -8,8 +8,8 @@ from gym import spaces
from gym.utils import seeding
import numpy as np
import pybullet
import bullet_client
import minitaur
from . import bullet_client
from . import minitaur
import os
NUM_SUBSTEPS = 5

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

@@ -5,54 +5,79 @@ import math
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):
self.racecarUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","racecar/racecar.urdf"), [0,0,.2])
self.maxForce = 20
self.nMotors = 2
self.motorizedwheels=[2]
self.inactiveWheels = [3,5,7]
for wheel in self.inactiveWheels:
self._p.setJointMotorControl2(self.racecarUniqueId,wheel,self._p.VELOCITY_CONTROL,targetVelocity=0,force=0)
def reset(self):
car = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","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.motorizedWheels = [2]
self.steeringLinks=[4,6]
self.speedMultiplier = 4.
#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)
def getActionDimension(self):
return self.nMotors
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)
def getObservationDimension(self):
return len(self.getObservation())
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)
def getObservation(self):
observation = []
pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId)
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)
observation.extend(list(pos))
observation.extend(list(orn))
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)
return observation
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)
def applyAction(self, motorCommands):
targetVelocity=motorCommands[0]*self.speedMultiplier
#print("targetVelocity")
#print(targetVelocity)
steeringAngle = motorCommands[1]
#print("steeringAngle")
#print(steeringAngle)
#print("maxForce")
#print(self.maxForce)
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
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)
def getObservationDimension(self):
return len(self.getObservation())
def getObservation(self):
observation = []
pos,orn=self._p.getBasePositionAndOrientation(self.racecarUniqueId)
observation.extend(list(pos))
observation.extend(list(orn))
return observation
def applyAction(self, motorCommands):
targetVelocity=motorCommands[0]*self.speedMultiplier
#print("targetVelocity")
#print(targetVelocity)
steeringAngle = motorCommands[1]*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)

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

@@ -8,7 +8,7 @@ import time
import pybullet
from . import racecar
import random
import bullet_client
from . import bullet_client
class RacecarGymEnv(gym.Env):
metadata = {
@@ -20,6 +20,7 @@ class RacecarGymEnv(gym.Env):
urdfRoot="",
actionRepeat=50,
isEnableSelfCollision=True,
isDiscrete=False,
renders=False):
print("init")
self._timeStep = 0.01
@@ -30,6 +31,7 @@ class RacecarGymEnv(gym.Env):
self._ballUniqueId = -1
self._envStepCounter = 0
self._renders = renders
self._isDiscrete = isDiscrete
if self._renders:
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
@@ -42,7 +44,13 @@ class RacecarGymEnv(gym.Env):
#print("observationDim")
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(9)
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)
self.observation_space = spaces.Box(-observation_high, observation_high)
self.viewer = None
@@ -53,10 +61,10 @@ class RacecarGymEnv(gym.Env):
#self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
#move the stadium objects slightly above 0
for i in stadiumobjects:
pos,orn = self._p.getBasePositionAndOrientation(i)
newpos = [pos[0],pos[1],pos[2]+0.1]
self._p.resetBasePositionAndOrientation(i,newpos,orn)
#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()
@@ -96,11 +104,15 @@ class RacecarGymEnv(gym.Env):
basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
#self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
fwd = [-5,-5,-5,0,0,0,5,5,5]
steerings = [-0.3,0,0.3,-0.3,0,0.3,-0.3,0,0.3]
forward = fwd[action]
steer = steerings[action]
realaction = [forward,steer]
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]
else:
realaction = action
self._racecar.applyAction(realaction)
for i in range(self._actionRepeat):
self._p.stepSimulation()

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

@@ -5,7 +5,8 @@ from gym import spaces
from gym.utils import seeding
import numpy as np
import time
import pybullet as p
import pybullet
from . import bullet_client
from . import racecar
import random
@@ -17,8 +18,9 @@ class RacecarZEDGymEnv(gym.Env):
def __init__(self,
urdfRoot="",
actionRepeat=100,
actionRepeat=10,
isEnableSelfCollision=True,
isDiscrete=True,
renders=True):
print("init")
self._timeStep = 0.01
@@ -30,11 +32,14 @@ class RacecarZEDGymEnv(gym.Env):
self._renders = renders
self._width = 100
self._height = 10
self._p = p
self._isDiscrete = isDiscrete
if self._renders:
p.connect(p.GUI)
self._p = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
p.connect(p.DIRECT)
self._p = bullet_client.BulletClient()
self._seed()
self.reset()
observationDim = len(self.getExtendedObservation())
@@ -42,22 +47,22 @@ class RacecarZEDGymEnv(gym.Env):
#print(observationDim)
observation_high = np.array([np.finfo(np.float32).max] * observationDim)
self.action_space = spaces.Discrete(6)
self.action_space = spaces.Discrete(9)
self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 4))
self.viewer = None
def _reset(self):
p.resetSimulation()
self._p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
#p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
stadiumobjects = p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
self._p.setTimeStep(self._timeStep)
#self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","plane.urdf"))
stadiumobjects = self._p.loadSDF(os.path.join(os.path.dirname(__file__),"../data","stadium.sdf"))
#move the stadium objects slightly above 0
for i in stadiumobjects:
pos,orn = p.getBasePositionAndOrientation(i)
pos,orn = self._p.getBasePositionAndOrientation(i)
newpos = [pos[0],pos[1],pos[2]+0.1]
p.resetBasePositionAndOrientation(i,newpos,orn)
self._p.resetBasePositionAndOrientation(i,newpos,orn)
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
@@ -66,39 +71,39 @@ class RacecarZEDGymEnv(gym.Env):
bally = dist * math.cos(ang)
ballz = 1
self._ballUniqueId = p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","sphere2red.urdf"),[ballx,bally,ballz])
p.setGravity(0,0,-10)
self._racecar = racecar.Racecar(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._ballUniqueId = self._p.loadURDF(os.path.join(os.path.dirname(__file__),"../data","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):
p.stepSimulation()
self._p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def __del__(self):
p.disconnect()
self._p = 0
def _seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def getExtendedObservation(self):
carpos,carorn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
carmat = p.getMatrixFromQuaternion(carorn)
ballpos,ballorn = p.getBasePositionAndOrientation(self._ballUniqueId)
invCarPos,invCarOrn = p.invertTransform(carpos,carorn)
ballPosInCar,ballOrnInCar = p.multiplyTransforms(invCarPos,invCarOrn,ballpos,ballorn)
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 = p.computeViewMatrix(eyePos,targetPos,up)
#viewMat = p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
viewMat = self._p.computeViewMatrix(eyePos,targetPos,up)
#viewMat = self._p.computeViewMatrixFromYawPitchRoll(carpos,1,0,0,0,2)
#print("projectionMatrix:")
#print(p.getDebugVisualizerCamera()[3])
#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 = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
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
@@ -106,17 +111,18 @@ class RacecarZEDGymEnv(gym.Env):
def _step(self, action):
if (self._renders):
basePos,orn = p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
#p.resetDebugVisualizerCamera(1, 30, -40, basePos)
basePos,orn = self._p.getBasePositionAndOrientation(self._racecar.racecarUniqueId)
#self._p.resetDebugVisualizerCamera(1, 30, -40, basePos)
fwd = [5,0,5,10,10,10]
steerings = [-0.5,0,0.5,-0.3,0,0.3]
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]
self._racecar.applyAction(realaction)
for i in range(self._actionRepeat):
p.stepSimulation()
self._p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
@@ -137,7 +143,7 @@ class RacecarZEDGymEnv(gym.Env):
return self._envStepCounter>1000
def _reward(self):
closestPoints = p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
closestPoints = self._p.getClosestPoints(self._racecar.racecarUniqueId,self._ballUniqueId,10000)
numPt = len(closestPoints)
reward=-1000

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_arm.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_carrier_cover.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_leftshaft.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_motor_cover.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_pinion.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_rightshaft.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.stl Normal file
View File

Binary file not shown.

198
examples/pybullet/gym/pybullet_envs/data/differential/diff_ring.urdf Normal file
View File

@@ -0,0 +1,198 @@
<?xml version="1.0" ?>
<robot name="husky_robot" xmlns:xacro="http://ros.org/wiki/xacro">
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="world"/>
<link name="diff_ring">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_ring.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_carrier.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.025 0"/>
<geometry>
<cylinder length="0.01" radius="0.05"/>
</geometry>
</collision>
</link>
<joint name="diff_ring_world" type="continuous">
<parent link="world"/>
<child link="diff_ring"/>
<origin rpy="0 0 0" xyz="0.256 0.2854 0.03282"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_spiderA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_spiderA_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_spiderA"/>
<origin rpy="0 0 1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_spiderB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_spider.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_spiderB_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_spiderB"/>
<origin rpy="0 0 -1.570795" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_sideA">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="1.570795 0 0" xyz="0 0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_sideA_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_sideA"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
<link name="diff_sideB">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<damping value="1000"/>
</contact>
<inertial>
<mass value="2.637"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
</inertial>
<visual>
<origin rpy="-1.570795 0 0" xyz="0 0 0"/>
<geometry>
<mesh scale="0.001 0.001 0.001" filename="differential/diff_side.stl"/>
</geometry>
<material name="DarkGrey"/>
</visual>
<collision>
<origin rpy="-1.570795 0 0" xyz="0 -0.01 0"/>
<geometry>
<cylinder length="0.01" radius="0.015"/>
</geometry>
</collision>
</link>
<joint name="diff_sideB_ring" type="continuous">
<parent link="diff_ring"/>
<child link="diff_sideB"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
</joint>
</robot>

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_side.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_spider.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_spider_shaft.stl Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_envs/data/differential/diff_stand.stl Normal file
View File

Binary file not shown.

2
examples/pybullet/gym/pybullet_envs/data/differential/modelorigin.txt Normal file
View File

@@ -0,0 +1,2 @@
stl files were copied from http://www.otvinta.com/download09.html
URDF file was manually created, along with mimicJointConstraint.py

31
examples/pybullet/gym/pybullet_envs/data/racecar/racecar_differential.urdf
View File

@@ -35,7 +35,7 @@
<contact>
<lateral_friction value=".5"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
<inertial>
@@ -68,7 +68,7 @@
<contact>
<lateral_friction value=".5"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
<inertial>
@@ -165,7 +165,7 @@
<contact>
<lateral_friction value=".8"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
<inertial>
@@ -209,7 +209,7 @@
<contact>
<lateral_friction value="0.8"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
<inertial>
@@ -337,7 +337,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -377,7 +377,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -416,7 +416,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -456,7 +456,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -492,7 +492,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -528,7 +528,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -568,7 +568,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -607,7 +607,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -645,7 +645,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -681,7 +681,7 @@
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="30000"/>
<stiffness value="300000"/>
<damping value="1000"/>
</contact>
@@ -736,8 +736,5 @@
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

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

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

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

@@ -5,30 +5,34 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv
environment = RacecarGymEnv(renders=True)
isDiscrete = True
environment = RacecarGymEnv(renders=True, isDiscrete=isDiscrete)
environment.reset()
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
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")

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

@@ -9,7 +9,7 @@ print ("hello")
environment = RacecarZEDGymEnv(renders=True)
targetVelocitySlider = environment._p.addUserDebugParameter("wheelVelocity",-1,1,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-0.5,0.5,0)
steeringSlider = environment._p.addUserDebugParameter("steering",-1,1,0)
while (True):
targetVelocity = environment._p.readUserDebugParameter(targetVelocitySlider)
@@ -27,8 +27,8 @@ while (True):
discreteAction=discreteAction+2
else:
discreteAction=discreteAction+1
action=discreteAction
state, reward, done, info = environment.step(action)
obs = environment.getExtendedObservation()
print("obs")

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

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

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

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