class Chunk(Entity):
chunkmass = 5.0
def __init__(self, world, blocks, pos, hpr=Point3(0,0,0), diff = Vec3(0,0,0), angVel = Vec3(0,0,0), linVel = Vec3(0,0,0)):
super(Chunk, self).__init__()
self.mass = len(blocks)*Chunk.chunkmass
self.world = world
self.blocks = blocks
self.bnode = BulletRigidBodyNode()
self.bnode.setMass(self.mass)
self.bnode.setAngularFactor(Vec3(0,1,0))
self.bnode.setLinearSleepThreshold(20)
self.bnode.setAngularSleepThreshold(20)
self.bnode.setAngularVelocity(angVel)
self.bnode.setLinearVelocity(linVel)
self.np = utilities.app.render.attachNewNode(self.bnode)
self.np.setPos(pos.x,20,pos.y)
self.np.setHpr(hpr)
self.np.setPos(self.np, diff)
self.bnode.setPythonTag("entity", self)
self.inScope = False # culling not done yet
# centre the blocks around the np and add their shapes in.
self.minMax()
cog = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0)
for block in blocks:
block.rebuild(self, cog)
world.bw.attachRigidBody(self.bnode)
self.hitlist = dict()
def update(self, timer):
for index in self.hitlist:
# returns true if the wall is destroyed by the hit
if self.blocks[index].hitby(self.hitlist[index]):
self.blocks[index].destroy()
self.rebuild(index)
self.hitlist.clear()
if self.playerDistance() > 40.0:
self.bnode.setAngularSleepThreshold(1)
self.bnode.setLinearSleepThreshold(1)
else:
self.bnode.setAngularSleepThreshold(0)
self.bnode.setLinearSleepThreshold(0)
def playerDistance(self):
sp = self.np.getPos()
pp = self.world.player.node.getPos()
distance = hypot(sp.x - pp.x, sp.z - pp.z)
return distance
# remove an element and rebuild
# TODO add another method for removing multiple
# blocks in a single go
def rebuild(self, index):
deadblock = self.blocks[index]
out = list()
for block in self.blocks:
for edge in block.edges:
if edge == deadblock:
block.edges.remove(edge)
for block in deadblock.edges:
chunk = list()
self.searchBlock(block, chunk, deadblock)
out.append(chunk)
#out is a list of lists of populated blocks
#remove duplicate chunks
results = list()
for chunk in out:
chunk.sort(compareBlock)
if not chunk in results and len(chunk) > 0:
results.append(chunk)
for result in results:
self.minMax(result)
#diff = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0)
diff = Vec3((self.minX+self.maxX) / 2.0, 0, (self.minY+self.maxY) / 2.0)
p = self.np.getPos()
pos = Point2(p.x, p.z)
h = self.np.getHpr()
self.world.entities.append(Chunk(self.world, result, pos, h, diff, self.bnode.getAngularVelocity(), self.bnode.getLinearVelocity()))
self.destroyNow()
self.remove = True
def searchBlock(self, block, lst, deleted):
if block in lst:
return
else:
lst.append(block)
for newblock in block.edges:
self.searchBlock(newblock, lst, deleted)
def minMax(self, blocks=None):
if blocks == None:
blocks = self.blocks
self.minX = blocks[0].pos.x
self.minY = blocks[0].pos.y
self.maxX = blocks[0].pos.x
self.maxY = blocks[0].pos.y
for point in blocks:
self.minX = min(point.pos.x, self.minX)
self.minY = min(point.pos.y, self.minY)
self.maxX = max(point.pos.x, self.maxX)
self.maxY = max(point.pos.y, self.maxY)
# Need to clear the bulletrigidbody immediately
# so that the phys object isn't present during next sim phase
# which occurs before cleanup normally. Otherwise shit will
# fly everywhere since two things are inside each other
def destroyNow(self):
self.world.bw.removeRigidBody(self.bnode)
return
# since this is handled earlier nothing happens
def destroy(self):
return
def hitby(self, projectile, index):
self.hitlist[index] = projectile
class Player(Entity):
walkspeed = 50
damping = 0.9
topspeed = 15
leftMove = False
rightMove = False
jumpToggle = False
crouchToggle = False
def __init__(self, world):
super(Player, self).__init__()
self.obj = utilities.loadObject("player", depth=20)
self.world = world
self.health = 100
self.inventory = dict()
self.depth = self.obj.getPos().y
self.location = Point2(0,0)
self.velocity = Vec3(0)
self.pt = 0.0
self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49))
self.bnode = BulletRigidBodyNode('Box')
self.bnode.setMass(1.0)
self.bnode.setAngularVelocity(Vec3(0))
self.bnode.setAngularFactor(Vec3(0))
self.bnode.addShape(self.shape)
self.bnode.setLinearDamping(0.95)
self.bnode.setLinearSleepThreshold(0)
world.bw.attachRigidBody(self.bnode)
self.bnode.setPythonTag("Entity", self)
self.bnode.setIntoCollideMask(BitMask32.bit(0))
self.node = utilities.app.render.attachNewNode(self.bnode)
self.node.setPos(self.obj.getPos())
self.obj.setPos(0,0,0)
self.obj.setScale(1)
self.obj.reparentTo(self.node)
self.node.setPos(self.location.x, self.depth, self.location.y)
def initialise(self):
self.inventory["LightLaser"] = LightLaser(self.world, self)
self.inventory["Blowtorch"] = Blowtorch(self.world, self)
self.inventory["Grenade"] = Grenade(self.world, self)
for i in self.inventory:
self.inventory[i].initialise()
self.currentItem = self.inventory["Blowtorch"]
self.currentItem.equip()
self.armNode = self.obj.attachNewNode("arm")
self.armNode.setPos(0.20,0,0.08)
self.arm = utilities.loadObject("arm", scaleX = 0.5,scaleY = 0.5, depth = -0.2)
self.arm.reparentTo(self.armNode)
def activate(self):
self.currentItem.activate()
def update(self, timer):
self.velocity = self.bnode.getLinearVelocity()
if (self.leftMove):
self.bnode.applyCentralForce(Vec3(-Player.walkspeed,0,0))
if (self.rightMove):
self.bnode.applyCentralForce(Vec3(Player.walkspeed,0,0))
if (self.jumpToggle):
self.bnode.applyCentralForce(Vec3(0,0,Player.walkspeed))
if (self.crouchToggle):
self.bnode.applyCentralForce(Vec3(0,0,-Player.walkspeed))
if (self.velocity.x < -self.topspeed):
self.velocity.x = -self.topspeed
if (self.velocity.x > self.topspeed):
self.velocity.x = self.topspeed
mouse = utilities.app.mousePos
# extrude test
near = Point3()
far = Point3()
utilities.app.camLens.extrude(mouse, near, far)
camp = utilities.app.camera.getPos()
near *= 20 # player depth
if near.x != 0:
angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x)
#angle = atan2(near.z, near.x)
else:
angle = 90
self.angle = angle
# set current item to point at cursor
self.currentItem.update(timer)
# move the camera so the player is centred horizontally,
# but keep the camera steady vertically
utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z)
#move arm into correct position.
gunLength = 2.0
self.gunVector = Point2(cos(angle)*gunLength - self.armNode.getX()*5, sin(angle)*gunLength - self.armNode.getZ()*2)
armAngle = atan2(self.gunVector.y, self.gunVector.x)
self.arm.setHpr(self.armNode, 0, 0, -1 * degrees(armAngle))
def moveLeft(self, switch):
self.leftMove = switch
#self.bnode.applyCentralForce(Vec3(-500,0,0))
def moveRight(self, switch):
self.rightMove = switch
#self.bnode.applyCentralForce(Vec3(500,0,0))
def jump(self, switch):
self.jumpToggle = switch
def crouch(self, switch):
self.crouchToggle = switch
class DroneEnv(gym.Env):
def __init__(self):
self.visualize = False
if self.visualize == False:
from pandac.PandaModules import loadPrcFileData
loadPrcFileData("", "window-type none")
import direct.directbase.DirectStart
self.ep = 0
self.ep_rew = 0
self.t = 0
self.prevDis = 0
self.action_space = Box(-1, 1, shape=(3, ))
self.observation_space = Box(-50, 50, shape=(9, ))
self.target = 8 * np.random.rand(3)
self.construct()
self.percentages = []
self.percentMean = []
self.percentStd = []
taskMgr.add(self.stepTask, 'update')
taskMgr.add(self.lightTask, 'lights')
self.rotorForce = np.array([0, 0, 9.81], dtype=np.float)
def construct(self):
if self.visualize:
base.cam.setPos(17, 17, 1)
base.cam.lookAt(0, 0, 0)
wp = WindowProperties()
wp.setSize(1200, 500)
base.win.requestProperties(wp)
# World
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
#skybox
skybox = loader.loadModel('../models/skybox.gltf')
skybox.setTexGen(TextureStage.getDefault(),
TexGenAttrib.MWorldPosition)
skybox.setTexProjector(TextureStage.getDefault(), render, skybox)
skybox.setTexScale(TextureStage.getDefault(), 1)
skybox.setScale(100)
skybox.setHpr(0, -90, 0)
tex = loader.loadCubeMap('../textures/s_#.jpg')
skybox.setTexture(tex)
skybox.reparentTo(render)
render.setTwoSided(True)
#Light
plight = PointLight('plight')
plight.setColor((1.0, 1.0, 1.0, 1))
plnp = render.attachNewNode(plight)
plnp.setPos(0, 0, 0)
render.setLight(plnp)
# Create Ambient Light
ambientLight = AmbientLight('ambientLight')
ambientLight.setColor((0.15, 0.05, 0.05, 1))
ambientLightNP = render.attachNewNode(ambientLight)
render.setLight(ambientLightNP)
# Drone
shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
self.drone = BulletRigidBodyNode('Box')
self.drone.setMass(1.0)
self.drone.addShape(shape)
self.droneN = render.attachNewNode(self.drone)
self.droneN.setPos(0, 0, 3)
self.world.attachRigidBody(self.drone)
model = loader.loadModel('../models/drone.gltf')
model.setHpr(0, 90, 0)
model.flattenLight()
model.reparentTo(self.droneN)
blade = loader.loadModel("../models/blade.gltf")
blade.reparentTo(self.droneN)
blade.setHpr(0, 90, 0)
blade.setPos(Vec3(0.3, -3.0, 1.1))
rotation_interval = blade.hprInterval(0.2, Vec3(180, 90, 0))
rotation_interval.loop()
placeholder = self.droneN.attachNewNode("blade-placeholder")
placeholder.setPos(Vec3(0, 6.1, 0))
blade.instanceTo(placeholder)
placeholder = self.droneN.attachNewNode("blade-placeholder")
placeholder.setPos(Vec3(3.05, 3.0, 0))
blade.instanceTo(placeholder)
placeholder = self.droneN.attachNewNode("blade-placeholder")
placeholder.setPos(Vec3(-3.05, 3.0, 0))
blade.instanceTo(placeholder)
#drone ligths
self.plight2 = PointLight('plight')
self.plight2.setColor((0.9, 0.1, 0.1, 1))
plnp = self.droneN.attachNewNode(self.plight2)
plnp.setPos(0, 0, -1)
self.droneN.setLight(plnp)
#over light
plight3 = PointLight('plight')
plight3.setColor((0.9, 0.8, 0.8, 1))
plnp = self.droneN.attachNewNode(plight3)
plnp.setPos(0, 0, 2)
self.droneN.setLight(plnp)
#target object
self.targetObj = loader.loadModel("../models/target.gltf")
self.targetObj.reparentTo(render)
self.targetObj.setPos(
Vec3(self.target[0], self.target[1], self.target[2]))
def lightTask(self, task):
count = globalClock.getFrameCount()
rest = count % 100
if rest < 10:
self.plight2.setColor((0.1, 0.9, 0.1, 1))
elif rest > 30 and rest < 40:
self.plight2.setColor((0.9, 0.1, 0.1, 1))
elif rest > 65 and rest < 70:
self.plight2.setColor((0.9, 0.9, 0.9, 1))
elif rest > 75 and rest < 80:
self.plight2.setColor((0.9, 0.9, 0.9, 1))
else:
self.plight2.setColor((0.1, 0.1, 0.1, 1))
return task.cont
def getState(self):
#vel = self.drone.get_linear_velocity()
po = self.drone.transform.pos
ang = self.droneN.getHpr()
#velocity = np.nan_to_num(np.array([vel[0], vel[1], vel[2]]))
position = np.array([po[0], po[1], po[2]])
state = np.array([position, self.target]).reshape(6, )
state = np.around(state, decimals=3)
return state
def getReward(self):
reward = 0
s = self.getState()
d = np.linalg.norm(s[0:3] - s[3:6])
if d < 20:
reward = 20 - d
reward = reward / 40 #/4000
#if d < self.prevDis :
# reward *= 1.2
#self.prevDis = np.copy(d)
return reward
def reset(self):
#log
self.percentages.append(self.ep_rew)
me = np.mean(self.percentages[-500:])
self.percentMean.append(me)
self.percentStd.append(np.std(self.percentages[-500:]))
s = self.getState()
d = np.linalg.norm(np.abs(s[:3] - self.target))
ds = np.linalg.norm(s[:3] - np.array([0, 0, 4]))
if self.ep % 50 == 0:
self.PlotReward()
print(
f'{self.ep} {self.t} {self.ep_rew:+8.2f} {me:+6.2f} {d:6.2f} {ds:6.2f} {s}'
) #{s[:6]}
#physics reset
self.droneN.setPos(0, 0, 4)
self.droneN.setHpr(0, 0, 0)
self.drone.set_linear_velocity(Vec3(0, 0, 0))
self.drone.setAngularVelocity(Vec3(0, 0, 0))
self.rotorForce = np.array([0, 0, 9.81], dtype=np.float)
#define new target:
self.target = 8 * (2 * np.random.rand(3) - 1)
#self.target = np.zeros((3))
self.target[2] = np.abs(self.target[2])
self.target[1] = np.abs(self.target[1])
self.targetObj.setPos(
Vec3(self.target[0], self.target[1], self.target[2]))
self.ep += 1
self.t = 0
self.ep_rew = 0
state = self.getState()
return state
def stepTask(self, task):
dt = globalClock.getDt()
if self.visualize:
self.world.doPhysics(dt)
else:
self.world.doPhysics(0.1)
self.drone.setDeactivationEnabled(False)
#application of force
force = Vec3(self.rotorForce[0], self.rotorForce[1],
self.rotorForce[2])
self.drone.applyCentralForce(force) #should be action
po = self.drone.transform.pos
position = np.array([po[0], po[1], po[2]])
return task.cont
def step(self, action):
done = False
reward = 0
self.t += 1
reward = self.getReward()
self.ep_rew += reward
state = self.getState()
basis = np.array([0, 0, 9.81], dtype=np.float)
self.rotorForce = basis + 0.2 * action #0.1 *action
#10 sub steps in each step
for i in range(5):
c = taskMgr.step()
self.rotorForce -= 0.05 * (self.rotorForce - basis)
#time constraint
if self.t > 150:
done = True
return state, reward, done, {}
def PlotReward(self):
c = range(len(self.percentages))
plt.plot(self.percentMean, c='b', alpha=0.8)
plt.fill_between(
c,
np.array(self.percentMean) + np.array(self.percentStd),
np.array(self.percentMean) - np.array(self.percentStd),
color='g',
alpha=0.3,
label='Objective 1')
plt.grid()
plt.savefig('rews.png')
plt.close()
class Chunk(Entity):
chunkmass = 5.0
def __init__(self,
world,
blocks,
pos,
hpr=Point3(0, 0, 0),
diff=Vec3(0, 0, 0),
angVel=Vec3(0, 0, 0),
linVel=Vec3(0, 0, 0)):
super(Chunk, self).__init__()
self.mass = len(blocks) * Chunk.chunkmass
self.world = world
self.blocks = blocks
self.bnode = BulletRigidBodyNode()
self.bnode.setMass(self.mass)
self.bnode.setAngularFactor(Vec3(0, 1, 0))
self.bnode.setLinearSleepThreshold(20)
self.bnode.setAngularSleepThreshold(20)
self.bnode.setAngularVelocity(angVel)
self.bnode.setLinearVelocity(linVel)
self.np = utilities.app.render.attachNewNode(self.bnode)
self.np.setPos(pos.x, 20, pos.y)
self.np.setHpr(hpr)
self.np.setPos(self.np, diff)
self.bnode.setPythonTag("entity", self)
self.inScope = False # culling not done yet
# centre the blocks around the np and add their shapes in.
self.minMax()
cog = Point2((self.minX + self.maxX) / 2.0,
(self.minY + self.maxY) / 2.0)
for block in blocks:
block.rebuild(self, cog)
world.bw.attachRigidBody(self.bnode)
self.hitlist = dict()
def update(self, timer):
for index in self.hitlist:
# returns true if the wall is destroyed by the hit
if self.blocks[index].hitby(self.hitlist[index]):
self.blocks[index].destroy()
self.rebuild(index)
self.hitlist.clear()
if self.playerDistance() > 40.0:
self.bnode.setAngularSleepThreshold(1)
self.bnode.setLinearSleepThreshold(1)
else:
self.bnode.setAngularSleepThreshold(0)
self.bnode.setLinearSleepThreshold(0)
def playerDistance(self):
sp = self.np.getPos()
pp = self.world.player.node.getPos()
distance = hypot(sp.x - pp.x, sp.z - pp.z)
return distance
# remove an element and rebuild
# TODO add another method for removing multiple
# blocks in a single go
def rebuild(self, index):
deadblock = self.blocks[index]
out = list()
for block in self.blocks:
for edge in block.edges:
if edge == deadblock:
block.edges.remove(edge)
for block in deadblock.edges:
chunk = list()
self.searchBlock(block, chunk, deadblock)
out.append(chunk)
#out is a list of lists of populated blocks
#remove duplicate chunks
results = list()
for chunk in out:
chunk.sort(compareBlock)
if not chunk in results and len(chunk) > 0:
results.append(chunk)
for result in results:
self.minMax(result)
#diff = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0)
diff = Vec3((self.minX + self.maxX) / 2.0, 0,
(self.minY + self.maxY) / 2.0)
p = self.np.getPos()
pos = Point2(p.x, p.z)
h = self.np.getHpr()
self.world.entities.append(
Chunk(self.world, result, pos, h, diff,
self.bnode.getAngularVelocity(),
self.bnode.getLinearVelocity()))
self.destroyNow()
self.remove = True
def searchBlock(self, block, lst, deleted):
if block in lst:
return
else:
lst.append(block)
for newblock in block.edges:
self.searchBlock(newblock, lst, deleted)
def minMax(self, blocks=None):
if blocks == None:
blocks = self.blocks
self.minX = blocks[0].pos.x
self.minY = blocks[0].pos.y
self.maxX = blocks[0].pos.x
self.maxY = blocks[0].pos.y
for point in blocks:
self.minX = min(point.pos.x, self.minX)
self.minY = min(point.pos.y, self.minY)
self.maxX = max(point.pos.x, self.maxX)
self.maxY = max(point.pos.y, self.maxY)
# Need to clear the bulletrigidbody immediately
# so that the phys object isn't present during next sim phase
# which occurs before cleanup normally. Otherwise shit will
# fly everywhere since two things are inside each other
def destroyNow(self):
self.world.bw.removeRigidBody(self.bnode)
return
# since this is handled earlier nothing happens
def destroy(self):
return
def hitby(self, projectile, index):
self.hitlist[index] = projectile
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward_only = self._params.get('collision_reward_only',
False)
self._collision_reward = self._params.get('collision_reward', -10.0)
self._obs_shape = self._params.get('obs_shape', (64, 36))
self._steer_limits = params.get('steer_limits', (-30., 30.))
self._speed_limits = params.get('speed_limits', (-4.0, 4.0))
self._motor_limits = params.get('motor_limits', (-0.5, 0.5))
self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1]
and self._use_vel)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 120)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._env_time_step = 0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 0.5)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
self._setup_spec()
self.spec = EnvSpec(observation_im_space=self.observation_im_space,
action_space=self.action_space,
action_selection_space=self.action_selection_space,
observation_vec_spec=self.observation_vec_spec,
action_spec=self.action_spec,
action_selection_spec=self.action_selection_spec,
goal_spec=self.goal_spec)
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
def _setup_spec(self):
self.action_spec = OrderedDict()
self.action_selection_spec = OrderedDict()
self.observation_vec_spec = OrderedDict()
self.goal_spec = OrderedDict()
self.action_spec['steer'] = Box(low=-45., high=45.)
self.action_selection_spec['steer'] = Box(low=self._steer_limits[0],
high=self._steer_limits[1])
if self._use_vel:
self.action_spec['speed'] = Box(low=-4., high=4.)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['speed'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['speed'].high[0]
]))
self.action_selection_spec['speed'] = Box(
low=self._speed_limits[0], high=self._speed_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['speed'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['speed'].high[0]
]))
else:
self.action_spec['motor'] = Box(low=-self._accelClamp,
high=self._accelClamp)
self.action_space = Box(low=np.array([
self.action_spec['steer'].low[0],
self.action_spec['motor'].low[0]
]),
high=np.array([
self.action_spec['steer'].high[0],
self.action_spec['motor'].high[0]
]))
self.action_selection_spec['motor'] = Box(
low=self._motor_limits[0], high=self._motor_limits[1])
self.action_selection_space = Box(
low=np.array([
self.action_selection_spec['steer'].low[0],
self.action_selection_spec['motor'].low[0]
]),
high=np.array([
self.action_selection_spec['steer'].high[0],
self.action_selection_spec['motor'].high[0]
]))
assert (np.logical_and(
self.action_selection_space.low >= self.action_space.low - 1e-4,
self.action_selection_space.high <=
self.action_space.high + 1e-4).all())
self.observation_im_space = Box(low=0,
high=255,
shape=tuple(
self._get_observation()[0].shape))
self.observation_vec_spec['coll'] = Discrete(1)
self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14)
self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0)
@property
def _base_dir(self):
return os.path.join(os.path.dirname(os.path.abspath(__file__)),
'models')
@property
def horizon(self):
return np.inf
@property
def max_reward(self):
return np.inf
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
self._setup_map()
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_map(self):
if hasattr(self, '_model_path'):
# Collidable objects
self._setup_collision_object(self._model_path)
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
def _setup_collision_object(self,
path,
pos=(0.0, 0.0, 0.0),
hpr=(0.0, 0.0, 0.0),
scale=1):
visNP = loader.loadModel(path)
visNP.clearModelNodes()
visNP.reparentTo(render)
visNP.setPos(pos[0], pos[1], pos[2])
visNP.setHpr(hpr[0], hpr[1], hpr[2])
visNP.set_scale(scale, scale, scale)
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
bodyNP.setHpr(hpr[0], hpr[1], hpr[2])
bodyNP.set_scale(scale, scale, scale)
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
print("Issue")
def _setup_restart_pos(self):
self._restart_index = 0
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
# alight = AmbientLight('ambientLight')
# alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
# alightNP = render.attachNewNode(alight)
# render.clearLight()
# render.setLight(alightNP)
pass
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 0.0)
def _default_restart_pos(self):
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _get_heading(self):
h = np.array(self._vehicle_pointer.getHpr())[0]
ori = h * (pi / 180.)
while (ori > 2 * pi):
ori -= 2 * pi
while (ori < 0):
ori += 2 * pi
return ori
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
# for i in range(int(dt/self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(self._p * err + self._d * d_err,
-self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
for _ in range(int(dt / self._step)):
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self.process(self._obs[0], self._obs_shape))
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self.process(self._back_obs[0],
self._obs_shape))
observation_im = np.concatenate(observation, axis=2)
coll = self._collision
heading = self._get_heading()
speed = self._get_speed()
observation_vec = np.array([coll, heading, speed])
return observation_im, observation_vec
def _get_goal(self):
return np.array([])
def process(self, image, obs_shape):
if self._use_depth:
im = np.reshape(image, (image.shape[0], image.shape[1]))
if im.shape != obs_shape:
im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA)
return im.astype(np.uint8)
else:
image = np.dot(image[..., :3], [0.299, 0.587, 0.114])
im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA
) #TODO how does this deal with aspect ratio
# TODO might not be necessary
im = np.expand_dims(im, 2)
return im.astype(np.uint8)
def _get_reward(self):
if self._collision_reward_only:
if self._collision:
reward = self._collision_reward
else:
reward = 0.0
else:
if self._collision:
reward = self._collision_reward
else:
reward = self._get_speed()
assert (reward <= self.max_reward)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
info['env_time_step'] = self._env_time_step
return info
def _back_up(self):
assert (self._use_vel)
# logger.debug('Backing up!')
self._params['back_up'] = self._params.get('back_up', {})
back_up_vel = self._params['back_up'].get('vel', -1.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 3.0)
self._update(dt=duration)
self._des_vel = 0.
self._steering = 0.
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if hard_reset:
logger.debug('Hard resetting!')
if pos is None and hpr is None:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
self._env_time_step = 0
return self._get_observation(), self._get_goal()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._mass * action[1]
self._update(dt=self._dt)
observation = self._get_observation()
goal = self._get_goal()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
self._env_time_step += 1
return observation, goal, reward, done, info
class CarEnv(DirectObject):
def __init__(self, params={}):
self._params = params
if 'random_seed' in self._params:
np.random.seed(self._params['random_seed'])
self._use_vel = self._params.get('use_vel', True)
self._run_as_task = self._params.get('run_as_task', False)
self._do_back_up = self._params.get('do_back_up', False)
self._use_depth = self._params.get('use_depth', False)
self._use_back_cam = self._params.get('use_back_cam', False)
self._collision_reward = self._params.get('collision_reward', 0.)
if not self._params.get('visualize', False):
loadPrcFileData('', 'window-type offscreen')
# Defines base, render, loader
try:
ShowBase()
except:
pass
base.setBackgroundColor(0.0, 0.0, 0.0, 1)
# World
self._worldNP = render.attachNewNode('World')
self._world = BulletWorld()
self._world.setGravity(Vec3(0, 0, -9.81))
self._dt = params.get('dt', 0.25)
self._step = 0.05
# Vehicle
shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25))
ts = TransformState.makePos(Point3(0., 0., 0.25))
self._vehicle_node = BulletRigidBodyNode('Vehicle')
self._vehicle_node.addShape(shape, ts)
self._mass = self._params.get('mass', 10.)
self._vehicle_node.setMass(self._mass)
self._vehicle_node.setDeactivationEnabled(False)
self._vehicle_node.setCcdSweptSphereRadius(1.0)
self._vehicle_node.setCcdMotionThreshold(1e-7)
self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node)
self._world.attachRigidBody(self._vehicle_node)
self._vehicle = BulletVehicle(self._world, self._vehicle_node)
self._vehicle.setCoordinateSystem(ZUp)
self._world.attachVehicle(self._vehicle)
self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07)
self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07)
self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07)
# Camera
size = self._params.get('size', [160, 90])
hfov = self._params.get('hfov', 60)
near_far = self._params.get('near_far', [0.1, 100.])
self._camera_sensor = Panda3dCameraSensor(base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='front cam')
self._camera_node = self._camera_sensor.cam
self._camera_node.setPos(0.0, 0.5, 0.375)
self._camera_node.lookAt(0.0, 6.0, 0.0)
self._camera_node.reparentTo(self._vehicle_pointer)
if self._use_back_cam:
self._back_camera_sensor = Panda3dCameraSensor(
base,
color=not self._use_depth,
depth=self._use_depth,
size=size,
hfov=hfov,
near_far=near_far,
title='back cam')
self._back_camera_node = self._back_camera_sensor.cam
self._back_camera_node.setPos(0.0, -0.5, 0.375)
self._back_camera_node.lookAt(0.0, -6.0, 0.0)
self._back_camera_node.reparentTo(self._vehicle_pointer)
# Car Simulator
self._des_vel = None
self._setup()
# Input
self.accept('escape', self._doExit)
self.accept('r', self.reset)
self.accept('f1', self._toggleWireframe)
self.accept('f2', self._toggleTexture)
self.accept('f3', self._view_image)
self.accept('f5', self._doScreenshot)
self.accept('q', self._forward_0)
self.accept('w', self._forward_1)
self.accept('e', self._forward_2)
self.accept('a', self._left)
self.accept('s', self._stop)
self.accept('x', self._backward)
self.accept('d', self._right)
self.accept('m', self._mark)
self._steering = 0.0 # degree
self._engineForce = 0.0
self._brakeForce = 0.0
self._p = self._params.get('p', 1.25)
self._d = self._params.get('d', 0.0)
self._last_err = 0.0
self._curr_time = 0.0
self._accelClamp = self._params.get('accelClamp', 2.0)
self._engineClamp = self._accelClamp * self._mass
self._collision = False
if self._run_as_task:
self._mark_d = 0.0
taskMgr.add(self._update_task, 'updateWorld')
base.run()
# _____HANDLER_____
def _doExit(self):
sys.exit(1)
def _toggleWireframe(self):
base.toggleWireframe()
def _toggleTexture(self):
base.toggleTexture()
def _doScreenshot(self):
base.screenshot('Bullet')
def _forward_0(self):
self._des_vel = 1
self._brakeForce = 0.0
def _forward_1(self):
self._des_vel = 2
self._brakeForce = 0.0
def _forward_2(self):
self._des_vel = 4
self._brakeForce = 0.0
def _stop(self):
self._des_vel = 0.0
self._brakeForce = 0.0
def _backward(self):
self._des_vel = -4
self._brakeForce = 0.0
def _right(self):
self._steering = np.min([np.max([-30, self._steering - 5]), 0.0])
def _left(self):
self._steering = np.max([np.min([30, self._steering + 5]), 0.0])
def _view_image(self):
from matplotlib import pyplot as plt
image = self._camera_sensor.observe()[0]
if self._use_depth:
plt.imshow(image[:, :, 0], cmap='gray')
else:
import cv2
def rgb2gray(rgb):
return np.dot(rgb[..., :3], [0.299, 0.587, 0.114])
image = rgb2gray(image)
im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA
) # TODO how does this deal with aspect ratio
plt.imshow(im.astype(np.uint8), cmap='Greys_r')
plt.show()
def _mark(self):
self._mark_d = 0.0
# Setup
def _setup(self):
if hasattr(self, '_model_path'):
# Collidable objects
visNP = loader.loadModel(self._model_path)
visNP.clearModelNodes()
visNP.reparentTo(render)
pos = (0., 0., 0.)
visNP.setPos(pos[0], pos[1], pos[2])
bodyNPs = BulletHelper.fromCollisionSolids(visNP, True)
for bodyNP in bodyNPs:
bodyNP.reparentTo(render)
bodyNP.setPos(pos[0], pos[1], pos[2])
if isinstance(bodyNP.node(), BulletRigidBodyNode):
bodyNP.node().setMass(0.0)
bodyNP.node().setKinematic(True)
bodyNP.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(bodyNP.node())
else:
ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
ground.node().addShape(shape)
ground.setCollideMask(BitMask32.allOn())
self._world.attachRigidBody(ground.node())
self._place_vehicle()
self._setup_light()
self._setup_restart_pos()
def _setup_restart_pos(self):
self._restart_pos = []
self._restart_index = 0
if self._params.get('position_ranges', None) is not None:
ranges = self._params['position_ranges']
num_pos = self._params['num_pos']
if self._params.get('range_type', 'random') == 'random':
for _ in range(num_pos):
ran = ranges[np.random.randint(len(ranges))]
self._restart_pos.append(np.random.uniform(ran[0], ran[1]))
elif self._params['range_type'] == 'fix_spacing':
num_ran = len(ranges)
num_per_ran = num_pos // num_ran
for i in range(num_ran):
ran = ranges[i]
low = np.array(ran[0])
diff = np.array(ran[1]) - np.array(ran[0])
for j in range(num_per_ran):
val = diff * ((j + 0.0) / num_per_ran) + low
self._restart_pos.append(val)
elif self._params.get('positions', None) is not None:
self._restart_pos = self._params['positions']
else:
self._restart_pos = self._default_restart_pos()
def _next_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
pos_hpr = self._restart_pos[self._restart_index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _next_random_restart_pos_hpr(self):
num = len(self._restart_pos)
if num == 0:
return None, None
else:
index = np.random.randint(num)
pos_hpr = self._restart_pos[index]
self._restart_index = (self._restart_index + 1) % num
return pos_hpr[:3], pos_hpr[3:]
def _setup_light(self):
alight = AmbientLight('ambientLight')
alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
alightNP = render.attachNewNode(alight)
render.clearLight()
render.setLight(alightNP)
# Vehicle
def _default_pos(self):
return (0.0, 0.0, 0.3)
def _default_hpr(self):
return (0.0, 0.0, 3.14)
def _default_restart_pos():
return [self._default_pos() + self._default_hpr()]
def _get_speed(self):
vel = self._vehicle.getCurrentSpeedKmHour() / 3.6
return vel
def _update(self, dt=1.0, coll_check=True):
self._vehicle.setSteeringValue(self._steering, 0)
self._vehicle.setSteeringValue(self._steering, 1)
self._vehicle.setBrake(self._brakeForce, 0)
self._vehicle.setBrake(self._brakeForce, 1)
self._vehicle.setBrake(self._brakeForce, 2)
self._vehicle.setBrake(self._brakeForce, 3)
if dt >= self._step:
# TODO maybe change number of timesteps
for i in range(int(dt / self._step)):
if self._des_vel is not None:
vel = self._get_speed()
err = self._des_vel - vel
d_err = (err - self._last_err) / self._step
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(self._step, 1, self._step)
self._collision = self._is_contact()
elif self._run_as_task:
self._curr_time += dt
if self._curr_time > 0.05:
if self._des_vel is not None:
vel = self._get_speed()
self._mark_d += vel * self._curr_time
print(vel, self._mark_d, self._is_contact())
err = self._des_vel - vel
d_err = (err - self._last_err) / 0.05
self._last_err = err
self._engineForce = np.clip(
self._p * err + self._d * d_err, -self._accelClamp,
self._accelClamp) * self._mass
self._curr_time = 0.0
self._vehicle.applyEngineForce(self._engineForce, 0)
self._vehicle.applyEngineForce(self._engineForce, 1)
self._vehicle.applyEngineForce(self._engineForce, 2)
self._vehicle.applyEngineForce(self._engineForce, 3)
self._world.doPhysics(dt, 1, dt)
self._collision = self._is_contact()
else:
raise ValueError(
"dt {0} s is too small for velocity control".format(dt))
def _stop_car(self):
self._steering = 0.0
self._engineForce = 0.0
self._vehicle.setSteeringValue(0.0, 0)
self._vehicle.setSteeringValue(0.0, 1)
self._vehicle.applyEngineForce(0.0, 0)
self._vehicle.applyEngineForce(0.0, 1)
self._vehicle.applyEngineForce(0.0, 2)
self._vehicle.applyEngineForce(0.0, 3)
if self._des_vel is not None:
self._des_vel = 0
self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0))
self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0))
for i in range(self._vehicle.getNumWheels()):
wheel = self._vehicle.getWheel(i)
wheel.setRotation(0.0)
self._vehicle_node.clearForces()
def _place_vehicle(self, pos=None, hpr=None):
if pos is None:
pos = self._default_pos()
if hpr is None:
hpr = self._default_hpr()
self._vehicle_pointer.setPos(pos[0], pos[1], pos[2])
self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2])
self._stop_car()
def _addWheel(self, pos, front, radius=0.25):
wheel = self._vehicle.createWheel()
wheel.setChassisConnectionPointCs(pos)
wheel.setFrontWheel(front)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(radius)
wheel.setMaxSuspensionTravelCm(40.0)
wheel.setSuspensionStiffness(40.0)
wheel.setWheelsDampingRelaxation(2.3)
wheel.setWheelsDampingCompression(4.4)
wheel.setFrictionSlip(1e2)
wheel.setRollInfluence(0.1)
# Task
def _update_task(self, task):
dt = globalClock.getDt()
self._update(dt=dt)
self._get_observation()
return task.cont
# Helper functions
def _get_observation(self):
self._obs = self._camera_sensor.observe()
observation = []
observation.append(self._obs[0])
if self._use_back_cam:
self._back_obs = self._back_camera_sensor.observe()
observation.append(self._back_obs[0])
observation = np.concatenate(observation, axis=2)
return observation
def _get_reward(self):
reward = self._collision_reward if self._collision else self._get_speed(
)
return reward
def _get_done(self):
return self._collision
def _get_info(self):
info = {}
info['pos'] = np.array(self._vehicle_pointer.getPos())
info['hpr'] = np.array(self._vehicle_pointer.getHpr())
info['vel'] = self._get_speed()
info['coll'] = self._collision
return info
def _back_up(self):
assert (self._use_vel)
back_up_vel = self._params['back_up'].get('vel', -2.0)
self._des_vel = back_up_vel
back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0))
# TODO
self._steering = np.random.uniform(*back_up_steer)
self._brakeForce = 0.
duration = self._params['back_up'].get('duration', 1.0)
self._update(dt=duration)
self._des_vel = 0.0
self._steering = 0.0
self._update(dt=duration)
self._brakeForce = 0.
def _is_contact(self):
result = self._world.contactTest(self._vehicle_node)
num_contacts = result.getNumContacts()
return result.getNumContacts() > 0
# Environment functions
def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False):
if self._do_back_up and not hard_reset and \
pos is None and hpr is None:
if self._collision:
self._back_up()
else:
if pos is None and hpr is None:
if random_reset:
pos, hpr = self._next_random_restart_pos_hpr()
else:
pos, hpr = self._next_restart_pos_hpr()
self._place_vehicle(pos=pos, hpr=hpr)
self._collision = False
return self._get_observation()
def step(self, action):
self._steering = action[0]
if action[1] == 0.0:
self._brakeForce = 1000.
else:
self._brakeForce = 0.
if self._use_vel:
# Convert from m/s to km/h
self._des_vel = action[1]
else:
self._engineForce = self._engineClamp * \
((action[1] - 49.5) / 49.5)
self._update(dt=self._dt)
observation = self._get_observation()
reward = self._get_reward()
done = self._get_done()
info = self._get_info()
return observation, reward, done, info
class CharacterController(DynamicObject, FSM):
def __init__(self, game):
self.game = game
FSM.__init__(self, "Player")
# key states
# dx direction left or right, dy direction forward or backward
self.kh = self.game.kh
self.dx = 0
self.dy = 0
self.jumping = 0
self.crouching = 0
# maximum speed when only walking
self.groundSpeed = 5.0
# acceleration used when walking to rise to self.groundSpeed
self.groundAccel = 100.0
# maximum speed in the air (air friction)
self.airSpeed = 30.0
# player movement control when in the air
self.airAccel = 10.0
# horizontal speed on jump start
self.jumpSpeed = 5.5
self.turnSpeed = 5
self.moveSpeedVec = Vec3(0,0,0)
self.platformSpeedVec = Vec3(0,0,0)
h = 1.75
w = 0.4
self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp)
self.node = BulletRigidBodyNode('Player')
self.node.setMass(2)
self.node.addShape(self.shape)
self.node.setActive(True, True)
self.node.setDeactivationEnabled(False, True)
self.node.setFriction(200)
#self.node.setGravity(10)
#self.node.setFallSpeed(200)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.node.setAngularFactor(Vec3(0,0,1))
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(0, 0, 0)
self.np.setH(0)
#self.np.setCollideMask(BitMask32.allOn())
self.game.world.attachRigidBody(self.node)
self.playerModel = None
self.slice_able = False
def setActor(self, modelPath, animDic={}, flip = False, pos = (0,0,0), scale = 1.0):
self.playerModel = Actor(modelPath, animDic)
self.playerModel.reparentTo(self.np)
self.playerModel.setScale(scale) # 1ft = 0.3048m
if flip:
self.playerModel.setH(180)
self.playerModel.setPos(pos)
self.playerModel.setScale(scale)
#-------------------------------------------------------------------
# Speed
def getSpeedVec(self):
return self.node.getLinearVelocity()
def setSpeedVec(self, speedVec):
#print "setting speed to %s" % (speedVec)
self.node.setLinearVelocity(speedVec)
return speedVec
def setPlatformSpeed(self, speedVec):
self.platformSpeedVec = speedVec
def getSpeed(self):
return self.getSpeedVec().length()
def setSpeed(self, speed):
speedVec = self.getSpeedVec()
speedVec.normalize()
self.setSpeedVec(speedVec*speed)
def getLocalSpeedVec(self):
return self.np.getRelativeVector(self.getSpeed())
def getSpeedXY(self):
vec = self.getSpeedVec()
return Vec3(vec[0], vec[1], 0)
def setSpeedXY(self, speedX, speedY):
vec = self.getSpeedVec()
z = self.getSpeedZ()
self.setSpeedVec(Vec3(speedX, speedY, z))
def getSpeedH(self):
return self.getSpeedXY().length()
def getSpeedZ(self):
return self.getSpeedVec()[2]
def setSpeedZ(self, zSpeed):
vec = self.getSpeedVec()
speedVec = Vec3(vec[0], vec[1], zSpeed)
return self.setSpeedVec(speedVec)
def setLinearVelocity(self, speedVec):
return self.setSpeedVec(speedVec)
def setAngularVelocity(self, speed):
self.node.setAngularVelocity(Vec3(0, 0, speed))
def getFriction(self):
return self.node.getFriction()
def setFriction(self, friction):
return self.node.setFriction(friction)
#-------------------------------------------------------------------
# Acceleration
def doJump(self):
#self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed)
self.setSpeedZ(self.jumpSpeed)
def setForce(self, force):
self.node.applyCentralForce(force)
#-------------------------------------------------------------------
# contacts
#-------------------------------------------------------------------
# update
def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0):
#self.setR(0)
#self.setP(0)
self.jumping = jumping
self.crouching = crouching
self.dx = dx
self.dy = dy
#self.setAngularVelocity(dRot*self.turnSpeed)
#self.setAngularVelocity(0)
self.setH(self.game.camHandler.gameNp.getH())
speedVec = self.getSpeedVec() - self.platformSpeedVec
speed = speedVec.length()
localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec)
pushVec = self.game.render.getRelativeVector(self.np, Vec3(self.dx,self.dy,0))
if self.dx != 0 or self.dy != 0:
pushVec.normalize()
else:
pushVec = Vec3(0,0,0)
contacts = self.getContacts()
contact = self.checkFeet()
if self.jumping and contact in contacts:
self.setFriction(0)
self.doJump()
if self.jumping:
self.setForce(pushVec * self.airAccel)
if speed > self.airSpeed:
self.setSpeed(self.airSpeed)
else:
if contacts:
self.setForce(pushVec * self.groundAccel)
else:
self.setForce(pushVec * self.airAccel)
if self.dx == 0 and self.dy == 0:
self.setFriction(100)
else:
self.setFriction(0)
if speed > self.groundSpeed:
if contacts:
self.setSpeed(self.groundSpeed)
'''
class CharacterController(DynamicObject, FSM):
def __init__(self, game):
self.game = game
FSM.__init__(self, "Player")
# key states
# dx direction left or right, dy direction forward or backward
self.kh = self.game.kh
self.dx = 0
self.dy = 0
self.jumping = 0
self.crouching = 0
# maximum speed when only walking
self.groundSpeed = 5.0
# acceleration used when walking to rise to self.groundSpeed
self.groundAccel = 100.0
# maximum speed in the air (air friction)
self.airSpeed = 30.0
# player movement control when in the air
self.airAccel = 10.0
# horizontal speed on jump start
self.jumpSpeed = 5.5
self.turnSpeed = 5
self.moveSpeedVec = Vec3(0, 0, 0)
self.platformSpeedVec = Vec3(0, 0, 0)
h = 1.75
w = 0.4
self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp)
self.node = BulletRigidBodyNode('Player')
self.node.setMass(2)
self.node.addShape(self.shape)
self.node.setActive(True, True)
self.node.setDeactivationEnabled(False, True)
self.node.setFriction(200)
#self.node.setGravity(10)
#self.node.setFallSpeed(200)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.node.setAngularFactor(Vec3(0, 0, 1))
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(0, 0, 0)
self.np.setH(0)
#self.np.setCollideMask(BitMask32.allOn())
self.game.world.attachRigidBody(self.node)
self.playerModel = None
self.slice_able = False
def setActor(self,
modelPath,
animDic={},
flip=False,
pos=(0, 0, 0),
scale=1.0):
self.playerModel = Actor(modelPath, animDic)
self.playerModel.reparentTo(self.np)
self.playerModel.setScale(scale) # 1ft = 0.3048m
if flip:
self.playerModel.setH(180)
self.playerModel.setPos(pos)
self.playerModel.setScale(scale)
#-------------------------------------------------------------------
# Speed
def getSpeedVec(self):
return self.node.getLinearVelocity()
def setSpeedVec(self, speedVec):
#print "setting speed to %s" % (speedVec)
self.node.setLinearVelocity(speedVec)
return speedVec
def setPlatformSpeed(self, speedVec):
self.platformSpeedVec = speedVec
def getSpeed(self):
return self.getSpeedVec().length()
def setSpeed(self, speed):
speedVec = self.getSpeedVec()
speedVec.normalize()
self.setSpeedVec(speedVec * speed)
def getLocalSpeedVec(self):
return self.np.getRelativeVector(self.getSpeed())
def getSpeedXY(self):
vec = self.getSpeedVec()
return Vec3(vec[0], vec[1], 0)
def setSpeedXY(self, speedX, speedY):
vec = self.getSpeedVec()
z = self.getSpeedZ()
self.setSpeedVec(Vec3(speedX, speedY, z))
def getSpeedH(self):
return self.getSpeedXY().length()
def getSpeedZ(self):
return self.getSpeedVec()[2]
def setSpeedZ(self, zSpeed):
vec = self.getSpeedVec()
speedVec = Vec3(vec[0], vec[1], zSpeed)
return self.setSpeedVec(speedVec)
def setLinearVelocity(self, speedVec):
return self.setSpeedVec(speedVec)
def setAngularVelocity(self, speed):
self.node.setAngularVelocity(Vec3(0, 0, speed))
def getFriction(self):
return self.node.getFriction()
def setFriction(self, friction):
return self.node.setFriction(friction)
#-------------------------------------------------------------------
# Acceleration
def doJump(self):
#self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed)
self.setSpeedZ(self.jumpSpeed)
def setForce(self, force):
self.node.applyCentralForce(force)
#-------------------------------------------------------------------
# contacts
#-------------------------------------------------------------------
# update
def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0):
#self.setR(0)
#self.setP(0)
self.jumping = jumping
self.crouching = crouching
self.dx = dx
self.dy = dy
#self.setAngularVelocity(dRot*self.turnSpeed)
#self.setAngularVelocity(0)
self.setH(self.game.camHandler.gameNp.getH())
speedVec = self.getSpeedVec() - self.platformSpeedVec
speed = speedVec.length()
localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec)
pushVec = self.game.render.getRelativeVector(self.np,
Vec3(self.dx, self.dy, 0))
if self.dx != 0 or self.dy != 0:
pushVec.normalize()
else:
pushVec = Vec3(0, 0, 0)
contacts = self.getContacts()
contact = self.checkFeet()
if self.jumping and contact in contacts:
self.setFriction(0)
self.doJump()
if self.jumping:
self.setForce(pushVec * self.airAccel)
if speed > self.airSpeed:
self.setSpeed(self.airSpeed)
else:
if contacts:
self.setForce(pushVec * self.groundAccel)
else:
self.setForce(pushVec * self.airAccel)
if self.dx == 0 and self.dy == 0:
self.setFriction(100)
else:
self.setFriction(0)
if speed > self.groundSpeed:
if contacts:
self.setSpeed(self.groundSpeed)
'''
class Window3D:
def __init__(self, window_id, rect):
print("Creating window ID={}".format(window_id))
self.rect = rect
self.window_id = window_id
self.box = WindowBox(globals.front_texture, globals.back_texture)
self.box.update_vertices(rect, globals.desk_rect)
x0 = int(rect[0] * 1.1) - 2000 + int(0.7 * rect[2])
y0 = 1980
z0 = int(rect[1] * 1.1) - 500
self.default_position = Point3(x0, y0, z0)
self.box.node.setTag('wid', str(window_id))
size = [0.5 * (a - b) for a, b in zip(self.box.mx, self.box.mn)]
self.shape = BulletBoxShape(Vec3(size[0], size[1], size[2] + 10))
self.phy_node = BulletRigidBodyNode('window')
volume = size[0] * size[1] * size[2]
self.phy_node.setMass(0.001 * volume)
self.phy_node.addShape(self.shape)
self.node = globals.gui.render.attachNewNode(self.phy_node)
self.phy_node.setLinearDamping(0.6)
self.phy_node.setAngularDamping(0.6)
globals.gui.world.attachRigidBody(self.phy_node)
self.node.setCollideMask(BitMask32.bit(1))
self.box.node.reparentTo(self.node)
self.box.node.setPos(0, 0, size[2] - 10)
self.node.setPos(x0, y0, z0)
self.set_physics(False)
def reset_position(self):
self.node.setPos(self.default_position)
self.node.setHpr(0, 0, 0)
self.set_physics(False)
def get_hook_pos(self):
return self.node.getPos() + self.box.node.getPos()
def update_rectangle(self, rect):
self.rect = rect
self.box.update_vertices(rect, globals.desk_rect)
def set_physics(self, state):
if state:
self.phy_node.setMass(1.0)
else:
self.phy_node.setMass(0.0)
self.phy_node.setLinearVelocity(LVector3(0, 0, 0))
self.phy_node.setAngularVelocity(LVector3(0, 0, 0))
def rotate(self, dx, dy):
dx = norm_rot(dx)
dy = norm_rot(dy)
v = self.box.node.getHpr()
v = v + LVecBase3(dx, dy, 0)
self.box.node.setHpr(v)
def move(self, delta):
v = self.node.getPos()
for i in range(len(delta)):
v[i] = v[i] + delta[i]
self.node.setPos(v)
# self.phy_node.setAngularVelocity(LVector3(0,0,0))
def close(self):
self.box.close()
def calc_desktop_coords(self, v):
if v[2] > 0:
return None
x = int(self.rect[0] + (0.5 * self.rect[2] + v[0]))
y = int(self.rect[1] - v[2])
return x, y
class Player(Entity):
walkspeed = 5
damping = 0.9
topspeed = 15
leftMove = False
rightMove = False
jumpToggle = False
crouchToggle = False
def __init__(self, world):
super(Player, self).__init__()
self.obj = utilities.loadObject("tdplayer", depth=20)
self.world = world
self.health = 100
self.inventory = list()
self.depth = self.obj.getPos().y
self.location = Point2(10,0)
self.velocity = Vec3(0)
self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49))
self.bnode = BulletRigidBodyNode('Box')
self.bnode.setMass(0.1)
self.bnode.setAngularVelocity(Vec3(0))
self.bnode.setAngularFactor(Vec3(0))
self.bnode.addShape(self.shape)
self.bnode.setLinearDamping(0.95)
self.bnode.setLinearSleepThreshold(0)
world.bw.attachRigidBody(self.bnode)
self.bnode.setPythonTag("entity", self)
self.bnode.setIntoCollideMask(BitMask32.bit(0))
self.node = utilities.app.render.attachNewNode(self.bnode)
self.node.setPos(self.obj.getPos())
self.obj.setPos(0,0,0)
self.obj.setScale(1)
self.obj.reparentTo(self.node)
self.node.setPos(self.location.x, self.depth, self.location.y)
def initialise(self):
self.inventory.append(LightLaser(self.world, self))
self.inventory.append(Blowtorch(self.world, self))
#self.inventory["Grenade"] = Grenade(self.world, self)
for item in self.inventory:
item.initialise()
self.currentItemIndex = 1
self.currentItem = self.inventory[self.currentItemIndex]
self.currentItem.equip()
def activate(self):
self.currentItem.activate()
def update(self, timer):
self.velocity = self.bnode.getLinearVelocity()
if (self.leftMove):
self.bnode.applyCentralForce(Vec3(-Player.walkspeed,0,0))
if (self.rightMove):
self.bnode.applyCentralForce(Vec3(Player.walkspeed,0,0))
if (self.jumpToggle):
self.bnode.applyCentralForce(Vec3(0,0,Player.walkspeed))
if (self.crouchToggle):
self.bnode.applyCentralForce(Vec3(0,0,-Player.walkspeed))
if (self.velocity.x < -self.topspeed):
self.velocity.x = -self.topspeed
if (self.velocity.x > self.topspeed):
self.velocity.x = self.topspeed
mouse = utilities.app.mousePos
# extrude test
near = Point3()
far = Point3()
utilities.app.camLens.extrude(mouse, near, far)
camp = utilities.app.camera.getPos()
near *= 20 # player depth
if near.x != 0:
angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x)
#angle = atan2(near.z, near.x)
else:
angle = 90
self.angle = angle
# set current item to point at cursor
self.currentItem.update(timer)
# move the camera so the player is centred horizontally,
# but keep the camera steady vertically
utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z)
self.obj.setHpr(0, 0, -1 * degrees(angle))
self.location.x = self.node.getPos().x
self.location.y = self.node.getPos().z
def moveLeft(self, switch):
self.leftMove = switch
def moveRight(self, switch):
self.rightMove = switch
def jump(self, switch):
self.jumpToggle = switch
def crouch(self, switch):
self.crouchToggle = switch
def scrollItem(self, number):
self.currentItem.unequip()
self.currentItemIndex = self.currentItemIndex + number
if self.currentItemIndex < 0:
self.currentItemIndex = len(self.inventory) - 1
if self.currentItemIndex > len(self.inventory) - 1:
self.currentItemIndex = 0
self.currentItem = self.inventory[self.currentItemIndex]
self.currentItem.equip()
def selectItem(self, number):
if (number - 1 < len(self.inventory) and number - 1 >= 0):
self.currentItem.unequip()
self.currentItemIndex = number - 1
self.currentItem = self.inventory[self.currentItemIndex]
self.currentItem.equip()
def hitby(self, projectile, index):
self.health -= projectile.damage
if (self.health < 0):
self.obj.setColor(1,0,0,1)
return True
greyscale = 0.3 + (0.01 * self.health)
self.obj.setColor(greyscale, greyscale,greyscale,greyscale)
return False
class Catcher(Enemy):
damage = 25
def __init__(self, location, player, cmap, world):
super(Catcher, self).__init__(location)
self.player = player
self.cmap = cmap
self.obj = utilities.loadObject("robot", depth=20)
self.world = world
self.health = 100
self.depth = self.obj.getPos().y
self.location = location
self.velocity = Vec3(0)
self.shape = BulletBoxShape(Vec3(0.5, 1.0, 0.5))
self.bnode = BulletRigidBodyNode('Box')
self.bnode.setMass(0.1)
self.bnode.setAngularVelocity(Vec3(0))
self.bnode.setAngularFactor(Vec3(0))
self.bnode.addShape(self.shape)
self.bnode.setLinearDamping(0.75)
self.bnode.setLinearSleepThreshold(0)
world.bw.attachRigidBody(self.bnode)
self.bnode.setPythonTag("entity", self)
self.bnode.setIntoCollideMask(BitMask32.bit(0))
self.node = utilities.app.render.attachNewNode(self.bnode)
self.node.setPos(self.obj.getPos())
self.obj.setPos(0,0,0)
self.obj.setScale(1)
self.obj.reparentTo(self.node)
self.node.setPos(self.location.x, self.depth, self.location.y)
def update(self, time):
self.location = Point2(self.node.getPos().x - self.player.location.x, self.node.getPos().z - self.player.location.y)
if self.location.x > 20 or self.location.x < -20:
return
if self.location.y > 20 or self.location.y < -20:
return
path = findPath(Point2(self.location + Point2(20,20)), Point2(20,20), self.world.cmap)
if len(path) > 1:
move = path[1] - self.location
self.bnode.applyCentralForce(Vec3(move.x-20,0,move.y-20))
def hitby(self, projectile, index):
self.health -= projectile.damage
if (self.health < 0):
self.remove = True
greyscale = 0.3 + (0.01 * self.health)
self.obj.setColor(1, greyscale,greyscale,1)
return False
def removeOnHit(self):
return
def destroy(self):
self.obj.hide()
self.world.bw.removeRigidBody(self.bnode)
class Catcher(Enemy):
damage = 25
def __init__(self, location, player, cmap, world):
super(Catcher, self).__init__(location)
self.player = player
self.cmap = cmap
self.obj = utilities.loadObject("robot", depth=20)
self.world = world
self.health = 100
self.depth = self.obj.getPos().y
self.location = location
self.velocity = Vec3(0)
self.shape = BulletBoxShape(Vec3(0.5, 1.0, 0.5))
self.bnode = BulletRigidBodyNode('Box')
self.bnode.setMass(0.1)
self.bnode.setAngularVelocity(Vec3(0))
self.bnode.setAngularFactor(Vec3(0))
self.bnode.addShape(self.shape)
self.bnode.setLinearDamping(0.75)
self.bnode.setLinearSleepThreshold(0)
world.bw.attachRigidBody(self.bnode)
self.bnode.setPythonTag("entity", self)
self.bnode.setIntoCollideMask(BitMask32.bit(0))
self.node = utilities.app.render.attachNewNode(self.bnode)
self.node.setPos(self.obj.getPos())
self.obj.setPos(0, 0, 0)
self.obj.setScale(1)
self.obj.reparentTo(self.node)
self.node.setPos(self.location.x, self.depth, self.location.y)
def update(self, time):
self.location = Point2(self.node.getPos().x - self.player.location.x,
self.node.getPos().z - self.player.location.y)
if self.location.x > 20 or self.location.x < -20:
return
if self.location.y > 20 or self.location.y < -20:
return
path = findPath(Point2(self.location + Point2(20, 20)), Point2(20, 20),
self.world.cmap)
if len(path) > 1:
move = path[1] - self.location
self.bnode.applyCentralForce(Vec3(move.x - 20, 0, move.y - 20))
def hitby(self, projectile, index):
self.health -= projectile.damage
if (self.health < 0):
self.remove = True
greyscale = 0.3 + (0.01 * self.health)
self.obj.setColor(1, greyscale, greyscale, 1)
return False
def removeOnHit(self):
return
def destroy(self):
self.obj.hide()
self.world.bw.removeRigidBody(self.bnode)
class Player(Entity):
walkspeed = 5
damping = 0.9
topspeed = 15
leftMove = False
rightMove = False
jumpToggle = False
crouchToggle = False
def __init__(self, world):
super(Player, self).__init__()
self.obj = utilities.loadObject("tdplayer", depth=20)
self.world = world
self.health = 100
self.inventory = list()
self.depth = self.obj.getPos().y
self.location = Point2(10, 0)
self.velocity = Vec3(0)
self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49))
self.bnode = BulletRigidBodyNode('Box')
self.bnode.setMass(0.1)
self.bnode.setAngularVelocity(Vec3(0))
self.bnode.setAngularFactor(Vec3(0))
self.bnode.addShape(self.shape)
self.bnode.setLinearDamping(0.95)
self.bnode.setLinearSleepThreshold(0)
world.bw.attachRigidBody(self.bnode)
self.bnode.setPythonTag("entity", self)
self.bnode.setIntoCollideMask(BitMask32.bit(0))
self.node = utilities.app.render.attachNewNode(self.bnode)
self.node.setPos(self.obj.getPos())
self.obj.setPos(0, 0, 0)
self.obj.setScale(1)
self.obj.reparentTo(self.node)
self.node.setPos(self.location.x, self.depth, self.location.y)
def initialise(self):
self.inventory.append(LightLaser(self.world, self))
self.inventory.append(Blowtorch(self.world, self))
#self.inventory["Grenade"] = Grenade(self.world, self)
for item in self.inventory:
item.initialise()
self.currentItemIndex = 1
self.currentItem = self.inventory[self.currentItemIndex]
self.currentItem.equip()
def activate(self):
self.currentItem.activate()
def update(self, timer):
self.velocity = self.bnode.getLinearVelocity()
if (self.leftMove):
self.bnode.applyCentralForce(Vec3(-Player.walkspeed, 0, 0))
if (self.rightMove):
self.bnode.applyCentralForce(Vec3(Player.walkspeed, 0, 0))
if (self.jumpToggle):
self.bnode.applyCentralForce(Vec3(0, 0, Player.walkspeed))
if (self.crouchToggle):
self.bnode.applyCentralForce(Vec3(0, 0, -Player.walkspeed))
if (self.velocity.x < -self.topspeed):
self.velocity.x = -self.topspeed
if (self.velocity.x > self.topspeed):
self.velocity.x = self.topspeed
mouse = utilities.app.mousePos
# extrude test
near = Point3()
far = Point3()
utilities.app.camLens.extrude(mouse, near, far)
camp = utilities.app.camera.getPos()
near *= 20 # player depth
if near.x != 0:
angle = atan2(near.z + camp.z - self.node.getPos().z,
near.x + camp.x - self.node.getPos().x)
#angle = atan2(near.z, near.x)
else:
angle = 90
self.angle = angle
# set current item to point at cursor
self.currentItem.update(timer)
# move the camera so the player is centred horizontally,
# but keep the camera steady vertically
utilities.app.camera.setPos(self.node.getPos().x, 0,
self.node.getPos().z)
self.obj.setHpr(0, 0, -1 * degrees(angle))
self.location.x = self.node.getPos().x
self.location.y = self.node.getPos().z
def moveLeft(self, switch):
self.leftMove = switch
def moveRight(self, switch):
self.rightMove = switch
def jump(self, switch):
self.jumpToggle = switch
def crouch(self, switch):
self.crouchToggle = switch
def scrollItem(self, number):
self.currentItem.unequip()
self.currentItemIndex = self.currentItemIndex + number
if self.currentItemIndex < 0:
self.currentItemIndex = len(self.inventory) - 1
if self.currentItemIndex > len(self.inventory) - 1:
self.currentItemIndex = 0
self.currentItem = self.inventory[self.currentItemIndex]
self.currentItem.equip()
def selectItem(self, number):
if (number - 1 < len(self.inventory) and number - 1 >= 0):
self.currentItem.unequip()
self.currentItemIndex = number - 1
self.currentItem = self.inventory[self.currentItemIndex]
self.currentItem.equip()
def hitby(self, projectile, index):
self.health -= projectile.damage
if (self.health < 0):
self.obj.setColor(1, 0, 0, 1)
return True
greyscale = 0.3 + (0.01 * self.health)
self.obj.setColor(greyscale, greyscale, greyscale, greyscale)
return False
