class DynamicModel(DynamicObject):
def __init__(self, name, modelPath, game, pos):
self.name = name
self.modelPath = modelPath
self.game = game
self.model = self.game.loader.loadModel(self.modelPath)
geomNodes = self.model.findAllMatches('**/+GeomNode')
self.geomNode = geomNodes.getPath(0).node()
self.geom = self.geomNode.getGeom(0)
self.shape = BulletConvexHullShape()
self.shape.addGeom(self.geom)
self.node = BulletRigidBodyNode(self.name)
self.node.setMass(10.0)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.slice_able = True
class DynamicPlatform(DynamicObject):
def __init__(self, name, game, x, y, z, pos):
self.name = name
self.game = game
self.shape = BulletBoxShape(Vec3(x, y, z))
self.node = BulletRigidBodyNode(self.name)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
#self.model = self.game.loader.loadModel("models/crate.egg")
#self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
#self.node.setFriction(5)
#self.debugOff()
self.speedVec = Vec3(0, 0, 0)
self.lastPos = Vec3(self.getPos())
self.slice_able = False
#self.node.setCcdMotionThreshold(1e-7)
#self.node.setCcdSweptSphereRadius(0.5)
def update(self, dt):
return
class DynamicNp(DynamicObject): def __init__(self, name, model, game, pos): self.name = name self.game = game self.model = model self.geomNode = self.model.node() self.geom = self.geomNode.getGeom(0) # with triangle mesh it crashes #mesh = BulletTriangleMesh() #mesh.addGeom(self.geom) #self.shape = BulletTriangleMeshShape(mesh, dynamic=True) # with convex hull self.shape = BulletConvexHullShape() self.shape.addGeom(self.geom) self.node = BulletRigidBodyNode(self.name) self.node.setMass(10.0) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.slice_able = True
def __init__(self):
PhysicsNodePath.__init__(self, 'can')
self.shapeGroup = BitMask32.allOn()
sph = BulletBoxShape(Vec3(2, 2, 2))
rbnode = BulletRigidBodyNode('can_body')
rbnode.setMass(100.0)
rbnode.addShape(sph, TransformState.makePos(Point3(0, 0, 1)))
rbnode.setCcdMotionThreshold(1e-7)
rbnode.setCcdSweptSphereRadius(0.5)
self.mdl = loader.loadModel("phase_5/models/props/safe-mod.bam")
self.mdl.setScale(6)
self.mdl.setZ(-1)
self.mdl.reparentTo(self)
self.mdl.showThrough(CIGlobals.ShadowCameraBitmask)
self.setupPhysics(rbnode)
#self.makeShadow(0.2)
self.reparentTo(render)
self.setPos(base.localAvatar.getPos(render) + (0, 0, 20))
self.setQuat(base.localAvatar.getQuat(render))
def __init__(self):
PhysicsNodePath.__init__(self, 'can')
self.shapeGroup = BitMask32.allOn()
sph = BulletCylinderShape(0.5, 1.2, ZUp)
rbnode = BulletRigidBodyNode('can_body')
rbnode.setMass(10.0)
rbnode.addShape(sph)
rbnode.setCcdMotionThreshold(1e-7)
rbnode.setCcdSweptSphereRadius(0.5)
self.mdl = loader.loadModel("phase_5/models/props/can.bam")
self.mdl.setScale(11.0)
self.mdl.setZ(-0.55)
self.mdl.reparentTo(self)
self.mdl.showThrough(CIGlobals.ShadowCameraBitmask)
self.setupPhysics(rbnode)
#self.makeShadow(0.2)
self.reparentTo(render)
self.setPos(base.localAvatar.getPos(render))
self.setQuat(base.localAvatar.getQuat(render))
dir = self.getQuat(render).xform(Vec3.forward())
amp = 10
self.node().setLinearVelocity(dir * amp)
class DynamicPlatform(DynamicObject):
def __init__(self, name, game, x, y, z, pos):
self.name = name
self.game = game
self.shape = BulletBoxShape(Vec3(x, y, z))
self.node = BulletRigidBodyNode(self.name)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
#self.model = self.game.loader.loadModel("models/crate.egg")
#self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
#self.node.setFriction(5)
#self.debugOff()
self.speedVec = Vec3(0,0,0)
self.lastPos = Vec3(self.getPos())
self.slice_able = False
#self.node.setCcdMotionThreshold(1e-7)
#self.node.setCcdSweptSphereRadius(0.5)
def update(self, dt):
return
class DynamicModel(DynamicObject):
def __init__(self, name, modelPath, game, pos):
self.name = name
self.modelPath = modelPath
self.game = game
self.model = self.game.loader.loadModel(self.modelPath)
geomNodes = self.model.findAllMatches('**/+GeomNode')
self.geomNode = geomNodes.getPath(0).node()
self.geom = self.geomNode.getGeom(0)
self.shape = BulletConvexHullShape()
self.shape.addGeom(self.geom)
self.node = BulletRigidBodyNode(self.name)
self.node.setMass(10.0)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.slice_able = True
class DynamicNp(DynamicObject):
def __init__(self, name, model, game, pos):
self.name = name
self.game = game
self.model = model
self.geomNode = self.model.node()
self.geom = self.geomNode.getGeom(0)
# with triangle mesh it crashes
#mesh = BulletTriangleMesh()
#mesh.addGeom(self.geom)
#self.shape = BulletTriangleMeshShape(mesh, dynamic=True)
# with convex hull
self.shape = BulletConvexHullShape()
self.shape.addGeom(self.geom)
self.node = BulletRigidBodyNode(self.name)
self.node.setMass(10.0)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.slice_able = True
def getPhysBody(self):
shape = BulletCylinderShape(0.3925, 1.4, ZUp)
body = BulletRigidBodyNode('tntBody')
body.addShape(shape)
body.setKinematic(True)
body.setCcdMotionThreshold(1e-7)
body.setCcdSweptSphereRadius(0.3925)
return body
def _initAgents(self):
# Load agents
for agent in self.scene.scene.findAllMatches('**/agents/agent*'):
transform = TransformState.makeIdentity()
if self.agentMode == 'capsule':
shape = BulletCapsuleShape(
self.agentRadius, self.agentHeight - 2 * self.agentRadius)
elif self.agentMode == 'sphere':
shape = BulletCapsuleShape(self.agentRadius,
2 * self.agentRadius)
# XXX: use BulletCharacterControllerNode class, which already handles local transform?
node = BulletRigidBodyNode('physics')
node.setMass(self.agentMass)
node.setStatic(False)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.addShape(shape)
self.bulletWorld.attach(node)
# Constrain the agent to have fixed position on the Z-axis
node.setLinearFactor(Vec3(1.0, 1.0, 0.0))
# Constrain the agent not to be affected by rotations
node.setAngularFactor(Vec3(0.0, 0.0, 0.0))
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
# Enable continuous collision detection (CCD)
node.setCcdMotionThreshold(1e-7)
node.setCcdSweptSphereRadius(0.50)
if node.isStatic():
agent.setTag('physics-mode', 'static')
else:
agent.setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = NodePath(node)
physicsNp.setTransform(transform)
# Reparent all child nodes below the new physic node
for child in agent.getChildren():
child.reparentTo(physicsNp)
physicsNp.reparentTo(agent)
# NOTE: we need this to update the transform state of the internal bullet node
physicsNp.node().setTransformDirty()
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(agent.getNetTransform().getMat()),
atol=1e-6)
def getPhysBody(self):
shape = BulletCylinderShape(0.3925, 1.4, ZUp)
body = BulletRigidBodyNode('tntBody')
body.addShape(shape)
body.setCcdMotionThreshold(1e-7)
body.setCcdSweptSphereRadius(0.3925)
body.setKinematic(False)
body.setMass(5.0)
body.setAngularDamping(0.3)
body.setLinearDamping(0.8)
return body
class DynamicBox(DynamicObject):
def __init__(self, name, game, pos):
self.name = name
self.game = game
self.shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
self.node = BulletRigidBodyNode(name)
self.node.setMass(10.0)
self.node.addShape(self.shape)
self.setFriction(20)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model = self.game.loader.loadModel("models/crate.egg")
self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.debugOff()
self.slice_able = True
def update(self, dt):
#self.activate()
#if self.node.isActive():
# return
contact = self.checkFeet()
contacts = self.getContacts()
if contact in self.game.objects and contact in contacts:
obj = self.game.objects[contact]
if type(obj) is KinematicPlatform:
self.activate()
self.setAngularVelocity(self.getAngularVelocity()*0.9)
self.setFriction(0.1)
#print "%s is a box sliding on %s" % (self.name, obj.name)
#self.addSpeedVec(self.game.objects[contact].getSpeedVec())
#self.setForce(obj.getSpeedVec()*self.node.getMass() * 100)
platform_speed = obj.getSpeedVec()
force = platform_speed * self.node.getMass() * 50
#self.setSpeedXY(platform_speed[0], platform_speed[1])
self.setSpeedVec(platform_speed)
#self.setAngularVelocity(0)
#self.node.addLin(obj.getSpeedVec()*self.node.getMass() * 100)
#self.node.applyCentralForce(force)
#self.setForce(force)
#self.setFriction(20)
#self.setPos(self.getPos() + obj.getSpeedVec())
#self.node.setActive(False)
else:
self.setFriction(20)
class DynamicBox(DynamicObject):
def __init__(self, name, game, pos):
self.name = name
self.game = game
self.shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
self.node = BulletRigidBodyNode(name)
self.node.setMass(10.0)
self.node.addShape(self.shape)
self.setFriction(20)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model = self.game.loader.loadModel("models/crate.egg")
self.model.reparentTo(self.np)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
self.debugOff()
self.slice_able = True
def update(self, dt):
#self.activate()
#if self.node.isActive():
# return
contact = self.checkFeet()
contacts = self.getContacts()
if contact in self.game.objects and contact in contacts:
obj = self.game.objects[contact]
if type(obj) is KinematicPlatform:
self.activate()
self.setAngularVelocity(self.getAngularVelocity() * 0.9)
self.setFriction(0.1)
#print "%s is a box sliding on %s" % (self.name, obj.name)
#self.addSpeedVec(self.game.objects[contact].getSpeedVec())
#self.setForce(obj.getSpeedVec()*self.node.getMass() * 100)
platform_speed = obj.getSpeedVec()
force = platform_speed * self.node.getMass() * 50
#self.setSpeedXY(platform_speed[0], platform_speed[1])
self.setSpeedVec(platform_speed)
#self.setAngularVelocity(0)
#self.node.addLin(obj.getSpeedVec()*self.node.getMass() * 100)
#self.node.applyCentralForce(force)
#self.setForce(force)
#self.setFriction(20)
#self.setPos(self.getPos() + obj.getSpeedVec())
#self.node.setActive(False)
else:
self.setFriction(20)
def __emitShell(self):
def __shellThink(shell, task):
if task.time > 3.0:
base.physicsWorld.remove(shell.node())
shell.removeNode()
return task.done
if not hasattr(task, 'didHitNoise'):
task.didHitNoise = False
if not task.didHitNoise:
contact = base.physicsWorld.contactTest(shell.node())
if contact.getNumContacts() > 0:
task.didHitNoise = True
hitNoise = base.loadSfxOnNode(
self.ShellContactSoundPath.format(
random.randint(*self.ShellContactSoundRange)),
shell)
hitNoise.play()
return task.cont
from panda3d.bullet import BulletCylinderShape, BulletRigidBodyNode, ZUp
scale = 0.75
shape = BulletCylinderShape(0.07 * scale, 0.47 * scale, ZUp)
rbnode = BulletRigidBodyNode('shellrbnode')
rbnode.setMass(1.0)
rbnode.addShape(shape)
rbnode.setCcdMotionThreshold(1e-7)
rbnode.setCcdSweptSphereRadius(0.1)
rbnp = render.attachNewNode(rbnode)
mdl = loader.loadModel(self.ShellPath)
mdl.reparentTo(rbnp)
mdl.setScale(0.3 * scale, 0.7 * scale, 0.3 * scale)
mdl.setP(90)
mdl.setTransparency(True, 1)
rbnp.setPos(camera, (1, 2, -0.5))
rbnp.setHpr(camera, (0, -90, 0))
localEjectDir = Vec3(1, 0, 0.3)
rbnode.applyCentralImpulse(
camera.getQuat(render).xform(localEjectDir) * 7)
base.physicsWorld.attach(rbnode)
taskMgr.add(__shellThink,
'shellThink',
extraArgs=[rbnp],
appendTask=True)
def getPhysBody(self):
bsph = BulletSphereShape(0.6)
bcy = BulletCylinderShape(0.25, 0.35, ZUp)
body = BulletRigidBodyNode('tntBody')
body.addShape(
bsph,
TransformState.makePosHpr((0.05, 0, 0.43),
(86.597, 24.5539, 98.1435)))
body.addShape(
bcy,
TransformState.makePosHpr((0.05, 0.655, 0.35),
(86.597, 24.5539, 98.1435)))
body.setKinematic(True)
body.setCcdMotionThreshold(1e-7)
body.setCcdSweptSphereRadius(0.6)
return body
class KinematicPlatform(BulletObject):
def __init__(self, name, game, x, y, z, pos):
self.name = name
self.game = game
self.shape = BulletBoxShape(Vec3(x, y, z))
self.node = BulletRigidBodyNode(self.name)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model = self.game.loader.loadModel("models/platform.egg")
self.model.reparentTo(self.np)
self.model.setScale(x*2,y*2,z*2)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
#self.node.setFriction(5)
#self.debugOff()
self.speedVec = Vec3(0,0,0)
self.lastPos = Vec3(self.getPos())
self.slice_able = False
def getSpeedVec(self):
return self.speedVec
def update(self, dt):
currentPos = Vec3(self.getPos())
currentPos.setZ(0)
dVec = currentPos - self.lastPos
self.speedVec = dVec * (1.0 / dt)
#print "speedVec = %s, currentPos = %s, lastPos = %s, dVec = %s" % (self.speedVec, currentPos, self.lastPos, dVec)
self.lastPos = currentPos
self.lastPos.setZ(0)
class KinematicPlatform(BulletObject):
def __init__(self, name, game, x, y, z, pos):
self.name = name
self.game = game
self.shape = BulletBoxShape(Vec3(x, y, z))
self.node = BulletRigidBodyNode(self.name)
self.node.addShape(self.shape)
self.np = self.game.render.attachNewNode(self.node)
self.np.setPos(pos)
self.game.world.attachRigidBody(self.node)
self.model = self.game.loader.loadModel("models/platform.egg")
self.model.reparentTo(self.np)
self.model.setScale(x * 2, y * 2, z * 2)
self.node.setCcdMotionThreshold(1e-7)
self.node.setCcdSweptSphereRadius(0.5)
#self.node.setFriction(5)
#self.debugOff()
self.speedVec = Vec3(0, 0, 0)
self.lastPos = Vec3(self.getPos())
self.slice_able = False
def getSpeedVec(self):
return self.speedVec
def update(self, dt):
currentPos = Vec3(self.getPos())
currentPos.setZ(0)
dVec = currentPos - self.lastPos
self.speedVec = dVec * (1.0 / dt)
#print "speedVec = %s, currentPos = %s, lastPos = %s, dVec = %s" % (self.speedVec, currentPos, self.lastPos, dVec)
self.lastPos = currentPos
self.lastPos.setZ(0)
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 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 Flame(Entity):
"""
The thing that comes out the end of the thing you hold
"""
animspeed = 0.1
depth = 20
playerWidth = 3
speed = 20
def __init__(self, world, pos, hpr):
super(Flame, self).__init__()
self.shape = BulletBoxShape(Vec3(0.1,0.05,0.05))
self.bnode = BulletRigidBodyNode()
self.bnode.setMass(1.0)
self.bnode.addShape(self.shape)
self.np = utilities.app.render.attachNewNode(self.bnode)
self.world =world
self.anim = list()
self.anim.append(utilities.loadObject("flame1", depth=0))
self.anim.append(utilities.loadObject("flame2", depth=0))
self.anim.append(utilities.loadObject("flame3", depth=0))
world.bw.attachRigidBody(self.bnode)
self.curspr = 0
self.obj = self.anim[self.curspr]
self.obj.show()
self.livetime = 0
self.delta = 0
self.pos = pos
self.pos.y = Flame.depth
#self.pos.z -= 0.2
self.hpr = hpr
self.vel = Point2()
self.vel.x = cos(world.player.angle)*Flame.speed
self.vel.y = sin(world.player.angle)*Flame.speed
tv = Vec3(self.vel.x, 0, self.vel.y)
# this makes the shot miss the target if the player has any velocity
tv += world.player.bnode.getLinearVelocity()
self.bnode.setLinearVelocity(tv)
tv.normalize()
# initial position of RB and draw plane
self.np.setHpr(hpr)
self.np.setPos(pos+tv/2)
self.bnode.setAngularFactor(Vec3(0,0,0))
self.bnode.setLinearFactor(Vec3(1,0,1))
self.bnode.setGravity(Vec3(0,0,0))
self.bnode.setCcdMotionThreshold(1e-7)
self.bnode.setCcdSweptSphereRadius(0.10)
self.bnode.notifyCollisions(True)
self.bnode.setIntoCollideMask(BitMask32.bit(1))
self.bnode.setPythonTag("Entity", self)
self.noCollideFrames = 4
for a in self.anim:
a.hide()
a.reparentTo(self.np)
a.setScale(0.25, 1, 0.25)
a.setPos(0, -0.1,0)
def update(self, timer):
#animation
self.delta += timer
self.livetime += timer
if self.noCollideFrames == 0:
self.bnode.setIntoCollideMask(BitMask32.allOn())
if self.noCollideFrames > -1:
self.noCollideFrames -= 1
if self.delta > Flame.animspeed:
self.delta = 0
self.obj.hide()
self.curspr += 1
if self.curspr > len(self.anim)-1:
self.curspr = 0
self.obj = self.anim[self.curspr]
self.obj.show()
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 Vehicle:
def __init__(self, app, model_name):
model_file_name = 'assets/cars/{}/{}.bam'.format(
model_name, model_name)
self.app = app
def animation_path(model, animation):
base_path = 'assets/cars/animations/{}/{}-{}.bam'
return base_path.format(model, model, animation)
self.model = Actor(
model_file_name,
{
# AIRBRAKE: 'assets/cars/animations/{}-{}.bam'.format(model_name, AIRBRAKE),
# AIRBRAKE: animation_path(model_name, AIRBRAKE),
# STABILIZER_FINS: animation_path(model_name, STABILIZER_FINS),
})
self.model.enableBlend()
self.model.setControlEffect(AIRBRAKE, 1)
self.model.setControlEffect(STABILIZER_FINS, 1)
# FIXME: This code fails due to a bug in Actor
# airbrake_joints = [joint.name
# for joint in self.model.getJoints()
# if joint.name.startswith(AIRBRAKE)
# ]
# self.model.makeSubpart(AIRBRAKE, airbrake_joints)
# stabilizer_joints = [joint.name
# for joint in self.model.getJoints()
# if joint.name.startswith(STABILIZER_FINS)
# ]
# self.model.makeSubpart(STABILIZER_FINS, stabilizer_joints)
puppet = self.app.loader.load_model(model_file_name)
puppet.find("**/armature").hide()
puppet.reparentTo(self.model)
# Get the vehicle data
self.vehicle_data = VehicleData(puppet, model_name, 'cars')
# Configure the physics node
self.physics_node = BulletRigidBodyNode('vehicle')
self.physics_node.set_friction(self.vehicle_data.friction)
self.physics_node.set_linear_sleep_threshold(0)
self.physics_node.set_angular_sleep_threshold(0)
self.physics_node.setCcdMotionThreshold(1e-7)
self.physics_node.setCcdSweptSphereRadius(0.5)
self.physics_node.setMass(self.vehicle_data.mass)
shape = BulletConvexHullShape()
for geom_node in self.model.find_all_matches("**/+GeomNode"):
for geom in geom_node.node().get_geoms():
vertices = GeomVertexReader(geom.get_vertex_data(), 'vertex')
while not vertices.is_at_end():
v_geom = vertices.getData3f()
v_model = self.model.get_relative_point(geom_node, v_geom)
shape.add_point(v_model)
self.physics_node.add_shape(shape)
self.vehicle = NodePath(self.physics_node)
self.vehicle.set_collide_mask(CM_VEHICLE | CM_COLLIDE)
self.model.reparent_to(self.vehicle)
# Navigational aids
self.target_node = self.app.loader.load_model('models/zup-axis')
self.target_node.reparent_to(self.model)
self.target_node.set_scale(1)
self.target_node.set_render_mode_wireframe()
self.target_node.hide()
self.delta_node = self.app.loader.load_model('models/smiley')
self.delta_node.set_pos(1, 10, 1)
self.delta_node.reparent_to(base.cam)
self.delta_node.hide()
self.airbrake_state = 0.0
self.airbrake_factor = 0.5
self.airbrake_speed = 1 / self.vehicle_data.airbrake_duration
self.stabilizer_fins_state = 0.0
self.stabilizer_fins_speed = 1 / self.vehicle_data.stabilizer_fins_duration
self.centroid = base.loader.load_model('models/smiley')
self.centroid.reparent_to(self.vehicle)
self.centroid.hide()
# Gyro sound
sound_file = 'assets/cars/{}/{}.wav'.format(
model_name,
GYROSCOPE_SOUND,
)
sound = base.audio3d.load_sfx(sound_file)
self.model.set_python_tag(GYROSCOPE_SOUND, sound)
base.audio3d.attach_sound_to_object(sound, self.model)
sound.set_volume(0)
sound.set_play_rate(0)
sound.set_loop(True)
sound.play()
# Thruster limiting
self.thruster_state = 0.0
self.thruster_heat = 0.0
for repulsor in self.vehicle_data.repulsor_nodes:
self.add_repulsor(repulsor, model_name)
for thruster in self.vehicle_data.thruster_nodes:
self.add_thruster(thruster, model_name)
# ECU data storage from frame to frame
self.last_flight_height = None
# FIXME: Move into a default controller
self.inputs = {
# Repulsors
REPULSOR_ACTIVATION: 0.0,
ACCELERATE: 0.0,
TURN: 0.0,
STRAFE: 0.0,
HOVER: 0.0,
FULL_REPULSORS: False,
# Gyro
ACTIVE_STABILIZATION_ON_GROUND: PASSIVE,
ACTIVE_STABILIZATION_CUTOFF_ANGLE: PASSIVE,
ACTIVE_STABILIZATION_IN_AIR: PASSIVE,
TARGET_ORIENTATION: Vec3(0, 0, 0),
# Thrust
THRUST: 0.0,
# Air foils
AIRBRAKE: 0.0,
STABILIZER_FINS: 0.0,
}
self.sensors = {}
self.commands = {}
def np(self):
return self.vehicle
def place(self, spawn_point):
# FIXME: Pass a root node to __init__ instead
self.vehicle.reparent_to(self.app.environment.model)
connector = self.model.find("**/" + SPAWN_POINT_CONNECTOR)
self.vehicle.set_hpr(-connector.get_hpr(spawn_point))
self.vehicle.set_pos(-connector.get_pos(spawn_point))
self.app.environment.add_physics_node(self.physics_node)
def set_inputs(self, inputs):
self.inputs = inputs
def add_repulsor(self, node, model_name):
ground_contact = self.app.loader.load_model(
"assets/effect/repulsorhit.egg")
ground_contact.set_scale(1)
ground_contact.reparent_to(self.app.render)
node.set_python_tag('ray_end', ground_contact)
sound_file = 'assets/cars/{}/{}.wav'.format(
model_name,
REPULSOR_SOUND,
)
sound = base.audio3d.load_sfx(sound_file)
node.set_python_tag(REPULSOR_SOUND, sound)
base.audio3d.attach_sound_to_object(sound, node)
sound.set_volume(0)
sound.set_play_rate(0)
sound.set_loop(True)
sound.play()
def add_thruster(self, node, model_name):
node.set_python_tag(THRUSTER_POWER, 0.0)
node.set_python_tag(THRUSTER_OVERHEATED, False)
# Basic jet sound
sound_file = 'assets/cars/{}/{}.wav'.format(
model_name,
THRUSTER_SOUND,
)
sound = base.audio3d.load_sfx(sound_file)
node.set_python_tag(THRUSTER_SOUND, sound)
base.audio3d.attach_sound_to_object(sound, node)
sound.set_volume(0)
sound.set_play_rate(0)
sound.set_loop(True)
sound.play()
# Overheating sound
sound_file = 'assets/cars/{}/{}.wav'.format(
model_name,
THRUSTER_OVERHEAT_SOUND,
)
sound = base.audio3d.load_sfx(sound_file)
node.set_python_tag(THRUSTER_OVERHEAT_SOUND, sound)
base.audio3d.attach_sound_to_object(sound, node)
def game_loop(self):
self.gather_sensors()
self.ecu()
self.apply_air_drag()
self.apply_repulsors()
self.apply_gyroscope()
self.apply_thrusters()
self.apply_airbrake()
self.apply_stabilizer_fins()
def gather_sensors(self):
# Gather data repulsor ray collisions with ground
repulsor_data = []
for node in self.vehicle_data.repulsor_nodes:
data = RepulsorData()
repulsor_data.append(data)
max_distance = node.get_python_tag(ACTIVATION_DISTANCE)
data.position = node.get_pos(self.vehicle)
data.direction = self.app.render.get_relative_vector(
node,
Vec3(0, 0, -max_distance),
)
# FIXME: `self.app.environment.physics_world` is ugly.
feeler = self.app.environment.physics_world.ray_test_closest(
base.render.get_relative_point(self.vehicle, data.position),
base.render.get_relative_point(self.vehicle,
data.position + data.direction),
CM_TERRAIN,
)
if feeler.has_hit():
data.active = True
data.fraction = feeler.get_hit_fraction()
data.contact = self.vehicle.get_relative_point(
self.app.render,
feeler.get_hit_pos(),
)
else:
data.active = False
data.fraction = 1.0
# Find the local ground's normal vector
local_up = False
contacts = [
data.contact for data in repulsor_data
if data.active and self.inputs[REPULSOR_ACTIVATION]
]
if len(contacts) > 2:
local_up = True
target_mode = self.inputs[ACTIVE_STABILIZATION_ON_GROUND]
# We can calculate a local up as the smallest base vector of the
# point cloud of contacts.
centroid = reduce(lambda a, b: a + b, contacts) / len(contacts)
covariance = [
[c.x, c.y, c.z]
for c in [contact - centroid for contact in contacts]
][0:3] # We need exactly 3, no PCA yet. :(
eigenvalues, eigenvectors = numpy.linalg.eig(
numpy.array(covariance))
# FIXME: These few lines look baaaad...
indexed_eigenvalues = enumerate(eigenvalues)
def get_magnitude(indexed_element):
index, value = indexed_element
return abs(value)
sorted_indexed_eigenvalues = sorted(
indexed_eigenvalues,
key=get_magnitude,
)
# The smallest eigenvalue leads to the ground plane's normal
up_vec_idx, _ = sorted_indexed_eigenvalues[0]
up_vec = VBase3(*eigenvectors[:, up_vec_idx])
# Point into the upper half-space
if up_vec.z < 0:
up_vec *= -1
# Calculate the forward of the centroid
centroid_forward = Vec3(0, 1,
0) - up_vec * (Vec3(0, 1, 0).dot(up_vec))
# FIXME: Huh?
forward_planar = centroid_forward - up_vec * (
centroid_forward.dot(up_vec))
# Now let's orient and place the centroid
self.centroid.set_pos(self.vehicle, (0, 0, 0))
self.centroid.heads_up(forward_planar, up_vec)
self.centroid.set_pos(self.vehicle, centroid)
# Flight height for repulsor attenuation
flight_height = -self.centroid.get_z(self.vehicle)
if self.last_flight_height is not None:
climb_speed = (flight_height -
self.last_flight_height) / globalClock.dt
else:
climb_speed = 0
self.last_flight_height = flight_height
else:
local_up = False
self.last_flight_height = None
flight_height = 0.0
climb_speed = 0.0
# Air drag
movement = self.physics_node.get_linear_velocity()
drag_coefficient = self.vehicle_data.drag_coefficient
drag_area = self.vehicle_data.drag_area + \
self.vehicle_data.airbrake_area * self.airbrake_state + \
self.vehicle_data.stabilizer_fins_area * self.stabilizer_fins_state
air_density = self.app.environment.env_data.air_density
air_speed = -self.vehicle.get_relative_vector(
base.render,
movement,
)
# drag_force = 1/2*drag_coefficient*mass_density*area*flow_speed**2
result = Vec3(air_speed)
result = (result**3) / result.length()
result.componentwise_mult(drag_area)
result.componentwise_mult(self.vehicle_data.drag_coefficient)
drag_force = result * 1 / 2 * air_density
self.sensors = {
CURRENT_ORIENTATION: self.vehicle.get_hpr(self.app.render),
CURRENT_MOVEMENT: movement,
CURRENT_ROTATION: self.physics_node.get_angular_velocity(),
LOCAL_DRAG_FORCE: drag_force,
REPULSOR_DATA: repulsor_data,
LOCAL_UP: local_up,
FLIGHT_HEIGHT: flight_height,
CLIMB_SPEED: climb_speed,
}
def ecu(self):
repulsor_target_angles = self.ecu_repulsor_reorientation()
repulsor_activation, delta_height, projected_delta_height, \
power_frac_needed = self.ecu_repulsor_activation()
gyro_rotation = self.ecu_gyro_stabilization()
thruster_activation = [
self.inputs[THRUST] for _ in self.vehicle_data.thruster_nodes
]
airbrake = self.inputs[AIRBRAKE]
stabilizer_fins = self.inputs[STABILIZER_FINS]
self.commands = {
# Steering commands
REPULSOR_TARGET_ORIENTATIONS: repulsor_target_angles,
REPULSOR_ACTIVATION: repulsor_activation,
GYRO_ROTATION: gyro_rotation,
THRUSTER_ACTIVATION: thruster_activation,
AIRBRAKE: airbrake,
STABILIZER_FINS: stabilizer_fins,
# ECU data output; Interesting numbers we found along the way.
HEIGHT_OVER_TARGET: delta_height,
HEIGHT_OVER_TARGET_PROJECTED: projected_delta_height,
REPULSOR_POWER_FRACTION_NEEDED: power_frac_needed,
}
def ecu_repulsor_reorientation(self):
# Calculate effective repulsor motion blend values
accelerate = self.inputs[ACCELERATE]
turn = self.inputs[TURN]
strafe = self.inputs[STRAFE]
hover = self.inputs[HOVER]
length = sum([abs(accelerate), abs(turn), abs(strafe), hover])
if length > 1:
accelerate /= length
turn /= length
strafe /= length
hover /= length
# Split the turn signal into animation blend factors
if turn > 0.0:
turn_left = 0.0
turn_right = turn
else:
turn_left = -turn
turn_right = 0.0
# Blend the repulsor angle
repulsor_target_angles = []
for node in self.vehicle_data.repulsor_nodes:
acc_angle = -(node.get_python_tag(ACCELERATE)) * accelerate
turn_left_angle = node.get_python_tag(TURN_LEFT) * turn_left
turn_right_angle = node.get_python_tag(TURN_RIGHT) * turn_right
strafe_angle = node.get_python_tag(STRAFE) * strafe
hover_angle = node.get_python_tag(HOVER) * hover
angle = acc_angle + turn_left_angle + turn_right_angle + \
strafe_angle + hover_angle
repulsor_target_angles.append(angle)
return repulsor_target_angles
def ecu_repulsor_activation(self):
# Do we know how high we are flying?
if self.sensors[LOCAL_UP]:
tau = self.inputs[TARGET_FLIGHT_HEIGHT_TAU]
# What would we be at in tau seconds if we weren't using repulsors?
flight_height = self.sensors[FLIGHT_HEIGHT]
target_flight_height = self.inputs[TARGET_FLIGHT_HEIGHT]
delta_height = flight_height - target_flight_height
gravity_z = self.centroid.get_relative_vector(
self.app.render,
self.app.environment.physics_world.get_gravity(),
).get_z()
# Since gravity is an acceleration
gravity_h = 1 / 2 * gravity_z * tau**2
climb_rate = self.sensors[CLIMB_SPEED] * tau
projected_delta_height = delta_height + gravity_h + climb_rate
# Are we sinking?
if projected_delta_height <= 0:
# Our projected height will be under our target height, so we
# will need to apply the repulsors to make up the difference.
# How much climb can each repulsor provide at 100% power right
# now?
max_powers = [
node.get_python_tag(FORCE)
for node in self.vehicle_data.repulsor_nodes
]
transferrable_powers = [
max_power * cos(0.5 * pi * data.fraction)
for max_power, data in zip(
max_powers,
self.sensors[REPULSOR_DATA],
)
]
angle_ratios = [
cos(node.get_quat(self.vehicle).get_angle_rad())
for node in self.vehicle_data.repulsor_nodes
]
angled_powers = [
power * ratio
for power, ratio in zip(transferrable_powers, angle_ratios)
]
# We don't want to activate the repulsors unevenly, so we'll
# have to go by the weakest link.
total_angled_power = min(angled_powers) * len(angled_powers)
# How high can we climb under 100% repulsor power?
max_climb = 1 / 2 * total_angled_power * tau**2 / self.vehicle_data.mass
# The fraction of power needed to achieve the desired climb
power_frac_needed = -projected_delta_height / max_climb
# ...and store it.
repulsor_activation = [
power_frac_needed for _ in self.vehicle_data.repulsor_nodes
]
else:
# We're not sinking.
repulsor_activation = [
0.0 for _ in self.vehicle_data.repulsor_nodes
]
power_frac_needed = 0.0
else: # We do not have ground contact.
repulsor_activation = [
0.0 for _ in self.vehicle_data.repulsor_nodes
]
delta_height = 0.0
projected_delta_height = 0.0
power_frac_needed = 0.0
# The driver gives 100% repulsor power, no matter how high we are, or
# whether we even have ground contact.
if self.inputs[FULL_REPULSORS]:
repulsor_activation = [
self.inputs[REPULSOR_ACTIVATION]
for _ in self.vehicle_data.repulsor_nodes
]
return repulsor_activation, delta_height, projected_delta_height,\
power_frac_needed
def ecu_gyro_stabilization(self):
# Active stabilization and angular dampening
# FIXME: Get from self.inputs
tau = 0.2 # Seconds until target orientation is reached
if self.sensors[LOCAL_UP]:
target_mode = self.inputs[ACTIVE_STABILIZATION_ON_GROUND]
else:
target_mode = self.inputs[ACTIVE_STABILIZATION_IN_AIR]
if target_mode == TO_HORIZON:
# Stabilize to the current heading, but in a horizontal
# orientation
self.target_node.set_hpr(
self.app.render,
self.vehicle.get_h(),
0,
0,
)
elif target_mode == TO_GROUND:
# Stabilize towards the local up
self.target_node.set_hpr(self.centroid, (0, 0, 0))
if target_mode != PASSIVE:
xyz_driver_modification = self.inputs[TARGET_ORIENTATION]
hpr_driver_modification = VBase3(
xyz_driver_modification.z,
xyz_driver_modification.x,
xyz_driver_modification.y,
)
self.target_node.set_hpr(
self.target_node,
hpr_driver_modification,
)
# Now comes the math.
orientation = self.vehicle.get_quat(self.app.render)
target_orientation = self.target_node.get_quat(self.app.render)
delta_orientation = target_orientation * invert(orientation)
self.delta_node.set_quat(invert(delta_orientation))
delta_angle = delta_orientation.get_angle_rad()
if abs(delta_angle) < (pi / 360 * 0.1) or isnan(delta_angle):
delta_angle = 0
axis_of_torque = VBase3(0, 0, 0)
else:
axis_of_torque = delta_orientation.get_axis()
axis_of_torque.normalize()
axis_of_torque = self.app.render.get_relative_vector(
self.vehicle,
axis_of_torque,
)
if delta_angle > pi:
delta_angle -= 2 * pi
# If the mass was standing still, this would be the velocity that
# has to be reached to achieve the targeted orientation in tau
# seconds.
target_angular_velocity = axis_of_torque * delta_angle / tau
else: # `elif target_mode == PASSIVE:`, since we might want an OFF mode
# Passive stabilization, so this is the pure commanded impulse
target_angular_velocity = self.app.render.get_relative_vector(
self.vehicle,
self.inputs[TARGET_ORIENTATION] * tau / pi,
)
# But we also have to cancel out the current velocity for that.
angular_velocity = self.physics_node.get_angular_velocity()
countering_velocity = -angular_velocity
# An impulse of 1 causes an angular velocity of 2.5 rad on a unit mass,
# so we have to adjust accordingly.
target_impulse = target_angular_velocity / 2.5 * self.vehicle_data.mass
countering_impulse = countering_velocity / 2.5 * self.vehicle_data.mass
# Now just sum those up, and we have the impulse that needs to be
# applied to steer towards target.
impulse = target_impulse + countering_impulse
return impulse
def apply_air_drag(self):
drag_force = self.sensors[LOCAL_DRAG_FORCE]
global_drag_force = base.render.get_relative_vector(
self.vehicle,
drag_force,
)
if not isnan(global_drag_force.x):
self.physics_node.apply_central_impulse(
global_drag_force * globalClock.dt, )
def apply_repulsors(self):
dt = globalClock.dt
repulsor_data = zip(self.vehicle_data.repulsor_nodes,
self.sensors[REPULSOR_DATA],
self.commands[REPULSOR_ACTIVATION],
self.commands[REPULSOR_TARGET_ORIENTATIONS])
for node, data, activation, angle in repulsor_data:
active = data.active
frac = data.fraction
# Repulse in current orientation
if active and activation:
if activation > 1.0:
activation = 1.0
if activation < 0.0:
activation = 0.0
# Repulsor power at zero distance
base_strength = node.get_python_tag(FORCE)
# Effective fraction of repulsors force
transfer_frac = cos(0.5 * pi * frac)
# Effective repulsor force
strength = base_strength * activation * transfer_frac
# Resulting impulse
impulse_dir = Vec3(0, 0, 1)
impulse_dir_world = self.app.render.get_relative_vector(
node,
impulse_dir,
)
impulse = impulse_dir_world * strength
# Apply
repulsor_pos = node.get_pos(self.vehicle)
# FIXME! The position at which an impulse is applied seems to be
# centered around node it is applied to, but offset in the world
# orientation. So, right distance, wrong angle. This is likely a
# bug in Panda3D's Bullet wrapper. Or an idiosyncracy of Bullet.
self.physics_node.apply_impulse(
impulse * dt,
self.app.render.get_relative_vector(
self.vehicle,
repulsor_pos,
),
)
# Contact visualization node
max_distance = node.get_python_tag(ACTIVATION_DISTANCE)
contact_distance = -impulse_dir_world * max_distance * frac
contact_node = node.get_python_tag('ray_end')
contact_node.set_pos(
node.get_pos(self.app.render) + contact_distance, )
#contact_node.set_hpr(node, 0, -90, 0) # Look towards repulsor
#contact_node.set_hpr(0, -90, 0) # Look up
contact_node.set_hpr(0, -90,
contact_node.getR() +
4) # Look up and rotate
contact_node.set_scale(1 - frac)
contact_node.show()
# Sound
sound = node.get_python_tag(REPULSOR_SOUND)
sound.set_volume(activation * 20)
sound.set_play_rate((1 + activation / 2) * 2)
else:
node.get_python_tag('ray_end').hide()
sound = node.get_python_tag(REPULSOR_SOUND)
sound.set_volume(0.0)
sound.set_play_rate(0.0)
# Reorient
old_hpr = node.get_python_tag(REPULSOR_OLD_ORIENTATION)
want_hpr = VBase3(angle.z, angle.x, angle.y)
delta_hpr = want_hpr - old_hpr
max_angle = node.get_python_tag(REPULSOR_TURNING_ANGLE) * dt
if delta_hpr.length() > max_angle:
delta_hpr = delta_hpr / delta_hpr.length() * max_angle
new_hpr = old_hpr + delta_hpr
node.set_hpr(new_hpr)
node.set_python_tag(REPULSOR_OLD_ORIENTATION, new_hpr)
def apply_gyroscope(self):
impulse = self.commands[GYRO_ROTATION]
# Clamp the impulse to what the "motor" can produce.
max_impulse = self.vehicle_data.max_gyro_torque
if impulse.length() > max_impulse:
clamped_impulse = impulse / impulse.length() * max_impulse
else:
clamped_impulse = impulse
self.physics_node.apply_torque_impulse(clamped_impulse)
impulse_ratio = clamped_impulse.length() / max_impulse
sound = self.model.get_python_tag(GYROSCOPE_SOUND)
sound.set_volume(0.5 * impulse_ratio)
sound.set_play_rate(0.9 + 0.1 * impulse_ratio)
def apply_thrusters(self):
dt = globalClock.dt
thruster_data = zip(
self.vehicle_data.thruster_nodes,
self.commands[THRUSTER_ACTIVATION],
)
for node, thrust in thruster_data:
if self.thruster_heat >= 1.0:
thrust = 0.0
current_power = node.get_python_tag(THRUSTER_POWER)
# Adjust thrust to be within bounds of thruster's ability to adjust
# thrust.
ramp_time = node.get_python_tag(THRUSTER_RAMP_TIME)
ramp_step = (1 / ramp_time) * globalClock.dt
thrust = adjust_within_limit(thrust, current_power, ramp_step)
node.set_python_tag(THRUSTER_POWER, thrust)
max_force = node.get_python_tag(FORCE)
real_force = max_force * thrust
# FIXME: See repulsors above for the shortcoming that this suffers
thruster_pos = base.render.get_relative_vector(
self.vehicle,
node.get_pos(self.vehicle),
)
thrust_direction = self.app.render.get_relative_vector(
node, Vec3(0, 0, 1))
self.physics_node.apply_impulse(
thrust_direction * real_force * dt,
thruster_pos,
)
heating = node.get_python_tag(THRUSTER_HEATING)
cooling = node.get_python_tag(THRUSTER_COOLING)
effective_heating = -cooling + (heating - cooling) * thrust
self.thruster_heat += effective_heating * dt
if self.thruster_heat < 0.0:
self.thruster_heat = 0.0
# Sound
sound = node.get_python_tag(THRUSTER_SOUND)
sound.set_volume(thrust * 5)
sound.set_play_rate((1 + thrust / 20) / 3)
was_overheated = node.get_python_tag(THRUSTER_OVERHEATED)
is_overheated = self.thruster_heat > 1.0
if is_overheated and not was_overheated:
sound = node.get_python_tag(THRUSTER_OVERHEAT_SOUND)
sound.set_volume(5)
sound.play()
node.set_python_tag(THRUSTER_OVERHEATED, True)
if not is_overheated:
node.set_python_tag(THRUSTER_OVERHEATED, False)
def apply_airbrake(self):
# Animation and state update only, since the effect is in air drag
dt = globalClock.dt
target = self.commands[AIRBRAKE]
max_movement = self.airbrake_speed * dt
# Clamp change to available speed
delta = target - self.airbrake_state
if abs(delta) > max_movement:
self.airbrake_state += copysign(max_movement, delta)
else:
self.airbrake_state = target
if self.airbrake_state > 1.0:
self.airbrake_state = 1.0
if self.airbrake_state < 0.0:
self.airbrake_state = 0.0
#self.model.pose(AIRBRAKE, self.airbrake_state, partName=AIRBRAKE)
self.model.pose(AIRBRAKE, self.airbrake_state)
def apply_stabilizer_fins(self):
# Animation and state update only, since the effect is in air drag
dt = globalClock.dt
target = self.commands[STABILIZER_FINS]
max_movement = self.stabilizer_fins_speed * dt
# Clamp change to available speed
delta = target - self.stabilizer_fins_state
if abs(delta) > max_movement:
self.stabilizer_fins_state += copysign(max_movement, delta)
else:
self.stabilizer_fins_state = target
if self.stabilizer_fins_state > 1.0:
self.stabilizer_fins_state = 1.0
if self.stabilizer_fins_state < 0.0:
self.stabilizer_fins_state = 0.0
self.model.pose(
STABILIZER_FINS,
self.stabilizer_fins_state,
#partName=STABILIZER_FINS,
)
# FIXME: Implement stabilizing effect
def shock(self, x=0, y=0, z=0):
self.physics_node.apply_impulse(
Vec3(0, 0, 0),
Vec3(random(), random(), random()) * 10,
)
self.physics_node.apply_torque_impulse(Vec3(x, y, z))
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)
'''
