def test_get_steering():
world = BulletWorld()
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
body = BulletRigidBodyNode('Vehicle')
body.addShape(shape)
world.attach(body)
# Vehicle
vehicle = BulletVehicle(world, body)
world.attachVehicle(vehicle)
# Wheel
wheel = vehicle.createWheel()
wheel.setSteering(30.0)
# Returns the steering angle in degrees.
assert wheel.getSteering() == approx(30.0)
class Game(DirectObject):
def __init__(self):
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
base.setFrameRateMeter(True)
base.cam.setPos(0, -20, 4)
base.cam.lookAt(0, 0, 0)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
inputState.watchWithModifiers('forward', 'w')
inputState.watchWithModifiers('left', 'a')
inputState.watchWithModifiers('reverse', 's')
inputState.watchWithModifiers('right', 'd')
inputState.watchWithModifiers('turnLeft', 'q')
inputState.watchWithModifiers('turnRight', 'e')
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def doReset(self):
self.cleanup()
self.setup()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
# ____TASK___
def processInput(self, dt):
engineForce = 0.0
brakeForce = 0.0
if inputState.isSet('forward'):
engineForce = 1000.0
brakeForce = 0.0
if inputState.isSet('reverse'):
engineForce = 0.0
brakeForce = 100.0
if inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
# Apply steering to front wheels
self.vehicle.setSteeringValue(self.steering, 0)
self.vehicle.setSteeringValue(self.steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(engineForce, 2)
self.vehicle.applyEngineForce(engineForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
def update(self, task):
dt = globalClock.getDt()
self.processInput(dt)
self.world.doPhysics(dt, 10, 0.008)
#print self.vehicle.getWheel(0).getRaycastInfo().isInContact()
#print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs()
#print self.vehicle.getChassis().isKinematic()
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, -1)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
np.node().addShape(shape, ts)
np.setPos(0, 0, 1)
np.node().setMass(800.0)
np.node().setDeactivationEnabled(False)
self.world.attachRigidBody(np.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(self.world, np.node())
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('car/yugo.egg')
self.yugoNP.reparentTo(np)
# Right front wheel
np = loader.loadModel('car/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('car/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('car/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('car/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
# Steering info
self.steering = 0.0 # degree
self.steeringClamp = 45.0 # degree
self.steeringIncrement = 120.0 # degree per second
def addWheel(self, pos, front, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.25)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0)
wheel.setRollInfluence(0.1)
class Game(DirectObject):
def __init__(self):
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
base.setFrameRateMeter(True)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
# ROS
self.crash_pub = rospy.Publisher('crash',
std_msgs.msg.Empty,
queue_size=100)
### subscribers (info sent to Arduino)
self.cmd_steer_sub = rospy.Subscriber(
'cmd/steer',
std_msgs.msg.Float32,
callback=self._cmd_steer_callback)
self.cmd_motor_sub = rospy.Subscriber(
'cmd/motor',
std_msgs.msg.Float32,
callback=self._cmd_motor_callback)
self.cmd_vel_sub = rospy.Subscriber('cmd/vel',
std_msgs.msg.Float32,
callback=self._cmd_vel_callback)
self.reset_sub = rospy.Subscriber('reset',
std_msgs.msg.Empty,
callback=self._reset_callback)
self.cmd_steer_queue = Queue.Queue(maxsize=1)
self.cmd_motor_queue = Queue.Queue(maxsize=1)
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
print('Starting ROS thread')
threading.Thread(target=self._ros_servos_thread).start()
threading.Thread(target=self._ros_crash_thread).start()
threading.Thread(target=self._ros_image_thread).start()
# Callbacks
def _cmd_steer_callback(self, msg):
if msg.data >= 0 and msg.data <= 99.0:
self.cmd_steer_queue.put(msg.data)
def _cmd_motor_callback(self, msg):
if msg.data >= 0 and msg.data <= 99.0:
self.cmd_motor_queue.put(msg.data)
def _cmd_vel_callback(self, msg):
data = numpy.clip(msg.data * 3 + 49.5, 0, 99.0)
self.cmd_motor_queue.put(data)
def _reset_callback(self, msg):
self.doReset()
# ROS thread
def _ros_servos_thread(self):
"""
Publishes/subscribes to Ros.
"""
self.steering = 0.0 # degree
self.steeringClamp = rospy.get_param('~steeringClamp')
self.engineForce = 0.0
self.engineClamp = rospy.get_param('~engineClamp')
r = rospy.Rate(60)
while not rospy.is_shutdown():
r.sleep()
for var, queue in (('steer', self.cmd_steer_queue),
('motor', self.cmd_motor_queue)):
if not queue.empty():
try:
if var == 'steer':
self.steering = self.steeringClamp * (
(queue.get() - 49.5) / 49.5)
self.vehicle.setSteeringValue(self.steering, 0)
self.vehicle.setSteeringValue(self.steering, 1)
elif var == 'motor':
self.vehicle.setBrake(100.0, 2)
self.vehicle.setBrake(100.0, 3)
self.engineForce = self.engineClamp * (
(queue.get() - 49.5) / 49.5)
self.vehicle.applyEngineForce(self.engineForce, 2)
self.vehicle.applyEngineForce(self.engineForce, 3)
except Exception as e:
print(e)
def _ros_crash_thread(self):
crash = 0
r = rospy.Rate(30)
while not rospy.is_shutdown():
r.sleep()
try:
result = self.world.contactTest(self.vehicle_node)
if result.getNumContacts() > 0:
self.crash_pub.publish(std_msgs.msg.Empty())
except Exception as e:
print(e)
def _ros_image_thread(self):
camera_pub = ImageROSPublisher("image")
depth_pub = ImageROSPublisher("depth")
r = rospy.Rate(30)
i = 0
while not rospy.is_shutdown():
r.sleep()
# sometimes fails to observe
try:
obs = self.camera_sensor.observe()
camera_pub.publish_image(obs[0])
depth_pub.publish_image(obs[1], image_format="passthrough")
except:
pass
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def doReset(self):
self.cleanup()
self.setup()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
# ____TASK___
def update(self, task):
dt = globalClock.getDt()
self.world.doPhysics(dt, 10, 0.008)
#print self.vehicle.getWheel(0).getRaycastInfo().isInContact()
#print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs()
#print self.vehicle.getChassis().isKinematic()
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
np = self.ground = self.worldNP.attachNewNode(
BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, -1)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
# collision
self.maze = []
for pos in [(0.0, 72.0, 0.0), (-11.0, 60.0, 0.0), (11.0, 60.0, 0.0),
(-11.0, 48.0, 0.0), (11.0, 48.0, 0.0), (-11.0, 36.0, 0.0),
(11.0, 36.0, 0.0), (-11.0, 24.0, 0.0), (11.0, 24.0, 0.0),
(-11.0, 12.0, 0.0), (11.0, 12.0, 0.0), (-11.0, 0.0, 0.0),
(11.0, 0.0, 0.0), (0.0, -12.0, 0.0), (0.5, 12.0, 1.0),
(-0.5, 12.0, 1.0)]:
translate = False
if (abs(pos[0]) == 0.5):
translate = True
visNP = loader.loadModel('../models/ball.egg')
else:
visNP = loader.loadModel('../models/maze.egg')
visNP.clearModelNodes()
visNP.reparentTo(self.ground)
visNP.setPos(pos[0], pos[1], pos[2])
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
if translate:
bodyNP.setPos(pos[0], pos[1], pos[2] - 1)
else:
bodyNP.setPos(pos[0], pos[1], pos[2])
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
self.maze.append(bodyNP)
for bodyNP in self.maze:
self.world.attachRigidBody(bodyNP.node())
# Chassis
mass = rospy.get_param('~mass')
#chassis_shape = rospy.get_param('~chassis_shape')
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
np.node().addShape(shape, ts)
rand_vals = numpy.random.random(2) * 8 - 4.0
np.setPos(rand_vals[0], 0.0, -0.6)
np.node().setMass(mass)
np.node().setDeactivationEnabled(False)
first_person = rospy.get_param('~first_person')
self.camera_sensor = Panda3dCameraSensor(base,
color=True,
depth=True,
size=(160, 90))
self.camera_node = self.camera_sensor.cam
if first_person:
self.camera_node.reparentTo(np)
self.camera_node.setPos(0.0, 1.0, 1.0)
self.camera_node.lookAt(0.0, 6.0, 0.0)
else:
self.camera_node.reparentTo(np)
self.camera_node.setPos(0.0, -10.0, 5.0)
self.camera_node.lookAt(0.0, 5.0, 0.0)
base.cam.reparentTo(np)
base.cam.setPos(0.0, -10.0, 5.0)
base.cam.lookAt(0.0, 5.0, 0.0)
self.world.attachRigidBody(np.node())
np.node().setCcdSweptSphereRadius(1.0)
np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle_node = np.node()
self.vehicle = BulletVehicle(self.world, np.node())
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('../models/yugo/yugo.egg')
self.yugoNP.reparentTo(np)
# Right front wheel
np = loader.loadModel('../models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('../models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('../models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('../models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
# Collision handle
#base.cTrav = CollisionTraverser()
#self.notifier = CollisionHandlerEvent()
#self.notifier.addInPattern("%fn")
#self.accept("Vehicle", self.onCollision)
def onCollision(self):
print("crash")
def addWheel(self, pos, front, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.25)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e3)
wheel.setRollInfluence(0.0)
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 Game(DirectObject):
def __init__(self, model):
self.model = model
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
base.setFrameRateMeter(True)
base.cam.setPos(0, -20, 4)
base.cam.lookAt(0, 0, 0)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
inputState.watchWithModifiers('forward', 'w')
inputState.watchWithModifiers('left', 'a')
inputState.watchWithModifiers('reverse', 's')
inputState.watchWithModifiers('right', 'd')
inputState.watchWithModifiers('turnLeft', 'q')
inputState.watchWithModifiers('turnRight', 'e')
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def doReset(self):
self.cleanup()
self.setup()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
# ____TASK___
def calculate_moves(self):
self.y = self.model.predict(self.x)
self.moves = self.y > 0 # 0.5
def processInput(self, dt):
engineForce = 0.0
brakeForce = 0.0
if self.moves[0]: #inputState.isSet('forward'):
engineForce = 1000.0
brakeForce = 0.0
if not self.moves[0]: #inputState.isSet('reverse'):
engineForce = 0.0
brakeForce = 100.0
if self.moves[1]: #inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if not self.moves[1]: #inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
"""
if inputState.isSet('forward'):
engineForce = 1000.0
brakeForce = 0.0
if inputState.isSet('reverse'):
engineForce = 0.0
brakeForce = 100.0
if inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
"""
# Apply steering to front wheels
self.vehicle.setSteeringValue(self.steering, 0)
self.vehicle.setSteeringValue(self.steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(engineForce, 2)
self.vehicle.applyEngineForce(engineForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
def raycast(self):
"""pFrom = render.getRelativePoint(self.yugoNP,Point3(0,0,0))#Point3(0,0,0)
pFrom -= Point3(0,0,pFrom[2])
pRel = render.getRelativePoint(base.cam,self.yugoNP.getPos()) # FIXME THIS IS IT!! get rid of z component
pRel -= Point3(0,0,pRel[2])
p45 = Point3(pRel[0] - pRel[1], pRel[1] + pRel[0],0)
pn45 = Point3(pRel[0] + pRel[1], pRel[1] - pRel[0],0)
#print(render.getRelativePoint(self.yugoNP,Point3(0,0,0)))
#print(dir(self.yugoNP))
pTo = [pFrom + pn45, pFrom + pRel, pFrom + p45]#[pFrom + Vec3(-10,10,0)*999,pFrom + Vec3(0,10,0)*999,pFrom + Vec3(10,10,0)*999]# FIXME should be relative to front of car, getting cloe! #self.yugoNP.getPosDelta()*99999]#[Point3(-10,10,0) * 99999,Point3(0,10,0) * 99999,Point3(10,10,0) * 99999]
#self.ray = CollisionRay(0,0,0,100,0,0)
result = [self.world.rayTestClosest(pFrom,pt) for pt in pTo]
#print(dir(self.yugoNP))
#print(result.getHitPos())
return tuple([res.getHitPos().length() for res in result])
"""#queue = CollisionHandlerQueue()
#traverser.addCollider(fromObject, queue)
#traverser.traverse(render)
#queue.sortEntries()
#for entry in queue.getEntries():
#print(entry)
#print(result.getHitPos())
#if result.getNode() != None:
#print(self.yugoNP.getPos(result.getNode()))
#print(self.cTrav)
self.cTrav.traverse(render)
entries = list(self.colHandler.getEntries())
entries.sort(key=lambda y: y.getSurfacePoint(render).getY())
#for entry in entries: print(entry.getFromNodePath().getName())
if entries: # and len(result) > 1:
for r in entries:
if r.getIntoNodePath().getName(
) == 'Box' and r.getFromNodePath().getName() in [
'ray%d' % i for i in range(3)
]:
self.ray_col_vec_dict[
r.getFromNodePath().getName()].append(
numpy.linalg.norm(
list(r.getSurfacePoint(
r.getFromNodePath()))[:-1]))
self.ray_col_vec_dict = {
k: (min(self.ray_col_vec_dict[k])
if len(self.ray_col_vec_dict[k]) >= 1 else 10000)
for k in self.ray_col_vec_dict
}
self.x = numpy.array(list(self.ray_col_vec_dict.values()))
#return entries
def update(self, task):
dt = globalClock.getDt()
self.raycast()
self.calculate_moves()
self.ray_col_vec_dict = {k: [] for k in self.ray_col_vec_dict}
self.processInput(dt)
self.world.doPhysics(dt, 10, 0.008)
#print(dir(result[1]))
#print(numpy.linalg.norm(list(result[1].getSurfacePoint(result[1].getFromNodePath()))[:-1]))
#base.camera.setPos(0,-40,10)
#print self.vehicle.getWheel(0).getRaycastInfo().isInContact()
#print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs()
#print self.vehicle.getChassis().isKinematic()
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
#terrain = GeoMipTerrain("mySimpleTerrain")
#terrain.setHeightfield("./models/heightfield_2.png")
#terrain.getRoot().reparentTo(self.worldNP)#render)
#terrain.generate()
# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, -1)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track'))
np.node().setMass(5000.0)
np.setPos(3, 0, 10)
np.setCollideMask(BitMask32.allOn()) #(0x0f))
#self.track = BulletVehicle(self.world, np.node())
#self.track.setCoordinateSystem(ZUp)
self.track_np = loader.loadModel('models/race_track.egg')
self.track_np.setScale(100)
self.track_np.reparentTo(np)
self.track_np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
self.track_np = np
#self.track_np.show()
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
np.node().addShape(shape, ts)
np.setPos(0, 0, 1)
np.node().setMass(800.0)
np.node().setDeactivationEnabled(False)
self.world.attachRigidBody(np.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
self.cTrav = CollisionTraverser()
# Vehicle
self.vehicle = BulletVehicle(self.world, np.node())
self.vehicle.setCoordinateSystem(ZUp)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.reparentTo(np)
self.colHandler = CollisionHandlerQueue()
self.ray_col_np = {}
self.ray_col_vec_dict = {}
for ray_dir in range(-1, 2): # populate collision rays
self.ray = CollisionRay()
self.ray.setOrigin(ray_dir, 0.5, 0.5)
self.ray.setDirection(ray_dir, 1, 0)
self.ray_col = CollisionNode('ray%d' % (ray_dir + 1))
self.ray_col.addSolid(self.ray)
self.ray_col.setFromCollideMask(
BitMask32.allOn()) #(0x0f))#CollideMask.bit(0)
#self.ray_col.setIntoCollideMask(CollideMask.allOff())
self.ray_col_np['ray%d' %
(ray_dir + 1)] = self.yugoNP.attachNewNode(
self.ray_col)
self.cTrav.addCollider(self.ray_col_np['ray%d' % (ray_dir + 1)],
self.colHandler)
self.ray_col_np['ray%d' % (ray_dir + 1)].show()
self.ray_col_vec_dict['ray%d' % (ray_dir + 1)] = []
self.world.attachVehicle(self.vehicle)
self.cTrav.showCollisions(render)
# FIXME
base.camera.reparentTo(self.yugoNP)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
# Steering info
self.steering = 0.0 # degree
self.steeringClamp = 45.0 # degree
self.steeringIncrement = 120.0 # degree per second
# Box
for i, j in [(0, 8), (-3, 5), (6, -5), (8, 3), (-4, -4)]:
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
# https://discourse.panda3d.org/t/wall-collision-help/23606
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box'))
np.node().setMass(1.0)
np.node().addShape(shape)
np.setPos(i, j, 2)
np.setCollideMask(BitMask32.allOn()) #(0x0f))
self.world.attachRigidBody(np.node())
self.boxNP = np
#self.colHandler2 = CollisionHandlerQueue()
visualNP = loader.loadModel('models/box.egg')
visualNP.reparentTo(self.boxNP)
#self.cTrav.addCollider(self.boxNP,self.colHandler)
"""
aNode = CollisionNode("TheRay")
self.ray = CollisionRay()
self.ray.setOrigin( self.yugoNP.getPos() )
self.ray.setDirection( Vec3(0, 10, 0) )
#self.ray.show()
aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") )
aNodePath.node().addSolid(self.ray)
aNodePath.show()
"""
#aNode.addSolid(self.ray)
#self.ray = CollisionRay(0,0,0,10,0,0)
#self.ray.reparentTo(self.yugoNP)
#self.rayColl = CollisionNode('PlayerRay')
#self.rayColl.addSolid(self.ray)
#self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl )
#self.playerRayNode.show()
#base.myTraverser.addCollider (self.playerRayNode, base.floor)
#base.floor.addCollider( self.playerRayNode, self.yugoNP)
"""
MyEvent=CollisionHandlerFloor()
MyEvent.setReach(100)
MyEvent.setOffset(15.0)
aNode = CollisionNode("TheRay")
ray = CollisionRay()
ray.setOrigin( self.boxNP.getPos() )
ray.setDirection( Vec3(10, 0, 0) )
aNode.addSolid(ray)
aNodePath = MyModel.attachNewNode( aNode )
Collision = ( aNode, "TheRay" )
Collision[0].setFromCollideMask( BitMask32.bit( 1 ) )
"""
def addWheel(self, pos, front, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.25)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0)
wheel.setRollInfluence(0.1)
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 RonnieRacer(DirectObject):
gameState = 'INIT'
gameLevel = 1
distanceTravelled = 0
speed = 0
score = 0
triesLeft = 3
count = 0
rot = 0
time = 0
pause = False
def __init__(self):
self.imageObject = OnscreenImage(image = 'media/images/splashscreen.png', pos=(0,0,0), scale=(1.4,1,1))
self.loseScreen = OnscreenImage(image = 'media/images/gameover.png', pos=(0,0,0), scale=(1,1,0.8))
self.loseScreen.hide()
self.retryScreen = OnscreenImage(image = 'media/images/retry.png', pos=(0,0,0), scale=(1,1,0.8))
self.retryScreen.hide()
self.congratScreen = OnscreenImage(image = 'media/images/congratulations.png', pos=(0,0,0), scale = (1,1,0.8))
self.congratScreen.hide()
self.winScreen = OnscreenImage(image = 'media/images/victory.png', pos=(0,0,0), scale = (1,1,0.8))
self.winScreen.hide()
self.pauseScreen = OnscreenImage(image = 'media/images/pause.png', pos=(0,0,0), scale = (1,1,0.8))
self.pauseScreen.hide()
self.instructionScreen = OnscreenImage(image = 'media/images/instructions.png', pos=(0,0,0), scale = (1,1,0.8))
self.instructionScreen.hide()
preloader = Preloader()
base.setBackgroundColor(0, 0, 0, 1)
base.setFrameRateMeter(True)
# Audio
self.loseSound = base.loader.loadSfx("media/audio/sfxboo.wav")
self.winSound = base.loader.loadSfx("media/audio/cheer2.aif")
self.menuMusic = base.loader.loadSfx("media/audio/Scattershot.mp3")
self.gameMusic = base.loader.loadSfx("media/audio/Ghostpocalypse - 7 Master.mp3")
self.menuMusic.setLoop(True)
self.menuMusic.setLoopCount(0)
self.gameMusic.setLoop(True)
self.gameMusic.setLoopCount(0)
#setup buttons
self.retryBtn = DirectButton(text="Retry", scale = 0.1, pos = (0,0,0), command = self.doRetry)
self.retryBtn.hide()
self.menuBtn = DirectButton(text="Main Menu", scale = 0.1, pos = (0,0,0), command = self.doMenu)
self.menuBtn.hide()
self.nextBtn = DirectButton(text='Next', scale = 0.1, pos = (0,0,0), command = self.doNext)
self.nextBtn.hide()
self.backBtn = DirectButton(text='back', scale = 0.1, pos = (-0.7,0,-0.7), command = self.doBack)
self.backBtn.hide()
#setup font
self.font = loader.loadFont('media/SHOWG.TTF')
self.font.setPixelsPerUnit(60)
#setup text
self.text = OnscreenText(text = '', pos = (0, 0), scale = 0.07, font = self.font)
self.rpmText = OnscreenText(text = '',
pos = (-0.9, -0.9), scale = 0.07, font = self.font)
self.speedText = OnscreenText(text = '',
pos = (0, -0.9), scale = 0.07, font = self.font)
self.distanceText = OnscreenText(text = '',
pos = (0.9, -0.9), scale = 0.07, font = self.font)
self.triesLeftText = OnscreenText(text = '',
pos = (1.0, 0.9), scale = 0.07, font = self.font)
self.gameLevelText = OnscreenText(text = '',
pos = (-1.0, 0.9), scale = 0.07, font = self.font)
self.timeText = OnscreenText(text = '',
pos = (0, 0.9), scale = 0.07, font = self.font)
self.scoreText = OnscreenText(text = '',
pos = (1.0, 0.8), scale = 0.07, font = self.font)
self.finalScoreText = OnscreenText(text = '',
pos = (0, 0.2), scale = 0.07, font = self.font)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
inputState.watchWithModifiers('forward', 'w')
inputState.watchWithModifiers('left', 'a')
inputState.watchWithModifiers('reverse', 's')
inputState.watchWithModifiers('right', 'd')
inputState.watchWithModifiers('turnLeft', 'a')
inputState.watchWithModifiers('turnRight', 'd')
# Task
taskMgr.add(self.update, 'updateWorld')
# _____HANDLER_____
def doExit(self):
sys.exit(1)
def doReset(self):
self.cleanup()
self.terrain.getRoot().removeNode()
self.setup()
def doBack(self):
self.backBtn.hide()
self.instructionScreen.hide()
self.imageObject.show()
self.helpBtn.show()
self.startBtn.show()
self.exitBtn.show()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
# ____TASK___
def processInput(self, dt):
# Process input
engineForce = 0.0
brakeForce = 0.0
self.accept('p', self.doPause)
if inputState.isSet('forward'):
engineForce = 15.0
brakeForce = 0.0
if inputState.isSet('reverse'):
engineForce = -25.0
brakeForce = 25.0
if inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
# Apply steering to front wheels
self.vehicle.setSteeringValue(self.steering, 0)
self.vehicle.setSteeringValue(self.steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(engineForce, 2)
self.vehicle.applyEngineForce(engineForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
def processContacts(self, dt):
result = self.world.contactTestPair(self.vehicleNP.node(), self.flagNP.node())
if(result.getNumContacts() > 0):
self.gameState = 'WIN'
self.doContinue()
def doContinue(self):
if(self.gameState == 'INIT'):
self.gameState = 'MENU'
self.menuMusic.play()
self.text.hide()
self.startBtn = DirectButton(text=("Play"), scale = 0.1, pos = (0.5,0,0),command=self.playGame)
self.helpBtn = DirectButton(text=("Help"), scale = 0.1, pos = (0.5,0,-0.2),command=self.help)
self.exitBtn = DirectButton(text=("Exit"), scale = 0.1, pos = (0.5,0,-0.4), command = self.doExit)
return
if(self.gameState == 'RETRY'):
self.retryScreen.show()
self.retryBtn.show()
self.loseSound.play()
return
if(self.gameState == 'LOSE'):
self.loseScreen.show()
self.menuBtn.show()
return
if(self.gameState == 'WIN'):
if(self.gameLevel < 3):
self.congratScreen.show()
self.nextBtn.show()
elif(self.gameLevel >= 3):
self.winScreen.show()
self.menuBtn.show()
self.finalScoreText.setText('Your Score: '+str(int(self.score)))
self.finalScoreText.show()
self.winSound.play()
def help(self):
self.gameState = 'HELP'
self.startBtn.hide()
self.exitBtn.hide()
self.helpBtn.hide()
self.imageObject.hide()
self.instructionScreen.show()
self.backBtn.show()
def doNext(self):
self.nextBtn.hide()
self.finalScoreText.hide()
self.congratScreen.hide()
self.gameLevel += 1
if(self.gameLevel == 2):
self.score += 2000
elif(self.gameLevel == 3):
self.score += 3000
self.doReset()
self.triesLeft = 3
self.gameState = 'PLAY'
def doRetry(self):
self.doReset()
self.gameState = 'PLAY'
self.retryScreen.hide()
self.retryBtn.hide()
self.triesLeft -= 1
def doMenu(self):
self.cleanup()
self.terrain.getRoot().removeNode()
self.gameState = 'MENU'
self.score = 0
self.imageObject.show()
self.startBtn.show()
self.exitBtn.show()
self.helpBtn.show()
self.loseScreen.hide()
self.menuBtn.hide()
self.winScreen.hide()
self.finalScoreText.hide()
self.speedText.hide()
self.distanceText.hide()
self.rpmText.hide()
self.scoreText.hide()
self.gameLevelText.hide()
self.timeText.hide()
self.triesLeftText.hide()
self.gameMusic.stop()
self.menuMusic.play()
def doPause(self):
self.pause = not self.pause
if(self.pause):
self.pauseScreen.show()
else:
self.pauseScreen.hide()
def playGame(self):
self.gameState = 'PLAY'
self.triesLeft = 3
self.gameLevel = 1
self.imageObject.hide()
self.startBtn.hide()
self.exitBtn.hide()
self.helpBtn.hide()
self.menuMusic.stop()
self.gameMusic.play()
self.speedText.show()
self.distanceText.show()
self.rpmText.show()
self.scoreText.show()
self.gameLevelText.show()
self.triesLeftText.show()
self.timeText.show()
# Physics
self.setup()
def update(self, task):
dt = globalClock.getDt()
if(not self.pause):
if(self.gameState == 'RETRY'):
return task.cont
if (self.gameState == 'INIT'):
self.accept('space', self.doContinue)
self.text.setText('Press Space to Continue')
if(self.gameState == 'PLAY'):
if (self.steering > 0):
self.steering -= dt * 50
if (self.steering < 0):
self.steering += dt * 50
playerOldSpeed = self.vehicle.getCurrentSpeedKmHour()
self.processInput(dt)
self.processContacts(dt)
self.world.doPhysics(dt, 10, 0.008)
#calculate speed,rpm,distance and display text
self.speed = self.vehicle.getCurrentSpeedKmHour()
if(self.speed<0):
self.speed = -self.speed
self.speedText.setText('Speed: ' + str(int(self.speed)) + 'Km/h')
self.distanceTravelled += self.speed*(dt/3600)
self.distanceText.setText('Distance: '+str(float(int(self.distanceTravelled * 1000))/1000) + 'Km')
playerNewSpeed = self.vehicle.getCurrentSpeedKmHour()
playerAcceleration = (playerNewSpeed - playerOldSpeed) / (dt/60)
#playerPosText = self.vehicleNP.getPos()
#self.text.setText('Player position: %s'%playerPosText)
self.rpmText.setText('Engine RPM: ' + str(int(((self.vehicle.getCurrentSpeedKmHour() / 60) * 1000) / (2 * 0.4 * 3.14159265))) + ' Rpm')
self.triesLeftText.setText('Tries Left: ' + str(self.triesLeft))
self.gameLevelText.setText('Level: '+ str(self.gameLevel))
#update camera
#position
d = self.vehicleNP.getPos() - base.cam.getPos()
if(d.length() > 8):
base.cam.setX(base.cam.getX() + d.getX()*dt)
base.cam.setY(base.cam.getY() + d.getY()*dt)
base.cam.setZ(self.vehicleNP.getZ() + 4)
#lookat
base.cam.lookAt(self.vehicleNP.getPos()+Vec3(0,0,1))
if(self.gameLevel == 1):
if(self.vehicleNP.getZ() < -17):
if(self.triesLeft > 0):
self.gameState = 'RETRY'
else:
self.gameState = 'LOSE'
self.doContinue()
elif(self.gameLevel == 2):
if(self.vehicleNP.getZ() < -20):
if(self.triesLeft > 0):
self.gameState = 'RETRY'
else:
self.gameState = 'LOSE'
self.doContinue()
elif(self.gameLevel == 3):
if(self.vehicleNP.getZ() < -17):
if(self.triesLeft > 0):
self.gameState = 'RETRY'
else:
self.gameState = 'LOSE'
self.doContinue()
if(self.speed < 5):
self.steeringIncrement = 120
elif(self.speed >= 5 and self.speed < 10):
self.steeringIncrement = 100
elif(self.speed >= 10 and self.speed < 15):
self.steeringIncrement = 80
elif(self.speed >=15 and self.speed < 30):
self.steeringIncrement = 60
#spin the flag
self.rot += 1
self.flagNP.setHpr(self.rot,0,0)
#time
self.time += dt
self.timeText.setText('Time: ' + str(int(self.time)))
if(self.score > 0):
self.score -= dt
self.scoreText.setText('Score: '+str(int(self.score)))
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
def setup(self):
# Steering info
self.steering = 0.0 # degree
self.steeringClamp = 30.0 # degree
self.steeringIncrement = 80.0 # degree per second
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
#self.debugNP.show()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
if(self.gameLevel == 1):
#set score
print('GameLevel')
self.score = 1000
self.distanceTravelled = 0
self.time = 0
# Plane
img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png'))
shape = BulletHeightfieldShape(img, 50.0, ZUp)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, 0)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
#skybox
skybox = loader.loadModel('media/models/skybox/skybox_01.X')
skybox.reparentTo(render)
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 1.0))
self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
self.vehicleNP.node().addShape(shape, ts)
self.vehicleNP.setPos(-93, -88, -7)#-93, -88, -7) #(-82,65.8,-8) #(55,8.38,-6)#(45, -19, -8)#(-93, -88, -7)
self.vehicleNP.setHpr(-90,0,0)
self.vehicleNP.node().setMass(5.0)
self.vehicleNP.node().setDeactivationEnabled(False)
base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2)
self.world.attachRigidBody(self.vehicleNP.node())
# Vehicle
self.vehicle = BulletVehicle(self.world, self.vehicleNP.node())
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.hummerNP = loader.loadModel('media/models/vehicle/body.X')
self.hummerNP.reparentTo(self.vehicleNP)
# Right front wheel
np = loader.loadModel('media/models/vehicle/front_right.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.8, 0.9, 0.8), True, np)
# Left front wheel
np = loader.loadModel('media/models/vehicle/front_left.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.8, 0.9, 0.8), True, np)
# Right rear wheel
np = loader.loadModel('media/models/vehicle/back_right.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.8, -0.7, 0.8), False, np)
# Left rear wheel
np = loader.loadModel('media/models/vehicle/back_left.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.8, -0.7, 0.8), False, np)
#Obstacles
self.setupObstacleOne(Vec3(50, -5, -4), 1.8, Vec3(60, 0, 0))
self.setupObstacleFour(Vec3(63.3, 59.2, -10), 1.5, Vec3(0,0,0))
self.setupObstacleFour(Vec3(41, 57, -10), 1.5, Vec3(0,0,0))
self.setupObstacleFour(Vec3(7.5, 53.8, -10), 1.5, Vec3(0,0,0))
self.setupObstacleFour(Vec3(-28, 81.4, -10), 1.5, Vec3(0,0,0))
self.setupObstacleSix(Vec3(-91, 81 , -6), 1, Vec3(60,0,0))
#Goal
self.setupGoal(Vec3(-101,90.6,-6.5))
#self.vehicleNP.setPos(Vec3(6,52,-6))
self.setupTerrain()
elif(self.gameLevel == 2):
self.distanceTravelled = 0
self.time = 0
# Plane
img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png'))
shape = BulletHeightfieldShape(img, 50.0, ZUp)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, 0)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
#skybox
skybox = loader.loadModel('media/models/skybox/skybox_01.X')
skybox.reparentTo(render)
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 1.0))
self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
self.vehicleNP.node().addShape(shape, ts)
self.vehicleNP.setPos(-99.6,105,-11.8)#(88, 21, -11)#(34.3,8.4,-11.8)#(-99.6,105,-11.8)#(86.4,41.2,-12)
self.vehicleNP.setHpr(-130,0,0)
self.vehicleNP.node().setMass(5.0)
self.vehicleNP.node().setDeactivationEnabled(False)
base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2)
self.world.attachRigidBody(self.vehicleNP.node())
# Vehicle
self.vehicle = BulletVehicle(self.world, self.vehicleNP.node())
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.hummerNP = loader.loadModel('media/models/vehicle/body.X')
self.hummerNP.reparentTo(self.vehicleNP)
# Right front wheel
np = loader.loadModel('media/models/vehicle/front_right.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.8, 0.9, 0.8), True, np)
# Left front wheel
np = loader.loadModel('media/models/vehicle/front_left.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.8, 0.9, 0.8), True, np)
# Right rear wheel
np = loader.loadModel('media/models/vehicle/back_right.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.8, -0.7, 0.8), False, np)
# Left rear wheel
np = loader.loadModel('media/models/vehicle/back_left.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.8, -0.7, 0.8), False, np)
self.setupObstacleFive(Vec3(91, 3, -9),1,Vec3(90,0,0))
self.setupObstacleFive(Vec3(94,-19, -10),0.9,Vec3(90,0,0))
self.setupObstacleFive(Vec3(85,-40, -10),1,Vec3(90,0,0))
self.setupObstacleFour(Vec3(-33.5, 23.4,-14.5),1,Vec3(0,0,0))
self.setupObstacleFour(Vec3(-43.3, 24.2,-14.5),1,Vec3(0,0,0))
self.setupObstacleTwo(Vec3(34.7,20.9,-8.5),1,Vec3(90,0,0))
self.setupObstacleTwo(Vec3(26.8,20.3,-8.5),1,Vec3(90,0,0))
self.setupObstacleTwo(Vec3(42.1,22.5,-8.5),1,Vec3(90,0,0))
#self.setupObstacleFive(Vec3(91,0.2, -8),2.1,Vec3(90,0,0))
#Goal
self.setupGoal(Vec3(94,-89.7,-10))
self.setupTerrain()
elif(self.gameLevel == 3):
self.distanceTravelled = 0
self.time = 0
# Plane
img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png'))
shape = BulletHeightfieldShape(img, 50.0, ZUp)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, 0)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
#skybox
skybox = loader.loadModel('media/models/skybox/skybox_01.X')
skybox.reparentTo(render)
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 1.0))
self.vehicleNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
self.vehicleNP.node().addShape(shape, ts)
self.vehicleNP.setPos(-110, -110, 0)
self.vehicleNP.setHpr(-40,0,0)
self.vehicleNP.node().setMass(5.0)
self.vehicleNP.node().setDeactivationEnabled(False)
base.cam.setPos(self.vehicleNP.getPos().getX()+2,self.vehicleNP.getPos().getY()+2,self.vehicleNP.getPos().getZ()+2)
self.world.attachRigidBody(self.vehicleNP.node())
# Vehicle
self.vehicle = BulletVehicle(self.world, self.vehicleNP.node())
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.hummerNP = loader.loadModel('media/models/vehicle/body.X')
self.hummerNP.reparentTo(self.vehicleNP)
# Right front wheel
np = loader.loadModel('media/models/vehicle/front_right.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.8, 0.9, 0.8), True, np)
# Left front wheel
np = loader.loadModel('media/models/vehicle/front_left.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.8, 0.9, 0.8), True, np)
# Right rear wheel
np = loader.loadModel('media/models/vehicle/back_right.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.8, -0.7, 0.8), False, np)
# Left rear wheel
np = loader.loadModel('media/models/vehicle/back_left.X')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.8, -0.7, 0.8), False, np)
self.setupTerrain()
#Goal
self.setupGoal(Vec3(114,100,-13))
#Obstacles
self.setupObstacleFour(Vec3(-60, -73, -9), 1, Vec3(0, 0, 0))
self.setupObstacleFour(Vec3(-63, -77, -9), 1, Vec3(0, 0, 0))
self.setupObstacleTwo(Vec3(-15, -40, -3), 1, Vec3(0, 0, 0))
self.setupObstacleFour(Vec3(-60, 12, -11), 1, Vec3(0, 0, 0))
self.setupObstacleSix(Vec3(-15, 90, -6), 1.5, Vec3(-30, 0, 0))
self.setupObstacleFour(Vec3(28, 87, -11), 1, Vec3(0, 0, 0))
self.setupObstacleFour(Vec3(32, 90, -11), 1, Vec3(0, 0, 0))
def addWheel(self, pos, front, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.4)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0);
wheel.setRollInfluence(0.1)
def setupTerrain(self):
if(self.gameLevel == 1):
#terrain setting
img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_1.png'))
self.terrain = GeoMipTerrain("myTerrain")
self.terrain.setHeightfield(img)
self.terrain.getRoot().setSz(50)
self.terrain.setBlockSize(4)
#self.terrain.setFactor(10)
#self.terrain.setMinLevel(0)
self.terrain.setNear(50)
self.terrain.setFar(1000)
self.terrain.setFocalPoint(base.camera)
self.terrain.getRoot().reparentTo(render)
offset = img.getXSize() / 2.0 - 0.5
self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0)
self.terrain.generate()
#load textures
tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_d.png")
tex0.setMinfilter(Texture.FTLinearMipmapLinear)
tex1 = loader.loadTexture("media/terrain/longGrass.png")
tex1.setMinfilter(Texture.FTLinearMipmapLinear)
tex2 = loader.loadTexture("media/terrain/bigRockFace.png")
tex2.setMinfilter(Texture.FTLinearMipmapLinear)
tex3 = loader.loadTexture("media/terrain/greenrough.png")
tex3.setMinfilter(Texture.FTLinearMipmapLinear)
tex4 = loader.loadTexture("media/terrain/grayRock.png")
tex4.setMinfilter(Texture.FTLinearMipmapLinear)
tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_c.png")
tex5.setMinfilter(Texture.FTLinearMipmapLinear)
tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_1_l.png")
tex6.setMinfilter(Texture.FTLinearMipmapLinear)
#set mutiltextures
self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 )
self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 )
self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 )
self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 )
self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 )
self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 )
self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 )
#load shader
self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha'))
elif(self.gameLevel == 2):
#terrain setting
img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_2.png'))
self.terrain = GeoMipTerrain("myTerrain")
self.terrain.setHeightfield(img)
self.terrain.getRoot().setSz(50)
self.terrain.setBlockSize(4)
#self.terrain.setFactor(10)
#self.terrain.setMinLevel(0)
self.terrain.setNear(50)
self.terrain.setFar(100)
self.terrain.setFocalPoint(base.camera)
self.terrain.getRoot().reparentTo(render)
offset = img.getXSize() / 2.0 - 0.5
self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0)
self.terrain.generate()
#load textures
tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_d.png")
tex0.setMinfilter(Texture.FTLinearMipmapLinear)
tex1 = loader.loadTexture("media/terrain/sandripple.png")
tex1.setMinfilter(Texture.FTLinearMipmapLinear)
tex2 = loader.loadTexture("media/terrain/orangesand.png")
tex2.setMinfilter(Texture.FTLinearMipmapLinear)
tex3 = loader.loadTexture("media/terrain/grayRock.png")
tex3.setMinfilter(Texture.FTLinearMipmapLinear)
tex4 = loader.loadTexture("media/terrain/bigRockFace.png")
tex4.setMinfilter(Texture.FTLinearMipmapLinear)
tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_c.png")
tex5.setMinfilter(Texture.FTLinearMipmapLinear)
tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_2_l.png")
tex6.setMinfilter(Texture.FTLinearMipmapLinear)
#set mutiltextures
self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 )
self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 )
self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 )
self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 )
self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 )
self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 )
self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 )
#load shader
self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha'))
elif(self.gameLevel == 3):
#terrain setting
img = PNMImage(Filename('media/terrain/SIMP_Assignment_2_Terrain_3.png'))
self.terrain = GeoMipTerrain("myTerrain")
self.terrain.setHeightfield(img)
self.terrain.getRoot().setSz(50)
self.terrain.setBlockSize(4)
#self.terrain.setFactor(10)
#self.terrain.setMinLevel(0)
self.terrain.setNear(50)
self.terrain.setFar(100)
self.terrain.setFocalPoint(base.camera)
self.terrain.getRoot().reparentTo(render)
offset = img.getXSize() / 2.0 - 0.5
self.terrain.getRoot().setPos(-offset, -offset, -50 / 2.0)
self.terrain.generate()
#load textures
tex0 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_d.png")
tex0.setMinfilter(Texture.FTLinearMipmapLinear)
tex1 = loader.loadTexture("media/terrain/hardDirt.png")
tex1.setMinfilter(Texture.FTLinearMipmapLinear)
tex2 = loader.loadTexture("media/terrain/littlerocks.png")
tex2.setMinfilter(Texture.FTLinearMipmapLinear)
tex3 = loader.loadTexture("media/terrain/greenrough.png")
tex3.setMinfilter(Texture.FTLinearMipmapLinear)
tex4 = loader.loadTexture("media/terrain/bigRockFace.png")
tex4.setMinfilter(Texture.FTLinearMipmapLinear)
tex5 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_c.png")
tex5.setMinfilter(Texture.FTLinearMipmapLinear)
tex6 = loader.loadTexture("media/terrain/SIMP_Assignment_2_Terrain_3_l.png")
tex6.setMinfilter(Texture.FTLinearMipmapLinear)
#set mutiltextures
self.terrain.getRoot().setTexture( TextureStage('tex0'),tex0 )
self.terrain.getRoot().setTexture( TextureStage('tex1'),tex1 )
self.terrain.getRoot().setTexture( TextureStage('tex2'),tex2 )
self.terrain.getRoot().setTexture( TextureStage('tex3'),tex3 )
self.terrain.getRoot().setTexture( TextureStage('tex4'),tex4 )
self.terrain.getRoot().setTexture( TextureStage('tex5'),tex5 )
self.terrain.getRoot().setTexture( TextureStage('tex6'),tex6 )
#load shader
self.terrain.getRoot().setShader(loader.loadShader('terraintexture.sha'))
def setupObstacleOne(self, pos, scale, turn):
#box A
shape = BulletBoxShape(Vec3(3, 0.1, 0.1) * scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(3, 0.1, 0.1)*2 * scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box C
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.9)*scale)
bodyC = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearDamping(0.5)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 0.9)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
pivotA = Point3(0, 0, -0.1 * scale)
pivotB = Point3(0, 0, 1 * scale)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
# Box B
shape = BulletBoxShape(Vec3(3, 2, 0.1)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearDamping(0.5)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn);
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(3, 2, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(0, 0, 0)
pivotB = Point3(0, 0, -1 * scale)
hinge = BulletHingeConstraint(bodyB, bodyC, pivotA, pivotB, axisA, axisB, True)
hinge.setLimit(0,360, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
def setupObstacleTwo(self,pos,scale,turn):
#box A
shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(100.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 1, 1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(2, 0, 0)
pivotB = Point3(0, 0, 2)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
# Box C
shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale)
bodyC = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(100.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 1, 1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
pivotA = Point3(-2, 0, 0)
pivotB = Point3(0, 0, 2)
hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True)
self.world.attachConstraint(hinge)
def setupObstacleThree(self, pos, scale, turn):
# Box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1))
bodyA = BulletRigidBodyNode('Box A')
bodyA.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
#Box B
shape = BulletBoxShape(Vec3(0.1,0.1,0.1))
bodyB = BulletRigidBodyNode('Box B')
bodyB.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1,0.1,0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Slider
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(0)
slider.setUpperLinearLimit(12)
slider.setLowerAngularLimit(-90)
slider.setUpperAngularLimit(-85)
self.world.attachConstraint(slider)
# Box C
shape = BulletBoxShape(Vec3(1, 3, 0.1))
bodyC = BulletRigidBodyNode('Box C')
bodyC.setRestitution(1.0)
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(0.1)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(Vec3(pos.getX() + 3, pos.getY() - 4, pos.getZ()))
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(1, 3, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
bodyNP.node().setLinearVelocity(-100)
# Slider
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 0))
slider = BulletSliderConstraint(bodyA, bodyC, frameA, frameB, True)
slider.setDebugDrawSize(2.0)
slider.setLowerLinearLimit(2)
slider.setUpperLinearLimit(6)
slider.setLowerAngularLimit(-90)
slider.setUpperAngularLimit(-85)
self.world.attachConstraint(slider)
def setupObstacleFour(self, pos, scale, turn):
#Start Here
# Box A
shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletSphereShape(0.5*scale)
bodyB = BulletRigidBodyNode('Sphere B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(10.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001)
visNP = loader.loadModel('media/models/ball.egg')
visNP.clearModelNodes()
visNP.setScale(1.25*scale)
visNP.reparentTo(bodyNP)
bodyNP.node().setLinearVelocity(100)
self.world.attachRigidBody(bodyB)
# Cone
frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90))
frameB = TransformState.makePosHpr(Point3(2, 0, 0)*scale, Vec3(0, 0, 0))
cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
cone.setDebugDrawSize(2.0)
cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0)
self.world.attachConstraint(cone)
# Box C
shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale)
bodyC = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3)
self.world.attachRigidBody(bodyC)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
def setupObstacleSix(self, pos, scale, turn):
#box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOff())
bodyNP.setPos(pos.getX()-2,pos.getY(),pos.getZ()+2.5)#-2,0,2.5)
bodyNP.setHpr(turn)
# Box B
shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearDamping(0.5)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX()-3,pos.getY(), pos.getZ())#, 0, 0)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(2, 0.1, 3)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(-2, 0, -3)
pivotB = Point3(-2, 0, -3)
axisA = Vec3(0, 0, 1)
axisB = Vec3(0, 0, 1)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(0,90, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
#box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOff())
bodyNP.setPos(pos.getX()+2,pos.getY(),pos.getZ()+2.5)#2,0,2.5)
bodyNP.setHpr(turn)
# Box B
shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearDamping(0.5)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX()+4, pos.getY(), pos.getZ())# 0, 0)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(2, 0.1, 3)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
pivotA = Point3(2, 0, -3)
pivotB = Point3(2, 0, -3)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setLimit(-90,0, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
def setupObstacleFive(self, pos, scale, turn):
#box A
shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(3, 2, 0.1)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(3, 2, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(0, 0, 0)
pivotB = Point3(0, 0, 5)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-50,50, softness=0.5, bias=0.3, relaxation=0.6)
self.world.attachConstraint(hinge)
# Box C
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.9)*scale)
bodyC = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 0.1, 0.9)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
pivotA = Point3(0, 0, -1.1)
pivotB = Point3(0, 0, 1)
hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True)
hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
def setupGoal(self, pos):
# Goal
shape = BulletBoxShape(Vec3(1, 1, 1))
body = BulletRigidBodyNode('Flag')
self.flagNP = self.worldNP.attachNewNode(body)
self.flagNP.node().addShape(shape)
self.flagNP.setCollideMask(BitMask32.allOn())
self.flagNP.setPos(pos)
visNP = loader.loadModel('media/models/Flag.X')
visNP.clearModelNodes()
visNP.reparentTo(self.flagNP)
self.world.attachRigidBody(body)
class Game(DirectObject):
def __init__(self):
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
base.setFrameRateMeter(True)
base.cam.setPos(0, -20, 4)
base.cam.lookAt(0, 0, 0)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
inputState.watchWithModifiers('forward', 'w')
inputState.watchWithModifiers('left', 'a')
inputState.watchWithModifiers('reverse', 's')
inputState.watchWithModifiers('right', 'd')
inputState.watchWithModifiers('turnLeft', 'q')
inputState.watchWithModifiers('turnRight', 'e')
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def doReset(self):
self.cleanup()
self.setup()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
# ____TASK___
def processInput(self, dt):
engineForce = 0.0
brakeForce = 0.0
if inputState.isSet('forward'):
engineForce = 1000.0
brakeForce = 0.0
if inputState.isSet('reverse'):
engineForce = 0.0
brakeForce = 100.0
if inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
# Apply steering to front wheels
self.vehicle.setSteeringValue(self.steering, 0);
self.vehicle.setSteeringValue(self.steering, 1);
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(engineForce, 2);
self.vehicle.applyEngineForce(engineForce, 3);
self.vehicle.setBrake(brakeForce, 2);
self.vehicle.setBrake(brakeForce, 3);
def update(self, task):
dt = globalClock.getDt()
self.processInput(dt)
self.world.doPhysics(dt, 10, 0.008)
#print self.vehicle.getWheel(0).getRaycastInfo().isInContact()
#print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs()
#print self.vehicle.getChassis().isKinematic()
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
def setup(self):
self.worldNP = render.attachNewNode('World')
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, -1)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
np.node().addShape(shape, ts)
np.setPos(0, 0, 1)
np.node().setMass(800.0)
np.node().setDeactivationEnabled(False)
self.world.attachRigidBody(np.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(self.world, np.node())
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.reparentTo(np)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3( 0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
# Steering info
self.steering = 0.0 # degree
self.steeringClamp = 45.0 # degree
self.steeringIncrement = 120.0 # degree per second
def addWheel(self, pos, front, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.25)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0);
wheel.setRollInfluence(0.1)
class Game(DirectObject):
def __init__(self, model):
self.model = model
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
base.setFrameRateMeter(True)
base.cam.setPos(0, -20, 4)
base.cam.lookAt(0, 0, 0)
w = WindowProperties()
w.setFullscreen(False)
w.setOrigin(5, 20)
w.setSize(865, 800)
base.win.requestProperties(w)
# Light
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
dlight = DirectionalLight('directionalLight')
dlight.setDirection(Vec3(1, 1, -1))
dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
dlightNP = render.attachNewNode(dlight)
render.clearLight()
render.setLight(alightNP)
render.setLight(dlightNP)
# Input
self.accept('escape', self.doExit)
self.accept('r', self.doReset)
self.accept('f1', self.toggleWireframe)
self.accept('f2', self.toggleTexture)
self.accept('f3', self.toggleDebug)
self.accept('f5', self.doScreenshot)
inputState.watchWithModifiers('forward', 'w')
inputState.watchWithModifiers('left', 'a')
inputState.watchWithModifiers('reverse', 's')
inputState.watchWithModifiers('right', 'd')
inputState.watchWithModifiers('turnLeft', 'q')
inputState.watchWithModifiers('turnRight', 'e')
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def endLoop(self):
self.penalized_distance = self.distance * (numpy.exp(
-self.time_max_steering / self.total_time))
pickle.dump(self.penalized_distance, open('distance.p', 'wb'))
sys.exit()
#quit()
#print("Distance was: ",self.distance)
#os.execv(sys.executable,['python']+[__file__])
#taskMgr.running = False
def doReset(self):
self.cleanup()
self.setup()
def toggleWireframe(self):
base.toggleWireframe()
def toggleTexture(self):
base.toggleTexture()
def toggleDebug(self):
if self.debugNP.isHidden():
self.debugNP.show()
else:
self.debugNP.hide()
def doScreenshot(self):
base.screenshot('Bullet')
# ____TASK___
def calculate_moves(self):
self.y = self.model.predict(self.x)
#print(self.y)
self.moves = self.y > 0 #+ self.model_offset # 0.5
#self.moves[0] = True # FIXME test
def processInput(self, dt):
engineForce = 0.0
brakeForce = 0.0
if self.moves[
0]: #inputState.isSet('forward'): FIXME maybe engine and brake can be linked to self.y, rectified, continuous
engineForce = 2000.0 # 1000.
brakeForce = 0.0
if not self.moves[0]: #inputState.isSet('reverse'):
engineForce = 200.0 #0.0
brakeForce = 100.0
self.steering = self.y[1]
if self.moves[1]:
self.steering = min(self.steering, self.steeringClamp)
if not self.moves[1]:
self.steering = max(self.steering, -self.steeringClamp)
""" # FIXME option may be better if self.steering is fed into self.y[3] for 4th element
if not self.moves[2]: # enabled steering lock
if self.moves[1]:#inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if not self.moves[1]:#inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
""" """
if inputState.isSet('forward'):
engineForce = 1000.0
brakeForce = 0.0
if inputState.isSet('reverse'):
engineForce = 0.0
brakeForce = 100.0
if inputState.isSet('turnLeft'):
self.steering += dt * self.steeringIncrement
self.steering = min(self.steering, self.steeringClamp)
if inputState.isSet('turnRight'):
self.steering -= dt * self.steeringIncrement
self.steering = max(self.steering, -self.steeringClamp)
"""
# Apply steering to front wheels
self.vehicle.setSteeringValue(self.steering, 0)
self.vehicle.setSteeringValue(self.steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(engineForce, 2)
self.vehicle.applyEngineForce(engineForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
def check_collisions(self):
"""pFrom = render.getRelativePoint(self.yugoNP,Point3(0,0,0))#Point3(0,0,0)
pFrom -= Point3(0,0,pFrom[2])
pRel = render.getRelativePoint(base.cam,self.yugoNP.getPos()) # FIXME THIS IS IT!! get rid of z component
pRel -= Point3(0,0,pRel[2])
p45 = Point3(pRel[0] - pRel[1], pRel[1] + pRel[0],0)
pn45 = Point3(pRel[0] + pRel[1], pRel[1] - pRel[0],0)
#print(render.getRelativePoint(self.yugoNP,Point3(0,0,0)))
#print(dir(self.yugoNP))
pTo = [pFrom + pn45, pFrom + pRel, pFrom + p45]#[pFrom + Vec3(-10,10,0)*999,pFrom + Vec3(0,10,0)*999,pFrom + Vec3(10,10,0)*999]# FIXME should be relative to front of car, getting cloe! #self.yugoNP.getPosDelta()*99999]#[Point3(-10,10,0) * 99999,Point3(0,10,0) * 99999,Point3(10,10,0) * 99999]
#self.ray = CollisionRay(0,0,0,100,0,0)
result = [self.world.rayTestClosest(pFrom,pt) for pt in pTo]
#print(dir(self.yugoNP))
#print(result.getHitPos())
return tuple([res.getHitPos().length() for res in result])
"""#queue = CollisionHandlerQueue()
#traverser.addCollider(fromObject, queue)
#traverser.traverse(render)
#queue.sortEntries()
#for entry in queue.getEntries():
#print(entry)
#print(result.getHitPos())
#if result.getNode() != None:
#print(self.yugoNP.getPos(result.getNode()))
#print(self.cTrav)
self.cTrav.traverse(render)
entries = list(self.colHandler.getEntries())
#print(entries)
entries.sort(key=lambda y: y.getSurfacePoint(render).getY())
#for entry in entries: print(entry.getFromNodePath().getName())
if entries: # and len(result) > 1:
#print(entries)
for r in entries:
#print(r.getIntoNodePath().getName())
if r.getIntoNodePath().getName(
) == 'Plane' and r.getFromNodePath().getName() == 'yugo_box':
self.endLoop()
if r.getIntoNodePath().getName(
) == 'Plane' and r.getFromNodePath().getName() in [
'ray%d' % i for i in range(self.n_rays)
]: #Box
self.ray_col_vec_dict[
r.getFromNodePath().getName()].append(
numpy.linalg.norm(
list(r.getSurfacePoint(
r.getFromNodePath()))[:-1]))
self.ray_col_vec_dict = {
k: (min(self.ray_col_vec_dict[k])
if len(self.ray_col_vec_dict[k]) >= 1 else 10000)
for k in self.ray_col_vec_dict
}
self.x = numpy.array(list(self.ray_col_vec_dict.values()))
#print(self.x)
result = self.world.contactTest(self.yugoNP.node())
#print(result.getNumContacts())
#print(dir(self.yugoNP))
#return entries
def check_prevPos(self):
if len(self.prevPos) > 80:
#print(self.prevPos)
#print(numpy.linalg.norm(self.prevPos[-1] - self.prevPos[0]))
if numpy.linalg.norm(self.prevPos[-1] - self.prevPos[0]) < 4.5:
#print("ERROR")
self.endLoop()
del self.prevPos[0:len(self.prevPos) - 80]
def update(self, task):
self.prevPos.append(self.yugoNP.getPos(render))
dx = numpy.linalg.norm(self.prevPos[-1] - self.prevPos[-2])
self.distance += dx
self.distance_text.setText('Distance=%.3f' % (self.distance))
#print(len(self.prevPos))
dt = globalClock.getDt()
self.total_time += dt
if abs(self.steering) == abs(self.steeringClamp):
self.time_max_steering += dt
self.time_text.setText('TotalTime=%.3f' % (self.total_time))
#self.time_maxsteer_text.setText('TotalTimeMaxSteer=%f'%(self.time_max_steering))
#self.penalized_distance = self.distance*(1.-numpy.exp(-self.time_max_steering/self.total_time))
if self.distance > 10000:
self.endLoop()
self.check_prevPos()
self.speed = dx / dt
self.speed_text.setText('Speed=%.3f' % (self.speed))
self.check_collisions()
self.calculate_moves()
self.model.plot_NN()
#self.nn_image.setImage('neural_net_vis.png')
self.ray_col_vec_dict = {k: [] for k in self.ray_col_vec_dict}
self.processInput(dt)
self.world.doPhysics(dt, 10, 0.008)
# FIXME KEEP TRACK OF TOTAL DEGREES TURNED AND PENALIZE
#self.doReset()
#print(dir(result[1]))
#print(numpy.linalg.norm(list(result[1].getSurfacePoint(result[1].getFromNodePath()))[:-1]))
#base.camera.setPos(0,-40,10)
#print self.vehicle.getWheel(0).getRaycastInfo().isInContact()
#print self.vehicle.getWheel(0).getRaycastInfo().getContactPointWs()
#print self.vehicle.getChassis().isKinematic()
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
def setup(self):
self.worldNP = render.attachNewNode('World')
self.distance_text = OnscreenText(
text='Distance=0', pos=(0.75, 0.85), scale=0.08,
mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0))
self.speed_text = OnscreenText(
text='Speed=0', pos=(0.75, 0.78), scale=0.08,
mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0))
self.time_text = OnscreenText(
text='TotalTime=0', pos=(0.75, 0.71), scale=0.08,
mayChange=1) #Directxxxxxx(distance='Distance=%d'%(0))
#self.time_maxsteer_text = OnscreenText(text='TotalTimeMaxSteer=0', pos = (0.85,0.70), scale = 0.05, mayChange=1)#Directxxxxxx(distance='Distance=%d'%(0))
#self.nn_image = OnscreenImage(image='blank.png', pos= (0.85,0,0.15), scale=0.45) # http://dev-wiki.gestureworks.com/index.php/GestureWorksCore:Python_%26_Panda3D:_Getting_Started_II_(Hello_Multitouch)#8._Create_a_method_to_draw_touchpoint_data
self.total_time = 0.
self.time_max_steering = 0.
# World
self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
self.debugNP.show()
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
#terrain = GeoMipTerrain("mySimpleTerrain")
#terrain.setHeightfield("./models/heightfield_2.png")
#terrain.getRoot().reparentTo(self.worldNP)#render)
#terrain.generate()
# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
np.node().addShape(shape)
np.setPos(0, 0, -1)
np.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(np.node())
#np = self.worldNP.attachNewNode(BulletRigidBodyNode('Track'))
#np.node().setMass(5000.0)
#np.setPos(3, 0, 10)
#np.setCollideMask(BitMask32.allOn())#(0x0f))
#self.track = BulletVehicle(self.world, np.node())
#self.track.setCoordinateSystem(ZUp)
self.track_np = loader.loadModel(
'models/race_track_2.egg'
) # https://discourse.panda3d.org/t/panda3d-and-bullet-physics/15724/10
self.track_np.setPos(-72, -7, -3.5)
self.track_np.setScale(10)
self.track_np.reparentTo(render)
self.track_np.setCollideMask(BitMask32.allOn()) #(0))#.allOn())
self.world.attachRigidBody(np.node())
self.track_np = np
#self.track_np.show()
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Vehicle'))
np.node().addShape(shape, ts)
np.setPos(0, 0, 0.05)
np.node().setMass(800.0)
np.node().setDeactivationEnabled(False)
self.world.attachRigidBody(np.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
self.cTrav = CollisionTraverser()
# Vehicle
self.vehicle = BulletVehicle(self.world, np.node())
self.vehicle.setCoordinateSystem(ZUp)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.setCollideMask(BitMask32(0)) #.allOn())
self.yugoNP.reparentTo(np)
self.colHandler = CollisionHandlerQueue()
# travel distance
self.distance = 0.
"""self.sphere = CollisionSphere(0,0,0,2)
self.sphere_col = CollisionNode('yugo')
self.sphere_col.addSolid(self.sphere)
self.sphere_col.setFromCollideMask(BitMask32.allOn())
self.sphere_col_np = self.yugoNP.attachNewNode(self.sphere_col)
self.cTrav.addCollider(self.sphere_col_np,self.colHandler)
self.sphere_col_np.show()"""
self.yugo_col = CollisionNode('yugo_box')
self.yugo_col.addSolid(CollisionBox(Point3(0, 0, 0.7), 0.9, 1.6, 0.05))
self.yugo_col.setFromCollideMask(BitMask32(1))
self.box_col_np = self.yugoNP.attachNewNode(self.yugo_col)
self.cTrav.addCollider(self.box_col_np, self.colHandler)
self.box_col_np.show()
self.ray_col_np = {}
self.ray_col_vec_dict = {}
self.n_rays = self.model.shape[0]
for i, ray_dir in enumerate(
numpy.linspace(-numpy.pi / 4, numpy.pi / 4,
self.n_rays)): # populate collision rays
#print(ray_dir)
self.ray = CollisionRay()
y_dir, x_dir = numpy.cos(ray_dir), numpy.sin(ray_dir)
self.ray.setOrigin(1.3 * x_dir, 1.3 * y_dir, 0.5)
self.ray.setDirection(x_dir, y_dir, 0)
self.ray_col = CollisionNode('ray%d' % (i))
self.ray_col.addSolid(self.ray)
self.ray_col.setFromCollideMask(
BitMask32.allOn()) #(0x0f))#CollideMask.bit(0)
#self.ray_col.setIntoCollideMask(CollideMask.allOff())
self.ray_col_np['ray%d' % (i)] = self.yugoNP.attachNewNode(
self.ray_col)
self.cTrav.addCollider(self.ray_col_np['ray%d' % (i)],
self.colHandler)
self.ray_col_np['ray%d' % (i)].show()
self.ray_col_vec_dict['ray%d' % (i)] = []
self.world.attachVehicle(self.vehicle)
self.cTrav.showCollisions(render)
# FIXME
base.camera.reparentTo(self.yugoNP)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(self.worldNP)
self.addWheel(Point3(-0.70, -1.05, 0.3), False, np)
# Steering info
self.steering = 0.0 # degree
self.steeringClamp = 38.0 #45.0 # degree
self.steeringIncrement = 105.0 #120.0 # degree per second
# add previous positions
self.prevPos = []
self.prevPos.append(self.yugoNP.getPos(render))
self.model_offset = 0.5 if self.model.activation == 'relu' else 0.
# Box
"""
for i,j in [(0,8),(-3,5),(6,-5),(8,3),(-4,-4),(0,0)]:
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
# https://discourse.panda3d.org/t/wall-collision-help/23606
np = self.worldNP.attachNewNode(BulletRigidBodyNode('Box'))
np.node().setMass(1.0)
np.node().addShape(shape)
np.setPos(i, j, 2)
np.setCollideMask(BitMask32.allOn())#(0x0f))
self.world.attachRigidBody(np.node())
self.boxNP = np
#self.colHandler2 = CollisionHandlerQueue()
visualNP = loader.loadModel('models/box.egg')
visualNP.reparentTo(self.boxNP)
#self.cTrav.addCollider(self.boxNP,self.colHandler)
"""
"""
aNode = CollisionNode("TheRay")
self.ray = CollisionRay()
self.ray.setOrigin( self.yugoNP.getPos() )
self.ray.setDirection( Vec3(0, 10, 0) )
#self.ray.show()
aNodePath = self.yugoNP.attachNewNode( CollisionNode("TheRay") )
aNodePath.node().addSolid(self.ray)
aNodePath.show()
"""
#aNode.addSolid(self.ray)
#self.ray = CollisionRay(0,0,0,10,0,0)
#self.ray.reparentTo(self.yugoNP)
#self.rayColl = CollisionNode('PlayerRay')
#self.rayColl.addSolid(self.ray)
#self.playerRayNode = self.yugoNP.attachNewNode( self.rayColl )
#self.playerRayNode.show()
#base.myTraverser.addCollider (self.playerRayNode, base.floor)
#base.floor.addCollider( self.playerRayNode, self.yugoNP)
"""
MyEvent=CollisionHandlerFloor()
MyEvent.setReach(100)
MyEvent.setOffset(15.0)
aNode = CollisionNode("TheRay")
ray = CollisionRay()
ray.setOrigin( self.boxNP.getPos() )
ray.setDirection( Vec3(10, 0, 0) )
aNode.addSolid(ray)
aNodePath = MyModel.attachNewNode( aNode )
Collision = ( aNode, "TheRay" )
Collision[0].setFromCollideMask( BitMask32.bit( 1 ) )
"""
def addWheel(self, pos, front, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.25)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0)
wheel.setRollInfluence(0.1)
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
def __init__(self):
ShowBase.__init__(self)
scene = BulletWorld()
scene.setGravity(Vec3(0, 0, -9.81))
base.setBackgroundColor(0.6, 0.9, 0.9)
#Variables
self.steering = 0
#Controls
inputState.watchWithModifiers("F", "arrow_up")
inputState.watchWithModifiers("B", "arrow_down")
inputState.watchWithModifiers("L", "arrow_left")
inputState.watchWithModifiers("R", "arrow_right")
#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.track_model = loader.loadModel("Models/Track.egg")
self.track_model.reparentTo(self.render)
self.track_model.setPos(0, 0, -7)
self.track_tex = loader.loadTexture("Textures/Road.jpg")
self.track_model.setTexture(self.track_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, 0)
Car_np.setHpr(0, 0, 0)
Car_np.node().setDeactivationEnabled(False)
scene.attachRigidBody(Car_node)
#Load and transform the Car Actor
self.car_model = loader.loadModel("Models/Car.egg")
self.car_model.setPos(0, 20, -3)
self.car_model.setHpr(180, 0, 0)
self.car_model.reparentTo(Car_np)
self.Car_sim = BulletVehicle(scene, Car_np.node())
self.Car_sim.setCoordinateSystem(ZUp)
scene.attachVehicle(self.Car_sim)
#Camera
#base.disableMouse()
camera.reparentTo(Car_np)
camera.setPos(0, 0, 0)
camera.setHpr(0, 0, 0)
def Wheel(pos, r, f):
w = self.Car_sim.createWheel()
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
Wheel(Point3(-1, 1, -0.6), 0.4, False)
Wheel(Point3(-1.1, -1.2, -0.6), 0.4, True)
Wheel(Point3(1.1, -1, -0.6), 0.4, True)
Wheel(Point3(1, 1, -0.6), 0.4, False)
def ProcessInput(dt):
engineForce = 0.0
self.steeringClamp = 35.0
self.steeringIncrement = 70
#Get the vehicle's current speed
self.carspeed = self.Car_sim.getCurrentSpeedKmHour()
#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
if inputState.isSet("F"):
engineForce = 35
if inputState.isSet("B"):
engineForce = -35
#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)
#Apply forces to wheels
self.Car_sim.applyEngineForce(engineForce, 0)
self.Car_sim.applyEngineForce(engineForce, 3)
def Update(task):
dt = globalClock.getDt()
ProcessInput(dt)
scene.doPhysics(dt, 5, 1.0 / 180.0)
return task.cont
taskMgr.add(Update, "Update")
