def __set_vehicle(self):
self.vehicle = BulletVehicle(PhysMgr().root, self.pnode)
self.vehicle.setCoordinateSystem(ZUp)
self.vehicle.setPitchControl(self.pitch_control)
tuning = self.vehicle.getTuning()
tuning.setSuspensionCompression(self.suspension_compression)
tuning.setSuspensionDamping(self.suspension_damping)
PhysMgr().attach_vehicle(self.vehicle)
def setupVehicle(self, bulletWorld):
# Chassis
shape = BulletBoxShape(Vec3(1, 2.2, 0.5))
ts = TransformState.makePos(Point3(0, 0, .7))
if self.isMainChar:
self.chassisNode = BulletRigidBodyNode(self.username)
else:
self.chassisNode = BulletRigidBodyNode('vehicle')
self.chassisNode.setTag("username", str(self.username))
self.chassisNP = render.attachNewNode(self.chassisNode)
self.chassisNP.node().addShape(shape, ts)
if self.isMainChar:
self.chassisNP.node().notifyCollisions(True)
else:
self.chassisNP.node().notifyCollisions(False)
self.setVehiclePos(self.pos[0], self.pos[1], self.pos[2], self.pos[3],
self.pos[4], self.pos[5])
# self.chassisNP.setPos(-5.34744, 114.773, 6)
#self.chassisNP.setPos(49.2167, 64.7968, 10)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
self.carNP = loader.loadModel('models/batmobile-chassis.egg')
#self.yugoNP.setScale(.7)
self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, 1.1, .7), True, np)
# Left front wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, 1.1, .7), True, np)
# Right rear wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, -2, .7), False, np)
# Left rear wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, -2, .7), False, np)
def __init__(self, tempworld, bulletWorld, type, playerId):
self.world = tempworld
self.speed = 0
self.acceleration = 1.5
self.brakes = .7
self.min_speed = 0
self.max_speed = 150
self.reverse_speed = 20
self.reverse_limit = -40
self.armor = 100
self.health = 100
self.a_timer_start = time.time()
self.a_timer_end = time.time()
self.power_ups = [0, 0, 0]
self.playerId = playerId
if type == 0:
self.actor = Actor("models/batmobile-chassis")
self.actor.setScale(0.7)
carRadius = 3
elif type == 1:
self.actor = Actor("models/policecarpainted", {})
self.actor.setScale(0.30)
self.actor.setH(180) # elif type == 2:
# self.actor = loader.loadModel("knucklehead.egg")
# self.tex = loader.loadTexture("knucklehead.jpg")
# self.actor.setTexture(self.car_tex, 1)
shape = BulletBoxShape(Vec3(1.0, 1.5, 0.4))
ts = TransformState.makePos(Point3(0, 0, 0.6))
self.chassisNP = render.attachNewNode(BulletRigidBodyNode('Vehicle'))
self.chassisNP.node().addShape(shape, ts)
self.chassisNP.setPos(50 * random.random(), 50 * random.random(), 1)
self.chassisNP.setH(180)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
self.actor.reparentTo(self.chassisNP)
self.actor.setH(180)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
for fb, y in (("F", 1.1), ("B", -1.1)):
for side, x in (("R", 0.75), ("L", -0.75)):
np = loader.loadModel("models/tire%s.egg" % side)
np.reparentTo(render)
isFront = fb == "F"
self.addWheel(Point3(x, y, 0.55), isFront, np)
def __set_vehicle(self):
self.vehicle = BulletVehicle(self.eng.phys_mgr.root._wld, self.pnode)
# access to a protected member
self.vehicle.set_coordinate_system(ZUp)
self.vehicle.set_pitch_control(self.pitch_control)
tuning = self.vehicle.get_tuning()
tuning.set_suspension_compression(self.suspension_compression)
# default .83
tuning.set_suspension_damping(self.suspension_damping)
# default .88
self.eng.phys_mgr.attach_vehicle(self.vehicle)
def _add_chassis(self, pg_physics_world: PGPhysicsWorld):
para = self.get_config()
self.LENGTH = self.vehicle_config["vehicle_length"]
self.WIDTH = self.vehicle_config["vehicle_width"]
chassis = BaseVehicleNode(BodyName.Base_vehicle, self)
chassis.setIntoCollideMask(BitMask32.bit(CollisionGroup.EgoVehicle))
chassis_shape = BulletBoxShape(
Vec3(self.WIDTH / 2, self.LENGTH / 2,
para[Parameter.vehicle_height] / 2))
ts = TransformState.makePos(
Vec3(0, 0, para[Parameter.chassis_height] * 2))
chassis.addShape(chassis_shape, ts)
heading = np.deg2rad(-para[Parameter.heading] - 90)
chassis.setMass(para[Parameter.mass])
self.chassis_np = self.node_path.attachNewNode(chassis)
# not random spawn now
self.chassis_np.setPos(Vec3(*self.spawn_place, 1))
self.chassis_np.setQuat(
LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(
True
) # advance collision check, do callback in pg_collision_callback
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Base_vehicle_beneath)
chassis_beneath.setIntoCollideMask(
BitMask32.bit(CollisionGroup.EgoVehicleBeneath))
chassis_beneath.addShape(chassis_shape)
self.chassis_beneath_np = self.chassis_np.attachNewNode(
chassis_beneath)
self.dynamic_nodes.append(chassis_beneath)
self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis)
self.system.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(
self.system
) # detach chassis will also detach system, so a waring will generate
if self.render:
if self.MODEL is None:
model_path = 'models/ferra/scene.gltf'
self.MODEL = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.MODEL.setZ(para[Parameter.vehicle_vis_z])
self.MODEL.setY(para[Parameter.vehicle_vis_y])
self.MODEL.setH(para[Parameter.vehicle_vis_h])
self.MODEL.set_scale(para[Parameter.vehicle_vis_scale])
self.MODEL.instanceTo(self.chassis_np)
def __init__(self, game, pos, vel):
self.initCordPos = numpy.array([pos[0], pos[1]])
line = game.segLine
lineLen = 0
prevPoint = line[0]
self.initNum = 0
for point in line:
norm = numpy.linalg.norm(point - prevPoint)
if pos[0] >= lineLen and pos[0] < lineLen + norm:
ldirec = (point - prevPoint) / norm
rdirec = numpy.array([ldirec[1], -ldirec[0]])
post = prevPoint + (pos[0] -
lineLen) * ldirec + rdirec * pos[1]
self.initPos = Vec3(post[0], post[1], pos[2])
break
lineLen = lineLen + norm
prevPoint = point
self.initNum = self.initNum + 1
chassis = setChassis(game, self.initPos, vel)
BulletVehicle.__init__(self, game.world, chassis.node())
self.setCoordinateSystem(ZUp)
game.world.attachVehicle(self)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.reparentTo(chassis)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(game.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 = 10.0 # degree per second
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)
def _add_chassis(self, pg_physics_world: PGPhysicsWorld):
para = self.get_config()
chassis = BulletRigidBodyNode(BodyName.Ego_vehicle_top)
chassis.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_shape = BulletBoxShape(
Vec3(para[Parameter.vehicle_width] / 2,
para[Parameter.vehicle_length] / 2,
para[Parameter.vehicle_height] / 2))
ts = TransformState.makePos(
Vec3(0, 0, para[Parameter.chassis_height] * 2))
chassis.addShape(chassis_shape, ts)
heading = np.deg2rad(-para[Parameter.heading] - 90)
chassis.setMass(para[Parameter.mass])
self.chassis_np = self.node_path.attachNewNode(chassis)
# not random born now
self.chassis_np.setPos(Vec3(*self.born_place, 1))
self.chassis_np.setQuat(
LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(True) # advance collision check
self.pg_world.physics_world.dynamic_world.setContactAddedCallback(
PythonCallbackObject(self._collision_check))
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Ego_vehicle)
chassis_beneath.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_beneath.addShape(chassis_shape)
self.chassis_beneath_np = self.chassis_np.attachNewNode(
chassis_beneath)
self.dynamic_nodes.append(chassis_beneath)
self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis)
self.system.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(
self.system
) # detach chassis will also detach system, so a waring will generate
self.LENGTH = para[Parameter.vehicle_length]
self.WIDTH = para[Parameter.vehicle_width]
if self.render:
model_path = 'models/ferra/scene.gltf'
self.chassis_vis = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.chassis_vis.setZ(para[Parameter.vehicle_vis_z])
self.chassis_vis.setY(para[Parameter.vehicle_vis_y])
self.chassis_vis.setH(para[Parameter.vehicle_vis_h])
self.chassis_vis.set_scale(para[Parameter.vehicle_vis_scale])
self.chassis_vis.reparentTo(self.chassis_np)
def __init__(self,game,pos,vel):
self.initCordPos=numpy.array([pos[0],pos[1]])
line=game.segLine
lineLen=0
prevPoint=line[0]
self.initNum=0
for point in line:
norm=numpy.linalg.norm(point-prevPoint)
if pos[0]>=lineLen and pos[0]<lineLen+norm:
ldirec=(point-prevPoint)/norm
rdirec=numpy.array([ldirec[1],-ldirec[0]])
post=prevPoint+(pos[0]-lineLen)*ldirec+rdirec*pos[1]
self.initPos=Vec3(post[0],post[1],pos[2])
break
lineLen=lineLen+norm
prevPoint=point
self.initNum=self.initNum+1
chassis=setChassis(game,self.initPos,vel)
BulletVehicle.__init__(self,game.world,chassis.node())
self.setCoordinateSystem(ZUp)
game.world.attachVehicle(self)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.reparentTo(chassis)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3( 0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3( 0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(game.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 = 10.0 # degree per second
def __createVehicle(self,bulletWorld):
'''
Creates a vehicle, sets up wheels and does all the things
'''
self._nodePath.node().setMass(800.0)
# Chassis geometry
np = loader.loadModel('media/tankBody.x')
np.setHpr(90,0,0)
np.reparentTo(self._nodePath)
#np.setScale(self._tankSize*2)
np.setPos(-self._tankSize+Vec3(1, 0, .5))
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self._nodePath.node())
self.vehicle.setCoordinateSystem(2)
bulletWorld.attachVehicle(self.vehicle)
wheelNP = loader.loadModel('box')
wheelNP.setScale(.01,.01,.01)
wheelPos = [Point3(1, 1, 0),Point3(-1, 1, 0),
Point3(1, -1, 0),Point3(-1, -1, 0)]
for i in range(4):
wheel = self.vehicle.createWheel()
wheel.setWheelAxleCs(Vec3(-2*(i%2)+1, 0, 0))
wheel.setChassisConnectionPointCs(wheelPos[i])
self.__createWheel(wheel)
self.vehicle.setSteeringValue(0,i)
wheel.setRollInfluence((-2*(i%2)+1)*0.2)
def setupVehicle(self, main):
scale = 0.5
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5 * scale))
name = self.username
self.chassisNode = BulletRigidBodyNode(name)
self.chassisNode.setTag('username', str(name))
self.chassisNP = main.worldNP.attachNewNode(self.chassisNode)
self.chassisNP.setName(str(name))
self.chassisNP.node().addShape(shape, ts)
self.chassisNP.setScale(scale)
self.chassisNP.setPos(self.pos)
if self.isCurrentPlayer:
self.chassisNP.node().notifyCollisions(True)
self.chassisNP.node().setMass(800.0)
else:
self.chassisNP.node().notifyCollisions(True)
self.chassisNP.node().setMass(400.0)
self.chassisNP.node().setDeactivationEnabled(False)
main.world.attachRigidBody(self.chassisNP.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(main.world, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
main.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.reparentTo(self.chassisNP)
#self.carNP = loader.loadModel('models/batmobile-chassis.egg')
#self.yugoNP.setScale(.7)
#self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3( 0.70 * scale, 1.05 * scale, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3(-0.70 * scale, 1.05 * scale, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3( 0.70 * scale, -1.05 * scale, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3(-0.70 * scale, -1.05 * scale, 0.3), False, np)
def reset(self):
car = loader.loadModel("models/car.egg")
car.flattenLight()
car_bounds = car.getTightBounds()
car_shape = BulletBoxShape(Vec3((car_bounds[1].x - car_bounds[0].x) / 2,
(car_bounds[1].y - car_bounds[0].y) / 2,
(car_bounds[1].z - car_bounds[0].z) / 2))
car_ts = TransformState.makePos(Point3(0, 0, 0.5))
if self.car_node is not None:
self.car_node.removeNode()
self.car_node = render.attachNewNode(BulletRigidBodyNode('Car'))
self.car_node.node().setDeactivationEnabled(False)
self.car_node.node().setMass(600)
self.car_node.node().addShape(car_shape, car_ts)
self.world.attachRigidBody(self.car_node.node())
car.reparentTo(self.car_node)
self.car_node.setPos(0, 6, 1)
self.car = BulletVehicle(self.world, self.car_node.node())
self.car.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.car)
self.car_node.setPos(0, 6, 1)
self.car_node.setHpr(0, 0, 0)
self.car.resetSuspension()
self.car_node.node().clearForces()
wheel_fl = loader.loadModel("models/wheelL")
wheel_fl.reparentTo(render)
self.make_wheel(Point3(-0.4, 1.28, 0), True, wheel_fl)
wheel_fr = loader.loadModel("models/wheelR")
wheel_fr.reparentTo(render)
self.make_wheel(Point3(0.4, 1.28, 0), True, wheel_fr)
wheel_rl = loader.loadModel("models/wheelL")
wheel_rl.reparentTo(render)
self.make_wheel(Point3(-0.4, -1.35, 0), False, wheel_rl)
wheel_rr = loader.loadModel("models/wheelR")
wheel_rr.reparentTo(render)
self.make_wheel(Point3(0.4, -1.35, 0), False, wheel_rr)
def setupVehicle(self, bulletWorld):
# Chassis
shape = BulletBoxShape(Vec3(1, 2.2, 0.5))
ts = TransformState.makePos(Point3(0, 0, .7))
if self.isMainChar:
self.chassisNode = BulletRigidBodyNode(self.username)
else:
self.chassisNode = BulletRigidBodyNode('vehicle')
self.chassisNode.setTag("username", str(self.username))
self.chassisNP = render.attachNewNode(self.chassisNode)
self.chassisNP.node().addShape(shape, ts)
if self.isMainChar:
self.chassisNP.node().notifyCollisions(True)
else:
self.chassisNP.node().notifyCollisions(False)
self.setVehiclePos(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5])
# self.chassisNP.setPos(-5.34744, 114.773, 6)
#self.chassisNP.setPos(49.2167, 64.7968, 10)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
self.carNP = loader.loadModel('models/batmobile-chassis.egg')
#self.yugoNP.setScale(.7)
self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, 1.1, .7), True, np)
# Left front wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, 1.1, .7), True, np)
# Right rear wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, -2, .7), False, np)
# Left rear wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, -2, .7), False, np)
def setupVehicle(self, bulletWorld):
# Chassis
shape = BulletBoxShape(Vec3(1, 2.2, 0.5))
ts = TransformState.makePos(Point3(0, 0, .7))
self.chassisNode = BulletRigidBodyNode('Vehicle')
self.chassisNP = render.attachNewNode(self.chassisNode)
self.chassisNP.node().addShape(shape, ts)
self.chassisNP.node().notifyCollisions(True)
self.chassisNP.setPosHpr(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5])
# self.chassisNP.setPos(-5.34744, 114.773, 6)
# self.chassisNP.setPos(49.2167, 64.7968, 10)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
# np.node().setCcdSweptSphereRadius(1.0)
# np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
self.carNP = loader.loadModel('models/swiftstar-chassis')
tex = loader.loadTexture("models/tex/Futuristic_Car_C.jpg")
self.carNP.setTexture(tex)
self.carNP.setScale(1, 1, .9)
self.carNP.setCollideMask(BitMask32.allOn())
self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/swiftstar-fr-tire')
np.setCollideMask(BitMask32.allOff())
np.reparentTo(render)
self.addWheel(Point3(1, 1.1, 1), True, np)
# Left front wheel
np = loader.loadModel('models/swiftstar-fl-tire')
np.setCollideMask(BitMask32.allOff())
np.reparentTo(render)
self.addWheel(Point3(-1, 1.1, 1), True, np)
# Right rear wheel
np = loader.loadModel('models/swiftstar-rr-tire')
np.setCollideMask(BitMask32.allOff())
np.reparentTo(render)
self.addWheel(Point3(1.2, -2, .8), False, np)
# Left rear wheel
np = loader.loadModel('models/swiftstar-rl-tire')
np.setCollideMask(BitMask32.allOff())
np.reparentTo(render)
self.addWheel(Point3(-1.2, -2, .8), False, np)
def _create_vehicle_chassis(self):
self.LENGTH = type(self).LENGTH
self.WIDTH = type(self).WIDTH
self.HEIGHT = type(self).HEIGHT
assert self.LENGTH < BaseVehicle.MAX_LENGTH, "Vehicle is too large!"
assert self.WIDTH < BaseVehicle.MAX_WIDTH, "Vehicle is too large!"
chassis = BaseRigidBodyNode(self.name, BodyName.Vehicle)
chassis.setIntoCollideMask(CollisionGroup.Vehicle)
chassis_shape = BulletBoxShape(
Vec3(self.WIDTH / 2, self.LENGTH / 2, self.HEIGHT / 2))
ts = TransformState.makePos(Vec3(0, 0, self.HEIGHT / 2))
chassis.addShape(chassis_shape, ts)
chassis.setMass(self.MASS)
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(
True
) # advance collision check, do callback in pg_collision_callback
physics_world = get_engine().physics_world
vehicle_chassis = BulletVehicle(physics_world.dynamic_world, chassis)
vehicle_chassis.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(vehicle_chassis)
return vehicle_chassis
def __init__(self, positionHpr, render, world, base):
self.base = base
loader = base.loader
position, hpr = positionHpr
vehicleType = "yugo"
self.vehicleDir = "data/vehicles/" + vehicleType + "/"
# Load vehicle description and specs
with open(self.vehicleDir + "vehicle.json") as vehicleData:
data = json.load(vehicleData)
self.specs = data["specs"]
# Chassis for collisions and mass uses a simple box shape
halfExtents = (0.5 * dim for dim in self.specs["dimensions"])
shape = BulletBoxShape(Vec3(*halfExtents))
ts = TransformState.makePos(Point3(0, 0, 0.5))
self.rigidNode = BulletRigidBodyNode("vehicle")
self.rigidNode.addShape(shape, ts)
self.rigidNode.setMass(self.specs["mass"])
self.rigidNode.setDeactivationEnabled(False)
self.np = render.attachNewNode(self.rigidNode)
self.np.setPos(position)
self.np.setHpr(hpr)
world.attachRigidBody(self.rigidNode)
# Vehicle physics model
self.vehicle = BulletVehicle(world, self.rigidNode)
self.vehicle.setCoordinateSystem(ZUp)
world.attachVehicle(self.vehicle)
# Vehicle graphical model
self.vehicleNP = loader.loadModel(self.vehicleDir + "car.egg")
self.vehicleNP.reparentTo(self.np)
# Create wheels
wheelPos = self.specs["wheelPositions"]
for fb, y in (("F", wheelPos[1]), ("B", -wheelPos[1])):
for side, x in (("R", wheelPos[0]), ("L", -wheelPos[0])):
np = loader.loadModel(self.vehicleDir + "tire%s.egg" % side)
np.reparentTo(render)
isFront = fb == "F"
self.addWheel(Point3(x, y, wheelPos[2]), isFront, np)
class HeightfieldVehicle(ShowBase):
def __init__(self, heightfield_fn="heightfield.png"):
# Store the heightfield's filename.
self.heightfield_fn = heightfield_fn
"""
Load some configuration variables, it's important for this to happen
before ShowBase is initialized
"""
load_prc_file_data("", """
sync-video #t
textures-power-2 none
###gl-coordinate-system default
notify-level-gobj warning
notify-level-grutil debug
notify-level-shader_terrain debug
notify-level-bullet debug
### model paths
model-path /usr/share/panda3d
model-path /home/juzzuj/code/prereq/bullet-samples/bullet-samples
""")
ShowBase.__init__(self)
base.set_background_color(0.1, 0.1, 0.8, 1)
base.set_frame_rate_meter(True)
# Increase camera Field Of View and set near and far planes
base.camLens.set_fov(90)
base.camLens.set_near_far(0.1, 50000)
# Lights
alight = AmbientLight('ambientLight')
alight.set_color(LVector4(0.5, 0.5, 0.5, 1))
alightNP = render.attach_new_node(alight)
dlight = DirectionalLight('directionalLight')
dlight.set_direction(LVector3(1, 1, -1))
dlight.set_color(LVector4(0.7, 0.7, 0.7, 1))
dlightNP = render.attach_new_node(dlight)
render.clear_light()
render.set_light(alightNP)
render.set_light(dlightNP)
# Basic game controls
self.accept('escape', self.do_exit)
self.accept('f1', base.toggle_wireframe)
self.accept('f2', base.toggle_texture)
self.accept('f3', self.toggle_debug)
self.accept('f5', self.do_screenshot)
self.accept('r', self.do_reset)
# Vehicle controls
inputState.watchWithModifiers('forward', 'w')
inputState.watchWithModifiers('turnLeft', 'a')
inputState.watchWithModifiers('reverse', 's')
inputState.watchWithModifiers('turnRight', 'd')
inputState.watchWithModifiers('forward', 'arrow_up')
inputState.watchWithModifiers('turnLeft', 'arrow_left')
inputState.watchWithModifiers('reverse', 'arrow_down')
inputState.watchWithModifiers('turnRight', 'arrow_right')
self.accept('space', self.reset_vehicle)
# Controls to do with the terrain
#self.accept('t', self.rise_in_front)
self.accept('t', self.deform_terrain, ["raise"])
self.accept('g', self.deform_terrain, ["depress"])
self.accept('b', self.drop_boxes)
# Some debugging and miscellaneous controls
self.accept('e', self.query_elevation)
self.accept('c', self.convert_coordinates)
self.accept('p', self.save)
self.accept('h', self.hide_terrain)
# Task
taskMgr.add(self.update, 'updateWorld')
self.setup()
"""
Macro-like function used to reduce the amount to code needed to create the
on screen instructions
"""
def genLabelText(self, i, text, parent="a2dTopLeft"):
return OnscreenText(text=text, parent=getattr(base, parent), scale=.05,
pos=(0.06, -.065 * i), fg=(1, 1, 1, 1),
align=TextNode.ALeft)
# _____HANDLER_____
def cleanup(self):
self.world = None
self.worldNP.remove_node()
self.terrain.remove_node()
self.skybox.remove_node()
del self.terrain_node
def do_exit(self):
self.cleanup()
sys.exit(1)
def do_reset(self):
self.cleanup()
self.setup()
def toggle_debug(self):
if self.debugNP.is_hidden():
self.debugNP.show()
else:
self.debugNP.hide()
def do_screenshot(self):
base.screenshot('HeightfieldVehicle')
# ____TASK___
def update(self, task):
# Get the time passed since the last frame
dt = globalClock.get_dt()
# Pass dt as parameter (we need it for sensible steering calculations)
self.process_input(dt)
"""
Basically, we want to put our camera where the camera floater is. We
need the floater's world (=render-relative) position (it's parented to
the vehicle)
"""
floater_pos = render.get_relative_point(self.camera_floater,
LVector3(0))
"""
If the camera floater is under the terrain surface, adjust it, so that
it stays above the terrain.
"""
elevation_at_floater_pos = self.query_elevation(floater_pos)
if elevation_at_floater_pos.z >= floater_pos.z:
floater_pos.z = elevation_at_floater_pos.z + 1.
# Now we set our camera's position and make it look at the vehicle.
base.cam.set_pos(floater_pos)
base.cam.look_at(self.vehicleNP)
# Let the Bullet library do physics calculations.
self.world.do_physics(dt, 10, 0.008)
return task.cont
def process_input(self, dt):
# Relax steering towards straight
self.steering *= 1 - self.steering_relax_factor * dt
engine_force = 0.0
brake_force = 0.0
if inputState.isSet('forward'):
engine_force = 1000.0
brake_force = 0.0
if inputState.isSet('reverse'):
engine_force = 0.0
brake_force = 100.0
if inputState.isSet('turnLeft'):
self.steering += dt * self.steering_increment
self.steering = min(self.steering, self.steering_clamp)
if inputState.isSet('turnRight'):
self.steering -= dt * self.steering_increment
self.steering = max(self.steering, -self.steering_clamp)
# Lower steering intensity for high speeds
self.steering *= 1. - (self.vehicle.get_current_speed_km_hour() *
self.steering_speed_reduction_factor)
# Apply steering to front wheels
#self.vehicle.get_wheels()[0].set_steering(self.steering)
#self.vehicle.get_wheels()[1].set_steering(self.steering)
#self.vehicle.wheels[0].steering = self.steering
#self.vehicle.wheels[1].steering = self.steering
self.vehicle.set_steering_value(self.steering, 0)
self.vehicle.set_steering_value(self.steering, 1)
# Apply engine and brake to rear wheels
#self.vehicle.wheels[2].engine_force = engine_force
#self.vehicle.wheels[3].engine_force = engine_force
#self.vehicle.wheels[2].brake = brake_force
#self.vehicle.wheels[3].brake = brake_force
self.vehicle.apply_engine_force(engine_force, 2)
self.vehicle.apply_engine_force(engine_force, 3)
self.vehicle.set_brake(brake_force, 2)
self.vehicle.set_brake(brake_force, 3)
def setup(self):
# Bullet physics world
self.worldNP = render.attach_new_node('World')
self.debugNP = self.worldNP.attach_new_node(BulletDebugNode('Debug'))
self.world = BulletWorld()
self.world.set_gravity(LVector3(0, 0, -9.81))
self.world.set_debug_node(self.debugNP.node())
# Vehicle
# Steering info
self.steering = 0.0 # degrees
self.steering_clamp = 45.0 # degrees
self.steering_increment = 120.0 # degrees per second
# How fast steering relaxes to straight ahead
self.steering_relax_factor = 2.0
# How much steering intensity decreases with higher speeds
self.steering_speed_reduction_factor = 0.003
# Chassis collision box (note: Bullet uses half-measures)
shape = BulletBoxShape(LVector3(0.6, 1.4, 0.5))
ts = TransformState.make_pos(LPoint3(0, 0, 0.5))
self.vehicleNP = self.worldNP.attach_new_node(
BulletRigidBodyNode('Vehicle'))
self.vehicleNP.node().add_shape(shape, ts)
# Set initial position
self.vehicleNP.set_pos(0, 70, -10)
self.vehicleNP.node().set_mass(800.0)
self.vehicleNP.node().set_deactivation_enabled(False)
self.world.attach(self.vehicleNP.node())
# Camera floater
self.attach_camera_floater()
# BulletVehicle
self.vehicle = BulletVehicle(self.world, self.vehicleNP.node())
self.world.attach(self.vehicle)
# Vehicle model
self.yugoNP = loader.load_model('models/yugo/yugo.egg')
self.yugoNP.reparent_to(self.vehicleNP)
# Right front wheel
np = loader.load_model('models/yugo/yugotireR.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3( 0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.load_model('models/yugo/yugotireL.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.load_model('models/yugo/yugotireR.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3( 0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.load_model('models/yugo/yugotireL.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3(-0.70, -1.05, 0.3), False, np)
# Load a skybox
self.skybox = self.loader.load_model(
"samples/shader-terrain/models/skybox.bam")
self.skybox.reparent_to(self.render)
self.skybox.set_scale(20000)
skybox_texture = self.loader.load_texture(
"samples/shader-terrain/textures/skybox.jpg")
skybox_texture.set_minfilter(SamplerState.FT_linear)
skybox_texture.set_magfilter(SamplerState.FT_linear)
skybox_texture.set_wrap_u(SamplerState.WM_repeat)
skybox_texture.set_wrap_v(SamplerState.WM_mirror)
skybox_texture.set_anisotropic_degree(16)
self.skybox.set_texture(skybox_texture)
skybox_shader = Shader.load(Shader.SL_GLSL,
"samples/shader-terrain/skybox.vert.glsl",
"samples/shader-terrain/skybox.frag.glsl")
self.skybox.set_shader(skybox_shader)
# Terrain
self.setup_terrain()
def add_wheel(self, pos, front, np):
wheel = self.vehicle.create_wheel()
wheel.set_node(np.node())
wheel.set_chassis_connection_point_cs(pos)
wheel.set_front_wheel(front)
wheel.set_wheel_direction_cs(LVector3(0, 0, -1))
wheel.set_wheel_axle_cs(LVector3(1, 0, 0))
wheel.set_wheel_radius(0.25)
wheel.set_max_suspension_travel_cm(40.0)
wheel.set_suspension_stiffness(40.0)
wheel.set_wheels_damping_relaxation(2.3)
wheel.set_wheels_damping_compression(4.4)
wheel.set_friction_slip(100.0)
wheel.set_roll_influence(0.1)
def attach_camera_floater(self):
"""
Set up an auxiliary camera floater, which is parented to the vehicle.
Every frame base.cam's position will be set to the camera floater's.
"""
camera_behind = 8
camera_above = 3
self.camera_floater = NodePath("camera_floater")
self.camera_floater.reparent_to(self.vehicleNP)
self.camera_floater.set_y(-camera_behind)
self.camera_floater.set_z(camera_above)
def reset_vehicle(self):
reset_pos = self.vehicleNP.get_pos()
reset_pos.z += 3
self.vehicleNP.node().clear_forces()
self.vehicleNP.node().set_linear_velocity(LVector3(0))
self.vehicleNP.node().set_angular_velocity(LVector3(0))
self.vehicleNP.set_pos(reset_pos)
self.vehicleNP.set_hpr(LVector3(0))
def drop_boxes(self):
"""
Puts a stack of boxes at a distance in front of the vehicle.
The boxes will not necessarily spawn above the terrain and will not
be cleaned up.
"""
model = loader.load_model('models/box.egg')
model.set_pos(-0.5, -0.5, -0.5)
model.flatten_light()
shape = BulletBoxShape(LVector3(0.5, 0.5, 0.5))
ahead = self.vehicleNP.get_pos() + self.vehicle.get_forward_vector()*15
for i in range(6):
node = BulletRigidBodyNode('Box')
node.set_mass(5.0)
node.add_shape(shape)
node.set_deactivation_enabled(False)
np = render.attach_new_node(node)
np.set_pos(ahead.x, ahead.y, ahead.z + i*2)
self.world.attach(node)
model.copy_to(np)
def query_elevation(self, xy_pos=None):
"""
Query elevation for xy_pos if present, else for vehicle position.
No xy_pos means verbose mode.
"""
query_pos = xy_pos or self.vehicleNP.get_pos()
"""
This method is accurate and may be useful for placing
objects on the terrain surface.
"""
result = self.world.ray_test_closest(
LPoint3(query_pos.x, query_pos.y, -10000),
LPoint3(query_pos.x, query_pos.y, 10000))
if result.has_hit():
hit_pos = result.get_hit_pos()
if not xy_pos:
print("Bullet heightfield elevation at "
"X {:.2f} | Y {:.2f} is {:.3f}".format(
hit_pos.x, hit_pos.y, hit_pos.z))
else:
hit_pos = None
if not xy_pos:
print("Could not query elevation at {}".format(xy_pos))
"""
This method is less accurate than the one above.
Under heavy ray-testing stress (ray tests are performed for all vehicle
wheels, the above elevation query etc.) Bullet sometimes seems to be a
little unreliable.
"""
texspace_pos = self.terrain.get_relative_point(render, query_pos)
stm_pos = self.terrain_node.uv_to_world(
LTexCoord(texspace_pos.x, texspace_pos.y))
if not xy_pos:
print("ShaderTerrainMesh elevation at "
"X {:.2f} | Y {:.2f} is {:.3f}".format(
stm_pos.x, stm_pos.y, stm_pos.z))
return hit_pos or stm_pos
def setup_terrain(self):
"""
Terrain info
Units are meters, which is preferable when working with Bullet.
"""
self.terrain_scale = LVector3(512, 512, 100)
self.terrain_pos = LVector3(-256, -256, -70)
# sample values for a 4096 x 4096px heightmap.
#self.terrain_scale = LVector3(4096, 4096, 1000)
#self.terrain_pos = LVector3(-2048, -2048, -70)
"""
Diamond_subdivision is an alternating triangulation scheme and may
produce better results.
"""
use_diamond_subdivision = True
"""
Construct the terrain
Without scaling, any ShaderTerrainMesh is 1x1x1 units.
"""
self.terrain_node = ShaderTerrainMesh()
"""
Set a heightfield, the heightfield should be a 16-bit png and
have a quadratic size of a power of two.
"""
heightfield = Texture()
heightfield.read(self.heightfield_fn)
heightfield.set_keep_ram_image(True)
self.terrain_node.heightfield = heightfield
# Display characteristic values of the heightfield texture
#minpoint, maxpoint, avg = LPoint3(), LPoint3(), LPoint3()
#heightfield.calc_min_max(minpoint, maxpoint)
#heightfield.calc_average_point(avg, 0.5, 0.5, 0.5)
#print("avg: {} min: {} max: {}".format(avg.x, minpoint.x, maxpoint.x))
"""
Set the target triangle width. For a value of 10.0 for example,
the ShaderTerrainMesh will attempt to make every triangle 10 pixels
wide on screen.
"""
self.terrain_node.target_triangle_width = 10.0
if use_diamond_subdivision:
"""
This has to be specified before calling .generate()
The default is false.
"""
load_prc_file_data("", "stm-use-hexagonal-layout true")
self.terrain_node.generate()
"""
Attach the terrain to the main scene and set its scale. With no scale
set, the terrain ranges from (0, 0, 0) to (1, 1, 1)
"""
self.terrain = self.render.attach_new_node(self.terrain_node)
self.terrain.set_scale(self.terrain_scale)
self.terrain.set_pos(self.terrain_pos)
"""
Set a vertex and a fragment shader on the terrain. The
ShaderTerrainMesh only works with an applied shader.
"""
terrain_shader = Shader.load(Shader.SL_GLSL,
"samples/shader-terrain/terrain.vert.glsl",
"samples/shader-terrain/terrain.frag.glsl")
self.terrain.set_shader(terrain_shader)
self.terrain.set_shader_input("camera", base.camera)
# Set some texture on the terrain
grass_tex = self.loader.load_texture(
"samples/shader-terrain/textures/grass.png")
grass_tex.set_minfilter(SamplerState.FT_linear_mipmap_linear)
grass_tex.set_anisotropic_degree(16)
self.terrain.set_texture(grass_tex)
"""
Set up the DynamicHeightfield (it's a type of PfmFile). We load the
same heightfield image as with ShaderTerrainMesh.
"""
self.DHF = DynamicHeightfield()
self.DHF.read(self.heightfield_fn)
"""
Set up empty PfmFiles to prepare stuff in that is going to
dynamically modify our terrain.
"""
self.StagingPFM = PfmFile()
self.RotorPFM = PfmFile()
"""
Set up the BulletHeightfieldShape (=collision terrain) and give it
some sensible physical properties.
"""
self.HFS = BulletHeightfieldShape(self.DHF, self.terrain_scale.z,
STM=True)
if use_diamond_subdivision:
self.HFS.set_use_diamond_subdivision(True)
HFS_rigidbody = BulletRigidBodyNode("BulletTerrain")
HFS_rigidbody.set_static(True)
friction = 2.0
HFS_rigidbody.set_anisotropic_friction(
LVector3(friction, friction, friction/1.3))
HFS_rigidbody.set_restitution(0.3)
HFS_rigidbody.add_shape(self.HFS)
self.world.attach(HFS_rigidbody)
HFS_NP = NodePath(HFS_rigidbody)
HFS_NP.reparent_to(self.worldNP)
"""
This aligns the Bullet terrain with the ShaderTerrainMesh rendered
terrain. It will be exact as long as the terrain vertex shader from
the STM sample is used and no additional tessellation shader.
For Bullet (as for other physics engines) the origin of objects is at
the center.
"""
HFS_NP.set_pos(self.terrain_pos + self.terrain_scale/2)
HFS_NP.set_sx(self.terrain_scale.x / heightfield.get_x_size())
HFS_NP.set_sy(self.terrain_scale.y / heightfield.get_y_size())
# Disables Bullet debug rendering for the terrain, because it is slow.
#HFS_NP.node().set_debug_enabled(False)
"""
Finally, link the ShaderTerrainMesh and the BulletHeightfieldShape to
the DynamicHeightfield. From now on changes to the DynamicHeightfield
will propagate to the (visible) ShaderTerrainMesh and the (collidable)
BulletHeightfieldShape.
"""
self.HFS.set_dynamic_heightfield(self.DHF)
self.terrain_node.set_dynamic_heightfield(self.DHF)
def deform_terrain(self, mode="raise", duration=1.0):
self.deform_duration = duration
"""
Calculate distance to the spot at which we want to raise the terrain.
At its current speed the vehicle would reach it in 2.5 seconds.
[km/h] / 3.6 = [m/s] * [s] = [m]
"""
distance = self.vehicle.get_current_speed_km_hour() / 3.6 * 2.5
spot_pos_world = (self.vehicleNP.get_pos() +
self.vehicle.get_forward_vector() * distance)
spot_pos_heightfield = self.terrain_node.world_to_heightfield(
spot_pos_world)
xcenter = spot_pos_heightfield.x
ycenter = spot_pos_heightfield.y
"""
To create a smooth hill/depression we call PfmFile.pull_spot to create
a sort of gradient. You can use self.cardmaker_debug to view it.
From the Panda3D API documentation of PfmFile.pull_spot:
Applies delta * t to the point values within radius (xr, yr)
distance of (xc, yc). The t value is scaled from 1.0 at the center
to 0.0 at radius (xr, yr), and this scale follows the specified
exponent. Returns the number of points affected.
"""
# Delta to apply to the point values in DynamicHeightfield array.
delta = LVector4(0.001)
# Make the raised spot elliptical.
xradius = 10.0 # meters
yradius = 6.0 # meters
# Choose an exponent
exponent = 0.6
# Counter-clockwise angle between Y-axis
angle = self.vehicle.get_forward_vector().signed_angle_deg(
LVector3.forward(), LVector3.down())
# Define all we need to repeatedly deform the terrain using a task.
self.spot_params = [delta, xcenter, ycenter, xradius, yradius,
exponent, mode]
# Clear staging area to a size that fits our raised region.
self.StagingPFM.clear(int(xradius)*2, int(yradius)*2, num_channels=1)
"""
There are two options:
(1) Rotate our hill/depression to be perpendicular
to the vehicle's trajectory. This is the default.
(2) Just put it in the terrain unrotated.
"""
# Option (1)
self.StagingPFM.pull_spot(delta,
xradius-0.5, yradius-0.5,
xradius, yradius,
exponent)
# Rotate wider side so it's perpendicular to vehicle's trajectory.
self.RotorPFM.rotate_from(self.StagingPFM, angle)
self.raise_start_time = globalClock.get_real_time()
taskMgr.do_method_later(0.03, self.deform_perpendicular,
"DeformPerpendicularSpot")
# Option (2)
#taskMgr.do_method_later(0.03, self.deform_regular, "RaiseASpot")
def deform_perpendicular(self, task):
if (globalClock.get_real_time() - self.raise_start_time <
self.deform_duration):
(delta, xcenter, ycenter,
xradius, yradius, exponent, mode) = self.spot_params
"""
Copy rotated hill to our DynamicHeightfield.
The values from RotorPFM are added to the ones found in the
DynamicHeightfield.
"""
self.DHF.add_sub_image(self.RotorPFM,
int(xcenter - self.RotorPFM.get_x_size()/2),
int(ycenter - self.RotorPFM.get_y_size()/2),
0, 0, self.RotorPFM.get_x_size(), self.RotorPFM.get_y_size(),
1.0 if mode == "raise" else -1.0)
# Output images of our PfmFiles.
#self.RotorPFM.write("dbg_RotorPFM.png")
#self.StagingPFM.write("dbg_StagingPFM.png")
return task.again
self.cardmaker_debug()
return task.done
def deform_regular(self, task):
if (globalClock.get_real_time() - self.raise_start_time <
self.deform_duration):
(delta, xcenter, ycenter,
xradius, yradius, exponent, mode) = self.spot_params
self.DHF.pull_spot(delta * (1.0 if mode == "raise" else -1.0),
xcenter, ycenter, xradius, yradius, exponent)
return task.again
return task.done
def convert_coordinates(self):
vpos = self.vehicleNP.get_pos()
print("VPOS world: ", vpos)
W2H = self.terrain_node.world_to_heightfield(vpos)
print("W2H: ", W2H)
H2W = self.terrain_node.heightfield_to_world(LTexCoord(W2H.x, W2H.y))
print("H2W: ", H2W)
W2U = self.terrain_node.world_to_uv(vpos)
print("W2U: ", W2U)
U2W = self.terrain_node.uv_to_world(LTexCoord(W2U.x, W2U.y))
print("U2W: ", U2W)
def hide_terrain(self):
if self.terrain.is_hidden():
self.terrain.show()
else:
self.terrain.hide()
def save(self):
self.DHF.write("dbg_dynamicheightfield.png")
def pfmvizzer_debug(self, pfm):
vizzer = PfmVizzer(pfm)
"""
To align the mesh we generate with PfmVizzer with our
ShaderTerrainMesh, BulletHeightfieldShape, and DynamicHeightfield, we
need to set a negative scale on the Y axis. This reverses the winding
order of the triangles in the mesh, which is why we choose
PfmVizzer.MF_back
Note that this does not accurately match up with our mesh because the
interpolation differs. Still, PfmVizzer can be useful when
inspecting, distorting, etc. PfmFiles.
"""
self.vizNP = vizzer.generate_vis_mesh(PfmVizzer.MF_back)
self.vizNP.set_texture(loader.load_texture("maps/grid.rgb"))
self.vizNP.reparent_to(render)
if pfm == self.DHF or pfm == self.terrain_node.heightfield:
self.vizNP.set_pos(self.terrain_pos.x, -self.terrain_pos.y,
self.terrain_pos.z)
self.vizNP.set_scale(self.terrain_scale.x, -self.terrain_scale.y,
self.terrain_scale.z)
def cardmaker_debug(self):
for node in render2d.find_all_matches("pfm"):
node.remove_node()
for text in base.a2dBottomLeft.find_all_matches("*"):
text.remove_node()
width = 0.2 # render2d coordinates range: [-1..1]
# Pseudo-normalize our PfmFile for better contrast.
normalized_pfm = PfmFile(self.RotorPFM)
max_p = LVector3()
normalized_pfm.calc_min_max(LVector3(), max_p)
normalized_pfm *= 1.0 / max_p.x
# Put it in a texture
tex = Texture()
tex.load(normalized_pfm)
# Apply the texture to a quad and put it in the lower left corner.
cm = CardMaker("pfm")
cm.set_frame(0, width, 0,
width / normalized_pfm.get_x_size() * normalized_pfm.get_y_size())
card = base.render2d.attach_new_node(cm.generate())
card.set_pos(-1, 0, -1)
card.set_texture(tex)
# Display max value text
self.genLabelText(-3,
"Max value: {:.3f} == {:.2f}m".format(max_p.x,
max_p.x * self.terrain_scale.z),
parent="a2dBottomLeft")
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)
class Tank(DynamicWorldObject):
'''Child of WorldObject, with all of the things that makes a Tank a tank.
Includes a Weapon
'''
def __init__(self, world, attach, name = '', position = Vec3(0,0,0), orientation = Vec3(0,0,0),
turretPitch = 0):
#Constant Relevant Instatiation Parameters
self._tankSize = Vec3(1, 1.5, .5) # Actually a half-size
self._tankSideLength = max(self._tankSize)
friction = .3
tankMass = 800.0
# Rewrite constructor to include these?
self._maxVel = 4
self._maxRotVel = 1
self._maxThrusterAccel = 5000
self._breakForce = 1000
turretRelPos = (0, 0, 0) #Relative to tank
self._shape = BulletBoxShape(Vec3(1,1.5,.5)) #chassis
self._transformState = TransformState.makePos(Point3(0, 0, 0)) #offset
DynamicWorldObject.__init__(self, world, attach, name, position, self._shape, orientation, Vec3(0,0,0), mass = tankMass) #Initial velocity must be 0
self.__createVehicle(self._tankWorld.getPhysics())
self._collisionCounter = 0
self._taskTimer = 0
self._nodePath.node().setFriction(friction)
self._nodePath.setPos(position)
# Set up turret nodepath
# (Nodepaths are how objects are managed in Panda3d)
self.onTask = 0
# Make collide mask (What collides with what)
self._nodePath.setCollideMask(0xFFFF0000)
#self._nodePath.setCollideMask(BitMask32.allOff())
self.movementPoint = Point3(10,10,0)
#print "Tank.__init__: " + name
# Set up the
def __createVehicle(self,bulletWorld):
'''
Creates a vehicle, sets up wheels and does all the things
'''
self._nodePath.node().setMass(800.0)
# Chassis geometry
np = loader.loadModel('media/tankBody.x')
np.setHpr(90,0,0)
np.reparentTo(self._nodePath)
#np.setScale(self._tankSize*2)
np.setPos(-self._tankSize+Vec3(1, 0, .5))
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self._nodePath.node())
self.vehicle.setCoordinateSystem(2)
bulletWorld.attachVehicle(self.vehicle)
wheelNP = loader.loadModel('box')
wheelNP.setScale(.01,.01,.01)
wheelPos = [Point3(1, 1, 0),Point3(-1, 1, 0),
Point3(1, -1, 0),Point3(-1, -1, 0)]
for i in range(4):
wheel = self.vehicle.createWheel()
wheel.setWheelAxleCs(Vec3(-2*(i%2)+1, 0, 0))
wheel.setChassisConnectionPointCs(wheelPos[i])
self.__createWheel(wheel)
self.vehicle.setSteeringValue(0,i)
wheel.setRollInfluence((-2*(i%2)+1)*0.2)
def __createWheel(self,wheel):
'''
sets up properties for wheel.
'''
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setFrontWheel(False)
wheel.setWheelRadius(0.15)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100.0)
def getWheels(self):
'''
returns a list of wheelPos
'''
return self.vehicle.getWheels()
def setWeaponHp(self, heading, pitch):
'''
float heading
float pitch
'''
self._weapon.setHp(heading,pitch)
def moveWeapon(self, heading = 0, pitch = 0):
rot = self.getHpr()
self.setWeaponHp(rot[0] + heading, rot[1] + pitch)
def distanceScan(self):
'''
This scan projects rays from the objects in the field toward the tank
in question. This scan does not perform as well as scan when the
objects are bunched together. When small objects are spread out
reasonably (more than 5 at a viewing range of 50), this scan performs
better.
Using this scan, large objects can hide behind small objects
This scan has the feature that it will pick up a lone object
guaranteed at any distance.
'''
self.wait(.1)
potentialNPs = self._tankWorld.render.getChildren()
found = []
for np in potentialNPs:
if type(np.node()) == BulletRigidBodyNode and np != self and not np.isHidden():
pFrom = np.getPos(render)
#Fix for cubeObjects (z = 0). They collide with the floor
if pFrom[2] < 1:
pFrom[2] = 1.1
pTo = self.getPos() + self.getPos() - pFrom
#pTo is a Vec3, turned to a point for rayTest
result = self._tankWorld.getPhysics().rayTestClosest(
pFrom, Point3(pTo[0], pTo[1], pTo[2]))
if result.hasHit() and result.getNode() == self._nodePath.node():
#found.append((np.node().getPrevTransform().getPos(),
# np.node().getName()))
found.append((np.getPos(render),
np.node().getName()))
elif result.hasHit():
#print "Tank.distanceScan: ",
#print np, result.getNode(), pFrom, pTo
#print "Neigh"
x = 1
return found
def __bulletRays(self, numPoints, relAngleRange, height):
'''Helper function for scans and pings. Runs through the
relAngleRange (two ints, in degrees) at the given numPoints and height.
Runs a Bullet rayTestClosest to find the nearest hit.
Returns a list of BulletClosestHitRayResult objects
'''
distanceOfMap = 100000
results = []
#scanResolution = numPoints / 360.0
angleSweep = relAngleRange[1] - relAngleRange[0] + 0.0
pos = self._nodePath.getPos()
heading = self._nodePath.getH()
for i in range(numPoints):
if angleSweep == 0:
angle = (heading + relAngleRange[0]) * (math.pi / 180)
else:
angle = (i / angleSweep + heading + relAngleRange[0]) * (math.pi / 180)
pFrom = Point3(math.sin(angle) * self._tankSideLength + pos[0],
math.cos(angle) * self._tankSideLength + pos[1], height)
pTo = Point3(math.sin(angle) * distanceOfMap + pos[0],
math.cos(angle) * distanceOfMap + pos[1], height)
result = self._tankWorld.getPhysics().rayTestClosest(pFrom, pTo)
results.append((result, angle * 180 / math.pi - heading))
return results
def scan(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
'''
This function scans the map to find the other objects on it. The scan
works iteratively, based on the angle range (given relative to the
tank's current heading) and the number of points given. This is a more
realistic scan, but does not work as well with smaller objects and
larger distances
'''
found = []
numFound = 0
results = self.__bulletRays(numPoints, relAngleRange, height)
prevNodes = dict()
for item in results:
result = item[0]
if result.hasHit():
newNode = result.getNode()
if newNode not in prevNodes:
found.append((newNode.getPrevTransform().getPos(),
newNode.getName()))
prevNodes[newNode] = 0
numFound = numFound + 1
return found
def pingPointsAbs(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
'''Returns a list of absolute coordinate points on each of the
'''
found = []
results = self.__bulletRays(numPoints, relAngleRange, height)
for item in results:
result = item[0]
if result.hasHit():
newNode = result.getNode()
found.append((result.getHitPos(), item[1], newNode.getName()))
return found
def pingPoints(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
found = self.pingPointsAbs(numPoints, relAngleRange, height)
pos = self.getPos()
for i in range(len(found)):
hitPos = found[i][0]
relPos = Point3(hitPos[0] - pos[0], hitPos[1] - pos[1], hitPos[2] - pos[2])
found[i] = (relPos, found[i][1], found[i][2])
return found
def pingDistance(self, numPoints = 360, relAngleRange = (-180, 180), height = 1):
found = self.pingPoints(numPoints, relAngleRange, height)
for i in range(len(found)):
relPos = found[i][0]
distance = math.sqrt(relPos[0]**2 + relPos[1]**2)
found[i] = (distance, found[i][1], found[i][2])
return found
def applyThrusters(self, right = 1, left = 1): #set acceleration
'''change acceleration to a percent of the maximum acceleration'''
#if right > 1 or amt < 0:
# raise ValueError("amt must be between 0 and 1")
# tankNode = self._nodePath.node()
# angle = self.nodePath.getH() #Apply force in current direction
# magnitude = amt * (self._maxThrusterAccel) + (tankNode.getFriction() *
# self._nodePath.node().getMass())
# force = Vec3(magnitude * math.cos(angle),
# magnitude * math.sin(angle), 0)
# self.nodePath.node().applyForce(force)
self.vehicle.reset_suspension()
self.applyBrakes(0)
vel = self.vehicle.getChassis().getLinearVelocity()
for i in range(4):
self.vehicle.applyEngineForce(0,i)
self.vehicle.setBrake(0,i)
if vel.length() < self._maxVel:
self.vehicle.applyEngineForce(-left*self._maxThrusterAccel,0)
self.vehicle.applyEngineForce(right*self._maxThrusterAccel,1)
self.vehicle.applyEngineForce(-left*self._maxThrusterAccel,2)
self.vehicle.applyEngineForce(right*self._maxThrusterAccel,3)
#for i in range(0,1):
# self.vehicle.applyEngineForce((left*(i)%2+right*(i+1)%2)*self._maxThrusterAccel,i)
#for i in range(2):
# self.vehicle.applyEngineForce((left*(i)%2+right*(i+1)%2)*self._maxThrusterAccel,i+2)
#self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i)
## for 1 and 3useleft
## for 0 and 2 use right
else:
for i in range(4):
#self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i)
self.vehicle.applyEngineForce(0,i)
def applyBrakes(self, amt=1):
for i in range(4):
#self.vehicle.applyEngineForce((2*i%2-1)*engineForce,i)
self.vehicle.applyEngineForce(0,i)
self.vehicle.setBrake(amt*self._breakForce,i)
def setVel(self, goal ):
pass
def move(self, dist):
'''Moves using a distance. Adds an updateMoveLoc task to the taskMgr.
'''
heading = self._nodePath.getH() * math.pi/180
pos = self.getPos()
self._stop = False
#Of the form (goalLoc, startLoc, distance)
self._moveLoc = (Point3(pos[0] + math.sin(heading) * dist, pos[1] - math.cos(heading) * dist, pos[2]), pos, dist)
self._tankWorld.taskMgr.add(self.updateMoveLoc,'userTank '+self.getName(),uponDeath=self.nextTask)
def rotate(self, angle):
'''Rotate function. All angles given between 0 and 360
Angle changes are between -180 and 180
'''
angle = angle % 360
if angle > 180:
angle -= 360
heading = self.fixAngle(self._nodePath.getH())
newH = self.fixAngle(heading + angle)
self._moveLoc = (newH, heading, angle)
self._stop = False
self._tankWorld.taskMgr.add(self.updateRotateLoc, 'userTank '+self.getName(), uponDeath=self.nextTask)
def moveTime(self, moveTime):
self._taskTimer = moveTime
self._tankWorld.taskMgr.add(self.updateMove,'userTank '+self.getName(),uponDeath=self.nextTask)
def rotateTime(self, rotateTime):
self._taskTimer = rotateTime
self._tankWorld.taskMgr.add(self.updateRotate,'userTank '+self.getName(),uponDeath=self.nextTask)
def wait(self, waitTime):
self._taskTimer = waitTime
self._tankWorld.taskMgr.add(self.updateWait,'userTank '+self.getName(),uponDeath=self.nextTask)
def updateWait(self, task):
'''
Tasks called to wait
'''
try:
if self._tankWorld.isRealTime():
dt = globalClock.getDt()
else:
dt = 1.0/60.0
#print "Tank.updateWait: ", dt
self._taskTimer -= dt
if self._taskTimer < 0:
return task.done
return task.cont
except:
print "error, tank.wait"
def updateRotateLoc(self, task):
heading = self.fixAngle(self._nodePath.getH())
toHeading = self._moveLoc[0]
w = self._nodePath.node().getAngularVelocity()[2]
#Right wheel direction for rotating in the direction of goal
rFor = self._moveLoc[2]/abs(self._moveLoc[2])
slowTheta = 1.5 * rFor
brakePercent = w**2 / (2 * slowTheta * self._maxThrusterAccel)
theta = self.fixAngle(toHeading - heading)
if theta > 180:
theta -= 360
thetaFromStart = heading - self._moveLoc[1]
thetaFromStart = thetaFromStart % 360
if thetaFromStart > 180:
thetaFromStart -= 360
if self._stop:
if abs(w) < .1:
self._nodePath.node().setAngularVelocity(Vec3(0,0,0))
self._nodePath.setH(self._moveLoc[0])
return task.done
self.applyBrakes()
return task.cont
if abs(theta) < slowTheta - .25:
self.applyThrusters(-rFor * brakePercent, rFor * brakePercent)
elif abs(theta) > slowTheta - .25:
if w < self._maxRotVel:
self.applyThrusters(rFor, -1 * rFor)
else:
self.applyThrusters(0,0)
else:
self.applyBrakes(brakePercent)
if abs(theta) < .1:
self._stop = True
if abs(thetaFromStart + .1) > abs(self._moveLoc[2]):
self._stop = True
#emergency stop based on a long time.
if task.time > 3:
#print "Tank.updateRotateLoc", "your rotate could not be completed, sorry"
return task.done
return task.cont
def updateMoveLoc(self, task):
'''Bases how much to slow down on a = v^2/2x.
x is slowDist, chosen to play nice.
Generally accurate to 5cm, rarely 20cm or worse
Stops at an arbitrary low velocity because it will never exit
at a lower minimum
'''
pos = self.getPos()
toLoc = self._moveLoc[0]
distance = math.sqrt((pos[0] - toLoc[0])**2 + (pos[1] - toLoc[1])**2)
v = self._nodePath.node().getLinearVelocity().length()
slowAccel = 2
slowDist = v**2 / (2 * slowAccel * self._breakForce)
brakePercent = slowAccel * self._nodePath.node().getMass() / self._breakForce
deltaPos = (self.getPos() - self._moveLoc[1])
distFromStart = deltaPos.length()
if self._stop:
if v < .4:
#self._nodePath.node().setLinearVelocity(Vec3(0,0,0))
#self._nodePath.setPos(self._moveLoc[0])
return task.done
self.applyBrakes()
return task.cont
if distance < slowDist:
self.applyBrakes(brakePercent * 1.1)
elif distance > slowDist:
if self._moveLoc[2] > 0:
self.applyThrusters(1,1)
else:
self.applyThrusters(-1, -1)
else:
self.applyBrakes(brakePercent)
if distance < .01:
self._stop = True
if abs(distFromStart) > abs(self._moveLoc[2]):
self._stop = True
return task.cont
def nextTask(self,task):
self._nodePath.node().setActive(True)
self.onTask += 1
if(self._tankWorld.isDead or self._tankWorld.isOver):
return
#if self.onTask >= len(self.taskList):
# return
def getNumUserTask():
taskAmount = 0
for t in self._tankWorld.taskMgr.getTasks():
if t.getName() == 'userTank '+self.getName():
taskAmount +=1
return taskAmount
pre = getNumUserTask()
try:
self.taskList.next()
if(getNumUserTask() == pre):
self.nextTask(task)
except StopIteration:
#print 'Tank.nextTask error'
pass
#self.taskList[self.onTask][0](self.taskList[self.onTask][1])
def runTasks(self):
self.onTask = 0
self.nextTask(None)
def setGenerator(self, gen):
self.taskList = gen
self.runTasks()
def updateMove(self, task):
'''
Task Called to do movement. This is called once perframe
'''
try:
#small hack to prevent the first frame from doing all the tasks.
dt = globalClock.getDt()
if dt > .1:
return task.cont
self._taskTimer -= dt
if self._taskTimer < 0:
self.applyBrakes(1)
else:
self.applyThrusters(1,1)
if self._taskTimer < -1: #one second to stop
return task.done
except:
print "ERROR in tank.updateMove"
return task.cont
def updateRotate(self, task):
''' called to do rotation. This is called once per frame
'''
try:
dt = globalClock.getDt()
#small hack to prevent the first frame from doing all the tasks.
if dt > .1:
return task.cont
self._taskTimer -= dt
if self._taskTimer < 0:
self.applyBrakes(1)
else:
self.applyThrusters(1,-1)
if self._taskTimer < -1: #one second to stop
return task.done
except:
print "ERROR in tank.updateRotate"
return task.cont
def update(self, task):
pass
def aimAt(self, point, amt = 1, aimLow = True):
return self._weapon.aimAt(point, aimLow)
def setWeapon(self, weopwn):
self._weapon = weopwn
def fire(self, amt = 1):
x = self._weapon.fire(amt)
self.wait(.1)
return x
def deleteAfter(self, task = None):
if not self._nodePath.is_empty():
x = self._nodePath.node()
self._tankWorld.removeRigidBody(x)
self.hide()
self._tankWorld.lose()
#self._nodePath.detachNode()
def handleCollision(self, collide, taskName):
self._collisionCounter += 1
forReal = True
hitBy = collide.getNode0()
if hitBy.getName() == self._nodePath.node().getName():
hitBy = collide.getNode1()
allowedNames = ['floor', 'wall', 'State']
for name in allowedNames:
if name in hitBy.getName():
forReal = False
if not self._nodePath.is_empty():
if forReal:
print "Tank.handleCollision:(pos) ", self.getPos(), 'obj 1', collide.getNode0().getName(), 'obj 2', collide.getNode1().getName()
self._tankWorld.taskMgr.remove(taskName)
self._tankWorld.doMethodLater(.01, self.deleteAfter, 'deleteAfter')
else:
print "Tank.handleCollision Failed to have _nodepath"
self._tankWorld.taskMgr.remove(taskName)
class BaseVehicle(DynamicElement):
MODEL = None
"""
Vehicle chassis and its wheels index
0 1
II-----II
|
| <---chassis
|
II-----II
2 3
"""
PARAMETER_SPACE = PGSpace(
VehicleParameterSpace.BASE_VEHICLE
) # it will not sample config from parameter space
COLLISION_MASK = CollisionGroup.EgoVehicle
STEERING_INCREMENT = 0.05
LENGTH = None
WIDTH = None
def __init__(
self,
pg_world: PGWorld,
vehicle_config: Union[dict, PGConfig] = None,
physics_config: dict = None,
random_seed: int = 0,
name: str = None,
):
"""
This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and
module parameters. And self.physics_config defines the physics feature of vehicles, such as length/width
:param pg_world: PGWorld
:param vehicle_config: mostly, vehicle module config
:param physics_config: vehicle height/width/length, find more physics para in VehicleParameterSpace
:param random_seed: int
"""
self.vehicle_config = PGConfig(vehicle_config)
# self.vehicle_config = self.get_vehicle_config(vehicle_config) \
# if vehicle_config is not None else self._default_vehicle_config()
# observation, action
self.action_space = self.get_action_space_before_init(
extra_action_dim=self.vehicle_config["extra_action_dim"])
super(BaseVehicle, self).__init__(random_seed, name=name)
# config info
self.set_config(self.PARAMETER_SPACE.sample())
if physics_config is not None:
self.set_config(physics_config)
self.increment_steering = self.vehicle_config["increment_steering"]
self.enable_reverse = self.vehicle_config["enable_reverse"]
self.max_speed = self.vehicle_config["max_speed"]
self.max_steering = self.vehicle_config["max_steering"]
self.pg_world = pg_world
self.node_path = NodePath("vehicle")
# create
self.spawn_place = (0, 0)
self._add_chassis(pg_world.physics_world)
self.wheels = self._create_wheel()
# modules
self.image_sensors = {}
self.lidar: Optional[Lidar] = None
self.side_detector: Optional[SideDetector] = None
self.lane_line_detector: Optional[LaneLineDetector] = None
self.routing_localization: Optional[RoutingLocalizationModule] = None
self.lane: Optional[AbstractLane] = None
self.lane_index = None
self.vehicle_panel = VehiclePanel(self.pg_world) if (
self.pg_world.mode == RENDER_MODE_ONSCREEN) else None
# state info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.spawn_place
self.last_heading_dir = self.heading
self.dist_to_left_side = None
self.dist_to_right_side = None
# collision info render
self.collision_info_np = self._init_collision_info_render(pg_world)
self.collision_banners = {} # to save time
self.current_banner = None
self.attach_to_pg_world(self.pg_world.pbr_render,
self.pg_world.physics_world)
# step info
self.out_of_route = None
self.on_lane = None
# self.step_info = None
self._init_step_info()
# others
self._add_modules_for_vehicle(self.vehicle_config["use_render"])
self.takeover = False
self._expert_takeover = False
self.energy_consumption = 0
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
def _add_modules_for_vehicle(self, use_render: bool):
# add self module for training according to config
vehicle_config = self.vehicle_config
self.add_routing_localization(
vehicle_config["show_navi_mark"]) # default added
if self.vehicle_config["side_detector"]["num_lasers"] > 0:
self.side_detector = SideDetector(
self.pg_world.render,
self.vehicle_config["side_detector"]["num_lasers"],
self.vehicle_config["side_detector"]["distance"],
self.vehicle_config["show_side_detector"])
if self.vehicle_config["lane_line_detector"]["num_lasers"] > 0:
self.lane_line_detector = LaneLineDetector(
self.pg_world.render,
self.vehicle_config["lane_line_detector"]["num_lasers"],
self.vehicle_config["lane_line_detector"]["distance"],
self.vehicle_config["show_lane_line_detector"])
if not self.vehicle_config["use_image"]:
if vehicle_config["lidar"]["num_lasers"] > 0 and vehicle_config[
"lidar"]["distance"] > 0:
self.add_lidar(
num_lasers=vehicle_config["lidar"]["num_lasers"],
distance=vehicle_config["lidar"]["distance"],
show_lidar_point=vehicle_config["show_lidar"])
else:
import logging
logging.warning(
"You have set the lidar config to: {}, which seems to be invalid!"
.format(vehicle_config["lidar"]))
if use_render:
rgb_cam_config = vehicle_config["rgb_cam"]
rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1],
self.chassis_np, self.pg_world)
self.add_image_sensor("rgb_cam", rgb_cam)
mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np,
self.pg_world)
self.add_image_sensor("mini_map", mini_map)
return
if vehicle_config["use_image"]:
# 3 types image observation
if vehicle_config["image_source"] == "rgb_cam":
rgb_cam_config = vehicle_config["rgb_cam"]
rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1],
self.chassis_np, self.pg_world)
self.add_image_sensor("rgb_cam", rgb_cam)
elif vehicle_config["image_source"] == "mini_map":
mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np,
self.pg_world)
self.add_image_sensor("mini_map", mini_map)
elif vehicle_config["image_source"] == "depth_cam":
cam_config = vehicle_config["depth_cam"]
depth_cam = DepthCamera(*cam_config, self.chassis_np,
self.pg_world)
self.add_image_sensor("depth_cam", depth_cam)
else:
raise ValueError("No module named {}".format(
vehicle_config["image_source"]))
# load more sensors for visualization when render, only for beauty...
if use_render:
if vehicle_config["image_source"] == "mini_map":
rgb_cam_config = vehicle_config["rgb_cam"]
rgb_cam = RGBCamera(rgb_cam_config[0], rgb_cam_config[1],
self.chassis_np, self.pg_world)
self.add_image_sensor("rgb_cam", rgb_cam)
else:
mini_map = MiniMap(vehicle_config["mini_map"], self.chassis_np,
self.pg_world)
self.add_image_sensor("mini_map", mini_map)
def _init_step_info(self):
# done info will be initialized every frame
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).init_collision_info()
self.out_of_route = False # re-route is required if is false
self.on_lane = True # on lane surface or not
# self.step_info = {"reward": 0, "cost": 0}
def _preprocess_action(self, action):
if self.vehicle_config["action_check"]:
assert self.action_space.contains(
action
), "Input {} is not compatible with action space {}!".format(
action, self.action_space)
# protect agent from nan error
action = safe_clip_for_small_array(action,
min_val=self.action_space.low[0],
max_val=self.action_space.high[0])
return action, {'raw_action': (action[0], action[1])}
def prepare_step(self, action):
"""
Save info and make decision before action
"""
# init step info to store info before each step
self._init_step_info()
action, step_info = self._preprocess_action(action)
self.last_position = self.position
self.last_heading_dir = self.heading
self.last_current_action.append(
action
) # the real step of physics world is implemented in taskMgr.step()
if self.increment_steering:
self.set_incremental_action(action)
else:
self.set_act(action)
if self.vehicle_panel is not None:
self.vehicle_panel.renew_2d_car_para_visualization(self)
return step_info
def update_state(self, pg_world=None, detector_mask="WRONG"):
# lidar
if self.lidar is not None:
self.lidar.perceive(self.position,
self.heading_theta,
self.pg_world.physics_world.dynamic_world,
extra_filter_node={self.chassis_np.node()},
detector_mask=detector_mask)
if self.routing_localization is not None:
self.lane, self.lane_index, = self.routing_localization.update_navigation_localization(
self)
if self.side_detector is not None:
self.side_detector.perceive(
self.position, self.heading_theta,
self.pg_world.physics_world.static_world)
if self.lane_line_detector is not None:
self.lane_line_detector.perceive(
self.position, self.heading_theta,
self.pg_world.physics_world.static_world)
self._state_check()
self.update_dist_to_left_right()
step_energy, episode_energy = self._update_energy_consumption()
self.out_of_route = self._out_of_route()
step_info = self._update_overtake_stat()
step_info.update({
"velocity": float(self.speed),
"steering": float(self.steering),
"acceleration": float(self.throttle_brake),
"step_energy": step_energy,
"episode_energy": episode_energy
})
return step_info
def _out_of_route(self):
left, right = self._dist_to_route_left_right()
return True if right < 0 or left < 0 else False
def _update_energy_consumption(self):
"""
The calculation method is from
https://www.researchgate.net/publication/262182035_Reduction_of_Fuel_Consumption_and_Exhaust_Pollutant_Using_Intelligent_Transport_System
default: 3rd gear, try to use ae^bx to fit it, dp: (90, 8), (130, 12)
:return: None
"""
distance = norm(*(self.last_position - self.position)) / 1000 # km
step_energy = 3.25 * math.pow(np.e, 0.01 * self.speed) * distance / 100
# step_energy is in Liter, we return mL
step_energy = step_energy * 1000
self.energy_consumption += step_energy # L/100 km
return step_energy, self.energy_consumption
def reset(self, map: Map, pos: np.ndarray = None, heading: float = 0.0):
"""
pos is a 2-d array, and heading is a float (unit degree)
if pos is not None, vehicle will be reset to the position
else, vehicle will be reset to spawn place
"""
if pos is None:
lane = map.road_network.get_lane(
self.vehicle_config["spawn_lane_index"])
pos = lane.position(self.vehicle_config["spawn_longitude"],
self.vehicle_config["spawn_lateral"])
heading = np.rad2deg(
lane.heading_at(self.vehicle_config["spawn_longitude"]))
self.spawn_place = pos
heading = -np.deg2rad(heading) - np.pi / 2
self.set_static(False)
self.chassis_np.setPos(panda_position(Vec3(*pos, 1)))
self.chassis_np.setQuat(
LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2)))
self.update_map_info(map)
self.chassis_np.node().clearForces()
self.chassis_np.node().setLinearVelocity(Vec3(0, 0, 0))
self.chassis_np.node().setAngularVelocity(Vec3(0, 0, 0))
self.system.resetSuspension()
# np.testing.assert_almost_equal(self.position, pos, decimal=4)
# done info
self._init_step_info()
# other info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.spawn_place
self.last_heading_dir = self.heading
self.update_dist_to_left_right()
self.takeover = False
self.energy_consumption = 0
# overtake_stat
self.front_vehicles = set()
self.back_vehicles = set()
# for render
if self.vehicle_panel is not None:
self.vehicle_panel.renew_2d_car_para_visualization(self)
if "depth_cam" in self.image_sensors and self.image_sensors[
"depth_cam"].view_ground:
for block in map.blocks:
block.node_path.hide(CamMask.DepthCam)
assert self.routing_localization
# Please note that if you respawn agent to some new place and might have a new destination,
# you should reset the routing localization too! Via: vehicle.routing_localization.set_route or
# vehicle.update
"""------------------------------------------- act -------------------------------------------------"""
def set_act(self, action):
steering = action[0]
self.throttle_brake = action[1]
self.steering = steering
self.system.setSteeringValue(self.steering * self.max_steering, 0)
self.system.setSteeringValue(self.steering * self.max_steering, 1)
self._apply_throttle_brake(action[1])
def set_incremental_action(self, action: np.ndarray):
self.throttle_brake = action[1]
self.steering += action[0] * self.STEERING_INCREMENT
self.steering = clip(self.steering, -1, 1)
steering = self.steering * self.max_steering
self.system.setSteeringValue(steering, 0)
self.system.setSteeringValue(steering, 1)
self._apply_throttle_brake(action[1])
def _apply_throttle_brake(self, throttle_brake):
max_engine_force = self.vehicle_config["max_engine_force"]
max_brake_force = self.vehicle_config["max_brake_force"]
for wheel_index in range(4):
if throttle_brake >= 0:
self.system.setBrake(2.0, wheel_index)
if self.speed > self.max_speed:
self.system.applyEngineForce(0.0, wheel_index)
else:
self.system.applyEngineForce(
max_engine_force * throttle_brake, wheel_index)
else:
if self.enable_reverse:
self.system.applyEngineForce(
max_engine_force * throttle_brake, wheel_index)
self.system.setBrake(0, wheel_index)
else:
self.system.applyEngineForce(0.0, wheel_index)
self.system.setBrake(
abs(throttle_brake) * max_brake_force, wheel_index)
"""---------------------------------------- vehicle info ----------------------------------------------"""
def update_dist_to_left_right(self):
self.dist_to_left_side, self.dist_to_right_side = self._dist_to_route_left_right(
)
def _dist_to_route_left_right(self):
current_reference_lane = self.routing_localization.current_ref_lanes[0]
_, lateral_to_reference = current_reference_lane.local_coordinates(
self.position)
lateral_to_left = lateral_to_reference + self.routing_localization.get_current_lane_width(
) / 2
lateral_to_right = self.routing_localization.get_current_lateral_range(
self.position, self.pg_world) - lateral_to_left
return lateral_to_left, lateral_to_right
@property
def position(self):
return pgdrive_position(self.chassis_np.getPos())
@property
def speed(self):
"""
km/h
"""
velocity = self.chassis_np.node().get_linear_velocity()
speed = norm(velocity[0], velocity[1]) * 3.6
return clip(speed, 0.0, 100000.0)
@property
def heading(self):
real_heading = self.heading_theta
# heading = np.array([math.cos(real_heading), math.sin(real_heading)])
heading = PGVector((math.cos(real_heading), math.sin(real_heading)))
return heading
@property
def heading_theta(self):
"""
Get the heading theta of vehicle, unit [rad]
:return: heading in rad
"""
return (pgdrive_heading(self.chassis_np.getH()) - 90) / 180 * math.pi
@property
def velocity(self) -> np.ndarray:
return self.speed * self.velocity_direction
@property
def velocity_direction(self):
direction = self.system.getForwardVector()
return np.asarray([direction[0], -direction[1]])
@property
def current_road(self):
return Road(*self.lane_index[0:-1])
"""---------------------------------------- some math tool ----------------------------------------------"""
def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
else:
raise ValueError("Unknown target lane type: {}".format(
type(target_lane)))
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def projection(self, vector):
# Projected to the heading of vehicle
# forward_vector = self.vehicle.get_forward_vector()
# forward_old = (forward_vector[0], -forward_vector[1])
forward = self.heading
# print(f"[projection] Old forward {forward_old}, new heading {forward}")
norm_velocity = norm(forward[0], forward[1]) + 1e-6
project_on_heading = (vector[0] * forward[0] +
vector[1] * forward[1]) / norm_velocity
side_direction = get_vertical_vector(forward)[1]
side_norm = norm(side_direction[0], side_direction[1]) + 1e-6
project_on_side = (vector[0] * side_direction[0] +
vector[1] * side_direction[1]) / side_norm
return project_on_heading, project_on_side
def lane_distance_to(self, vehicle, lane: AbstractLane = None) -> float:
assert self.routing_localization is not None, "a routing and localization module should be added " \
"to interact with other vehicles"
if not vehicle:
return np.nan
if not lane:
lane = self.lane
return lane.local_coordinates(
vehicle.position)[0] - lane.local_coordinates(self.position)[0]
"""-------------------------------------- for vehicle making ------------------------------------------"""
def _add_chassis(self, pg_physics_world: PGPhysicsWorld):
para = self.get_config()
self.LENGTH = self.vehicle_config["vehicle_length"]
self.WIDTH = self.vehicle_config["vehicle_width"]
chassis = BaseVehicleNode(BodyName.Base_vehicle, self)
chassis.setIntoCollideMask(BitMask32.bit(CollisionGroup.EgoVehicle))
chassis_shape = BulletBoxShape(
Vec3(self.WIDTH / 2, self.LENGTH / 2,
para[Parameter.vehicle_height] / 2))
ts = TransformState.makePos(
Vec3(0, 0, para[Parameter.chassis_height] * 2))
chassis.addShape(chassis_shape, ts)
heading = np.deg2rad(-para[Parameter.heading] - 90)
chassis.setMass(para[Parameter.mass])
self.chassis_np = self.node_path.attachNewNode(chassis)
# not random spawn now
self.chassis_np.setPos(Vec3(*self.spawn_place, 1))
self.chassis_np.setQuat(
LQuaternionf(math.cos(heading / 2), 0, 0, math.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(
True
) # advance collision check, do callback in pg_collision_callback
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Base_vehicle_beneath)
chassis_beneath.setIntoCollideMask(
BitMask32.bit(CollisionGroup.EgoVehicleBeneath))
chassis_beneath.addShape(chassis_shape)
self.chassis_beneath_np = self.chassis_np.attachNewNode(
chassis_beneath)
self.dynamic_nodes.append(chassis_beneath)
self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis)
self.system.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(
self.system
) # detach chassis will also detach system, so a waring will generate
if self.render:
if self.MODEL is None:
model_path = 'models/ferra/scene.gltf'
self.MODEL = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.MODEL.setZ(para[Parameter.vehicle_vis_z])
self.MODEL.setY(para[Parameter.vehicle_vis_y])
self.MODEL.setH(para[Parameter.vehicle_vis_h])
self.MODEL.set_scale(para[Parameter.vehicle_vis_scale])
self.MODEL.instanceTo(self.chassis_np)
def _create_wheel(self):
para = self.get_config()
f_l = para[Parameter.front_tire_longitude]
r_l = -para[Parameter.rear_tire_longitude]
lateral = para[Parameter.tire_lateral]
axis_height = para[Parameter.tire_radius] + 0.05
radius = para[Parameter.tire_radius]
wheels = []
for k, pos in enumerate([
Vec3(lateral, f_l, axis_height),
Vec3(-lateral, f_l, axis_height),
Vec3(lateral, r_l, axis_height),
Vec3(-lateral, r_l, axis_height)
]):
wheel = self._add_wheel(pos, radius, True if k < 2 else False,
True if k == 0 or k == 2 else False)
wheels.append(wheel)
return wheels
def _add_wheel(self, pos: Vec3, radius: float, front: bool, left):
wheel_np = self.node_path.attachNewNode("wheel")
if self.render:
# TODO something wrong with the wheel render
model_path = 'models/yugo/yugotireR.egg' if left else 'models/yugo/yugotireL.egg'
wheel_model = self.loader.loadModel(
AssetLoader.file_path(model_path))
wheel_model.reparentTo(wheel_np)
wheel_model.set_scale(1.4, radius / 0.25, radius / 0.25)
wheel = self.system.create_wheel()
wheel.setNode(wheel_np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
# TODO add them to PGConfig in the future
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40)
wheel.setSuspensionStiffness(30)
wheel.setWheelsDampingRelaxation(4.8)
wheel.setWheelsDampingCompression(1.2)
wheel.setFrictionSlip(self.vehicle_config["wheel_friction"])
wheel.setRollInfluence(1.5)
return wheel
def add_image_sensor(self, name: str, sensor: ImageBuffer):
self.image_sensors[name] = sensor
def add_lidar(self, num_lasers=240, distance=50, show_lidar_point=False):
assert num_lasers > 0
assert distance > 0
self.lidar = Lidar(self.pg_world.render, num_lasers, distance,
show_lidar_point)
def add_routing_localization(self, show_navi_mark: bool = False):
config = self.vehicle_config
self.routing_localization = RoutingLocalizationModule(
self.pg_world,
show_navi_mark=show_navi_mark,
random_navi_mark_color=config["random_navi_mark_color"],
show_dest_mark=config["show_dest_mark"],
show_line_to_dest=config["show_line_to_dest"])
def update_map_info(self, map):
"""
Update map info after reset()
:param map: new map
:return: None
"""
lane, new_l_index = ray_localization(np.array(self.heading.tolist()),
np.array(self.spawn_place),
self.pg_world)
self.routing_localization.update(map, current_lane_index=new_l_index)
assert lane is not None, "spawn place is not on road!"
self.lane_index = new_l_index
self.lane = lane
def _state_check(self):
"""
Check States and filter to update info
"""
result_1 = self.pg_world.physics_world.static_world.contactTest(
self.chassis_beneath_np.node(), True)
result_2 = self.pg_world.physics_world.dynamic_world.contactTest(
self.chassis_beneath_np.node(), True)
contacts = set()
for contact in result_1.getContacts() + result_2.getContacts():
node0 = contact.getNode0()
node1 = contact.getNode1()
name = [node0.getName(), node1.getName()]
name.remove(BodyName.Base_vehicle_beneath)
if name[0] == "Ground" or name[0] == BodyName.Lane:
continue
if name[0] == BodyName.Sidewalk:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_sidewalk = True
elif name[0] == BodyName.White_continuous_line:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_white_continuous_line = True
elif name[0] == BodyName.Yellow_continuous_line:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_yellow_continuous_line = True
elif name[0] == BodyName.Broken_line:
self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_broken_line = True
contacts.add(name[0])
if self.render:
self.render_collision_info(contacts)
@staticmethod
def _init_collision_info_render(pg_world):
if pg_world.mode == "onscreen":
info_np = NodePath("Collision info nodepath")
info_np.reparentTo(pg_world.aspect2d)
else:
info_np = None
return info_np
def render_collision_info(self, contacts):
contacts = sorted(list(contacts),
key=lambda c: COLLISION_INFO_COLOR[COLOR[c]][0])
text = contacts[0] if len(contacts) != 0 else None
if text is None:
text = "Normal" if time.time(
) - self.pg_world._episode_start_time > 10 else "Press H to see help message"
self.render_banner(text, COLLISION_INFO_COLOR["green"][1])
else:
if text == BodyName.Base_vehicle_beneath:
text = BodyName.Traffic_vehicle
self.render_banner(text, COLLISION_INFO_COLOR[COLOR[text]][1])
def render_banner(self, text, color=COLLISION_INFO_COLOR["green"][1]):
"""
Render the banner in the left bottom corner.
"""
if self.collision_info_np is None:
return
if self.current_banner is not None:
self.current_banner.detachNode()
if text in self.collision_banners:
self.collision_banners[text].reparentTo(self.collision_info_np)
self.current_banner = self.collision_banners[text]
else:
new_banner = NodePath(TextNode("collision_info:{}".format(text)))
self.collision_banners[text] = new_banner
text_node = new_banner.node()
text_node.setCardColor(color)
text_node.setText(text)
text_node.setCardActual(-5 * self.pg_world.w_scale,
5.1 * self.pg_world.w_scale, -0.3, 1)
text_node.setCardDecal(True)
text_node.setTextColor(1, 1, 1, 1)
text_node.setAlign(TextNode.A_center)
new_banner.setScale(0.05)
new_banner.setPos(-0.75 * self.pg_world.w_scale, 0,
-0.8 * self.pg_world.h_scale)
new_banner.reparentTo(self.collision_info_np)
self.current_banner = new_banner
def destroy(self, _=None):
self.chassis_np.node().getPythonTag(BodyName.Base_vehicle).destroy()
if self.chassis_np.node() in self.dynamic_nodes:
self.dynamic_nodes.remove(self.chassis_np.node())
super(BaseVehicle, self).destroy(self.pg_world)
self.pg_world.physics_world.dynamic_world.clearContactAddedCallback()
self.routing_localization.destroy()
self.routing_localization = None
if self.side_detector is not None:
self.side_detector.destroy()
if self.lane_line_detector is not None:
self.lane_line_detector.destroy()
self.side_detector = None
self.lane_line_detector = None
if self.lidar is not None:
self.lidar.destroy()
self.lidar = None
if len(self.image_sensors) != 0:
for sensor in self.image_sensors.values():
sensor.destroy(self.pg_world)
self.image_sensors = None
if self.vehicle_panel is not None:
self.vehicle_panel.destroy(self.pg_world)
self.pg_world = None
def set_position(self, position, height=0.4):
"""
Should only be called when restore traffic from episode data
:param position: 2d array or list
:return: None
"""
self.chassis_np.setPos(panda_position(position, height))
def set_heading(self, heading_theta) -> None:
"""
Should only be called when restore traffic from episode data
:param heading_theta: float in rad
:return: None
"""
self.chassis_np.setH((panda_heading(heading_theta) * 180 / np.pi) - 90)
def get_state(self):
return {
"heading":
self.heading_theta,
"position":
self.position.tolist(),
"done":
self.crash_vehicle or self.out_of_route or self.crash_sidewalk
or not self.on_lane
}
def set_state(self, state: dict):
self.set_heading(state["heading"])
self.set_position(state["position"])
def _update_overtake_stat(self):
if self.vehicle_config["overtake_stat"]:
surrounding_vs = self.lidar.get_surrounding_vehicles()
routing = self.routing_localization
ckpt_idx = routing._target_checkpoints_index
for surrounding_v in surrounding_vs:
if surrounding_v.lane_index[:-1] == (
routing.checkpoints[ckpt_idx[0]],
routing.checkpoints[ckpt_idx[1]]):
if self.lane.local_coordinates(self.position)[0] - \
self.lane.local_coordinates(surrounding_v.position)[0] < 0:
self.front_vehicles.add(surrounding_v)
if surrounding_v in self.back_vehicles:
self.back_vehicles.remove(surrounding_v)
else:
self.back_vehicles.add(surrounding_v)
return {"overtake_vehicle_num": self.get_overtake_num()}
def get_overtake_num(self):
return len(self.front_vehicles.intersection(self.back_vehicles))
@classmethod
def get_action_space_before_init(cls, extra_action_dim: int = 0):
return gym.spaces.Box(-1.0,
1.0,
shape=(2 + extra_action_dim, ),
dtype=np.float32)
def remove_display_region(self):
if self.render:
self.vehicle_panel.remove_display_region(self.pg_world)
self.vehicle_panel.buffer.set_active(False)
self.collision_info_np.detachNode()
self.routing_localization._arrow_node_path.detachNode()
for sensor in self.image_sensors.values():
sensor.remove_display_region(self.pg_world)
sensor.buffer.set_active(False)
def add_to_display(self):
if self.render:
self.vehicle_panel.add_to_display(
self.pg_world, self.vehicle_panel.default_region)
self.vehicle_panel.buffer.set_active(True)
self.collision_info_np.reparentTo(self.pg_world.aspect2d)
self.routing_localization._arrow_node_path.reparentTo(
self.pg_world.aspect2d)
for sensor in self.image_sensors.values():
sensor.add_to_display(self.pg_world, sensor.default_region)
sensor.buffer.set_active(True)
def __del__(self):
super(BaseVehicle, self).__del__()
self.pg_world = None
self.lidar = None
self.mini_map = None
self.rgb_cam = None
self.routing_localization = None
self.wheels = None
@property
def arrive_destination(self):
long, lat = self.routing_localization.final_lane.local_coordinates(
self.position)
flag = (self.routing_localization.final_lane.length - 5 < long <
self.routing_localization.final_lane.length + 5) and (
self.routing_localization.get_current_lane_width() / 2 >=
lat >=
(0.5 - self.routing_localization.get_current_lane_num()) *
self.routing_localization.get_current_lane_width())
return flag
@property
def crash_vehicle(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_vehicle
@property
def crash_object(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_object
@property
def crash_sidewalk(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).crash_sidewalk
@property
def on_yellow_continuous_line(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_yellow_continuous_line
@property
def on_white_continuous_line(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_white_continuous_line
@property
def on_broken_line(self):
return self.chassis_np.node().getPythonTag(
BodyName.Base_vehicle).on_broken_line
def set_static(self, flag):
self.chassis_np.node().setStatic(flag)
@property
def reference_lanes(self):
return self.routing_localization.current_ref_lanes
def set_wheel_friction(self, new_friction):
raise DeprecationWarning("Bug exists here")
for wheel in self.wheels:
wheel.setFrictionSlip(new_friction)
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)
"""
def load_vehicle(self, pos=(0.0, -20.0, -0.6), quat=None):
# Chassis
self._mass = self.params['mass']
#chassis_shape = self.params['chassis_shape']
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
self.vehicle_pointer = self.worldNP.attachNewNode(
BulletRigidBodyNode('Vehicle'))
self.vehicle_node = self.vehicle_pointer.node()
self.vehicle_node.addShape(shape, ts)
self.previous_pos = pos
self.vehicle_pointer.setPos(pos[0], pos[1], pos[2])
if quat is not None:
self.vehicle_pointer.setQuat(quat)
self.previous_quat = self.vehicle_pointer.getQuat()
self.vehicle_node.setMass(self._mass)
self.vehicle_node.setDeactivationEnabled(False)
# first_person = self.params['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.setPos(0.0, 1.0, 1.0)
# self.camera_node.lookAt(0.0, 6.0, 0.0)
# else:
# self.camera_node.setPos(0.0, -10.0, 5.0)
# self.camera_node.lookAt(0.0, 5.0, 0.0)
self.camera_node.reparentTo(self.vehicle_pointer)
self.camera_node.setPos(0.0, 1.0, 1.0)
self.camera_node.lookAt(0.0, 6.0, 0.0)
self.back_camera_sensor = Panda3dCameraSensor(base,
color=True,
depth=True,
size=(160, 90))
self.back_camera_node = self.back_camera_sensor.cam
# if first_person:
# self.camera_node.setPos(0.0, 1.0, 1.0)
# self.camera_node.lookAt(0.0, 6.0, 0.0)
# else:
# self.camera_node.setPos(0.0, -10.0, 5.0)
# self.camera_node.lookAt(0.0, 5.0, 0.0)
self.back_camera_node.reparentTo(self.vehicle_pointer)
self.back_camera_node.setPos(0.0, -1.0, 1.0)
self.back_camera_node.lookAt(0.0, -6.0, 0.0)
self.world.attachRigidBody(self.vehicle_node)
self.vehicle_node.setCcdSweptSphereRadius(1.0)
self.vehicle_node.setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(self.world, self.vehicle_node)
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('../models/yugo/yugo.egg')
self.yugoNP.reparentTo(self.vehicle_pointer)
self._wheels = []
# 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)
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))
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 CarPhys(PhysColleague):
def __init__(self, mediator, car_props, tuning, players):
PhysColleague.__init__(self, mediator)
self.pnode = self.vehicle = self.__track_phys = self.coll_mesh = \
self.max_speed = self.friction_slip = \
self.friction_slip_rear = self.cfg = None
self.turbo = False
self._tuning = tuning
self._players = players
self.roll_influence = []
self.ai_meshes = []
self.curr_speed_mul = 1.0
self.roll_influence_k = self.friction_slip_k = 1.0
self.__prev_speed = 0
self.__last_drift_time = 0
self.__finds = {} # cache for find's results
self.__whl2flytime = {}
self.cprops = car_props
self._load_phys()
self.__set_collision_mesh()
self.__set_ai_meshes()
self.__set_phys_node()
self.__set_vehicle()
self.__set_wheels()
self.eng.attach_obs(self.on_end_frame)
def _load_phys(self):
ppath = self.cprops.race_props.season_props.gameprops.phys_path
fpath = ppath % self.cprops.name
with open(fpath) as phys_file:
self.cfg = load(phys_file)
# they may be changed by drivers and tuning
self.cfg['max_speed'] = self.get_speed()
self.cfg['friction_slip'] = self.get_friction_static()[0]
self.cfg['friction_slip_rear'] = self.get_friction_static()[1]
self.cfg['roll_influence'] = self.get_roll_influence_static()
self.friction_slip = self.cfg['friction_slip']
self.friction_slip_rear = self.cfg['friction_slip_rear']
self.__log_props()
set_a = lambda field: setattr(self, field, self.cfg[field])
list(map(set_a, self.cfg.keys()))
def __log_props(self, starting=True):
s_s = self.cfg['max_speed'] if starting else self.max_speed
s_f = self.cfg['friction_slip'] if starting else \
self.get_friction_static()[0]
s_fr = self.cfg['friction_slip_rear'] if starting else \
self.get_friction_static()[1]
s_r = self.cfg['roll_influence'] if starting else \
self.get_roll_influence_static()
log_info = [('speed', self.cprops.name, round(s_s, 2),
self.cprops.driver_engine),
('friction 0', self.cprops.name, round(s_f[0], 2),
self.cprops.driver_tires),
('friction 1', self.cprops.name, round(s_f[1], 2),
self.cprops.driver_tires),
('friction_rear 0', self.cprops.name, round(s_fr[0], 2),
self.cprops.driver_tires),
('friction_rear 1', self.cprops.name, round(s_fr[1], 2),
self.cprops.driver_tires),
('roll min', self.cprops.name, round(s_r[0], 2),
self.cprops.driver_suspensions),
('roll max', self.cprops.name, round(s_r[1], 2),
self.cprops.driver_suspensions)]
for l_i in log_info:
info('%s %s: %s (%s)' % l_i)
def __set_collision_mesh(self):
fpath = self.cprops.race_props.coll_path % self.cprops.name
try:
self.coll_mesh = self.eng.load_model(fpath)
except OSError: # new cars don't have collision meshes
self.coll_mesh = self.eng.load_model(
fpath.replace('capsule', 'car'))
# chassis_shape = BulletConvexHullShape()
# for geom in self.eng.lib.find_geoms(
# self.coll_mesh, self.cprops.race_props.coll_name):
# chassis_shape.add_geom(geom.node().get_geom(0),
# geom.get_transform())
# self.mediator.gfx.nodepath.get_node().add_shape(chassis_shape)
chassis_shape = BulletBoxShape(tuple(self.cfg['box_size']))
boxpos = self.cfg['box_pos']
boxpos[2] += self.cfg['center_mass_offset']
pos = TransformState.makePos(tuple(boxpos))
self.mediator.gfx.nodepath.p3dnode.add_shape(chassis_shape, pos)
car_names = [player.car for player in self._players]
car_idx = car_names.index(self.cprops.name)
car_bit = BitMask32.bit(BitMasks.car(car_idx))
ghost_bit = BitMask32.bit(BitMasks.ghost)
track_bit = BitMask32.bit(BitMasks.track_merged)
mask = car_bit | ghost_bit | track_bit
self.mediator.gfx.nodepath.set_collide_mask(mask)
def __set_ai_meshes(self):
return
# if we attach these meshes (or even only one mesh, box, sphere,
# whatever) then the collision between the goal and the vehicle doesn't
# work properly
h = .5
boxsz = self.cfg['box_size']
hs = []
hs += [
h / 2 + boxsz[2] * 2 + self.cfg['box_pos'][2] +
self.cfg['center_mass_offset']
]
hs += [
-h / 2 - boxsz[2] * 2 + self.cfg['box_pos'][2] +
self.cfg['center_mass_offset']
]
for _h in hs:
shape = BulletBoxShape((boxsz[0], boxsz[1], h))
ghost = GhostNode('Vehicle')
pos = TransformState.makePos((0, 0, _h))
ghost.node.addShape(shape, pos)
self.ai_meshes += [self.eng.attach_node(ghost.node)]
car_names = self.cprops.race_props.season_props.car_names
car_idx = car_names.index(self.cprops.name)
car_bit = BitMask32.bit(BitMasks.car(car_idx))
ghost_bit = BitMask32.bit(BitMasks.ghost)
mask = car_bit | ghost_bit
self.ai_meshes[-1].set_collide_mask(mask)
self.eng.phys_mgr.attach_ghost(ghost.node)
def __set_phys_node(self):
self.pnode = self.mediator.gfx.nodepath.p3dnode
self.pnode.set_mass(self.mass) # default 0
self.pnode.set_deactivation_enabled(False)
self.eng.phys_mgr.attach_rigid_body(self.pnode)
self.eng.phys_mgr.add_collision_obj(self.pnode)
def __set_vehicle(self):
self.vehicle = BulletVehicle(self.eng.phys_mgr.root._wld, self.pnode)
# access to a protected member
self.vehicle.set_coordinate_system(ZUp)
self.vehicle.set_pitch_control(self.pitch_control)
tuning = self.vehicle.get_tuning()
tuning.set_suspension_compression(self.suspension_compression)
# default .83
tuning.set_suspension_damping(self.suspension_damping)
# default .88
self.eng.phys_mgr.attach_vehicle(self.vehicle)
def __set_wheels(self):
wheels = self.mediator.gfx.wheels
f_bounds = wheels['fr'].tight_bounds
f_radius = (f_bounds[1][2] - f_bounds[0][2]) / 2.0 + .01
r_bounds = wheels['rr'].tight_bounds
r_radius = (r_bounds[1][2] - r_bounds[0][2]) / 2.0 + .01
wheel_names = self.cprops.race_props.wheel_names
ffr = self.coll_mesh.find('**/' + wheel_names.frontrear.fr)
ffl = self.coll_mesh.find('**/' + wheel_names.frontrear.fl)
rrr = self.coll_mesh.find('**/' + wheel_names.frontrear.rr)
rrl = self.coll_mesh.find('**/' + wheel_names.frontrear.rl)
meth = self.coll_mesh.find
fr_node = ffr if ffr else meth('**/' + wheel_names.both.fr)
fl_node = ffl if ffl else meth('**/' + wheel_names.both.fl)
rr_node = rrr if rrr else meth('**/' + wheel_names.both.rr)
rl_node = rrl if rrl else meth('**/' + wheel_names.both.rl)
if not fr_node: # new cars
fr_node = meth('**/w_fr')
fl_node = meth('**/w_fl')
rr_node = meth('**/w_rr')
rl_node = meth('**/w_rl')
offset = self.cfg['center_mass_offset']
fr_pos = fr_node.get_pos() + (0, 0, f_radius + offset)
fl_pos = fl_node.get_pos() + (0, 0, f_radius + offset)
rr_pos = rr_node.get_pos() + (0, 0, r_radius + offset)
rl_pos = rl_node.get_pos() + (0, 0, r_radius + offset)
wheels_info = [(fr_pos, True, wheels['fr'], f_radius),
(fl_pos, True, wheels['fl'], f_radius),
(rr_pos, False, wheels['rr'], r_radius),
(rl_pos, False, wheels['rl'], r_radius)]
for i, (pos, front, nodepath, radius) in enumerate(wheels_info):
self.__add_wheel(pos, front, nodepath.p3dnode, radius, i)
def __add_wheel(self, pos, is_front, node, radius, i):
whl = self.vehicle.create_wheel()
whl.set_node(node)
whl.set_chassis_connection_point_cs(LPoint3f(*pos))
whl.set_front_wheel(is_front)
whl.set_wheel_direction_cs((0, 0, -1))
whl.set_wheel_axle_cs((1, 0, 0))
whl.set_wheel_radius(radius)
whl.set_suspension_stiffness(self.suspension_stiffness[0])
# default 5.88
whl.set_wheels_damping_relaxation(self.wheels_damping_relaxation[0])
# default .88
whl.set_wheels_damping_compression(self.wheels_damping_compression[0])
# default .83
idx = 0 if is_front else 1
whl.set_friction_slip(self.get_friction_static()[idx][0])
# default 10.5
# friction slip high -> more adherence
whl.set_roll_influence(self.roll_influence[0])
# low -> more stability # default .1
whl.set_max_suspension_force(self.max_suspension_force)
# default 6000
whl.set_max_suspension_travel_cm(self.max_suspension_travel_cm)
# default 500
whl.set_skid_info(self.skid_info) # default 0
self.__whl2flytime[i] = 0
@property
def lateral_force(self):
vel = self.vehicle.get_chassis().get_linear_velocity()
rot_mat = Mat4()
rot_mat.setRotateMat(-90, (0, 0, 1))
car_lat = rot_mat.xformVec(self.mediator.logic.car_vec._vec)
# access to a protected member
proj_frc = vel.project(car_lat)
return proj_frc.length()
@property
def lin_vel(self):
return self.vehicle.get_chassis().get_linear_velocity().length() * 3.6
@property
def is_flying(self): # no need to be cached
rays = [whl.get_raycast_info() for whl in self.vehicle.get_wheels()]
return not any(ray.is_in_contact() for ray in rays)
@property
def is_drifting(self):
return self.lateral_force > 4.0
@property
def last_drift_time(self):
return self.__last_drift_time
@property
def prev_speed(self):
return self.__prev_speed
@property
def prev_speed_ratio(self):
return max(0, min(1.0, self.prev_speed / self.max_speed))
def on_end_frame(self):
self.__prev_speed = self.speed
@property
def speed(self):
if self.mediator.fsm.getCurrentOrNextState() == 'Countdown':
return 0 # getCurrentSpeedKmHour returns odd values otherwise
return self.vehicle.get_current_speed_km_hour()
@property
def speed_ratio(self):
return max(0, min(1.0, self.speed / self.max_speed))
@property
def lin_vel_ratio(self):
return max(0, min(1.0, self.lin_vel / self.max_speed))
def set_forces(self, eng_frc, brake_frc, brk_ratio, steering):
idx = 1 if self.mediator.logic.is_drifting else 0
eng_frc_ratio = self.engine_acc_frc_ratio[idx]
self.vehicle.set_steering_value(steering, 0)
self.vehicle.set_steering_value(steering, 1)
self.vehicle.apply_engine_force(eng_frc * eng_frc_ratio, 0)
self.vehicle.apply_engine_force(eng_frc * eng_frc_ratio, 1)
self.vehicle.apply_engine_force(eng_frc * (1 - eng_frc_ratio), 2)
self.vehicle.apply_engine_force(eng_frc * (1 - eng_frc_ratio), 3)
self.vehicle.set_brake((1 - brk_ratio) * brake_frc, 2)
self.vehicle.set_brake((1 - brk_ratio) * brake_frc, 3)
self.vehicle.set_brake(brk_ratio * brake_frc, 0)
self.vehicle.set_brake(brk_ratio * brake_frc, 1)
def update_car_props(self):
wheels = zip(self.vehicle.get_wheels(), range(4))
speeds = list(map(lambda whi: self.__update_whl_props(*whi), wheels))
speeds = [speed for speed in speeds if speed]
if self.is_drifting:
self.__last_drift_time = self.eng.curr_time
self.curr_speed_mul = (sum(speeds) / len(speeds)) if speeds else 1.0
def __update_whl_props(self, whl, i):
susp_min = self.suspension_stiffness[0]
susp_max = self.suspension_stiffness[1]
susp_diff = susp_max - susp_min
whl.set_suspension_stiffness(susp_min + self.speed_ratio * susp_diff)
relax_min = self.wheels_damping_relaxation[0]
relax_max = self.wheels_damping_relaxation[1]
relax_diff = relax_max - relax_min
relax = relax_min + self.speed_ratio * relax_diff
whl.set_wheels_damping_relaxation(relax)
compr_min = self.wheels_damping_compression[0]
compr_max = self.wheels_damping_compression[1]
compr_diff = compr_max - compr_min
compr = compr_min + self.speed_ratio * compr_diff
whl.set_wheels_damping_compression(compr)
roll_infl_min = self.roll_influence[0]
roll_infl_max = self.roll_influence[1]
roll_infl_diff = roll_infl_max - roll_infl_min
roll_infl = roll_infl_min + self.speed_ratio * roll_infl_diff
whl.set_roll_influence(self.roll_influence_k * roll_infl)
contact_pt = whl.get_raycast_info().getContactPointWs()
gnd_name = self.gnd_name(contact_pt)
if not gnd_name or gnd_name in ['Vehicle', 'Wall', 'Respawn']:
return
if gnd_name not in self.__finds:
gnd = self.cprops.race.track.phys.model.find('**/' + gnd_name)
self.__finds[gnd_name] = gnd
gfx_node = self.__finds[gnd_name]
if not gfx_node:
print('ground error', gnd_name)
return
fric = 1.0
if gfx_node.has_tag('friction'):
fric = float(gfx_node.get_tag('friction'))
if not whl.get_raycast_info().is_in_contact():
self.__whl2flytime[i] = self.eng.curr_time
gnd_time = self.eng.curr_time - self.__whl2flytime[i]
gnd_recovery_time = .2 if whl.is_front_wheel() else .1
gnd_factor = min(1, gnd_time / gnd_recovery_time)
idx = 0 if whl.is_front_wheel() else 1
turbo_factor = 1.24 if self.turbo else 1.0
whl.setFrictionSlip(self.get_friction()[idx] * fric * gnd_factor *
turbo_factor)
if gfx_node.has_tag('speed'):
return float(gfx_node.get_tag('speed'))
@property
def gnd_names(self): # no need to be cached
whls = self.vehicle.get_wheels()
pos = list(
map(lambda whl: whl.get_raycast_info().get_contact_point_ws(),
whls))
return list(map(self.gnd_name, pos))
@staticmethod
def gnd_name(pos):
top, bottom = pos + (0, 0, 20), pos + (0, 0, -20)
result = CarPhys.eng.phys_mgr.ray_test_closest(bottom, top)
ground = result.get_node()
return ground.get_name() if ground else ''
@staticmethod
def gnd_height(pos): # this should be a method of the track
top, bottom = pos + (0, 0, 20), pos + (0, 0, -20)
result = CarPhys.eng.phys_mgr.ray_test_closest(bottom, top)
hit_pos = result.get_hit_pos()
return hit_pos.z if hit_pos else None
def apply_damage(self, reset=False):
# wheels = self.vehicle.get_wheels()
if reset:
self.max_speed = self.get_speed()
self.friction_slip_k = 1.0
self.roll_influence_k = 1.0
else:
self.max_speed *= .95
self.friction_slip_k *= .95
self.roll_influence_k *= 1.05
# map(lambda whl: whl.set_friction_slip(self.friction_slip), wheels)
self.__log_props(False)
def get_speed(self):
return self.cfg['max_speed'] * (1 + .01 * self.cprops.driver_engine)
def get_friction(self):
k = (1 + .01 * self.cprops.driver_tires)
return self.friction_slip[0] * k, self.friction_slip_rear[0] * k
def get_roll_influence_static(self):
min_r = self.cfg['roll_influence'][0]
max_r = self.cfg['roll_influence'][1]
k = 1 + .01 * self.cprops.driver_suspensions
return [min_r * k, max_r * k]
def get_friction_static(self):
k = 1 + .01 * self.cprops.driver_tires
fstr = 'friction_slip'
return [(self.cfg[fstr][0] * k, self.cfg[fstr][1] * k),
(self.cfg[fstr + '_rear'][0] * k,
self.cfg[fstr + '_rear'][1] * k)]
def get_roll_influence(self):
min_r = self.cfg['roll_influence'][0]
max_r = self.cfg['roll_influence'][1]
diff_r = max_r - min_r
curr_r = min_r + self.speed_ratio * diff_r
return curr_r * (1 + .01 * self.cprops.driver_suspensions)
def rotate(self):
self.pnode.apply_torque((0, 0, 80000))
self.mediator.logic.applied_torque = True
def destroy(self):
self.eng.detach_obs(self.on_end_frame)
self.eng.phys_mgr.remove_vehicle(self.vehicle)
self.pnode = self.vehicle = self.__finds = self.__track_phys = \
self.coll_mesh = None
PhysColleague.destroy(self)
class Vehicle(object):
COUNT = 0
def __init__(self, main, username, pos = LVecBase3(-5, -5, 1), isCurrentPlayer = False, carId=3):
self.username = username
self.main = main
self.isCurrentPlayer = isCurrentPlayer
self.boostCount = 0
self.boostActive = False
self.boostStep = 2
self.boostDuration = 0
self.moveStartTime = self.startTime = self.boostStartTime = time.time()
self.pos = pos
self.boostFactor = 1.2
self.specs = {"mass": 800.0,
"maxWheelForce": 2000.0,
"brakeForce": 100.0,
"steeringLock": 45.0,
"maxSpeed": 33.0,
"maxReverseSpeed": 10.0}
self.vehicleControlState = {"throttle": 0, "reverse": False, "brake": 0.0, "steering": 0.0, "health": 1}
# Steering change per second, normalised to steering lock
# Eg. 45 degrees lock and 1.0 rate means 45 degrees per second
self.steeringRate = 0.8
self.centreingRate = 1.2
self.speed = 0
self.setupVehicle(main)
self.props = VehicleProps(carId)
self.currentPowerups = {"powerup1": None, "powerup2": None, "powerup3": None}
if isCurrentPlayer:
#This command is required for Panda to render particles
base.enableParticles()
self.p = ParticleEffect()
# self.loadParticleConfig('steam.ptf')
def setPropHealth(self, health):
self.props.setHealth(health)
if not self.isCurrentPlayer:
self.main.updateStatusBars(self.username, self.props.health)
def loadParticleConfig(self, file):
#Start of the code from steam.ptf
self.p.cleanup()
self.p = ParticleEffect()
self.p.loadConfig(Filename(file))
# print type(main.worldNp)
self.p.softStart()
self.p.start(self.yugoNP)
# self.p.setPos(0.000, -0.700, 0.250)
self.p.setPos(0.000, -0.700, 0)
#self.setupVehicle(bulletWorld)
self.startTime = time.time()
#COUNT = 1
def move(self, steering, wheelForce, brakeForce, x, y, z, h, p, r):
self.applyForcesAndSteering(steering, wheelForce, brakeForce)
self.endTime = time.time()
#print self.endTime
elapsed = self.endTime - self.moveStartTime
#self.startTime = self.endTime
#if elapsed > 1:
self.moveStartTime = self.endTime
if not self.isCurrentPlayer:
self.setVehiclePos(x, y, z, h, p, r)
#print "Do Move"
def applyForcesAndSteering(self, steering, wheelForce, brakeForce):
# Apply steering to front wheels
self.vehicle.setSteeringValue(steering, 0);
self.vehicle.setSteeringValue(steering, 1);
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(wheelForce, 2);
self.vehicle.applyEngineForce(wheelForce, 3);
self.vehicle.setBrake(brakeForce, 2);
self.vehicle.setBrake(brakeForce, 3);
def addBoost(self):
if self.boostCount > 0:
self.boostCount -= 1
if not self.boostActive:
self.boostStartTime = time.time()
self.boostActive = True
self.boostDuration += self.boostStep
def checkDisableBoost(self):
if time.time() - self.boostStartTime > self.boostDuration:
self.boostActive = False
def reset(self):
self.chassisNP.setP(0)
self.chassisNP.setR(0)
def processInput(self, inputState, dt):
# print self.chassisNP.getPos()
#print self.chassisNP.getH()
"""Use controls to update the player's car"""
# For keyboard throttle and brake are either 0 or 1
self.checkDisableBoost()
if inputState.isSet('forward'):
self.vehicleControlState["throttle"] = 1.0
else:
self.vehicleControlState["throttle"] = 0.0
velocity = self.chassisNode.getLinearVelocity()
speed = math.sqrt(sum(v ** 2 for v in velocity))
self.speed = speed
# Update braking and reversing
if inputState.isSet('brake'):
if speed < 0.5 or self.vehicleControlState["reverse"]:
# If we're stopped, then start reversing
# Also keep reversing if we already were
self.vehicleControlState["reverse"] = True
self.vehicleControlState["throttle"] = 1.0
self.vehicleControlState["brake"] = 0.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 1.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 0.0
# steering is normalised from -1 to 1, corresponding
# to the steering lock right and left
steering = self.vehicleControlState["steering"]
if inputState.isSet('left'):
steering += dt * self.steeringRate
steering = min(steering, 1.0)
elif inputState.isSet('right'):
steering -= dt * self.steeringRate
steering = max(steering, -1.0)
else:
# gradually re-center the steering
if steering > 0.0:
steering -= dt * self.centreingRate
if steering < 0.0:
steering = 0.0
elif steering < 0.0:
steering += dt * self.centreingRate
if steering > 0.0:
steering = 0.0
self.vehicleControlState["steering"] = steering
# """Updates acceleration, braking and steering
# These are all passed in through a controlState dictionary
# """
# Update acceleration and braking
self.reversing = self.vehicleControlState["reverse"]
brakeForce = self.vehicleControlState["brake"] * self.specs["brakeForce"]
if self.reversing and self.speed > self.specs["maxReverseSpeed"]:
self.applyForcesAndSteering(steering, 0, brakeForce)
return
if not self.reversing and self.speed > self.specs["maxSpeed"]:
self.applyForcesAndSteering(steering, 0, brakeForce)
return
wheelForce = self.vehicleControlState["throttle"] * self.specs["maxWheelForce"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
# Update steering
# Steering control state is from -1 to 1
steering = self.vehicleControlState["steering"] * self.specs["steeringLock"]
if self.boostActive:
wheelForce *= self.boostFactor
self.applyForcesAndSteering(steering, wheelForce, brakeForce)
return [steering, wheelForce, brakeForce]
def getSpeed(self):
velocity = self.chassisNode.getLinearVelocity()
speed = math.sqrt(sum(v ** 2 for v in velocity))
return speed, speed/self.specs["maxSpeed"]
def getBoost(self):
maxBoost = 3.0
currentScaledBoost = self.boostCount / maxBoost
return currentScaledBoost
def updateHealth(self, damage):
self.vehicleControlState["health"] -= 0.25
if self.vehicleControlState["health"] <= 0.0:
self.killVehicle("Lost health")
def killVehicle(self, reason = ""):
print "Sent request to server for killing this player because: ", reason
def updateMovement(self, move, dt):
"""Use controls to update the player's car"""
# For keyboard throttle and brake are either 0 or 1
if move == 'f':
self.vehicleControlState["throttle"] = 1.0
else:
self.vehicleControlState["throttle"] = 0.0
velocity = self.chassisNode.getLinearVelocity()
speed = math.sqrt(sum(v ** 2 for v in velocity))
# Update braking and reversing
if move == 'b':
if speed < 0.5 or self.vehicleControlState["reverse"]:
# If we're stopped, then start reversing
# Also keep reversing if we already were
self.vehicleControlState["reverse"] = True
self.vehicleControlState["throttle"] = 1.0
self.vehicleControlState["brake"] = 0.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 1.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 0.0
# steering is normalised from -1 to 1, corresponding
# to the steering lock right and left
steering = self.vehicleControlState["steering"]
if move == 'l':
steering += dt * self.steeringRate
steering = min(steering, 1.0)
elif move == 'r':
steering -= dt * self.steeringRate
steering = max(steering, -1.0)
else:
# gradually re-center the steering
if steering > 0.0:
steering -= dt * self.centreingRate
if steering < 0.0:
steering = 0.0
elif steering < 0.0:
steering += dt * self.centreingRate
if steering > 0.0:
steering = 0.0
self.vehicleControlState["steering"] = steering
# """Updates acceleration, braking and steering
# These are all passed in through a controlState dictionary
# """
# Update acceleration and braking
wheelForce = self.vehicleControlState["throttle"] * self.specs["maxWheelForce"]
self.reversing = self.vehicleControlState["reverse"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
brakeForce = self.vehicleControlState["brake"] * self.specs["brakeForce"]
# Update steering
# Steering control state is from -1 to 1
steering = self.vehicleControlState["steering"] * self.specs["steeringLock"]
self.applyForcesAndSteering(steering, wheelForce, brakeForce)
return [steering, wheelForce, brakeForce]
def setVehiclePos(self, x,y, z, h, p, r):
#self.chassisNP.setX(x)
#self.chassisNP.setY(y)
#self.chassisNP.setP(p)
#self.chassisNP.setR(r)
self.chassisNP.setPosHpr(x, y, z, h, p, r)
return
def setupVehicle(self, main):
scale = 0.5
# Chassis
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5 * scale))
name = self.username
self.chassisNode = BulletRigidBodyNode(name)
self.chassisNode.setTag('username', str(name))
self.chassisNP = main.worldNP.attachNewNode(self.chassisNode)
self.chassisNP.setName(str(name))
self.chassisNP.node().addShape(shape, ts)
self.chassisNP.setScale(scale)
self.chassisNP.setPos(self.pos)
if self.isCurrentPlayer:
self.chassisNP.node().notifyCollisions(True)
self.chassisNP.node().setMass(800.0)
else:
self.chassisNP.node().notifyCollisions(True)
self.chassisNP.node().setMass(400.0)
self.chassisNP.node().setDeactivationEnabled(False)
main.world.attachRigidBody(self.chassisNP.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(main.world, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
main.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP.reparentTo(self.chassisNP)
#self.carNP = loader.loadModel('models/batmobile-chassis.egg')
#self.yugoNP.setScale(.7)
#self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3( 0.70 * scale, 1.05 * scale, 0.3), True, np)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3(-0.70 * scale, 1.05 * scale, 0.3), True, np)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3( 0.70 * scale, -1.05 * scale, 0.3), False, np)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(main.worldNP)
self.addWheel(Point3(-0.70 * scale, -1.05 * scale, 0.3), False, np)
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)
def reset(self):
#self.chassisNP.setP(0)
#self.chassisNP.setR(0)
#print "kegwe", self.chassisNP.getX(),self.chassisNP.getY(),self.chassisNP.getZ(),self.chassisNP.getH(),0,0
self.chassisNP.setPosHpr(self.chassisNP.getX(),self.chassisNP.getY(),self.chassisNP.getZ(),self.chassisNP.getH(),0,0)
def pickedPowerup(self, powerup):
if not powerup.pickable:
powerup.useAbility()
else:
if self.currentPowerups["powerup1"] is None:
self.currentPowerups["powerup1"] = powerup
elif self.currentPowerups["powerup2"] is None:
self.currentPowerups["powerup2"] = powerup
elif self.currentPowerups["powerup3"] is None:
self.currentPowerups["powerup3"] = powerup
def canPickUpPowerup(self):
return (self.currentPowerups["powerup1"] is None or
self.currentPowerups["powerup2"] is None or
self.currentPowerups["powerup3"] is None)
def usePowerup(self, powerupIndex):
# Move usePowerupN to this function
if powerupIndex == 0 and self.currentPowerups["powerup1"] is not None:
self.currentPowerups["powerup1"].useAbility()
self.currentPowerups["powerup1"] = None
elif powerupIndex == 1 and self.currentPowerups["powerup2"] is not None:
self.currentPowerups["powerup2"].useAbility()
self.currentPowerups["powerup2"] = None
elif powerupIndex == 2 and self.currentPowerups["powerup3"] is not None:
self.currentPowerups["powerup3"].useAbility()
self.currentPowerups["powerup3"] = None
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
class CarPhys(Phys):
def __init__(self, mdt, carphys_props):
Phys.__init__(self, mdt)
self.pnode = None
self.vehicle = None
self.curr_speed_factor = 1.0
self.__prev_speed = 0
self.__finds = {} # cache for find's results
self.props = carphys_props
self._load_phys()
self.__set_collision_mesh()
self.__set_phys_node()
self.__set_vehicle()
self.__set_wheels()
eng.attach_obs(self.on_end_frame)
def _load_phys(self):
fpath = self.props.phys_file % self.mdt.name
with open(fpath) as phys_file: # pass phys props as a class
self.cfg = load(phys_file)
self.cfg['max_speed'] = self.get_speed()
self.cfg['friction_slip'] = self.get_friction()
self.cfg['roll_influence'] = self.get_roll_influence()
s_a = (self.mdt.name, round(self.cfg['max_speed'],
2), self.props.driver_engine)
LogMgr().log('speed %s: %s (%s)' % s_a)
fr_slip = round(self.cfg['friction_slip'], 2)
f_a = (self.mdt.name, fr_slip, self.props.driver_tires)
LogMgr().log('friction %s: %s (%s)' % f_a)
r_a = (self.mdt.name, round(self.cfg['roll_influence'],
2), self.props.driver_suspensions)
LogMgr().log('roll %s: %s (%s)' % r_a)
s_a = lambda field: setattr(self, field, self.cfg[field])
map(s_a, self.cfg.keys())
def __set_collision_mesh(self):
fpath = self.props.coll_path % self.mdt.name
self.coll_mesh = loader.loadModel(fpath)
chassis_shape = BulletConvexHullShape()
for geom in PhysMgr().find_geoms(self.coll_mesh, self.props.coll_name):
chassis_shape.addGeom(geom.node().getGeom(0), geom.getTransform())
self.mdt.gfx.nodepath.node().addShape(chassis_shape)
self.mdt.gfx.nodepath.setCollideMask(
BitMask32.bit(1)
| BitMask32.bit(2 + self.props.cars.index(self.mdt.name)))
#nodepath = self.mdt.gfx.nodepath.attachNewNode(BulletGhostNode('car ghost'))
#nodepath.node().addShape(BulletCapsuleShape(4, 5, ZUp))
#eng.attach_ghost(nodepath.node())
#nodepath.node().notifyCollisions(False)
def __set_phys_node(self):
self.pnode = self.mdt.gfx.nodepath.node()
self.pnode.setMass(self.mass)
self.pnode.setDeactivationEnabled(False)
PhysMgr().attach_rigid_body(self.pnode)
PhysMgr().add_collision_obj(self.pnode)
def __set_vehicle(self):
self.vehicle = BulletVehicle(PhysMgr().root, self.pnode)
self.vehicle.setCoordinateSystem(ZUp)
self.vehicle.setPitchControl(self.pitch_control)
tuning = self.vehicle.getTuning()
tuning.setSuspensionCompression(self.suspension_compression)
tuning.setSuspensionDamping(self.suspension_damping)
PhysMgr().attach_vehicle(self.vehicle)
def __set_wheels(self):
fwheel_bounds = self.mdt.gfx.wheels['fr'].get_tight_bounds()
f_radius = (fwheel_bounds[1][2] - fwheel_bounds[0][2]) / 2.0 + .01
rwheel_bounds = self.mdt.gfx.wheels['rr'].get_tight_bounds()
r_radius = (rwheel_bounds[1][2] - rwheel_bounds[0][2]) / 2.0 + .01
ffr = self.coll_mesh.find('**/' + self.props.wheel_names[0][0])
ffl = self.coll_mesh.find('**/' + self.props.wheel_names[0][1])
rrr = self.coll_mesh.find('**/' + self.props.wheel_names[0][2])
rrl = self.coll_mesh.find('**/' + self.props.wheel_names[0][3])
meth = self.coll_mesh.find
fr_node = ffr if ffr else meth('**/' + self.props.wheel_names[1][0])
fl_node = ffl if ffl else meth('**/' + self.props.wheel_names[1][1])
rr_node = rrr if rrr else meth('**/' + self.props.wheel_names[1][2])
rl_node = rrl if rrl else meth('**/' + self.props.wheel_names[1][3])
wheel_fr_pos = fr_node.get_pos() + (0, 0, f_radius)
wheel_fl_pos = fl_node.get_pos() + (0, 0, f_radius)
wheel_rr_pos = rr_node.get_pos() + (0, 0, r_radius)
wheel_rl_pos = rl_node.get_pos() + (0, 0, r_radius)
frw = self.mdt.gfx.wheels['fr']
flw = self.mdt.gfx.wheels['fl']
rrw = self.mdt.gfx.wheels['rr']
rlw = self.mdt.gfx.wheels['rl']
wheels_info = [(wheel_fr_pos, True, frw, f_radius),
(wheel_fl_pos, True, flw, f_radius),
(wheel_rr_pos, False, rrw, r_radius),
(wheel_rl_pos, False, rlw, r_radius)]
for (pos, front, nodepath, radius) in wheels_info:
self.__add_wheel(pos, front, nodepath.node(), radius)
def __add_wheel(self, pos, front, node, radius):
whl = self.vehicle.createWheel()
whl.setNode(node)
whl.setChassisConnectionPointCs(LPoint3f(*pos))
whl.setFrontWheel(front)
whl.setWheelDirectionCs((0, 0, -1))
whl.setWheelAxleCs((1, 0, 0))
whl.setWheelRadius(radius)
whl.setSuspensionStiffness(self.suspension_stiffness)
whl.setWheelsDampingRelaxation(self.wheels_damping_relaxation)
whl.setWheelsDampingCompression(self.wheels_damping_compression)
whl.setFrictionSlip(self.friction_slip) # high -> more adherence
whl.setRollInfluence(self.roll_influence) # low -> more stability
whl.setMaxSuspensionForce(self.max_suspension_force)
whl.setMaxSuspensionTravelCm(self.max_suspension_travel_cm)
whl.setSkidInfo(self.skid_info)
@property
def lateral_force(self):
vel = self.vehicle.get_chassis().get_linear_velocity()
vel.normalize()
dir = self.mdt.logic.car_vec
lat = dir.dot(vel)
lat_force = 0
if lat > .5:
lat_force = min(1, (lat - 1.0) / -.2)
return lat_force
@property
def is_flying(self): # no need to be cached
rays = [whl.getRaycastInfo() for whl in self.vehicle.get_wheels()]
return not any(ray.isInContact() for ray in rays)
@property
def prev_speed(self):
return self.__prev_speed
@property
def prev_speed_ratio(self):
return max(0, min(1.0, self.prev_speed / self.max_speed))
def on_end_frame(self):
self.__prev_speed = self.speed
@property
def speed(self):
if self.mdt.fsm.getCurrentOrNextState() == 'Countdown':
return 0 # getCurrentSpeedKmHour returns odd values otherwise
return self.vehicle.getCurrentSpeedKmHour()
@property
def speed_ratio(self):
return max(0, min(1.0, self.speed / self.max_speed))
def set_forces(self, eng_frc, brake_frc, steering):
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
self.vehicle.applyEngineForce(eng_frc, 2)
self.vehicle.applyEngineForce(eng_frc, 3)
self.vehicle.setBrake(brake_frc, 2)
self.vehicle.setBrake(brake_frc, 3)
def update_car_props(self):
speeds = []
for whl in self.vehicle.get_wheels():
contact_pt = whl.get_raycast_info().getContactPointWs()
gnd_name = self.gnd_name(contact_pt)
if not gnd_name:
continue
if gnd_name not in self.__finds:
gnd = self.props.track_phys.find('**/' + gnd_name)
self.__finds[gnd_name] = gnd
gfx_node = self.__finds[gnd_name]
if gfx_node.has_tag('speed'):
speeds += [float(gfx_node.get_tag('speed'))]
if gfx_node.has_tag('friction'):
fric = float(gfx_node.get_tag('friction'))
whl.setFrictionSlip(self.friction_slip * fric)
self.curr_speed_factor = (sum(speeds) / len(speeds)) if speeds else 1.0
@property
def gnd_names(self): # no need to be cached
whls = self.vehicle.get_wheels()
pos = map(lambda whl: whl.get_raycast_info().getContactPointWs(), whls)
return map(self.gnd_name, pos)
@staticmethod
def gnd_name(pos):
top = pos + (0, 0, 20)
bottom = pos + (0, 0, -20)
result = PhysMgr().ray_test_closest(bottom, top)
ground = result.get_node()
return ground.get_name() if ground else ''
def apply_damage(self, reset=False):
if reset:
self.max_speed = self.get_speed()
self.friction_slip = self.get_friction()
self.roll_influence = self.get_roll_influence()
else:
self.max_speed *= .95
self.friction_slip *= .95
self.roll_influence *= 1.05
fric = lambda whl: whl.setFrictionSlip(self.friction_slip)
map(fric, self.vehicle.get_wheels())
roll = lambda whl: whl.setRollInfluence(self.roll_influence)
map(roll, self.vehicle.get_wheels())
s_a = (str(round(self.max_speed, 2)), self.props.driver_engine)
LogMgr().log('speed: %s (%s)' % s_a)
f_a = (str(round(self.friction_slip, 2)), self.props.driver_tires)
LogMgr().log('friction: %s (%s)' % f_a)
r_a = (str(round(self.roll_influence,
2)), self.props.driver_suspensions)
LogMgr().log('roll: %s (%s)' % r_a)
def get_speed(self):
return self.cfg['max_speed'] * (1 + .01 * self.props.driver_engine)
def get_friction(self):
return self.cfg['friction_slip'] * (1 + .01 * self.props.driver_tires)
def get_roll_influence(self):
return self.cfg['roll_influence'] * (
1 + .01 * self.props.driver_suspensions)
def destroy(self):
eng.detach_obs(self.on_end_frame)
eng.remove_vehicle(self.vehicle)
self.pnode = self.vehicle = self.__finds = self.__track_phys = \
self.coll_mesh = None
Phys.destroy(self)
class BaseVehicle(DynamicElement):
Ego_state_obs_dim = 9
"""
Vehicle chassis and its wheels index
0 1
II-----II
|
| <---chassis
|
II-----II
2 3
"""
PARAMETER_SPACE = PGSpace(
VehicleParameterSpace.BASE_VEHICLE
) # it will not sample config from parameter space
COLLISION_MASK = CollisionGroup.EgoVehicle
STEERING_INCREMENT = 0.05
default_vehicle_config = PGConfig(
dict(
lidar=(240, 50, 4), # laser num, distance, other vehicle info num
mini_map=(84, 84, 250), # buffer length, width
rgb_cam=(160, 120), # buffer length, width
depth_cam=(84, 84, True), # buffer length, width, view_ground
show_navi_mark=False,
increment_steering=False,
wheel_friction=0.6,
))
born_place = (5, 0)
LENGTH = None
WIDTH = None
def __init__(self,
pg_world: PGWorld,
vehicle_config: dict = None,
random_seed: int = 0,
config: dict = None):
"""
This Vehicle Config is different from self.get_config(), and it is used to define which modules to use, and
module parameters.
"""
super(BaseVehicle, self).__init__(random_seed)
self.pg_world = pg_world
self.node_path = NodePath("vehicle")
# config info
self.set_config(self.PARAMETER_SPACE.sample())
if config is not None:
self.set_config(config)
self.vehicle_config = self.get_vehicle_config(
vehicle_config
) if vehicle_config is not None else self.default_vehicle_config
self.increment_steering = self.vehicle_config["increment_steering"]
self.max_speed = self.get_config()[Parameter.speed_max]
self.max_steering = self.get_config()[Parameter.steering_max]
# create
self._add_chassis(pg_world.physics_world)
self.wheels = self._create_wheel()
# modules
self.image_sensors = {}
self.lidar = None
self.routing_localization = None
self.lane = None
self.lane_index = None
self.vehicle_panel = VehiclePanel(self.pg_world) if (
self.pg_world.mode == RENDER_MODE_ONSCREEN) else None
# other info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.born_place
self.last_heading_dir = self.heading
# collision info render
self.collision_info_np = self._init_collision_info_render(pg_world)
self.collision_banners = {} # to save time
self.current_banner = None
# done info
self.crash = False
self.out_of_road = False
self.attach_to_pg_world(self.pg_world.pbr_render,
self.pg_world.physics_world)
@classmethod
def get_vehicle_config(cls, new_config):
default = copy.deepcopy(cls.default_vehicle_config)
default.update(new_config)
return default
def prepare_step(self, action):
"""
Save info and make decision before action
"""
self.last_position = self.position
self.last_heading_dir = self.heading
self.last_current_action.append(
action
) # the real step of physics world is implemented in taskMgr.step()
if self.increment_steering:
self.set_incremental_action(action)
else:
self.set_act(action)
if self.vehicle_panel is not None:
self.vehicle_panel.renew_2d_car_para_visualization(self)
def update_state(self, pg_world=None):
if self.lidar is not None:
self.lidar.perceive(self.position, self.heading_theta,
self.pg_world.physics_world)
if self.routing_localization is not None:
self.lane, self.lane_index = self.routing_localization.update_navigation_localization(
self)
return self._state_check()
def reset(self, map: Map, pos: np.ndarray, heading: float):
"""
pos is a 2-d array, and heading is a float (unit degree)
"""
heading = -np.deg2rad(heading) - np.pi / 2
self.chassis_np.setPos(Vec3(*pos, 1))
self.chassis_np.setQuat(
LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2)))
self.update_map_info(map)
self.chassis_np.node().clearForces()
self.chassis_np.node().setLinearVelocity(Vec3(0, 0, 0))
self.chassis_np.node().setAngularVelocity(Vec3(0, 0, 0))
self.system.resetSuspension()
# done info
self.crash = False
self.out_of_road = False
# other info
self.throttle_brake = 0.0
self.steering = 0
self.last_current_action = deque([(0.0, 0.0), (0.0, 0.0)], maxlen=2)
self.last_position = self.born_place
self.last_heading_dir = self.heading
if "depth_cam" in self.image_sensors and self.image_sensors[
"depth_cam"].view_ground:
for block in map.blocks:
block.node_path.hide(CamMask.DepthCam)
"""------------------------------------------- act -------------------------------------------------"""
def set_act(self, action):
para = self.get_config()
steering = action[0]
self.throttle_brake = action[1]
self.steering = steering
self.system.setSteeringValue(
self.steering * para[Parameter.steering_max], 0)
self.system.setSteeringValue(
self.steering * para[Parameter.steering_max], 1)
self._apply_throttle_brake(action[1])
def set_incremental_action(self, action: np.ndarray):
self.throttle_brake = action[1]
self.steering += action[0] * self.STEERING_INCREMENT
self.steering = clip(self.steering, -1, 1)
steering = self.steering * self.max_steering
self.system.setSteeringValue(steering, 0)
self.system.setSteeringValue(steering, 1)
self._apply_throttle_brake(action[1])
def _apply_throttle_brake(self, throttle_brake):
para = self.get_config()
max_engine_force = para[Parameter.engine_force_max]
max_brake_force = para[Parameter.brake_force_max]
for wheel_index in range(4):
if throttle_brake >= 0:
self.system.setBrake(2.0, wheel_index)
if self.speed > self.max_speed:
self.system.applyEngineForce(0.0, wheel_index)
else:
self.system.applyEngineForce(
max_engine_force * throttle_brake, wheel_index)
else:
self.system.applyEngineForce(0.0, wheel_index)
self.system.setBrake(
abs(throttle_brake) * max_brake_force, wheel_index)
"""---------------------------------------- vehicle info ----------------------------------------------"""
@property
def position(self):
return pgdrive_position(self.chassis_np.getPos())
@property
def speed(self):
"""
km/h
"""
speed = self.chassis_np.node().get_linear_velocity().length() * 3.6
return clip(speed, 0.0, 100000.0)
@property
def heading(self):
real_heading = self.heading_theta
heading = np.array([np.cos(real_heading), np.sin(real_heading)])
return heading
@property
def heading_theta(self):
"""
Get the heading theta of vehicle, unit [rad]
:return: heading in rad
"""
return (pgdrive_heading(self.chassis_np.getH()) - 90) / 180 * math.pi
@property
def velocity(self) -> np.ndarray:
return self.speed * self.velocity_direction
@property
def velocity_direction(self):
direction = self.system.get_forward_vector()
return np.asarray([direction[0], -direction[1]])
@property
def forward_direction(self):
raise ValueError("This function id deprecated.")
# print("This function id deprecated.")
# direction = self.vehicle.get_forward_vector()
# return np.array([direction[0], -direction[1]])
@property
def current_road(self):
return self.lane_index[0:-1]
"""---------------------------------------- some math tool ----------------------------------------------"""
def heading_diff(self, target_lane):
lateral = None
if isinstance(target_lane, StraightLane):
lateral = np.asarray(
get_vertical_vector(target_lane.end - target_lane.start)[1])
elif isinstance(target_lane, CircularLane):
if target_lane.direction == -1:
lateral = self.position - target_lane.center
else:
lateral = target_lane.center - self.position
else:
raise ValueError("Unknown target lane type: {}".format(
type(target_lane)))
lateral_norm = norm(lateral[0], lateral[1])
forward_direction = self.heading
# print(f"Old forward direction: {self.forward_direction}, new heading {self.heading}")
forward_direction_norm = norm(forward_direction[0],
forward_direction[1])
if not lateral_norm * forward_direction_norm:
return 0
# cos = self.forward_direction.dot(lateral) / (np.linalg.norm(lateral) * np.linalg.norm(self.forward_direction))
cos = ((forward_direction[0] * lateral[0] +
forward_direction[1] * lateral[1]) /
(lateral_norm * forward_direction_norm))
# return cos
# Normalize to 0, 1
return clip(cos, -1.0, 1.0) / 2 + 0.5
def projection(self, vector):
# Projected to the heading of vehicle
# forward_vector = self.vehicle.get_forward_vector()
# forward_old = (forward_vector[0], -forward_vector[1])
forward = self.heading
# print(f"[projection] Old forward {forward_old}, new heading {forward}")
norm_velocity = norm(forward[0], forward[1]) + 1e-6
project_on_heading = (vector[0] * forward[0] +
vector[1] * forward[1]) / norm_velocity
side_direction = get_vertical_vector(forward)[1]
side_norm = norm(side_direction[0], side_direction[1]) + 1e-6
project_on_side = (vector[0] * side_direction[0] +
vector[1] * side_direction[1]) / side_norm
return project_on_heading, project_on_side
def lane_distance_to(self, vehicle, lane: AbstractLane = None) -> float:
assert self.routing_localization is not None, "a routing and localization module should be added " \
"to interact with other vehicles"
if not vehicle:
return np.nan
if not lane:
lane = self.lane
return lane.local_coordinates(
vehicle.position)[0] - lane.local_coordinates(self.position)[0]
"""-------------------------------------- for vehicle making ------------------------------------------"""
def _add_chassis(self, pg_physics_world: PGPhysicsWorld):
para = self.get_config()
chassis = BulletRigidBodyNode(BodyName.Ego_vehicle_top)
chassis.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_shape = BulletBoxShape(
Vec3(para[Parameter.vehicle_width] / 2,
para[Parameter.vehicle_length] / 2,
para[Parameter.vehicle_height] / 2))
ts = TransformState.makePos(
Vec3(0, 0, para[Parameter.chassis_height] * 2))
chassis.addShape(chassis_shape, ts)
heading = np.deg2rad(-para[Parameter.heading] - 90)
chassis.setMass(para[Parameter.mass])
self.chassis_np = self.node_path.attachNewNode(chassis)
# not random born now
self.chassis_np.setPos(Vec3(*self.born_place, 1))
self.chassis_np.setQuat(
LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(True) # advance collision check
self.pg_world.physics_world.dynamic_world.setContactAddedCallback(
PythonCallbackObject(self._collision_check))
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Ego_vehicle)
chassis_beneath.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_beneath.addShape(chassis_shape)
self.chassis_beneath_np = self.chassis_np.attachNewNode(
chassis_beneath)
self.dynamic_nodes.append(chassis_beneath)
self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis)
self.system.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(
self.system
) # detach chassis will also detach system, so a waring will generate
self.LENGTH = para[Parameter.vehicle_length]
self.WIDTH = para[Parameter.vehicle_width]
if self.render:
model_path = 'models/ferra/scene.gltf'
self.chassis_vis = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.chassis_vis.setZ(para[Parameter.vehicle_vis_z])
self.chassis_vis.setY(para[Parameter.vehicle_vis_y])
self.chassis_vis.setH(para[Parameter.vehicle_vis_h])
self.chassis_vis.set_scale(para[Parameter.vehicle_vis_scale])
self.chassis_vis.reparentTo(self.chassis_np)
def _create_wheel(self):
para = self.get_config()
f_l = para[Parameter.front_tire_longitude]
r_l = -para[Parameter.rear_tire_longitude]
lateral = para[Parameter.tire_lateral]
axis_height = para[Parameter.tire_radius] + 0.05
radius = para[Parameter.tire_radius]
wheels = []
for k, pos in enumerate([
Vec3(lateral, f_l, axis_height),
Vec3(-lateral, f_l, axis_height),
Vec3(lateral, r_l, axis_height),
Vec3(-lateral, r_l, axis_height)
]):
wheel = self._add_wheel(pos, radius, True if k < 2 else False,
True if k == 0 or k == 2 else False)
wheels.append(wheel)
return wheels
def _add_wheel(self, pos: Vec3, radius: float, front: bool, left):
wheel_np = self.node_path.attachNewNode("wheel")
if self.render:
model_path = 'models/yugo/yugotireR.egg' if left else 'models/yugo/yugotireL.egg'
wheel_model = self.loader.loadModel(
AssetLoader.file_path(model_path))
wheel_model.reparentTo(wheel_np)
wheel_model.set_scale(1.4, radius / 0.25, radius / 0.25)
wheel = self.system.create_wheel()
wheel.setNode(wheel_np.node())
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
# TODO add them to PGConfig in the future
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40)
wheel.setSuspensionStiffness(30)
wheel.setWheelsDampingRelaxation(4.8)
wheel.setWheelsDampingCompression(1.2)
wheel.setFrictionSlip(self.vehicle_config["wheel_friction"])
wheel.setRollInfluence(1.5)
return wheel
def add_image_sensor(self, name: str, sensor: ImageBuffer):
self.image_sensors[name] = sensor
def add_lidar(self, laser_num=240, distance=50):
self.lidar = Lidar(self.pg_world.render, laser_num, distance)
def add_routing_localization(self, show_navi_point: bool):
self.routing_localization = RoutingLocalizationModule(
self.pg_world, show_navi_point)
def update_map_info(self, map):
"""
Update map info after reset()
:param map: new map
:return: None
"""
self.routing_localization.update(map)
lane, new_l_index = ray_localization((self.born_place), self.pg_world)
assert lane is not None, "Born place is not on road!"
self.lane_index = new_l_index
self.lane = lane
def _state_check(self):
"""
Check States and filter to update info
"""
result = self.pg_world.physics_world.dynamic_world.contactTest(
self.chassis_beneath_np.node(), True)
contacts = set()
for contact in result.getContacts():
node0 = contact.getNode0()
node1 = contact.getNode1()
name = [node0.getName(), node1.getName()]
name.remove(BodyName.Ego_vehicle)
if name[0] == "Ground" or name[0] == BodyName.Lane:
continue
elif name[0] == BodyName.Side_walk:
self.out_of_road = True
contacts.add(name[0])
if self.render:
self.render_collision_info(contacts)
return contacts
def _collision_check(self, contact):
"""
It may lower the performance if overdone
"""
node0 = contact.getNode0().getName()
node1 = contact.getNode1().getName()
name = [node0, node1]
name.remove(BodyName.Ego_vehicle_top)
if name[0] == BodyName.Traffic_vehicle:
self.crash = True
logging.debug("Crash with {}".format(name[0]))
def _init_collision_info_render(self, pg_world):
if pg_world.mode == "onscreen":
info_np = NodePath("Collision info nodepath")
info_np.reparentTo(pg_world.aspect2d)
else:
info_np = None
return info_np
def render_collision_info(self, contacts):
contacts = sorted(list(contacts),
key=lambda c: COLLISION_INFO_COLOR[COLOR[c]][0])
text = contacts[0] if len(contacts) != 0 else None
if text is None:
text = "Normal" if time.time(
) - self.pg_world._episode_start_time > 10 else "Press H to see help message"
self.render_banner(text, COLLISION_INFO_COLOR["green"][1])
else:
self.render_banner(text, COLLISION_INFO_COLOR[COLOR[text]][1])
def render_banner(self, text, color=COLLISION_INFO_COLOR["green"][1]):
"""
Render the banner in the left bottom corner.
"""
if self.collision_info_np is None:
return
if self.current_banner is not None:
self.current_banner.detachNode()
if text in self.collision_banners:
self.collision_banners[text].reparentTo(self.collision_info_np)
self.current_banner = self.collision_banners[text]
else:
new_banner = NodePath(TextNode("collision_info:{}".format(text)))
self.collision_banners[text] = new_banner
text_node = new_banner.node()
text_node.setCardColor(color)
text_node.setText(text)
text_node.setCardActual(-5 * self.pg_world.w_scale,
5.1 * self.pg_world.w_scale, -0.3, 1)
text_node.setCardDecal(True)
text_node.setTextColor(1, 1, 1, 1)
text_node.setAlign(TextNode.A_center)
new_banner.setScale(0.05)
new_banner.setPos(-0.75 * self.pg_world.w_scale, 0,
-0.8 * self.pg_world.h_scale)
new_banner.reparentTo(self.collision_info_np)
self.current_banner = new_banner
def destroy(self, _=None):
self.dynamic_nodes.remove(self.chassis_np.node())
super(BaseVehicle, self).destroy(self.pg_world)
self.pg_world.physics_world.dynamic_world.clearContactAddedCallback()
self.routing_localization.destroy()
self.routing_localization = None
if self.lidar is not None:
self.lidar.destroy()
self.lidar = None
if len(self.image_sensors) != 0:
for sensor in self.image_sensors.values():
sensor.destroy(self.pg_world)
self.image_sensors = None
if self.vehicle_panel is not None:
self.vehicle_panel.destroy(self.pg_world)
self.pg_world = None
def set_position(self, position):
"""
Should only be called when restore traffic from episode data
:param position: 2d array or list
:return: None
"""
self.chassis_np.setPos(panda_position(position, 0.4))
def set_heading(self, heading_theta) -> None:
"""
Should only be called when restore traffic from episode data
:param heading_theta: float in rad
:return: None
"""
self.chassis_np.setH((panda_heading(heading_theta) * 180 / np.pi) - 90)
def get_state(self):
return {
"heading": self.heading_theta,
"position": self.position.tolist(),
"done": self.crash or self.out_of_road
}
def set_state(self, state: dict):
self.set_heading(state["heading"])
self.set_position(state["position"])
def __del__(self):
super(BaseVehicle, self).__del__()
self.pg_world = None
self.lidar = None
self.mini_map = None
self.rgb_cam = None
self.routing_localization = None
self.wheels = None
class CarSrv(DirectObject):
def __init__(self):
base.setBackgroundColor(0.1, 0.1, 0.8, 1)
assert ("offscreen" == base.config.GetString("window-type",
"offscreen"))
# 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)
self.params = rospy.get_param('~sim')
# Car Simulator
self.dt = rospy.get_param('~dt')
self.setup()
self.load_vehicle()
# ROS
self.crash_pub = rospy.Publisher('crash',
std_msgs.msg.Empty,
queue_size=1)
self.bridge = cv_bridge.CvBridge()
# taskMgr.add(self.update, 'updateWorld')
self.start_update_server()
# _____HANDLER_____
def doExit(self):
self.cleanup()
sys.exit(1)
def doReset(self, pos=None, quat=None):
self.cleanup()
self.setup()
if pos is None or quat is None:
self.load_vehicle()
else:
self.load_vehicle(pos=pos, quat=quat)
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')
def get_ros_image(self, cv_image, image_format="rgb8"):
return self.bridge.cv2_to_imgmsg(cv_image, image_format)
def get_handler(self):
def sim_env_handler(req):
start_time = time.time()
cmd_steer = req.steer
motor = req.motor
vel = req.vel
reset = req.reset
pose = req.pose
# If motor is default then use velocity
if motor == 0.0:
cmd_motor = numpy.clip(vel * 3 + 49.5, 0., 99.)
else:
cmd_motor = numpy.clip(motor, 0., 99.)
self.steering = self.steeringClamp * \
((cmd_steer - 49.5) / 49.5)
self.engineForce = self.engineClamp * \
((cmd_motor - 49.5) / 49.5)
if reset:
self.steering = 0.0 # degree
self.engineForce = 0.0
pos = pose.position.x, pose.position.y, pose.position.z
quat = pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w
if numpy.all(numpy.array(pos) == 0.):
self.doReset()
else:
self.doReset(pos=pos, quat=quat)
self.vehicle.setSteeringValue(self.steering, 0)
self.vehicle.setSteeringValue(self.steering, 1)
self.vehicle.setBrake(100.0, 2)
self.vehicle.setBrake(100.0, 3)
self.vehicle.applyEngineForce(self.engineForce, 2)
self.vehicle.applyEngineForce(self.engineForce, 3)
pos = numpy.array(self.vehicle_pointer.getPos())
np_quat = self.vehicle_pointer.getQuat()
quat = numpy.array(np_quat)
self.previous_pos = pos
self.previous_quat = np_quat
# TODO maybe change number of timesteps
self.world.doPhysics(self.dt, 10, 0.05)
# Collision detection
result = self.world.contactTest(self.vehicle_node)
collision = result.getNumContacts() > 0
if collision:
# TODO figure out why this causes problems
# self.crash_pub.publish(std_msgs.msg.Empty())
self.doReset(pos=self.previous_pos, quat=self.previous_quat)
state = geometry_msgs.msg.Pose()
pos = numpy.array(self.vehicle_pointer.getPos())
np_quat = self.vehicle_pointer.getQuat()
quat = numpy.array(np_quat)
self.previous_pos = pos
self.previous_quat = np_quat
state.position.x, state.position.y, state.position.z = pos
state.orientation.x, state.orientation.y, \
state.orientation.z, state.orientation.w = quat
# Get observation
obs = self.camera_sensor.observe()
back_obs = self.back_camera_sensor.observe()
cam_image = self.get_ros_image(obs[0])
cam_depth = self.get_ros_image(obs[1], image_format="passthrough")
self.camera_pub.publish_image(obs[0])
self.depth_pub.publish_image(obs[1], image_format="passthrough")
back_cam_image = self.get_ros_image(back_obs[0])
back_cam_depth = self.get_ros_image(back_obs[1],
image_format="passthrough")
self.back_camera_pub.publish_image(back_obs[0])
self.back_depth_pub.publish_image(back_obs[1],
image_format="passthrough")
return [
collision, cam_image, cam_depth, back_cam_image,
back_cam_depth, state
]
return sim_env_handler
def load_vehicle(self, pos=(0.0, -20.0, -0.6), quat=None):
# Chassis
self._mass = self.params['mass']
#chassis_shape = self.params['chassis_shape']
shape = BulletBoxShape(Vec3(0.6, 1.4, 0.5))
ts = TransformState.makePos(Point3(0, 0, 0.5))
self.vehicle_pointer = self.worldNP.attachNewNode(
BulletRigidBodyNode('Vehicle'))
self.vehicle_node = self.vehicle_pointer.node()
self.vehicle_node.addShape(shape, ts)
self.previous_pos = pos
self.vehicle_pointer.setPos(pos[0], pos[1], pos[2])
if quat is not None:
self.vehicle_pointer.setQuat(quat)
self.previous_quat = self.vehicle_pointer.getQuat()
self.vehicle_node.setMass(self._mass)
self.vehicle_node.setDeactivationEnabled(False)
# first_person = self.params['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.setPos(0.0, 1.0, 1.0)
# self.camera_node.lookAt(0.0, 6.0, 0.0)
# else:
# self.camera_node.setPos(0.0, -10.0, 5.0)
# self.camera_node.lookAt(0.0, 5.0, 0.0)
self.camera_node.reparentTo(self.vehicle_pointer)
self.camera_node.setPos(0.0, 1.0, 1.0)
self.camera_node.lookAt(0.0, 6.0, 0.0)
self.back_camera_sensor = Panda3dCameraSensor(base,
color=True,
depth=True,
size=(160, 90))
self.back_camera_node = self.back_camera_sensor.cam
# if first_person:
# self.camera_node.setPos(0.0, 1.0, 1.0)
# self.camera_node.lookAt(0.0, 6.0, 0.0)
# else:
# self.camera_node.setPos(0.0, -10.0, 5.0)
# self.camera_node.lookAt(0.0, 5.0, 0.0)
self.back_camera_node.reparentTo(self.vehicle_pointer)
self.back_camera_node.setPos(0.0, -1.0, 1.0)
self.back_camera_node.lookAt(0.0, -6.0, 0.0)
self.world.attachRigidBody(self.vehicle_node)
self.vehicle_node.setCcdSweptSphereRadius(1.0)
self.vehicle_node.setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(self.world, self.vehicle_node)
self.vehicle.setCoordinateSystem(ZUp)
self.world.attachVehicle(self.vehicle)
self.yugoNP = loader.loadModel('../models/yugo/yugo.egg')
self.yugoNP.reparentTo(self.vehicle_pointer)
self._wheels = []
# 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)
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)
self._wheels.append(np.node())
def start_update_server(self):
self.steering = 0.0 # degree
self.steeringClamp = self.params['steeringClamp']
self.engineForce = 0.0
self.engineClamp = self.params['engineClamp']
self.camera_pub = ImageROSPublisher("image")
self.depth_pub = ImageROSPublisher("depth")
self.back_camera_pub = ImageROSPublisher("back_image")
self.back_depth_pub = ImageROSPublisher("back_depth")
s = rospy.Service('sim_env', bair_car.srv.sim_env, self.get_handler())
rospy.spin()
def update(self, task):
dt = globalClock.getDt()
self.world.doPhysics(dt, 10, 0.008)
obs = self.camera_sensor.observe()
obs = self.back_camera_sensor.observe()
return task.cont
def cleanup(self):
self.world = None
self.worldNP.removeNode()
@abc.abstractmethod
def setup(self):
pass
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 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)
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)
"""
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()
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)
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 Vehicle(object):
def __init__(self, positionHpr, render, world, base):
self.base = base
loader = base.loader
position, hpr = positionHpr
vehicleType = "yugo"
self.vehicleDir = "data/vehicles/" + vehicleType + "/"
# Load vehicle description and specs
with open(self.vehicleDir + "vehicle.json") as vehicleData:
data = json.load(vehicleData)
self.specs = data["specs"]
# Chassis for collisions and mass uses a simple box shape
halfExtents = (0.5 * dim for dim in self.specs["dimensions"])
shape = BulletBoxShape(Vec3(*halfExtents))
ts = TransformState.makePos(Point3(0, 0, 0.5))
self.rigidNode = BulletRigidBodyNode("vehicle")
self.rigidNode.addShape(shape, ts)
self.rigidNode.setMass(self.specs["mass"])
self.rigidNode.setDeactivationEnabled(False)
self.np = render.attachNewNode(self.rigidNode)
self.np.setPos(position)
self.np.setHpr(hpr)
world.attachRigidBody(self.rigidNode)
# Vehicle physics model
self.vehicle = BulletVehicle(world, self.rigidNode)
self.vehicle.setCoordinateSystem(ZUp)
world.attachVehicle(self.vehicle)
# Vehicle graphical model
self.vehicleNP = loader.loadModel(self.vehicleDir + "car.egg")
self.vehicleNP.reparentTo(self.np)
# Create wheels
wheelPos = self.specs["wheelPositions"]
for fb, y in (("F", wheelPos[1]), ("B", -wheelPos[1])):
for side, x in (("R", wheelPos[0]), ("L", -wheelPos[0])):
np = loader.loadModel(self.vehicleDir + "tire%s.egg" % side)
np.reparentTo(render)
isFront = fb == "F"
self.addWheel(Point3(x, y, wheelPos[2]), isFront, np)
def addWheel(self, position, isFront, np):
wheel = self.vehicle.createWheel()
wheelSpecs = self.specs["wheels"]
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(position)
wheel.setFrontWheel(isFront)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(wheelSpecs["radius"])
wheel.setMaxSuspensionTravelCm(wheelSpecs["suspensionTravel"] * 100.0)
wheel.setSuspensionStiffness(wheelSpecs["suspensionStiffness"])
wheel.setWheelsDampingRelaxation(wheelSpecs["dampingRelaxation"])
wheel.setWheelsDampingCompression(wheelSpecs["dampingCompression"])
wheel.setFrictionSlip(wheelSpecs["frictionSlip"])
wheel.setRollInfluence(wheelSpecs["rollInfluence"])
def initialiseSound(self, audioManager):
"""Start the engine sound and set collision sounds"""
# Set sounds to play for collisions
self.collisionSound = CollisionSound(
nodePath=self.np, sounds=["data/sounds/09.wav"], thresholdForce=600.0, maxForce=800000.0
)
self.engineSound = audioManager.loadSfx(self.vehicleDir + "engine.wav")
audioManager.attachSoundToObject(self.engineSound, self.np)
self.engineSound.setLoop(True)
self.engineSound.setPlayRate(0.6)
self.engineSound.play()
self.gearSpacing = self.specs["sound"]["maxExpectedRotationRate"] / self.specs["sound"]["numberOfGears"]
self.reversing = False
def updateSound(self, dt):
"""Use vehicle speed to update sound pitch"""
soundSpecs = self.specs["sound"]
# Use rear wheel rotation speed as some measure of engine revs
wheels = (self.vehicle.getWheel(idx) for idx in (2, 3))
# wheelRate is in degrees per second
wheelRate = 0.5 * abs(sum(w.getDeltaRotation() / dt for w in wheels))
# Calculate which gear we're in, and what the normalised revs are
if self.reversing:
numberOfGears = 1
else:
numberOfGears = self.specs["sound"]["numberOfGears"]
gear = min(int(wheelRate / self.gearSpacing), numberOfGears - 1)
posInGear = (wheelRate - gear * self.gearSpacing) / self.gearSpacing
targetPlayRate = 0.6 + posInGear * (1.5 - 0.6)
currentRate = self.engineSound.getPlayRate()
self.engineSound.setPlayRate(0.8 * currentRate + 0.2 * targetPlayRate)
def updateControl(self, controlState, dt):
"""Updates acceleration, braking and steering
These are all passed in through a controlState dictionary
"""
# Update acceleration and braking
wheelForce = controlState["throttle"] * self.specs["maxWheelForce"]
self.reversing = controlState["reverse"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
brakeForce = controlState["brake"] * self.specs["brakeForce"]
# Update steering
# Steering control state is from -1 to 1
steering = controlState["steering"] * self.specs["steeringLock"]
# Apply steering to front wheels
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(wheelForce, 2)
self.vehicle.applyEngineForce(wheelForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
class Vehicle(object):
COUNT = 0
def __init__(self, bulletWorld, pos, username, isMainChar=False):
self.specs = {
"mass": 400.0,
"maxWheelForce": 2000.0,
"brakeForce": 100.0,
"steeringLock": 45.0
}
self.vehicleControlState = {
"throttle": 0,
"reverse": False,
"brake": 0.0,
"steering": 0.0
}
self.username = username
self.isMainChar = isMainChar
# Steering change per second, normalised to steering lock
# Eg. 45 degrees lock and 1.0 rate means 45 degrees per second
self.steeringRate = 0.8
self.centreingRate = 1.2
self.pos = pos
self.currentPowerups = {
"powerup1": None,
"powerup2": None,
"powerup3": None
}
self.setupVehicle(bulletWorld)
self.startTime = time.time()
COUNT = 1
def move(self, steering, wheelForce, brakeForce, x, y, z, h, p, r):
self.applyForcesAndSteering(steering, wheelForce, brakeForce)
self.endTime = time.time()
#print self.endTime
elapsed = self.endTime - self.startTime
#if elapsed > .1:
#print "Set Pos Now"
self.setVehiclePos(x, y, z, h, p, r)
self.startTime = self.endTime
#print "Do Move"
def applyForcesAndSteering(self, steering, wheelForce, brakeForce):
# Apply steering to front wheels
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(wheelForce, 2)
self.vehicle.applyEngineForce(wheelForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
def processInput(self, inputState, dt):
# print self.chassisNP.getPos()
#print self.chassisNP.getH()
"""Use controls to update the player's car"""
# For keyboard throttle and brake are either 0 or 1
if inputState.isSet('forward'):
self.vehicleControlState["throttle"] = 1.0
else:
self.vehicleControlState["throttle"] = 0.0
velocity = self.chassisNode.getLinearVelocity()
speed = math.sqrt(sum(v**2 for v in velocity))
# Update braking and reversing
if inputState.isSet('brake'):
if speed < 0.5 or self.vehicleControlState["reverse"]:
# If we're stopped, then start reversing
# Also keep reversing if we already were
self.vehicleControlState["reverse"] = True
self.vehicleControlState["throttle"] = 1.0
self.vehicleControlState["brake"] = 0.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 1.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 0.0
# steering is normalised from -1 to 1, corresponding
# to the steering lock right and left
steering = self.vehicleControlState["steering"]
if inputState.isSet('left'):
steering += dt * self.steeringRate
steering = min(steering, 1.0)
elif inputState.isSet('right'):
steering -= dt * self.steeringRate
steering = max(steering, -1.0)
else:
# gradually re-center the steering
if steering > 0.0:
steering -= dt * self.centreingRate
if steering < 0.0:
steering = 0.0
elif steering < 0.0:
steering += dt * self.centreingRate
if steering > 0.0:
steering = 0.0
self.vehicleControlState["steering"] = steering
# """Updates acceleration, braking and steering
# These are all passed in through a controlState dictionary
# """
# Update acceleration and braking
wheelForce = self.vehicleControlState["throttle"] * self.specs[
"maxWheelForce"]
self.reversing = self.vehicleControlState["reverse"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
brakeForce = self.vehicleControlState["brake"] * self.specs[
"brakeForce"]
# Update steering
# Steering control state is from -1 to 1
steering = self.vehicleControlState["steering"] * self.specs[
"steeringLock"]
self.applyForcesAndSteering(steering, wheelForce, brakeForce)
return [steering, wheelForce, brakeForce]
def setVehiclePos(self, x, y, z, h, p, r):
self.chassisNP.setPosHpr(x, y, z, h, p, r)
def setupVehicle(self, bulletWorld):
# Chassis
shape = BulletBoxShape(Vec3(1, 2.2, 0.5))
ts = TransformState.makePos(Point3(0, 0, .7))
if self.isMainChar:
self.chassisNode = BulletRigidBodyNode(self.username)
else:
self.chassisNode = BulletRigidBodyNode('vehicle')
self.chassisNode.setTag("username", str(self.username))
self.chassisNP = render.attachNewNode(self.chassisNode)
self.chassisNP.node().addShape(shape, ts)
if self.isMainChar:
self.chassisNP.node().notifyCollisions(True)
else:
self.chassisNP.node().notifyCollisions(False)
self.setVehiclePos(self.pos[0], self.pos[1], self.pos[2], self.pos[3],
self.pos[4], self.pos[5])
# self.chassisNP.setPos(-5.34744, 114.773, 6)
#self.chassisNP.setPos(49.2167, 64.7968, 10)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
#np.node().setCcdSweptSphereRadius(1.0)
#np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
self.carNP = loader.loadModel('models/batmobile-chassis.egg')
#self.yugoNP.setScale(.7)
self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, 1.1, .7), True, np)
# Left front wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, 1.1, .7), True, np)
# Right rear wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, -2, .7), False, np)
# Left rear wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, -2, .7), False, np)
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.33)
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 reset(self):
self.chassisNP.setP(0)
self.chassisNP.setR(0)
def pickedPowerup(self, powerup):
if not powerup.pickable:
powerup.useAbility()
else:
if self.currentPowerups["powerup1"] is None:
self.currentPowerups["powerup1"] = powerup
elif self.currentPowerups["powerup2"] is None:
self.currentPowerups["powerup2"] = powerup
elif self.currentPowerups["powerup3"] is None:
self.currentPowerups["powerup3"] = powerup
def canPickUpPowerup(self):
return (self.currentPowerups["powerup1"] is None
or self.currentPowerups["powerup2"] is None
or self.currentPowerups["powerup3"] is None)
def usePowerup(self, powerupIndex):
# Move usePowerupN to this function
if powerupIndex == 0 and self.currentPowerups["powerup1"] is not None:
self.currentPowerups["powerup1"].useAbility()
self.currentPowerups["powerup1"] = None
elif powerupIndex == 1 and self.currentPowerups["powerup2"] is not None:
self.currentPowerups["powerup2"].useAbility()
self.currentPowerups["powerup2"] = None
elif powerupIndex == 2 and self.currentPowerups["powerup3"] is not None:
self.currentPowerups["powerup3"].useAbility()
self.currentPowerups["powerup3"] = None
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, game, pos, vel, length, width, height, axisDis,
wheelDis, radius, wheelH):
self.initCordPos = numpy.array([pos[0], pos[1]])
'''self.length=length
self.width=width
self.height=height
self.axisDis=axisDis
self.wheelDis=wheelDis
self.radius=radius
self.wheelH=wheelH'''
line = game.segLine
lineLen = 0
prevPoint = line[0]
self.initNum = 0
for point in line:
norm = numpy.linalg.norm(point - prevPoint)
if pos[0] >= lineLen and pos[0] < lineLen + norm:
ldirec = (point - prevPoint) / norm
rdirec = numpy.array([ldirec[1], -ldirec[0]])
post = prevPoint + (pos[0] -
lineLen) * ldirec + rdirec * pos[1]
self.initPos = Vec3(post[0], post[1], pos[2])
break
lineLen = lineLen + norm
prevPoint = point
self.initNum = self.initNum + 1
chassis = setChassis(game, self.initPos, vel, length, width, height)
BulletVehicle.__init__(self, game.world, chassis.node())
self.setCoordinateSystem(ZUp)
game.world.attachVehicle(self)
#self.yugoNP = loader.loadModel('models/yugo/yugo.egg')
self.yugoNP = loader.loadModel('models/car.egg')
self.yugoNP.reparentTo(chassis)
# Right front wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(wheelDis / 2, axisDis / 2, wheelH), True, np,
radius)
# Left front wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(-wheelDis / 2, axisDis / 2, wheelH), True, np,
radius)
# Right rear wheel
np = loader.loadModel('models/yugo/yugotireR.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(wheelDis / 2, -axisDis / 2, wheelH), False, np,
radius)
# Left rear wheel
np = loader.loadModel('models/yugo/yugotireL.egg')
np.reparentTo(game.worldNP)
self.addWheel(Point3(-wheelDis / 2, -axisDis / 2, wheelH), False, np,
radius)
# Steering info
self.steering = 0.0 # degree
self.steeringClamp = 45.0 # degree
self.steeringIncrement = 10.0 # degree per second
def setup(self):
# Bullet physics world
self.worldNP = render.attach_new_node('World')
self.debugNP = self.worldNP.attach_new_node(BulletDebugNode('Debug'))
self.world = BulletWorld()
self.world.set_gravity(LVector3(0, 0, -9.81))
self.world.set_debug_node(self.debugNP.node())
# Vehicle
# Steering info
self.steering = 0.0 # degrees
self.steering_clamp = 45.0 # degrees
self.steering_increment = 120.0 # degrees per second
# How fast steering relaxes to straight ahead
self.steering_relax_factor = 2.0
# How much steering intensity decreases with higher speeds
self.steering_speed_reduction_factor = 0.003
# Chassis collision box (note: Bullet uses half-measures)
shape = BulletBoxShape(LVector3(0.6, 1.4, 0.5))
ts = TransformState.make_pos(LPoint3(0, 0, 0.5))
self.vehicleNP = self.worldNP.attach_new_node(
BulletRigidBodyNode('Vehicle'))
self.vehicleNP.node().add_shape(shape, ts)
# Set initial position
self.vehicleNP.set_pos(0, 70, -10)
self.vehicleNP.node().set_mass(800.0)
self.vehicleNP.node().set_deactivation_enabled(False)
self.world.attach(self.vehicleNP.node())
# Camera floater
self.attach_camera_floater()
# BulletVehicle
self.vehicle = BulletVehicle(self.world, self.vehicleNP.node())
self.world.attach(self.vehicle)
# Vehicle model
self.yugoNP = loader.load_model('models/yugo/yugo.egg')
self.yugoNP.reparent_to(self.vehicleNP)
# Right front wheel
np = loader.load_model('models/yugo/yugotireR.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3( 0.70, 1.05, 0.3), True, np)
# Left front wheel
np = loader.load_model('models/yugo/yugotireL.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3(-0.70, 1.05, 0.3), True, np)
# Right rear wheel
np = loader.load_model('models/yugo/yugotireR.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3( 0.70, -1.05, 0.3), False, np)
# Left rear wheel
np = loader.load_model('models/yugo/yugotireL.egg')
np.reparent_to(self.worldNP)
self.add_wheel(LPoint3(-0.70, -1.05, 0.3), False, np)
# Load a skybox
self.skybox = self.loader.load_model(
"samples/shader-terrain/models/skybox.bam")
self.skybox.reparent_to(self.render)
self.skybox.set_scale(20000)
skybox_texture = self.loader.load_texture(
"samples/shader-terrain/textures/skybox.jpg")
skybox_texture.set_minfilter(SamplerState.FT_linear)
skybox_texture.set_magfilter(SamplerState.FT_linear)
skybox_texture.set_wrap_u(SamplerState.WM_repeat)
skybox_texture.set_wrap_v(SamplerState.WM_mirror)
skybox_texture.set_anisotropic_degree(16)
self.skybox.set_texture(skybox_texture)
skybox_shader = Shader.load(Shader.SL_GLSL,
"samples/shader-terrain/skybox.vert.glsl",
"samples/shader-terrain/skybox.frag.glsl")
self.skybox.set_shader(skybox_shader)
# Terrain
self.setup_terrain()
class Character:
playerId = 1
type = 0
def __init__(self, tempworld, bulletWorld, type, playerId):
self.world = tempworld
self.speed = 0
self.acceleration = 1.5
self.brakes = .7
self.min_speed = 0
self.max_speed = 150
self.reverse_speed = 20
self.reverse_limit = -40
self.armor = 100
self.health = 100
self.a_timer_start = time.time()
self.a_timer_end = time.time()
self.power_ups = [0, 0, 0]
self.playerId = playerId
if type == 0:
self.actor = Actor("models/batmobile-chassis")
self.actor.setScale(0.7)
carRadius = 3
elif type == 1:
self.actor = Actor("models/policecarpainted", {})
self.actor.setScale(0.30)
self.actor.setH(180) # elif type == 2:
# self.actor = loader.loadModel("knucklehead.egg")
# self.tex = loader.loadTexture("knucklehead.jpg")
# self.actor.setTexture(self.car_tex, 1)
shape = BulletBoxShape(Vec3(1.0, 1.5, 0.4))
ts = TransformState.makePos(Point3(0, 0, 0.6))
self.chassisNP = render.attachNewNode(BulletRigidBodyNode('Vehicle'))
self.chassisNP.node().addShape(shape, ts)
self.chassisNP.setPos(50 * random.random(), 50 * random.random(), 1)
self.chassisNP.setH(180)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
self.actor.reparentTo(self.chassisNP)
self.actor.setH(180)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
for fb, y in (("F", 1.1), ("B", -1.1)):
for side, x in (("R", 0.75), ("L", -0.75)):
np = loader.loadModel("models/tire%s.egg" % side)
np.reparentTo(render)
isFront = fb == "F"
self.addWheel(Point3(x, y, 0.55), isFront, np)
def addWheel(self, position, isFront, np):
wheel = self.vehicle.createWheel()
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(position)
wheel.setFrontWheel(isFront)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(0.2)
wheel.setMaxSuspensionTravelCm(0.4 * 100.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(100)
wheel.setRollInfluence(0.1)
def getActor(self):
return self.actor
def get_speed(self):
return self.speed
def accelerate(self):
# check how long you were accelerating
time_elapsed = time.time() - self.a_timer_end
if time_elapsed > 1: # in seconds last accelerated
self.a_timer_start = time.time()
if self.speed < self.max_speed:
self.speed += self.acceleration * (time.time() - self.a_timer_start)
# print(time_elapsed)
else:
self.speed = self.max_speed
# reset timer
self.a_timer_end = time.time()
def friction(self, friction):
if self.speed > 0:
self.speed -= friction
else:
self.speed = 0
def brake(self):
if self.speed > self.min_speed:
self.speed -= self.brakes
# reset acceleration timer
self.a_timer_start = time.time()
def reverse(self):
if self.speed > self.reverse_limit:
self.speed -= self.reverse_speed
def walk(self):
self.actor.stop()
self.actor.pose("walk", 5)
self.isMoving = False
def run(self):
self.actor.loop("run")
self.isMoving = True
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
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 Vehicle(object):
COUNT = 0
def __init__(self, bulletWorld, pos, username):
self.world = bulletWorld
self.acceleration = 1.5
self.brakeForce = 100.0
self.mass = 800
self.max_speed = 150
self.reverse_limit = -40
self.mass = 800.0 # kg
self.max_speed = 150 # km
self.armor = 100
self.health = 100
self.friction = 0.2 #slows the car when not accelerating (based on the brakes)
self.maxWheelForce = 2000.0
self.maxWheelForce = 2000.0 # acceleration
self.power_ups = [0, 0, 0]
self.specs = {"mass": self.mass, "maxWheelForce": self.maxWheelForce, "brakeForce": self.brakeForce,
"steeringLock": 20.0}
self.vehicleControlState = {"throttle": 0, "reverse": False, "brake": 0.0, "steering": 0.0}
self.username = username
# Steering change per second, normalised to steering lock
# Eg. 45 degrees lock and 1.0 rate means 45 degrees per second
self.steeringRate = 0.7
self.centreingRate = 5.0
self.pos = pos
self.currentPowerups = {"powerup1": None, "powerup2": None, "powerup3": None}
self.setupVehicle(bulletWorld)
COUNT = 1
def processInput(self, inputState, dt):
# print self.chassisNP.getPos()
# print self.chassisNP.getH()
"""Use controls to update the player's car"""
# For keyboard throttle and brake are either 0 or 1
if inputState.isSet('forward') and self.vehicle.getCurrentSpeedKmHour() <= self.max_speed:
self.vehicleControlState["throttle"] = 1.0
else:
self.vehicleControlState["throttle"] = 0.0
velocity = self.chassisNode.getLinearVelocity()
speed = math.sqrt(sum(v ** 2 for v in velocity))
# Update braking and reversing
if inputState.isSet('brake'):
if speed < 0.5 or self.vehicleControlState["reverse"]:
# If we're stopped, then start reversing
# Also keep reversing if we already were
self.vehicleControlState["reverse"] = True
self.vehicleControlState["throttle"] = 1.0
self.vehicleControlState["brake"] = 0.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 1.0
else:
self.vehicleControlState["reverse"] = False
self.vehicleControlState["brake"] = 0.0
# steering is normalised from -1 to 1, corresponding
# to the steering lock right and left
steering = self.vehicleControlState["steering"]
if inputState.isSet('left'):
steering += dt * self.steeringRate
steering = min(steering, 1.0)
elif inputState.isSet('right'):
steering -= dt * self.steeringRate
steering = max(steering, -1.0)
else:
# gradually re-center the steering
if steering > 0.0:
steering -= dt * self.centreingRate
if steering < 0.0:
steering = 0.0
elif steering < 0.0:
steering += dt * self.centreingRate
if steering > 0.0:
steering = 0.0
self.vehicleControlState["steering"] = steering
# """Updates acceleration, braking and steering
# These are all passed in through a controlState dictionary
# """
# Update acceleration and braking
wheelForce = self.vehicleControlState["throttle"] * self.specs["maxWheelForce"]
self.reversing = self.vehicleControlState["reverse"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
brakeForce = self.vehicleControlState["brake"] * self.specs["brakeForce"]
#slows down vehicle when no key pressed
if self.vehicleControlState["throttle"] != 1.0 and self.vehicleControlState["reverse"] != 1.0 and self.vehicleControlState["brake"] != 1.0:
brakeForce = self.friction * self.specs["brakeForce"]
# Update steering
# Steering control state is from -1 to 1
steering = self.vehicleControlState["steering"] * self.specs["steeringLock"]
# Apply steering to front wheels
self.vehicle.setSteeringValue(steering, 0);
self.vehicle.setSteeringValue(steering, 1);
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(wheelForce, 2);
self.vehicle.applyEngineForce(wheelForce, 3);
self.vehicle.setBrake(brakeForce, 2);
self.vehicle.setBrake(brakeForce, 3);
def setupVehicle(self, bulletWorld):
# Chassis
shape = BulletBoxShape(Vec3(1, 2.2, 0.5))
ts = TransformState.makePos(Point3(0, 0, .7))
self.chassisNode = BulletRigidBodyNode('Vehicle')
self.chassisNP = render.attachNewNode(self.chassisNode)
self.chassisNP.node().addShape(shape, ts)
self.chassisNP.node().notifyCollisions(True)
self.chassisNP.setPosHpr(self.pos[0], self.pos[1], self.pos[2], self.pos[3], self.pos[4], self.pos[5])
# self.chassisNP.setPos(-5.34744, 114.773, 6)
# self.chassisNP.setPos(49.2167, 64.7968, 10)
self.chassisNP.node().setMass(800.0)
self.chassisNP.node().setDeactivationEnabled(False)
bulletWorld.attachRigidBody(self.chassisNP.node())
# np.node().setCcdSweptSphereRadius(1.0)
# np.node().setCcdMotionThreshold(1e-7)
# Vehicle
self.vehicle = BulletVehicle(bulletWorld, self.chassisNP.node())
self.vehicle.setCoordinateSystem(ZUp)
bulletWorld.attachVehicle(self.vehicle)
self.carNP = loader.loadModel('models/batmobile-chassis.egg')
# self.yugoNP.setScale(.7)
self.carNP.reparentTo(self.chassisNP)
# Right front wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, 1.1, .7), True, np)
# Left front wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, 1.1, .7), True, np)
# Right rear wheel
np = loader.loadModel('models/batmobile-wheel-right.egg')
np.reparentTo(render)
self.addWheel(Point3(1, -2, .7), False, np)
# Left rear wheel
np = loader.loadModel('models/batmobile-wheel-left.egg')
np.reparentTo(render)
self.addWheel(Point3(-1, -2, .7), False, np)
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.33)
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 reset(self):
self.chassisNP.setP(0)
self.chassisNP.setR(0)
def pickedPowerup(self, powerup):
if not powerup.pickable:
powerup.useAbility()
else:
if self.currentPowerups["powerup1"] is None:
self.currentPowerups["powerup1"] = powerup
elif self.currentPowerups["powerup2"] is None:
self.currentPowerups["powerup2"] = powerup
elif self.currentPowerups["powerup3"] is None:
self.currentPowerups["powerup3"] = powerup
def canPickUpPowerup(self):
return (self.currentPowerups["powerup1"] is None or
self.currentPowerups["powerup2"] is None or
self.currentPowerups["powerup3"] is None)
def usePowerup(self, powerupIndex):
# Move usePowerupN to this function
if powerupIndex == 0 and self.currentPowerups["powerup1"] is not None:
self.currentPowerups["powerup1"].useAbility()
self.currentPowerups["powerup1"] = None
elif powerupIndex == 1 and self.currentPowerups["powerup2"] is not None:
self.currentPowerups["powerup2"].useAbility()
self.currentPowerups["powerup2"] = None
elif powerupIndex == 2 and self.currentPowerups["powerup3"] is not None:
self.currentPowerups["powerup3"].useAbility()
self.currentPowerups["powerup3"] = None
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
