class Drive(ShowBase):
def __init__(self):
ShowBase.__init__(self)
#Setup
scene = BulletWorld()
scene.setGravity(Vec3(0, 0, -9.81))
base.setBackgroundColor(0.6,0.9,0.9)
fog = Fog("The Fog")
fog.setColor(0.9,0.9,1.0)
fog.setExpDensity(0.003)
render.setFog(fog)
#Lighting
#Sun light
sun = DirectionalLight("The Sun")
sun_np = render.attachNewNode(sun)
sun_np.setHpr(0,-60,0)
render.setLight(sun_np)
#Ambient light
amb = AmbientLight("The Ambient Light")
amb.setColor(VBase4(0.39,0.39,0.39, 1))
amb_np = render.attachNewNode(amb)
render.setLight(amb_np)
#Variables
self.gear = 0
self.start = 0
self.Pbrake = 0
self.terrain_var = 1
self.time = 0
self.headlight_var = 0
self.RPM = 0
self.clutch = 0
self.carmaxspeed = 100 #KPH
self.carmaxreversespeed = -40 #KPH
self.steering = 0
#Functions
def V1():
camera.setPos(0.25,-1.2,0.5)
camera.setHpr(0,-13,0)
def V2():
camera.setPos(0,-15,3)
camera.setHpr(0,-10,0)
def V3():
camera.setPos(0,0,9)
camera.setHpr(0,-90,0)
def up():
self.gear = self.gear -1
if self.gear < -1:
self.gear = -1
def down():
self.gear = self.gear +1
if self.gear > 1:
self.gear = 1
def start_function():
self.start = 1
self.start_sound.play()
self.engine_idle_sound.play()
self.RPM = 1000
def stop_function():
self.start = 0
self.engine_idle_sound.stop()
def parkingbrake():
self.Pbrake = (self.Pbrake + 1) % 2
def rotate():
Car_np.setHpr(0, 0, 0)
def horn():
self.horn_sound.play()
def set_time():
if self.time == -1:
sun.setColor(VBase4(0.4, 0.3, 0.3, 1))
base.setBackgroundColor(0.8,0.7,0.7)
if self.time == 0:
sun.setColor(VBase4(0.7, 0.7, 0.7, 1))
base.setBackgroundColor(0.6,0.9,0.9)
if self.time == 1:
sun.setColor(VBase4(0.2, 0.2, 0.2, 1))
base.setBackgroundColor(0.55,0.5,0.5)
if self.time == 2:
sun.setColor(VBase4(0.02, 0.02, 0.05, 1))
base.setBackgroundColor(0.3,0.3,0.3)
if self.time == -2:
self.time = -1
if self.time == 3:
self.time = 2
def time_forward():
self.time = self.time + 1
def time_backward():
self.time = self.time -1
def set_terrain():
if self.terrain_var == 1:
self.ground_model.setTexture(self.ground_tex, 1)
self.ground_model.setScale(3)
if self.terrain_var == 2:
self.ground_model.setTexture(self.ground_tex2, 1)
self.ground_model.setScale(3)
if self.terrain_var == 3:
self.ground_model.setTexture(self.ground_tex3, 1)
self.ground_model.setScale(4)
if self.terrain_var == 4:
self.terrain_var = 1
if self.terrain_var == 0:
self.terrain_var = 3
def next_terrain():
self.terrain_var = self.terrain_var + 1
def previous_terrain():
self.terrain_var = self.terrain_var - 1
def show_menu():
self.menu_win.show()
self.a1.show()
self.a2.show()
self.a3.show()
self.a4.show()
self.t1.show()
self.t2.show()
self.ok.show()
self.exit_button.show()
def hide_menu():
self.menu_win.hide()
self.a1.hide()
self.a2.hide()
self.a3.hide()
self.a4.hide()
self.ok.hide()
self.t1.hide()
self.t2.hide()
self.exit_button.hide()
def Menu():
self.menu_win = OnscreenImage(image = "Textures/menu.png", pos = (0.9,0,0), scale = (0.5))
self.menu_win.setTransparency(TransparencyAttrib.MAlpha)
#The Arrow Buttons
self.a1 = DirectButton(text = "<", scale = 0.2, pos = (0.55,0,0.25), command = previous_terrain)
self.a2 = DirectButton(text = ">", scale = 0.2, pos = (1.15,0,0.25), command = next_terrain)
self.a3 = DirectButton(text = "<", scale = 0.2, pos = (0.55,0,0.0), command = time_backward)
self.a4 = DirectButton(text = ">", scale = 0.2, pos = (1.15,0,0.0), command = time_forward)
#The Text
self.t1 = OnscreenText(text = "Terrain", pos = (0.85,0.25,0), scale = 0.1, fg = (0.4,0.4,0.5,1))
self.t2 = OnscreenText(text = "Time", pos = (0.85,0,0), scale = 0.1, fg = (0.4,0.4,0.5,1))
#The Buttons
self.ok = DirectButton(text = "Okay", scale = 0.11, pos = (0.87,0,-0.25), command = hide_menu)
self.exit_button = DirectButton(text = "Quit", scale = 0.11, pos = (0.87,0,-0.42), command = sys.exit)
Menu()
def take_screenshot():
base.screenshot("Screenshot")
def set_headlights():
if self.headlight_var == 1:
Headlight1.setColor(VBase4(9.0,8.9,8.9,1))
Headlight2.setColor(VBase4(9.0,8.9,8.9,1))
if self.headlight_var == 0:
Headlight1.setColor(VBase4(0,0,0,1))
Headlight2.setColor(VBase4(0,0,0,1))
def headlights():
self.headlight_var = (self.headlight_var + 1) % 2
def update_rpm():
#Simulate RPM
if self.start == 1:
if self.gear == 0:
self.RPM = self.RPM - self.RPM / 400
else:
self.RPM = self.RPM + self.carspeed / 9
self.RPM = self.RPM - self.RPM / 200
#Reset RPM to 0 when engine is off
if self.start == 0:
if self.RPM > 0.0:
self.RPM = self.RPM - 40
if self.RPM < 10:
self.RPM = 0.0
#Idle RPM power
if self.start == 1:
if self.RPM < 650:
self.RPM = self.RPM + 4
if self.RPM < 600:
self.clutch = 1
else:
self.clutch = 0
#RPM limit
if self.RPM > 6000:
self.RPM = 6000
#Controls
inputState.watchWithModifiers("F", "arrow_up")
inputState.watchWithModifiers("B", "arrow_down")
inputState.watchWithModifiers("L", "arrow_left")
inputState.watchWithModifiers("R", "arrow_right")
do = DirectObject()
do.accept("escape", show_menu)
do.accept("1", V1)
do.accept("2", V2)
do.accept("3", V3)
do.accept("page_up", up)
do.accept("page_down", down)
do.accept("x-repeat", start_function)
do.accept("x", stop_function)
do.accept("p", parkingbrake)
do.accept("backspace", rotate)
do.accept("enter", horn)
do.accept("f12", take_screenshot)
do.accept("h", headlights)
#The ground
self.ground = BulletPlaneShape(Vec3(0, 0, 1,), 1)
self.ground_node = BulletRigidBodyNode("The ground")
self.ground_node.addShape(self.ground)
self.ground_np = render.attachNewNode(self.ground_node)
self.ground_np.setPos(0, 0, -2)
scene.attachRigidBody(self.ground_node)
self.ground_model = loader.loadModel("Models/plane.egg")
self.ground_model.reparentTo(render)
self.ground_model.setPos(0,0,-1)
self.ground_model.setScale(3)
self.ground_tex = loader.loadTexture("Textures/ground.png")
self.ground_tex2 = loader.loadTexture("Textures/ground2.png")
self.ground_tex3 = loader.loadTexture("Textures/ground3.png")
self.ground_model.setTexture(self.ground_tex, 1)
#The car
Car_shape = BulletBoxShape(Vec3(1, 2.0, 1.0))
Car_node = BulletRigidBodyNode("The Car")
Car_node.setMass(1200.0)
Car_node.addShape(Car_shape)
Car_np = render.attachNewNode(Car_node)
Car_np.setPos(0,0,3)
Car_np.setHpr(0,0,0)
Car_np.node().setDeactivationEnabled(False)
scene.attachRigidBody(Car_node)
Car_model = loader.loadModel("Models/Car.egg")
Car_model.reparentTo(Car_np)
Car_tex = loader.loadTexture("Textures/Car1.png")
Car_model.setTexture(Car_tex, 1)
self.Car_sim = BulletVehicle(scene, Car_np.node())
self.Car_sim.setCoordinateSystem(ZUp)
scene.attachVehicle(self.Car_sim)
#The inside of the car
Car_int = loader.loadModel("Models/inside.egg")
Car_int.reparentTo(Car_np)
Car_int_tex = loader.loadTexture("Textures/inside.png")
Car_int.setTexture(Car_int_tex, 1)
Car_int.setTransparency(TransparencyAttrib.MAlpha)
#The steering wheel
Sw = loader.loadModel("Models/Steering wheel.egg")
Sw.reparentTo(Car_np)
Sw.setPos(0.25,0,-0.025)
#The first headlight
Headlight1 = Spotlight("Headlight1")
lens = PerspectiveLens()
lens.setFov(180)
Headlight1.setLens(lens)
Headlight1np = render.attachNewNode(Headlight1)
Headlight1np.reparentTo(Car_np)
Headlight1np.setPos(-0.8,2.5,-0.5)
Headlight1np.setP(-15)
render.setLight(Headlight1np)
#The second headlight
Headlight2 = Spotlight("Headlight2")
Headlight2.setLens(lens)
Headlight2np = render.attachNewNode(Headlight2)
Headlight2np.reparentTo(Car_np)
Headlight2np.setPos(0.8,2.5,-0.5)
Headlight2np.setP(-15)
render.setLight(Headlight2np)
#Sounds
self.horn_sound = loader.loadSfx("Sounds/horn.ogg")
self.start_sound = loader.loadSfx("Sounds/enginestart.ogg")
self.engine_idle_sound = loader.loadSfx("Sounds/engineidle.ogg")
self.engine_idle_sound.setLoop(True)
self.accelerate_sound = loader.loadSfx("Sounds/enginethrottle.ogg")
#Camera
base.disableMouse()
camera.reparentTo(Car_np)
camera.setPos(0,-15,3)
camera.setHpr(0,-10,0)
#Wheel function
def Wheel(pos, np, r, f):
w = self.Car_sim.createWheel()
w.setNode(np.node())
w.setChassisConnectionPointCs(pos)
w.setFrontWheel(f)
w.setWheelDirectionCs(Vec3(0, 0, -1))
w.setWheelAxleCs(Vec3(1, 0, 0))
w.setWheelRadius(r)
w.setMaxSuspensionTravelCm(40)
w.setSuspensionStiffness(120)
w.setWheelsDampingRelaxation(2.3)
w.setWheelsDampingCompression(4.4)
w.setFrictionSlip(50)
w.setRollInfluence(0.1)
#Wheels
w1_np = loader.loadModel("Models/Lwheel")
w1_np.reparentTo(render)
w1_np.setColorScale(0,6)
Wheel(Point3(-1,1,-0.6), w1_np, 0.4, False)
w2_np = loader.loadModel("Models/Rwheel")
w2_np.reparentTo(render)
w2_np.setColorScale(0,6)
Wheel(Point3(-1.1,-1.2,-0.6), w2_np, 0.4, True)
w3_np = loader.loadModel("Models/Lwheel")
w3_np.reparentTo(render)
w3_np.setColorScale(0,6)
Wheel(Point3(1.1,-1,-0.6), w3_np, 0.4, True)
w4_np = loader.loadModel("Models/Rwheel")
w4_np.reparentTo(render)
w4_np.setColorScale(0,6)
Wheel(Point3(1,1,-0.6), w4_np, 0.4, False)
#The engine and steering
def processInput(dt):
#Vehicle properties
self.steeringClamp = 35.0
self.steeringIncrement = 70
engineForce = 0.0
brakeForce = 0.0
#Get the vehicle's current speed
self.carspeed = self.Car_sim.getCurrentSpeedKmHour()
#Engage clutch when in gear 0
if self.gear == 0:
self.clutch = 1
#Slow the steering when at higher speeds
self.steeringIncrement = self.steeringIncrement - self.carspeed / 1.5
#Reset the steering
if not inputState.isSet("L") and not inputState.isSet("R"):
if self.steering < 0.00:
self.steering = self.steering + 0.6
if self.steering > 0.00:
self.steering = self.steering - 0.6
if self.steering < 1.0 and self.steering > -1.0:
self.steering = 0
#Slow the car down while it's moving
if self.clutch == 0:
brakeForce = brakeForce + self.carspeed / 5
else:
brakeForce = brakeForce + self.carspeed / 15
#Forward
if self.start == 1:
if inputState.isSet("F"):
self.RPM = self.RPM + 35
self.accelerate_sound.play()
if self.clutch == 0:
if self.gear == -1:
if self.carspeed > self.carmaxreversespeed:
engineForce = -self.RPM / 3
if self.gear == 1:
if self.carspeed < self.carmaxspeed:
engineForce = self.RPM / 1
#Brake
if inputState.isSet("B"):
engineForce = 0.0
brakeForce = 12.0
if self.gear != 0 and self.clutch == 0:
self.RPM = self.RPM - 20
#Left
if inputState.isSet("L"):
if self.steering < 0.0:
#This makes the steering reset at the correct speed when turning from right to left
self.steering += dt * self.steeringIncrement + 0.6
self.steering = min(self.steering, self.steeringClamp)
else:
#Normal steering
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
#Right
if inputState.isSet("R"):
if self.steering > 0.0:
#This makes the steering reset at the correct speed when turning from left to right
self.steering -= dt * self.steeringIncrement + 0.6
self.steering = max(self.steering, -self.steeringClamp)
else:
#Normal steering
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
#Park
if self.Pbrake == 1:
brakeForce = 10.0
if self.gear != 0 and self. clutch == 0:
self.RPM = self.RPM - 20
#Apply forces to wheels
self.Car_sim.applyEngineForce(engineForce, 0);
self.Car_sim.applyEngineForce(engineForce, 3);
self.Car_sim.setBrake(brakeForce, 1);
self.Car_sim.setBrake(brakeForce, 2);
self.Car_sim.setSteeringValue(self.steering, 0);
self.Car_sim.setSteeringValue(self.steering, 3);
#Steering wheel
Sw.setHpr(0,0,-self.steering*10)
#The HUD
self.gear_hud = OnscreenImage(image = "Textures/gear_hud.png", pos = (-1,0,-0.85), scale = (0.2))
self.gear_hud.setTransparency(TransparencyAttrib.MAlpha)
self.gear2_hud = OnscreenImage(image = "Textures/gear2_hud.png", pos = (-1,0,-0.85), scale = (0.2))
self.gear2_hud.setTransparency(TransparencyAttrib.MAlpha)
self.starter = OnscreenImage(image = "Textures/starter.png", pos = (-1.2,0,-0.85), scale = (0.15))
self.starter.setTransparency(TransparencyAttrib.MAlpha)
self.park = OnscreenImage(image = "Textures/pbrake.png", pos = (-0.8,0,-0.85), scale = (0.1))
self.park.setTransparency(TransparencyAttrib.MAlpha)
self.rev_counter = OnscreenImage(image = "Textures/dial.png", pos = (-1.6, 0.0, -0.70), scale = (0.6,0.6,0.4))
self.rev_counter.setTransparency(TransparencyAttrib.MAlpha)
self.rev_needle = OnscreenImage(image = "Textures/needle.png", pos = (-1.6, 0.0, -0.70), scale = (0.5))
self.rev_needle.setTransparency(TransparencyAttrib.MAlpha)
self.rev_text = OnscreenText(text = " ", pos = (-1.6, -0.90, 0), scale = 0.05)
self.speedometer = OnscreenImage(image = "Textures/dial.png", pos = (-1.68, 0.0, -0.10), scale = (0.7,0.7,0.5))
self.speedometer.setTransparency(TransparencyAttrib.MAlpha)
self.speedometer_needle = OnscreenImage(image = "Textures/needle.png", pos = (-1.68, 0.0, -0.10), scale = (0.5))
self.speedometer_needle.setTransparency(TransparencyAttrib.MAlpha)
self.speedometer_text = OnscreenText(text = " ", pos = (-1.68, -0.35, 0), scale = 0.05)
#Update the HUD
def Update_HUD():
#Move gear selector
if self.gear == -1:
self.gear2_hud.setPos(-1,0,-0.785)
if self.gear == 0:
self.gear2_hud.setPos(-1,0,-0.85)
if self.gear == 1:
self.gear2_hud.setPos(-1,0,-0.91)
#Rotate starter
if self.start == 0:
self.starter.setHpr(0,0,0)
else:
self.starter.setHpr(0,0,45)
#Update the parking brake light
if self.Pbrake == 1:
self.park.setImage("Textures/pbrake2.png")
self.park.setTransparency(TransparencyAttrib.MAlpha)
else:
self.park.setImage("Textures/pbrake.png")
self.park.setTransparency(TransparencyAttrib.MAlpha)
#Update the rev counter
self.rev_needle.setR(self.RPM/22)
rev_string = str(self.RPM)[:4]
self.rev_text.setText(rev_string+" RPM")
#Update the speedometer
if self.carspeed > 0.0:
self.speedometer_needle.setR(self.carspeed*2.5)
if self.carspeed < 0.0:
self.speedometer_needle.setR(-self.carspeed*2.5)
speed_string = str(self.carspeed)[:3]
self.speedometer_text.setText(speed_string+" KPH")
#Update the program
def update(task):
dt = globalClock.getDt()
processInput(dt)
Update_HUD()
set_time()
set_terrain()
set_headlights()
update_rpm()
scene.doPhysics(dt, 5, 1.0/180.0)
return task.cont
taskMgr.add(update, "Update")
class Tank(DynamicWorldObject):
'''Child of WorldObject, with all of the things that makes a Tank a tank.
Includes a Weapon
'''
def __init__(self, world, attach, name = '', position = Vec3(0,0,0), orientation = Vec3(0,0,0),
turretPitch = 0):
#Constant Relevant Instatiation Parameters
self._tankSize = Vec3(1, 1.5, .5) # Actually a half-size
self._tankSideLength = max(self._tankSize)
friction = .3
tankMass = 800.0
# Rewrite constructor to include these?
self._maxVel = 4
self._maxRotVel = 1
self._maxThrusterAccel = 5000
self._breakForce = 1000
turretRelPos = (0, 0, 0) #Relative to tank
self._shape = BulletBoxShape(Vec3(1,1.5,.5)) #chassis
self._transformState = TransformState.makePos(Point3(0, 0, 0)) #offset
DynamicWorldObject.__init__(self, world, attach, name, position, self._shape, orientation, Vec3(0,0,0), mass = tankMass) #Initial velocity must be 0
self.__createVehicle(self._tankWorld.getPhysics())
self._collisionCounter = 0
self._taskTimer = 0
self._nodePath.node().setFriction(friction)
self._nodePath.setPos(position)
# Set up turret nodepath
# (Nodepaths are how objects are managed in Panda3d)
self.onTask = 0
# Make collide mask (What collides with what)
self._nodePath.setCollideMask(0xFFFF0000)
#self._nodePath.setCollideMask(BitMask32.allOff())
self.movementPoint = Point3(10,10,0)
#print "Tank.__init__: " + name
# Set up the
def __createVehicle(self,bulletWorld):
'''
Creates a vehicle, sets up wheels and does all the things
'''
self._nodePath.node().setMass(800.0)
# Chassis geometry
np = loader.loadModel('media/tankBody.x')
np.setHpr(90,0,0)
np.reparentTo(self._nodePath)
#np.setScale(self._tankSize*2)
np.setPos(-self._tankSize+Vec3(1, 0, .5))
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self._nodePath.node())
self.vehicle.setCoordinateSystem(2)
bulletWorld.attachVehicle(self.vehicle)
wheelNP = loader.loadModel('box')
wheelNP.setScale(.01,.01,.01)
wheelPos = [Point3(1, 1, 0),Point3(-1, 1, 0),
Point3(1, -1, 0),Point3(-1, -1, 0)]
for i in range(4):
wheel = self.vehicle.createWheel()
wheel.setWheelAxleCs(Vec3(-2*(i%2)+1, 0, 0))
wheel.setChassisConnectionPointCs(wheelPos[i])
self.__createWheel(wheel)
self.vehicle.setSteeringValue(0,i)
wheel.setRollInfluence((-2*(i%2)+1)*0.2)
def __createWheel(self,wheel):
'''
sets up properties for wheel.
'''
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setFrontWheel(False)
wheel.setWheelRadius(0.15)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0)
def getWheels(self):
'''
returns a list of wheelPos
'''
return self.vehicle.getWheels()
def setWeaponHp(self, heading, pitch):
'''
float heading
float pitch
'''
self._weapon.setHp(heading,pitch)
def moveWeapon(self, heading = 0, pitch = 0):
rot = self.getHpr()
self.setWeaponHp(rot[0] + heading, rot[1] + pitch)
def distanceScan(self):
'''
This scan projects rays from the objects in the field toward the tank
in question. This scan does not perform as well as scan when the
objects are bunched together. When small objects are spread out
reasonably (more than 5 at a viewing range of 50), this scan performs
better.
Using this scan, large objects can hide behind small objects
This scan has the feature that it will pick up a lone object
guaranteed at any distance.
'''
self.wait(.1)
potentialNPs = self._tankWorld.render.getChildren()
found = []
for np in potentialNPs:
if type(np.node()) == BulletRigidBodyNode and np != self and not np.isHidden():
pFrom = np.getPos(render)
#Fix for cubeObjects (z = 0). They collide with the floor
if pFrom[2] < 1:
pFrom[2] = 1.1
pTo = self.getPos() + self.getPos() - pFrom
#pTo is a Vec3, turned to a point for rayTest
result = self._tankWorld.getPhysics().rayTestClosest(
pFrom, Point3(pTo[0], pTo[1], pTo[2]))
if result.hasHit() and result.getNode() == self._nodePath.node():
#found.append((np.node().getPrevTransform().getPos(),
# np.node().getName()))
found.append((np.getPos(render),
np.node().getName()))
elif result.hasHit():
#print "Tank.distanceScan: ",
#print np, result.getNode(), pFrom, pTo
#print "Neigh"
x = 1
return found
def __bulletRays(self, numPoints, relAngleRange, height):
'''Helper function for scans and pings. Runs through the
relAngleRange (two ints, in degrees) at the given numPoints and height.
Runs a Bullet rayTestClosest to find the nearest hit.
Returns a list of BulletClosestHitRayResult objects
'''
distanceOfMap = 100000
results = []
#scanResolution = numPoints / 360.0
angleSweep = relAngleRange[1] - relAngleRange[0] + 0.0
pos = self._nodePath.getPos()
heading = self._nodePath.getH()
for i in range(numPoints):
if angleSweep == 0:
angle = (heading + relAngleRange[0]) * (math.pi / 180)
else:
angle = (i / angleSweep + heading + relAngleRange[0]) * (math.pi / 180)
pFrom = Point3(math.sin(angle) * self._tankSideLength + pos[0],
math.cos(angle) * self._tankSideLength + pos[1], height)
pTo = Point3(math.sin(angle) * distanceOfMap + pos[0],
math.cos(angle) * distanceOfMap + pos[1], height)
result = self._tankWorld.getPhysics().rayTestClosest(pFrom, pTo)
results.append((result, angle * 180 / math.pi - heading))
return results
def scan(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
'''
This function scans the map to find the other objects on it. The scan
works iteratively, based on the angle range (given relative to the
tank's current heading) and the number of points given. This is a more
realistic scan, but does not work as well with smaller objects and
larger distances
'''
found = []
numFound = 0
results = self.__bulletRays(numPoints, relAngleRange, height)
prevNodes = dict()
for item in results:
result = item[0]
if result.hasHit():
newNode = result.getNode()
if newNode not in prevNodes:
found.append((newNode.getPrevTransform().getPos(),
newNode.getName()))
prevNodes[newNode] = 0
numFound = numFound + 1
return found
def pingPointsAbs(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
'''Returns a list of absolute coordinate points on each of the
'''
found = []
results = self.__bulletRays(numPoints, relAngleRange, height)
for item in results:
result = item[0]
if result.hasHit():
newNode = result.getNode()
found.append((result.getHitPos(), item[1], newNode.getName()))
return found
def pingPoints(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
found = self.pingPointsAbs(numPoints, relAngleRange, height)
pos = self.getPos()
for i in range(len(found)):
hitPos = found[i][0]
relPos = Point3(hitPos[0] - pos[0], hitPos[1] - pos[1], hitPos[2] - pos[2])
found[i] = (relPos, found[i][1], found[i][2])
return found
def pingDistance(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
found = self.pingPoints(numPoints, relAngleRange, height)
for i in range(len(found)):
relPos = found[i][0]
distance = math.sqrt(relPos[0]**2 + relPos[1]**2)
found[i] = (distance, found[i][1], found[i][2])
return found
def applyThrusters(self, right = 1, left = 1): #set acceleration
'''change acceleration to a percent of the maximum acceleration'''
#if right > 1 or amt < 0:
# raise ValueError("amt must be between 0 and 1")
# tankNode = self._nodePath.node()
# angle = self.nodePath.getH() #Apply force in current direction
# magnitude = amt * (self._maxThrusterAccel) + (tankNode.getFriction() *
# self._nodePath.node().getMass())
# force = Vec3(magnitude * math.cos(angle),
# magnitude * math.sin(angle), 0)
# self.nodePath.node().applyForce(force)
self.vehicle.reset_suspension()
self.applyBrakes(0)
vel = self.vehicle.getChassis().getLinearVelocity()
for i in range(4):
self.vehicle.applyEngineForce(0,i)
self.vehicle.setBrake(0,i)
if vel.length() < self._maxVel:
self.vehicle.applyEngineForce(-left*self._maxThrusterAccel,0)
self.vehicle.applyEngineForce(right*self._maxThrusterAccel,1)
self.vehicle.applyEngineForce(-left*self._maxThrusterAccel,2)
self.vehicle.applyEngineForce(right*self._maxThrusterAccel,3)
#for i in range(0,1):
# self.vehicle.applyEngineForce((left*(i)%2+right*(i+1)%2)*self._maxThrusterAccel,i)
#for i in range(2):
# self.vehicle.applyEngineForce((left*(i)%2+right*(i+1)%2)*self._maxThrusterAccel,i+2)
#self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i)
## for 1 and 3useleft
## for 0 and 2 use right
else:
for i in range(4):
#self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i)
self.vehicle.applyEngineForce(0,i)
def applyBrakes(self, amt=1):
for i in range(4):
#self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i)
self.vehicle.applyEngineForce(0,i)
self.vehicle.setBrake(amt*self._breakForce,i)
def setVel(self, goal ):
pass
def move(self, dist):
'''Moves using a distance. Adds an updateMoveLoc task to the taskMgr.
'''
heading = self._nodePath.getH() * math.pi/180
pos = self.getPos()
self._stop = False
#Of the form (goalLoc, startLoc, distance)
self._moveLoc = (Point3(pos[0] + math.sin(heading) * dist, pos[1] - math.cos(heading) * dist, pos[2]), pos, dist)
self._tankWorld.taskMgr.add(self.updateMoveLoc,'userTank '+self.getName(),uponDeath=self.nextTask)
def rotate(self, angle):
'''Rotate function. All angles given between 0 and 360
Angle changes are between -180 and 180
'''
angle = angle % 360
if angle > 180:
angle -= 360
heading = self.fixAngle(self._nodePath.getH())
newH = self.fixAngle(heading + angle)
self._moveLoc = (newH, heading, angle)
self._stop = False
self._tankWorld.taskMgr.add(self.updateRotateLoc, 'userTank '+self.getName(), uponDeath=self.nextTask)
def moveTime(self, moveTime):
self._taskTimer = moveTime
self._tankWorld.taskMgr.add(self.updateMove,'userTank '+self.getName(),uponDeath=self.nextTask)
def rotateTime(self, rotateTime):
self._taskTimer = rotateTime
self._tankWorld.taskMgr.add(self.updateRotate,'userTank '+self.getName(),uponDeath=self.nextTask)
def wait(self, waitTime):
self._taskTimer = waitTime
self._tankWorld.taskMgr.add(self.updateWait,'userTank '+self.getName(),uponDeath=self.nextTask)
def updateWait(self, task):
'''
Tasks called to wait
'''
try:
if self._tankWorld.isRealTime():
dt = globalClock.getDt()
else:
dt = 1.0/60.0
#print "Tank.updateWait: ", dt
self._taskTimer -= dt
if self._taskTimer < 0:
return task.done
return task.cont
except:
print "error, tank.wait"
def updateRotateLoc(self, task):
heading = self.fixAngle(self._nodePath.getH())
toHeading = self._moveLoc[0]
w = self._nodePath.node().getAngularVelocity()[2]
#Right wheel direction for rotating in the direction of goal
rFor = self._moveLoc[2]/abs(self._moveLoc[2])
slowTheta = 1.5 * rFor
brakePercent = w**2 / (2 * slowTheta * self._maxThrusterAccel)
theta = self.fixAngle(toHeading - heading)
if theta > 180:
theta -= 360
thetaFromStart = heading - self._moveLoc[1]
thetaFromStart = thetaFromStart % 360
if thetaFromStart > 180:
thetaFromStart -= 360
if self._stop:
if abs(w) < .1:
self._nodePath.node().setAngularVelocity(Vec3(0,0,0))
self._nodePath.setH(self._moveLoc[0])
return task.done
self.applyBrakes()
return task.cont
if abs(theta) < slowTheta - .25:
self.applyThrusters(-rFor * brakePercent, rFor * brakePercent)
elif abs(theta) > slowTheta - .25:
if w < self._maxRotVel:
self.applyThrusters(rFor, -1 * rFor)
else:
self.applyThrusters(0,0)
else:
self.applyBrakes(brakePercent)
if abs(theta) < .1:
self._stop = True
if abs(thetaFromStart + .1) > abs(self._moveLoc[2]):
self._stop = True
#emergency stop based on a long time.
if task.time > 3:
#print "Tank.updateRotateLoc", "your rotate could not be completed, sorry"
return task.done
return task.cont
def updateMoveLoc(self, task):
'''Bases how much to slow down on a = v^2/2x.
x is slowDist, chosen to play nice.
Generally accurate to 5cm, rarely 20cm or worse
Stops at an arbitrary low velocity because it will never exit
at a lower minimum
'''
pos = self.getPos()
toLoc = self._moveLoc[0]
distance = math.sqrt((pos[0] - toLoc[0])**2 + (pos[1] - toLoc[1])**2)
v = self._nodePath.node().getLinearVelocity().length()
slowAccel = 2
slowDist = v**2 / (2 * slowAccel * self._breakForce)
brakePercent = slowAccel * self._nodePath.node().getMass() / self._breakForce
deltaPos = (self.getPos() - self._moveLoc[1])
distFromStart = deltaPos.length()
if self._stop:
if v < .4:
#self._nodePath.node().setLinearVelocity(Vec3(0,0,0))
#self._nodePath.setPos(self._moveLoc[0])
return task.done
self.applyBrakes()
return task.cont
if distance < slowDist:
self.applyBrakes(brakePercent * 1.1)
elif distance > slowDist:
if self._moveLoc[2] > 0:
self.applyThrusters(1,1)
else:
self.applyThrusters(-1, -1)
else:
self.applyBrakes(brakePercent)
if distance < .01:
self._stop = True
if abs(distFromStart) > abs(self._moveLoc[2]):
self._stop = True
return task.cont
def nextTask(self,task):
self._nodePath.node().setActive(True)
self.onTask += 1
if(self._tankWorld.isDead or self._tankWorld.isOver):
return
#if self.onTask >= len(self.taskList):
# return
def getNumUserTask():
taskAmount = 0
for t in self._tankWorld.taskMgr.getTasks():
if t.getName() == 'userTank '+self.getName():
taskAmount +=1
return taskAmount
pre = getNumUserTask()
try:
self.taskList.next()
if(getNumUserTask() == pre):
self.nextTask(task)
except StopIteration:
#print 'Tank.nextTask error'
pass
#self.taskList[self.onTask][0](self.taskList[self.onTask][1])
def runTasks(self):
self.onTask = 0
self.nextTask(None)
def setGenerator(self, gen):
self.taskList = gen
self.runTasks()
def updateMove(self, task):
'''
Task Called to do movement. This is called once perframe
'''
try:
#small hack to prevent the first frame from doing all the tasks.
dt = globalClock.getDt()
if dt > .1:
return task.cont
self._taskTimer -= dt
if self._taskTimer < 0:
self.applyBrakes(1)
else:
self.applyThrusters(1,1)
if self._taskTimer < -1: #one second to stop
return task.done
except:
print "ERROR in tank.updateMove"
return task.cont
def updateRotate(self, task):
''' called to do rotation. This is called once per frame
'''
try:
dt = globalClock.getDt()
#small hack to prevent the first frame from doing all the tasks.
if dt > .1:
return task.cont
self._taskTimer -= dt
if self._taskTimer < 0:
self.applyBrakes(1)
else:
self.applyThrusters(1,-1)
if self._taskTimer < -1: #one second to stop
return task.done
except:
print "ERROR in tank.updateRotate"
return task.cont
def update(self, task):
pass
def aimAt(self, point, amt = 1, aimLow = True):
return self._weapon.aimAt(point, aimLow)
def setWeapon(self, weopwn):
self._weapon = weopwn
def fire(self, amt = 1):
x = self._weapon.fire(amt)
self.wait(.1)
return x
def deleteAfter(self, task = None):
if not self._nodePath.is_empty():
x = self._nodePath.node()
self._tankWorld.removeRigidBody(x)
self.hide()
self._tankWorld.lose()
#self._nodePath.detachNode()
def handleCollision(self, collide, taskName):
self._collisionCounter += 1
forReal = True
hitBy = collide.getNode0()
if hitBy.getName() == self._nodePath.node().getName():
hitBy = collide.getNode1()
allowedNames = ['floor', 'wall', 'State']
for name in allowedNames:
if name in hitBy.getName():
forReal = False
if not self._nodePath.is_empty():
if forReal:
print "Tank.handleCollision:(pos) ", self.getPos(), 'obj 1', collide.getNode0().getName(), 'obj 2', collide.getNode1().getName()
self._tankWorld.taskMgr.remove(taskName)
self._tankWorld.doMethodLater(.01, self.deleteAfter, 'deleteAfter')
else:
print "Tank.handleCollision Failed to have _nodepath"
self._tankWorld.taskMgr.remove(taskName)
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward = self._params.get('collision_reward', 0.)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 60)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 2.0)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
import cv2
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
if hasattr(self, '_model_path'):
# Collidable objects
visNP = loader.loadModel(self._model_path)
visNP.clearModelNodes()
visNP.reparentTo(render)
pos = (0., 0., 0.)
visNP.setPos(pos[0], pos[1], pos[2])
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_restart_pos(self):
self._restart_pos = []
self._restart_index = 0
if self._params.get('position_ranges', None) is not None:
ranges = self._params['position_ranges']
num_pos = self._params['num_pos']
if self._params.get('range_type', 'random') == 'random':
for _ in range(num_pos):
ran = ranges[np.random.randint(len(ranges))]
self._restart_pos.append(np.random.uniform(ran[0], ran[1]))
elif self._params['range_type'] == 'fix_spacing':
num_ran = len(ranges)
num_per_ran = num_pos // num_ran
for i in range(num_ran):
ran = ranges[i]
low = np.array(ran[0])
diff = np.array(ran[1]) - np.array(ran[0])
for j in range(num_per_ran):
val = diff * ((j + 0.0) / num_per_ran) + low
self._restart_pos.append(val)
elif self._params.get('positions', None) is not None:
self._restart_pos = self._params['positions']
else:
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _next_random_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
index = np.random.randint(num)
pos_hpr = self._restart_pos[index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
render.clearLight()
render.setLight(alightNP)
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 3.14)
def _default_restart_pos():
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
for i in range(int(dt / self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self._obs[0])
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self._back_obs[0])
observation = np.concatenate(observation, axis=2)
return observation
def _get_reward(self):
reward = self._collision_reward if self._collision else self._get_speed(
)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
return info
def _back_up(self):
assert (self._use_vel)
back_up_vel = self._params['back_up'].get('vel', -2.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
# TODO
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 1.0)
self._update(dt=duration)
self._des_vel = 0.0
self._steering = 0.0
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
num_contacts = result.getNumContacts()
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if pos is None and hpr is None:
if random_reset:
pos, hpr = self._next_random_restart_pos_hpr()
else:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
return self._get_observation()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._engineClamp * \
((action[1] - 49.5) / 49.5)
self._update(dt=self._dt)
observation = self._get_observation()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
return observation, reward, done, info
class CarPhys(Phys):
def __init__(self, mdt, carphys_props):
Phys.__init__(self, mdt)
self.pnode = None
self.vehicle = None
self.curr_speed_factor = 1.0
self.__prev_speed = 0
self.__finds = {} # cache for find's results
self.props = carphys_props
self._load_phys()
self.__set_collision_mesh()
self.__set_phys_node()
self.__set_vehicle()
self.__set_wheels()
eng.attach_obs(self.on_end_frame)
def _load_phys(self):
fpath = self.props.phys_file % self.mdt.name
with open(fpath) as phys_file: # pass phys props as a class
self.cfg = load(phys_file)
self.cfg['max_speed'] = self.get_speed()
self.cfg['friction_slip'] = self.get_friction()
self.cfg['roll_influence'] = self.get_roll_influence()
s_a = (self.mdt.name, round(self.cfg['max_speed'],
2), self.props.driver_engine)
LogMgr().log('speed %s: %s (%s)' % s_a)
fr_slip = round(self.cfg['friction_slip'], 2)
f_a = (self.mdt.name, fr_slip, self.props.driver_tires)
LogMgr().log('friction %s: %s (%s)' % f_a)
r_a = (self.mdt.name, round(self.cfg['roll_influence'],
2), self.props.driver_suspensions)
LogMgr().log('roll %s: %s (%s)' % r_a)
s_a = lambda field: setattr(self, field, self.cfg[field])
map(s_a, self.cfg.keys())
def __set_collision_mesh(self):
fpath = self.props.coll_path % self.mdt.name
self.coll_mesh = loader.loadModel(fpath)
chassis_shape = BulletConvexHullShape()
for geom in PhysMgr().find_geoms(self.coll_mesh, self.props.coll_name):
chassis_shape.addGeom(geom.node().getGeom(0), geom.getTransform())
self.mdt.gfx.nodepath.node().addShape(chassis_shape)
self.mdt.gfx.nodepath.setCollideMask(
BitMask32.bit(1)
| BitMask32.bit(2 + self.props.cars.index(self.mdt.name)))
#nodepath = self.mdt.gfx.nodepath.attachNewNode(BulletGhostNode('car ghost'))
#nodepath.node().addShape(BulletCapsuleShape(4, 5, ZUp))
#eng.attach_ghost(nodepath.node())
#nodepath.node().notifyCollisions(False)
def __set_phys_node(self):
self.pnode = self.mdt.gfx.nodepath.node()
self.pnode.setMass(self.mass)
self.pnode.setDeactivationEnabled(False)
PhysMgr().attach_rigid_body(self.pnode)
PhysMgr().add_collision_obj(self.pnode)
def __set_vehicle(self):
self.vehicle = BulletVehicle(PhysMgr().root, self.pnode)
self.vehicle.setCoordinateSystem(ZUp)
self.vehicle.setPitchControl(self.pitch_control)
tuning = self.vehicle.getTuning()
tuning.setSuspensionCompression(self.suspension_compression)
tuning.setSuspensionDamping(self.suspension_damping)
PhysMgr().attach_vehicle(self.vehicle)
def __set_wheels(self):
fwheel_bounds = self.mdt.gfx.wheels['fr'].get_tight_bounds()
f_radius = (fwheel_bounds[1][2] - fwheel_bounds[0][2]) / 2.0 + .01
rwheel_bounds = self.mdt.gfx.wheels['rr'].get_tight_bounds()
r_radius = (rwheel_bounds[1][2] - rwheel_bounds[0][2]) / 2.0 + .01
ffr = self.coll_mesh.find('**/' + self.props.wheel_names[0][0])
ffl = self.coll_mesh.find('**/' + self.props.wheel_names[0][1])
rrr = self.coll_mesh.find('**/' + self.props.wheel_names[0][2])
rrl = self.coll_mesh.find('**/' + self.props.wheel_names[0][3])
meth = self.coll_mesh.find
fr_node = ffr if ffr else meth('**/' + self.props.wheel_names[1][0])
fl_node = ffl if ffl else meth('**/' + self.props.wheel_names[1][1])
rr_node = rrr if rrr else meth('**/' + self.props.wheel_names[1][2])
rl_node = rrl if rrl else meth('**/' + self.props.wheel_names[1][3])
wheel_fr_pos = fr_node.get_pos() + (0, 0, f_radius)
wheel_fl_pos = fl_node.get_pos() + (0, 0, f_radius)
wheel_rr_pos = rr_node.get_pos() + (0, 0, r_radius)
wheel_rl_pos = rl_node.get_pos() + (0, 0, r_radius)
frw = self.mdt.gfx.wheels['fr']
flw = self.mdt.gfx.wheels['fl']
rrw = self.mdt.gfx.wheels['rr']
rlw = self.mdt.gfx.wheels['rl']
wheels_info = [(wheel_fr_pos, True, frw, f_radius),
(wheel_fl_pos, True, flw, f_radius),
(wheel_rr_pos, False, rrw, r_radius),
(wheel_rl_pos, False, rlw, r_radius)]
for (pos, front, nodepath, radius) in wheels_info:
self.__add_wheel(pos, front, nodepath.node(), radius)
def __add_wheel(self, pos, front, node, radius):
whl = self.vehicle.createWheel()
whl.setNode(node)
whl.setChassisConnectionPointCs(LPoint3f(*pos))
whl.setFrontWheel(front)
whl.setWheelDirectionCs((0, 0, -1))
whl.setWheelAxleCs((1, 0, 0))
whl.setWheelRadius(radius)
whl.setSuspensionStiffness(self.suspension_stiffness)
whl.setWheelsDampingRelaxation(self.wheels_damping_relaxation)
whl.setWheelsDampingCompression(self.wheels_damping_compression)
whl.setFrictionSlip(self.friction_slip) # high -> more adherence
whl.setRollInfluence(self.roll_influence) # low -> more stability
whl.setMaxSuspensionForce(self.max_suspension_force)
whl.setMaxSuspensionTravelCm(self.max_suspension_travel_cm)
whl.setSkidInfo(self.skid_info)
@property
def lateral_force(self):
vel = self.vehicle.get_chassis().get_linear_velocity()
vel.normalize()
dir = self.mdt.logic.car_vec
lat = dir.dot(vel)
lat_force = 0
if lat > .5:
lat_force = min(1, (lat - 1.0) / -.2)
return lat_force
@property
def is_flying(self): # no need to be cached
rays = [whl.getRaycastInfo() for whl in self.vehicle.get_wheels()]
return not any(ray.isInContact() for ray in rays)
@property
def prev_speed(self):
return self.__prev_speed
@property
def prev_speed_ratio(self):
return max(0, min(1.0, self.prev_speed / self.max_speed))
def on_end_frame(self):
self.__prev_speed = self.speed
@property
def speed(self):
if self.mdt.fsm.getCurrentOrNextState() == 'Countdown':
return 0 # getCurrentSpeedKmHour returns odd values otherwise
return self.vehicle.getCurrentSpeedKmHour()
@property
def speed_ratio(self):
return max(0, min(1.0, self.speed / self.max_speed))
def set_forces(self, eng_frc, brake_frc, steering):
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
self.vehicle.applyEngineForce(eng_frc, 2)
self.vehicle.applyEngineForce(eng_frc, 3)
self.vehicle.setBrake(brake_frc, 2)
self.vehicle.setBrake(brake_frc, 3)
def update_car_props(self):
speeds = []
for whl in self.vehicle.get_wheels():
contact_pt = whl.get_raycast_info().getContactPointWs()
gnd_name = self.gnd_name(contact_pt)
if not gnd_name:
continue
if gnd_name not in self.__finds:
gnd = self.props.track_phys.find('**/' + gnd_name)
self.__finds[gnd_name] = gnd
gfx_node = self.__finds[gnd_name]
if gfx_node.has_tag('speed'):
speeds += [float(gfx_node.get_tag('speed'))]
if gfx_node.has_tag('friction'):
fric = float(gfx_node.get_tag('friction'))
whl.setFrictionSlip(self.friction_slip * fric)
self.curr_speed_factor = (sum(speeds) / len(speeds)) if speeds else 1.0
@property
def gnd_names(self): # no need to be cached
whls = self.vehicle.get_wheels()
pos = map(lambda whl: whl.get_raycast_info().getContactPointWs(), whls)
return map(self.gnd_name, pos)
@staticmethod
def gnd_name(pos):
top = pos + (0, 0, 20)
bottom = pos + (0, 0, -20)
result = PhysMgr().ray_test_closest(bottom, top)
ground = result.get_node()
return ground.get_name() if ground else ''
def apply_damage(self, reset=False):
if reset:
self.max_speed = self.get_speed()
self.friction_slip = self.get_friction()
self.roll_influence = self.get_roll_influence()
else:
self.max_speed *= .95
self.friction_slip *= .95
self.roll_influence *= 1.05
fric = lambda whl: whl.setFrictionSlip(self.friction_slip)
map(fric, self.vehicle.get_wheels())
roll = lambda whl: whl.setRollInfluence(self.roll_influence)
map(roll, self.vehicle.get_wheels())
s_a = (str(round(self.max_speed, 2)), self.props.driver_engine)
LogMgr().log('speed: %s (%s)' % s_a)
f_a = (str(round(self.friction_slip, 2)), self.props.driver_tires)
LogMgr().log('friction: %s (%s)' % f_a)
r_a = (str(round(self.roll_influence,
2)), self.props.driver_suspensions)
LogMgr().log('roll: %s (%s)' % r_a)
def get_speed(self):
return self.cfg['max_speed'] * (1 + .01 * self.props.driver_engine)
def get_friction(self):
return self.cfg['friction_slip'] * (1 + .01 * self.props.driver_tires)
def get_roll_influence(self):
return self.cfg['roll_influence'] * (
1 + .01 * self.props.driver_suspensions)
def destroy(self):
eng.detach_obs(self.on_end_frame)
eng.remove_vehicle(self.vehicle)
self.pnode = self.vehicle = self.__finds = self.__track_phys = \
self.coll_mesh = None
Phys.destroy(self)
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward_only = self._params.get('collision_reward_only',
False)
self._collision_reward = self._params.get('collision_reward', -10.0)
self._obs_shape = self._params.get('obs_shape', (64, 36))
self._steer_limits = params.get('steer_limits', (-30., 30.))
self._speed_limits = params.get('speed_limits', (-4.0, 4.0))
self._motor_limits = params.get('motor_limits', (-0.5, 0.5))
self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1]
and self._use_vel)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 120)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._env_time_step = 0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 0.5)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
self._setup_spec()
self.spec = EnvSpec(observation_im_space=self.observation_im_space,
action_space=self.action_space,
action_selection_space=self.action_selection_space,
observation_vec_spec=self.observation_vec_spec,
action_spec=self.action_spec,
action_selection_spec=self.action_selection_spec,
goal_spec=self.goal_spec)
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
def _setup_spec(self):
self.action_spec = OrderedDict()
self.action_selection_spec = OrderedDict()
self.observation_vec_spec = OrderedDict()
self.goal_spec = OrderedDict()
self.action_spec['steer'] = Box(low=-45., high=45.)
self.action_selection_spec['steer'] = Box(low=self._steer_limits[0],
high=self._steer_limits[1])
if self._use_vel:
self.action_spec['speed'] = Box(low=-4., high=4.)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['speed'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['speed'].high[0]
]))
self.action_selection_spec['speed'] = Box(
low=self._speed_limits[0], high=self._speed_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['speed'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['speed'].high[0]
]))
else:
self.action_spec['motor'] = Box(low=-self._accelClamp,
high=self._accelClamp)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['motor'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['motor'].high[0]
]))
self.action_selection_spec['motor'] = Box(
low=self._motor_limits[0], high=self._motor_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['motor'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['motor'].high[0]
]))
assert (np.logical_and(
self.action_selection_space.low >= self.action_space.low - 1e-4,
self.action_selection_space.high <=
self.action_space.high + 1e-4).all())
self.observation_im_space = Box(low=0,
high=255,
shape=tuple(
self._get_observation()[0].shape))
self.observation_vec_spec['coll'] = Discrete(1)
self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14)
self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0)
@property
def _base_dir(self):
return os.path.join(os.path.dirname(os.path.abspath(__file__)),
'models')
@property
def horizon(self):
return np.inf
@property
def max_reward(self):
return np.inf
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
self._setup_map()
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_map(self):
if hasattr(self, '_model_path'):
# Collidable objects
self._setup_collision_object(self._model_path)
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
def _setup_collision_object(self,
path,
pos=(0.0, 0.0, 0.0),
hpr=(0.0, 0.0, 0.0),
scale=1):
visNP = loader.loadModel(path)
visNP.clearModelNodes()
visNP.reparentTo(render)
visNP.setPos(pos[0], pos[1], pos[2])
visNP.setHpr(hpr[0], hpr[1], hpr[2])
visNP.set_scale(scale, scale, scale)
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
bodyNP.setHpr(hpr[0], hpr[1], hpr[2])
bodyNP.set_scale(scale, scale, scale)
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
print("Issue")
def _setup_restart_pos(self):
self._restart_index = 0
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
# alight = AmbientLight('ambientLight')
# alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
# alightNP = render.attachNewNode(alight)
# render.clearLight()
# render.setLight(alightNP)
pass
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 0.0)
def _default_restart_pos(self):
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _get_heading(self):
h = np.array(self._vehicle_pointer.getHpr())[0]
ori = h * (pi / 180.)
while (ori > 2 * pi):
ori -= 2 * pi
while (ori < 0):
ori += 2 * pi
return ori
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
# for i in range(int(dt/self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(self._p * err + self._d * d_err,
-self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
for _ in range(int(dt / self._step)):
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self.process(self._obs[0], self._obs_shape))
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self.process(self._back_obs[0],
self._obs_shape))
observation_im = np.concatenate(observation, axis=2)
coll = self._collision
heading = self._get_heading()
speed = self._get_speed()
observation_vec = np.array([coll, heading, speed])
return observation_im, observation_vec
def _get_goal(self):
return np.array([])
def process(self, image, obs_shape):
if self._use_depth:
im = np.reshape(image, (image.shape[0], image.shape[1]))
if im.shape != obs_shape:
im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA)
return im.astype(np.uint8)
else:
image = np.dot(image[..., :3], [0.299, 0.587, 0.114])
im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA
) #TODO how does this deal with aspect ratio
# TODO might not be necessary
im = np.expand_dims(im, 2)
return im.astype(np.uint8)
def _get_reward(self):
if self._collision_reward_only:
if self._collision:
reward = self._collision_reward
else:
reward = 0.0
else:
if self._collision:
reward = self._collision_reward
else:
reward = self._get_speed()
assert (reward <= self.max_reward)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
info['env_time_step'] = self._env_time_step
return info
def _back_up(self):
assert (self._use_vel)
# logger.debug('Backing up!')
self._params['back_up'] = self._params.get('back_up', {})
back_up_vel = self._params['back_up'].get('vel', -1.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 3.0)
self._update(dt=duration)
self._des_vel = 0.
self._steering = 0.
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if hard_reset:
logger.debug('Hard resetting!')
if pos is None and hpr is None:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
self._env_time_step = 0
return self._get_observation(), self._get_goal()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._mass * action[1]
self._update(dt=self._dt)
observation = self._get_observation()
goal = self._get_goal()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
self._env_time_step += 1
return observation, goal, reward, done, info
class Vehicle(object):
def __init__(self, positionHpr, render, world, base):
self.base = base
loader = base.loader
position, hpr = positionHpr
vehicleType = "yugo"
self.vehicleDir = "data/vehicles/" + vehicleType + "/"
# Load vehicle description and specs
with open(self.vehicleDir + "vehicle.json") as vehicleData:
data = json.load(vehicleData)
self.specs = data["specs"]
# Chassis for collisions and mass uses a simple box shape
halfExtents = (0.5 * dim for dim in self.specs["dimensions"])
shape = BulletBoxShape(Vec3(*halfExtents))
ts = TransformState.makePos(Point3(0, 0, 0.5))
self.rigidNode = BulletRigidBodyNode("vehicle")
self.rigidNode.addShape(shape, ts)
self.rigidNode.setMass(self.specs["mass"])
self.rigidNode.setDeactivationEnabled(False)
self.np = render.attachNewNode(self.rigidNode)
self.np.setPos(position)
self.np.setHpr(hpr)
world.attachRigidBody(self.rigidNode)
# Vehicle physics model
self.vehicle = BulletVehicle(world, self.rigidNode)
self.vehicle.setCoordinateSystem(ZUp)
world.attachVehicle(self.vehicle)
# Vehicle graphical model
self.vehicleNP = loader.loadModel(self.vehicleDir + "car.egg")
self.vehicleNP.reparentTo(self.np)
# Create wheels
wheelPos = self.specs["wheelPositions"]
for fb, y in (("F", wheelPos[1]), ("B", -wheelPos[1])):
for side, x in (("R", wheelPos[0]), ("L", -wheelPos[0])):
np = loader.loadModel(self.vehicleDir + "tire%s.egg" % side)
np.reparentTo(render)
isFront = fb == "F"
self.addWheel(Point3(x, y, wheelPos[2]), isFront, np)
def addWheel(self, position, isFront, np):
wheel = self.vehicle.createWheel()
wheelSpecs = self.specs["wheels"]
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(position)
wheel.setFrontWheel(isFront)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(wheelSpecs["radius"])
wheel.setMaxSuspensionTravelCm(wheelSpecs["suspensionTravel"] * 100.0)
wheel.setSuspensionStiffness(wheelSpecs["suspensionStiffness"])
wheel.setWheelsDampingRelaxation(wheelSpecs["dampingRelaxation"])
wheel.setWheelsDampingCompression(wheelSpecs["dampingCompression"])
wheel.setFrictionSlip(wheelSpecs["frictionSlip"])
wheel.setRollInfluence(wheelSpecs["rollInfluence"])
def initialiseSound(self, audioManager):
"""Start the engine sound and set collision sounds"""
# Set sounds to play for collisions
self.collisionSound = CollisionSound(
nodePath=self.np, sounds=["data/sounds/09.wav"], thresholdForce=600.0, maxForce=800000.0
)
self.engineSound = audioManager.loadSfx(self.vehicleDir + "engine.wav")
audioManager.attachSoundToObject(self.engineSound, self.np)
self.engineSound.setLoop(True)
self.engineSound.setPlayRate(0.6)
self.engineSound.play()
self.gearSpacing = self.specs["sound"]["maxExpectedRotationRate"] / self.specs["sound"]["numberOfGears"]
self.reversing = False
def updateSound(self, dt):
"""Use vehicle speed to update sound pitch"""
soundSpecs = self.specs["sound"]
# Use rear wheel rotation speed as some measure of engine revs
wheels = (self.vehicle.getWheel(idx) for idx in (2, 3))
# wheelRate is in degrees per second
wheelRate = 0.5 * abs(sum(w.getDeltaRotation() / dt for w in wheels))
# Calculate which gear we're in, and what the normalised revs are
if self.reversing:
numberOfGears = 1
else:
numberOfGears = self.specs["sound"]["numberOfGears"]
gear = min(int(wheelRate / self.gearSpacing), numberOfGears - 1)
posInGear = (wheelRate - gear * self.gearSpacing) / self.gearSpacing
targetPlayRate = 0.6 + posInGear * (1.5 - 0.6)
currentRate = self.engineSound.getPlayRate()
self.engineSound.setPlayRate(0.8 * currentRate + 0.2 * targetPlayRate)
def updateControl(self, controlState, dt):
"""Updates acceleration, braking and steering
These are all passed in through a controlState dictionary
"""
# Update acceleration and braking
wheelForce = controlState["throttle"] * self.specs["maxWheelForce"]
self.reversing = controlState["reverse"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
brakeForce = controlState["brake"] * self.specs["brakeForce"]
# Update steering
# Steering control state is from -1 to 1
steering = controlState["steering"] * self.specs["steeringLock"]
# Apply steering to front wheels
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(wheelForce, 2)
self.vehicle.applyEngineForce(wheelForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
class Car(DirectObject):
def __init__(self, base, world, track):
super().__init__()
self.world = world
self.track = track
self.steering = 0
self.accelerator = 0
self.brake = 0
self.gear_ratios = [-4, 0, 3.9, 2.9, 2.3, 1.87, 1.68, 1.54, 1.46]
self.gear = 1
self.differential_ratio = 4.5
self.transmission_efficiency = 0.95
self.wheel_radius = 0.4
self.drag_coefficient = 0.48
self.engine_torque_curve = [
(0, 466),
(564, 469),
(1612, 469),
(2822, 518),
(3870, 517),
(4516, 597),
(5000, 613),
(5564, 600),
(6048, 655),
(6693, 681),
(7177, 716),
(7822, 696),
(8306, 696),
(11048, 569),
(13951, 391),
(15000, 339),
(15483, 301),
(16612, 247),
(17177, 65),
(18306, 55)
]
self.fw_wingspan = 0.64
self.fw_cord = 1.01
self.fw_clift = 0.7
self.rw_wingspan = 0.64
self.rw_cord = 0.51
self.rw_clift = 0.2
self.car_node = None
self.car = None
dial_scale = 0.2
speed_dial = OnscreenImage(image='tex/speed360.rgb', pos=(-dial_scale, 0, dial_scale),
scale=(dial_scale, 1, dial_scale),
parent=base.a2dBottomCenter)
speed_dial.setTransparency(TransparencyAttrib.MAlpha)
self.speed_dial = OnscreenImage(image='tex/dial.rgb', parent=speed_dial)
rpm_dial = OnscreenImage(image='tex/rpm20000.rgb', pos=(dial_scale, 0, dial_scale),
scale=(dial_scale, 1, dial_scale),
parent=base.a2dBottomCenter)
rpm_dial.setTransparency(TransparencyAttrib.MAlpha)
self.rpm_dial = OnscreenImage(image='tex/dial.rgb', parent=rpm_dial)
self.gear_text = OnscreenText(text='N', pos=(-0.02, -0.67), scale=0.4, parent=rpm_dial, fg=(255, 0, 0, 1))
self.reset()
taskMgr.add(self.update, 'update')
def make_wheel(self, pos, front, np):
wheel = self.car.createWheel(0.02)
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(self.wheel_radius)
wheel.setSuspensionStiffness(200)
wheel.setWheelsDampingRelaxation(23)
wheel.setWheelsDampingCompression(44)
wheel.setRollInfluence(0.1)
wheel.setMaxSuspensionTravelCm(10)
wheel.setFrictionSlip(1.2)
# wheel.setMaxSuspensionForce(1000)
def reset(self):
car = loader.loadModel("models/car.egg")
car.flattenLight()
car_bounds = car.getTightBounds()
car_shape = BulletBoxShape(Vec3((car_bounds[1].x - car_bounds[0].x) / 2,
(car_bounds[1].y - car_bounds[0].y) / 2,
(car_bounds[1].z - car_bounds[0].z) / 2))
car_ts = TransformState.makePos(Point3(0, 0, 0.5))
if self.car_node is not None:
self.car_node.removeNode()
self.car_node = render.attachNewNode(BulletRigidBodyNode('Car'))
self.car_node.node().setDeactivationEnabled(False)
self.car_node.node().setMass(600)
self.car_node.node().addShape(car_shape, car_ts)
self.world.attachRigidBody(self.car_node.node())
car.reparentTo(self.car_node)
self.car_node.setPos(0, 6, 1)
self.car = BulletVehicle(self.world, self.car_node.node())
self.car.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.car)
self.car_node.setPos(0, 6, 1)
self.car_node.setHpr(0, 0, 0)
self.car.resetSuspension()
self.car_node.node().clearForces()
wheel_fl = loader.loadModel("models/wheelL")
wheel_fl.reparentTo(render)
self.make_wheel(Point3(-0.4, 1.28, 0), True, wheel_fl)
wheel_fr = loader.loadModel("models/wheelR")
wheel_fr.reparentTo(render)
self.make_wheel(Point3(0.4, 1.28, 0), True, wheel_fr)
wheel_rl = loader.loadModel("models/wheelL")
wheel_rl.reparentTo(render)
self.make_wheel(Point3(-0.4, -1.35, 0), False, wheel_rl)
wheel_rr = loader.loadModel("models/wheelR")
wheel_rr.reparentTo(render)
self.make_wheel(Point3(0.4, -1.35, 0), False, wheel_rr)
def get_engine_torque(self, rpm):
min_rpm = min(self.engine_torque_curve, key=lambda p: p[0])
max_rpm = max(self.engine_torque_curve, key=lambda p: p[0])
if rpm <= min_rpm[0]:
return min_rpm[1]
elif rpm >= max_rpm[0]:
return 0
less_rpm = filter(lambda p: p[0] <= rpm, self.engine_torque_curve)
more_rpm = filter(lambda p: p[0] >= rpm, self.engine_torque_curve)
max_less_rpm = max(less_rpm, key=lambda p: p[0])
min_more_rpm = min(more_rpm, key=lambda p: p[0])
rpm_diff = min_more_rpm[0] - max_less_rpm[0]
torque_diff = min_more_rpm[1] - max_less_rpm[1]
slope = torque_diff / rpm_diff
diff = rpm - max_less_rpm[0]
return max_less_rpm[1] + (slope * diff)
@property
def pos(self):
return self.car_node.getPos()
@property
def forward_vector(self):
return self.car.getForwardVector()
def update(self, task):
car_pos = self.pos
car_vec = self.forward_vector
track_bounds = self.track.tight_bounds
if car_pos.x < track_bounds[0].x or \
car_pos.x > track_bounds[1].x or \
car_pos.y < track_bounds[0].y or \
car_pos.y > track_bounds[1].y or \
car_pos.z < track_bounds[0].z:
self.reset()
return task.cont
car_speed = self.car.getCurrentSpeedKmHour()
car_speed_ms = car_speed * 1000 / 3600
car_speed_abs = abs(car_speed)
self.car_node.node().clearForces()
self.apply_wing_force(car_speed_ms)
self.apply_drag_force(car_speed_ms, car_vec)
rpm = self.apply_engine_force(car_speed_ms)
self.apply_brake_force()
self.apply_steering_moment(car_speed_abs)
self.update_dials(rpm, car_speed_abs)
return task.cont
def apply_wing_force(self, car_speed_ms):
fw_downforce = 0.5 * self.fw_cord * self.fw_wingspan * self.fw_clift * 1.29 * car_speed_ms ** 2
rw_downforce = 0.5 * self.rw_cord * self.rw_wingspan * self.rw_clift * 1.29 * car_speed_ms ** 2
self.car_node.node().applyForce(Vec3(0, 0, -fw_downforce), Point3(-0.61, 2.32, 0.23))
self.car_node.node().applyForce(Vec3(0, 0, -fw_downforce), Point3(0.61, 2.32, 0.23))
self.car_node.node().applyForce(Vec3(0, 0, -rw_downforce), Point3(-0.61, -2.47, 1.2))
self.car_node.node().applyForce(Vec3(0, 0, -rw_downforce), Point3(0.61, -2.47, 1.2))
def apply_drag_force(self, car_speed_ms, car_vec):
drag_coefficient = 0.5 * self.drag_coefficient * 1.29 * 5
drag_force = drag_coefficient * car_speed_ms ** 2
rr_force = drag_coefficient * 30 * car_speed_ms
if car_speed_ms < 0:
drag_force = -drag_force
self.car_node.node().applyCentralForce(car_vec * -drag_force)
self.car_node.node().applyCentralForce(car_vec * -rr_force)
def apply_engine_force(self, car_speed_ms):
angular_velocity = car_speed_ms / self.wheel_radius
rpm = angular_velocity * self.gear_ratios[self.gear] * self.differential_ratio * 60 / (2 * math.pi)
rpm = max(0, rpm)
engine_torque = self.get_engine_torque(rpm)
max_engine_force = engine_torque * self.gear_ratios[self.gear] * self.differential_ratio * \
self.transmission_efficiency / self.wheel_radius
engine_force = max_engine_force * (self.accelerator / 128)
self.car.applyEngineForce(engine_force, 2)
self.car.applyEngineForce(engine_force, 3)
return rpm
def apply_brake_force(self):
brake_adj = 5
self.car.setBrake(self.brake * brake_adj, 0)
self.car.setBrake(self.brake * brake_adj, 1)
self.car.setBrake(self.brake * brake_adj, 2)
self.car.setBrake(self.brake * brake_adj, 3)
def apply_steering_moment(self, car_speed_abs):
steering_ratio = max((car_speed_abs / 3), 30)
self.car.setSteeringValue(self.steering / steering_ratio, 0)
self.car.setSteeringValue(self.steering / steering_ratio, 1)
def update_dials(self, rpm, car_speed_abs):
min_rpm = min(self.engine_torque_curve, key=lambda p: p[0])[0]
if self.gear == 1:
min_rpm = max(self.engine_torque_curve, key=lambda p: p[0])[0]
self.speed_dial.setHpr(0, 0, car_speed_abs * (270 / 360))
self.rpm_dial.setHpr(0, 0, max(rpm, min_rpm) * (270 / 20000) * (self.accelerator / 128))
def update_gear_text(self):
if self.gear >= 2:
text = str(self.gear - 1)
elif self.gear == 0:
text = "R"
elif self.gear == 1:
text = "N"
else:
text = ""
self.gear_text.setText(text)
def down_gear(self):
self.gear -= 1
if self.gear < 0:
self.gear = 0
self.update_gear_text()
def up_gear(self):
self.gear += 1
if self.gear >= len(self.gear_ratios):
self.gear = len(self.gear_ratios) - 1
self.update_gear_text()