def setupPlayer(self, x, y, z):
yetiHeight = 7
yetiRadius = 2
yetiShape = BulletCapsuleShape(yetiRadius, yetiHeight - 2 * yetiRadius, ZUp)
modelPrefix = "../res/models/yeti_"
self.yetiModel = Actor("../res/models/yeti.egg", {"idle":"../res/models/yeti_idle.egg", "walk":"../res/models/yeti_walking.egg"})
self.yetiModel.setH(90)
self.yetiModel.setPos(0, 0, SNOW_HEIGHT)
playerNode = BulletRigidBodyNode("Player")
playerNode.setMass(MASS)
playerNode.addShape(yetiShape)
# Without this set to 0,0,0, the Yeti would wobble like a Weeble but not fall down.
playerNode.setAngularFactor(Vec3(0,0,0))
# Without this disabled, things will weld together after a certain amount of time. It's really annoying.
playerNode.setDeactivationEnabled(False)
playerNP = self.worldNP.attachNewNode(playerNode)
playerNP.setPos(x, y, z)
playerNP.setH(270)
self.yetiModel.reparentTo(playerNP)
self.bulletWorld.attachRigidBody(playerNP.node())
# Hopefully Brandon will get those animation files to me so I can convert them.
# self.setAnimation('idle')
return playerNP
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 _initLayoutModels(self):
# Load layout objects as meshes
for model in self.scene.scene.findAllMatches('**/layouts/object*/model*'):
# NOTE: ignore models that have no geometry defined
if model.getTightBounds() is None:
logger.warning('Object %s has no geometry defined and will be ignored for physics!' % (str(model)))
continue
shape, transform = getCollisionShapeFromModel(
model, mode='mesh', defaultCentered=False)
node = BulletRigidBodyNode('physics')
node.setMass(0.0)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setStatic(True)
node.addShape(shape)
node.setDeactivationEnabled(True)
node.setIntoCollideMask(BitMask32.allOn())
self.bulletWorld.attach(node)
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Reparent render and acoustics nodes (if any) below the new physic node
# XXX: should be less error prone to just reparent all children
# (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
semanticsNp = model.getParent().find('**/semantics')
if not semanticsNp.isEmpty():
semanticsNp.reparentTo(physicsNp)
# 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(model.getNetTransform().getMat()), atol=1e-6)
def makeBall(self, num, pos = (0, 0, 0)):
shape = BulletSphereShape(0.5)
node = BulletRigidBodyNode('ball_' + str(num))
node.setMass(1.0)
node.setRestitution(.9)
node.setDeactivationEnabled(False)
node.addShape(shape)
physics = render.attachNewNode(node)
physics.setPos(*pos)
self.world.attachRigidBody(node)
model = loader.loadModel('models/ball')
color = self.makeColor()
model.setColor(color)
self.ballColors['ball_' + str(num)] = color
model.reparentTo(physics)
def _add_chassis(self, pg_physics_world: PGPhysicsWorld):
para = self.get_config()
chassis = BulletRigidBodyNode(BodyName.Ego_vehicle_top)
chassis.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_shape = BulletBoxShape(
Vec3(para[Parameter.vehicle_width] / 2,
para[Parameter.vehicle_length] / 2,
para[Parameter.vehicle_height] / 2))
ts = TransformState.makePos(
Vec3(0, 0, para[Parameter.chassis_height] * 2))
chassis.addShape(chassis_shape, ts)
heading = np.deg2rad(-para[Parameter.heading] - 90)
chassis.setMass(para[Parameter.mass])
self.chassis_np = self.node_path.attachNewNode(chassis)
# not random born now
self.chassis_np.setPos(Vec3(*self.born_place, 1))
self.chassis_np.setQuat(
LQuaternionf(np.cos(heading / 2), 0, 0, np.sin(heading / 2)))
chassis.setDeactivationEnabled(False)
chassis.notifyCollisions(True) # advance collision check
self.pg_world.physics_world.dynamic_world.setContactAddedCallback(
PythonCallbackObject(self._collision_check))
self.dynamic_nodes.append(chassis)
chassis_beneath = BulletGhostNode(BodyName.Ego_vehicle)
chassis_beneath.setIntoCollideMask(BitMask32.bit(self.COLLISION_MASK))
chassis_beneath.addShape(chassis_shape)
self.chassis_beneath_np = self.chassis_np.attachNewNode(
chassis_beneath)
self.dynamic_nodes.append(chassis_beneath)
self.system = BulletVehicle(pg_physics_world.dynamic_world, chassis)
self.system.setCoordinateSystem(ZUp)
self.dynamic_nodes.append(
self.system
) # detach chassis will also detach system, so a waring will generate
self.LENGTH = para[Parameter.vehicle_length]
self.WIDTH = para[Parameter.vehicle_width]
if self.render:
model_path = 'models/ferra/scene.gltf'
self.chassis_vis = self.loader.loadModel(
AssetLoader.file_path(model_path))
self.chassis_vis.setZ(para[Parameter.vehicle_vis_z])
self.chassis_vis.setY(para[Parameter.vehicle_vis_y])
self.chassis_vis.setH(para[Parameter.vehicle_vis_h])
self.chassis_vis.set_scale(para[Parameter.vehicle_vis_scale])
self.chassis_vis.reparentTo(self.chassis_np)
def createElement(self, name, type, start, end=None):
if type == "cell":
model_file = "sphere.dae"
elif type == "bit":
model_file = "box.dae"
elif type == "segment" or type == "synapse":
model_file = "cylinder.dae"
# Create the rigid body
body_node = BulletRigidBodyNode(name)
body_node.setDeactivationEnabled(False)
body_np = self.render.attachNewNode(body_node)
body_np.setName(name)
if type == "segment" or type == "synapse":
# Calculate the linear distance between the start and the end position of the segment.
length = (Point3(start) - Point3(end)).length()
body_np.setPos(start)
body_np.lookAt(end)
body_np.setScale(1, length / 2, 1)
else:
body_np.setPos(start)
# Load the 3d model file using the asset folder relative path and attach the geom node to rigid body
object_np = self.loader.loadModel(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", model_file)))
object_np.reparentTo(body_np)
object_np.setPosHpr((0, 0, 0), (0, 0, 0))
object_np.setName(name + "_geom")
object_np.setTexGen(TextureStage.getDefault(),
TexGenAttrib.MWorldPosition)
# Apply any transforms from modelling sotware to gain performance
object_np.flattenStrong()
# Create the shape used for collisions
geom_nodes = object_np.findAllMatches("**/+GeomNode")
mesh = BulletTriangleMesh()
for geom in geom_nodes[0].node().getGeoms():
mesh.addGeom(geom)
shape = BulletTriangleMeshShape(mesh, dynamic=True)
body_node.addShape(shape)
self.physics_manager.attachRigidBody(body_node)
return body_np
def setupDoor(self, _obj, _eggFile):
shape = BulletBoxShape(Vec3(1.7 / 2, 0.6 / 2, 0.2 / 2))
node = BulletRigidBodyNode(_obj.getTag("door"))
node.addShape(shape)
node.setMass(1)
node.setDeactivationEnabled(False)
np = self.rootNode.attachNewNode(node)
np.setCollideMask(BitMask32.bit(2))
np.setPos(_obj.getPos())
np.setHpr(_obj.getHpr())
self.parent.physics_world.attachRigidBody(node)
_obj.reparentTo(np)
_obj.setPos(np.getPos() - _obj.getPos())
# Setup hinge
if _obj.getTag("door") == "left" and self.isHingeLeftSet != True:
pos = Point3(-2.0, 0, 0) #hingeLeft.getPos()
axisA = Vec3(0, 1, 0)
hinge = BulletHingeConstraint(node, pos, axisA, True)
hinge.setDebugDrawSize(0.3)
hinge.setLimit(-10, 58, softness=0.9, bias=0.3, relaxation=1.0)
self.parent.physics_world.attachConstraint(hinge)
self.isHingeLeftSet = True
self.parent.game_doors["left"] = np
self.parent.game_doors["left_hinge"] = hinge
if _obj.getTag("door") == "right" and self.isHingeRightSet != True:
pos = Point3(2.0, 0, 0) #hingeLeft.getPos()
axisA = Vec3(0, -1, 0)
hinge = BulletHingeConstraint(node, pos, axisA, True)
hinge.setDebugDrawSize(0.3)
hinge.setLimit(-10, 58, softness=0.9, bias=0.3, relaxation=1.0)
self.parent.physics_world.attachConstraint(hinge)
self.isHingeRightSet = True
self.parent.game_doors["right"] = np
self.parent.game_doors["right_hinge"] = hinge
def _initObjects(self):
# Load objects
for model in self.scene.scene.findAllMatches(
'**/objects/object*/model*'):
modelId = model.getParent().getTag('model-id')
# XXX: we could create BulletGhostNode instance for non-collidable objects, but we would need to filter out the collisions later on
if not modelId in self.openedDoorModelIds:
shape, transform = getCollisionShapeFromModel(
model, self.objectMode, defaultCentered=True)
node = BulletRigidBodyNode('physics')
node.addShape(shape)
node.setFriction(self.defaultMaterialFriction)
node.setRestitution(self.defaultMaterialRestitution)
node.setIntoCollideMask(BitMask32.allOn())
node.setDeactivationEnabled(True)
if self.suncgDatasetRoot is not None:
# Check if it is a movable object
category = self.modelCatMapping.getCoarseGrainedCategoryForModelId(
modelId)
if category in self.movableObjectCategories:
# Estimate mass of object based on volumetric data and default material density
objVoxFilename = os.path.join(self.suncgDatasetRoot,
'object_vox',
'object_vox_data',
modelId,
modelId + '.binvox')
voxelData = ObjectVoxelData.fromFile(objVoxFilename)
mass = Panda3dBulletPhysics.defaultDensity * voxelData.getFilledVolume(
)
node.setMass(mass)
else:
node.setMass(0.0)
node.setStatic(True)
else:
node.setMass(0.0)
node.setStatic(True)
if node.isStatic():
model.getParent().setTag('physics-mode', 'static')
else:
model.getParent().setTag('physics-mode', 'dynamic')
# Attach the physic-related node to the scene graph
physicsNp = model.getParent().attachNewNode(node)
physicsNp.setTransform(transform)
# Reparent render and acoustics nodes (if any) below the new physic node
#XXX: should be less error prone to just reparent all children (except the hidden model)
renderNp = model.getParent().find('**/render')
if not renderNp.isEmpty():
renderNp.reparentTo(physicsNp)
acousticsNp = model.getParent().find('**/acoustics')
if not acousticsNp.isEmpty():
acousticsNp.reparentTo(physicsNp)
# NOTE: we need this to update the transform state of the internal bullet node
node.setTransformDirty()
# NOTE: we need to add the node to the bullet engine only after setting all attributes
self.bulletWorld.attach(node)
# Validation
assert np.allclose(
mat4ToNumpyArray(physicsNp.getNetTransform().getMat()),
mat4ToNumpyArray(
model.getParent().getNetTransform().getMat()),
atol=1e-6)
else:
logger.debug('Object %s ignored from physics' % (modelId))
class SMAI():
#This constructs the nods tree needed for an AIChar object.
#Takes in a model as a string, a speed double, a world object, a bullet physics world and n x, y and z positions
def __init__(self, model, speed, world, worldNP, x, y, z):
self.AISpeed = 80000
self.maxSpeed = speed
self.AIX = x
self.AIY = y
self.AIZ = z
self.worldNP = worldNP
bulletWorld = world
self.type = ""
self.AIModel = loader.loadModel(model)
self.AIModel.setScale(0.5)
self.AIModel.reparentTo(render)
AIShape = BulletBoxShape(Vec3(3, 3, 1))
self.AINode = BulletRigidBodyNode('AIChar')
self.AINode.setMass(100)
self.AINode.addShape(AIShape)
self.AINode.setDeactivationEnabled(False)
self.AINode.setAngularFactor(Vec3(0, 0, 0))
render.attachNewNode(self.AINode)
self.AIChar = self.worldNP.attachNewNode(self.AINode)
self.AIModel.reparentTo(self.AIChar)
self.AIChar.setPos(x, y, z)
bulletWorld.attachRigidBody(self.AIChar.node())
#This method needs to be called on your AIChar object to determine what type of AI you want.
#Takes in a type string; either flee or seek and also a target object.
def setBehavior(self, type, target):
if (type == "flee"):
self.type = type
self.target = target
print("Type set to flee")
elif (type == "seek"):
self.type = type
self.target = target
print("Type set to seek")
else:
print("Error @ Incorrect Type!")
def moveTo(self, target):
self.moveTo = True
self.target = target
def flee(self):
h = 1
hx = 1
hy = 1
if (self.AIChar.getDistance(self.target) < 40.0):
if (self.AINode.getLinearVelocity() > self.maxSpeed):
if (self.AINode.getLinearVelocity() < 100):
self.AIChar.setH(self.target.getH())
h = self.AIChar.getH()
else:
hx = sin(h * DEG_TO_RAD) * 10
hy = cos(h * DEG_TO_RAD) * 10
self.AINode.applyForce(
Vec3(-hx * self.AISpeed * globalClock.getDt(),
hy * self.AISpeed * globalClock.getDt(), 0), PNT)
# else:
# self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT)
def seek(self):
if (self.AIChar.getDistance(self.target) > 20.0):
if (self.AINode.getLinearVelocity() > self.maxSpeed):
self.AIChar.lookAt(self.target)
h = self.AIChar.getH()
hx = sin(h * DEG_TO_RAD) * 10
hy = cos(h * DEG_TO_RAD) * 10
self.AINode.applyForce(
Vec3(-hx * self.AISpeed * globalClock.getDt(),
hy * self.AISpeed * globalClock.getDt(), 0), PNT)
def moveToRun(self):
self.seek()
# if(self.target.getX()+5 <= self.AIChar.getX() and self.target.getX()-5 >= self.AIChar.getX() and self.target.getY()+5 <= self.AIChar.getY() and self.target.getY()-5 >= self.AIChar.getY()):
# print("It's stopped!")
# self.AIChar.clearForces()
# self.AIChar.setLinearVelocity(0)
# else:
# self.AINode.clearForces()
# self.AIChar.lookAt(self.target)
# self.AINode.setLinearVelocity(self.AISpeed)
#Gets called on every AIChar in the world's update method to allow the AI to function at all.
def AIUpdate(self):
if (self.moveTo == True):
self.moveToRun()
if (self.type == "seek"):
self.seek()
elif (self.type == "flee"):
self.flee()
else:
print("Error @ No AI Type")
class BulletNPC(DynamicObject):
def __init__(self, name, game, pos):
self.game = game
self.name = name
self.jumping = 0
self.crouching = 0
# maximum speed when only walking
self.groundSpeed = 4.0
# acceleration used when walking to rise to self.groundSpeed
self.groundAccel = 150.0
self.groundFriction = 0.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(self.name)
self.node.setMass(1.0)
self.node.addShape(self.shape)
self.node.setActive(True, True)
self.node.setDeactivationEnabled(False, True)
self.node.setFriction(self.groundFriction)
# self.node.setGravity(10)
# self.node.setFallSpeed(200)
self.node.setCcdMotionThreshold(1e-7)
# do not use setCcdSweptSphereRadius
# it messes up the bite contact test
# self.node.setCcdSweptSphereRadius(0.5)
self.node.setAngularFactor(Vec3(0, 0, 1))
self.np = self.game.render.attachNewNode(self.node)
self.setPos(pos)
self.setH(0)
# self.np.setCollideMask(BitMask32.allOn())
self.game.world.attachRigidBody(self.node)
self.playerModel = Actor(
"models/monsters/ghoul2.egg",
{"idle": "models/monsters/ghoul2-idle.egg", "walk": "models/monsters/ghoul2-walk.egg"},
)
self.playerModel.setScale(0.15)
self.playerModel.setZ(0.15)
self.playerModel.reparentTo(self.np)
interval = Wait(random.uniform(0, 1))
i2 = Func(self.startWalkAnim)
seq = Sequence()
seq.append(interval)
seq.append(i2)
seq.start()
self.growlTimer = 5.0
self.sound = None
self.alive = True
self.targetNp = NodePath(self.name)
self.brainTimer = 0.0
self.brainDelay = 1.2
self.currentForce = Vec3(0, 0, 0)
def startWalkAnim(self):
self.playerModel.loop("walk")
def stopWalkAnim(self):
self.playerModel.loop("idle")
def checkFront(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
direction = direction * 2.0
p2 = Point3(self.getPos() + direction)
result = self.game.world.rayTestClosest(p1, p2)
# ts1 = TransformState.makePos(p1)
# ts2 = TransformState.makePos(p2)
# result = self.game.world.sweepTestClosest(self.shape, ts1, ts2, 0)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkRight(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
right = direction.cross(Vec3(0, 0, 1))
right = right * 2.0
p2 = Point3(self.getPos() + right)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkLeft(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
left = -direction.cross(Vec3(0, 0, 1))
left = left * 2.0
p2 = Point3(self.getPos() + left)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def updateDirection(self):
direction = self.game.playerNp.getPos() - self.getPos()
direction.setZ(0)
direction.normalize()
direction = direction * self.groundAccel
right = direction.cross(Vec3(0, 0, 1))
left = -right
inFront = self.checkFront()
inRight = self.checkRight()
inLeft = self.checkLeft()
if inFront == "boxpack":
if inRight == "boxpack":
if inLeft == "boxPack":
self.currentForce = -direction
else:
self.currentForce = left
else:
self.currentForce = right
else:
self.currentForce = direction
def update(self, dt=0.0):
if not self.alive:
return
self.brainTimer -= dt
if self.brainTimer <= 0.0:
self.updateDirection()
self.brainTimer = self.brainDelay
self.setForce(self.currentForce)
self.capSpeedXY()
speedVec = self.getSpeedVec()
self.targetNp.setPos(self.getPos() + speedVec)
self.lookAt(self.targetNp)
# growl!
self.growlTimer -= dt
if self.growlTimer <= 0.0:
if self.sound:
self.game.soundDic3D[self.sound].stop()
self.game.detachSound(self.sound)
growlIndex = random.randint(1, len(self.game.zombieGrowlSounds))
soundName = "monster-" + str(growlIndex)
self.sound = soundName
self.game.attachSound(soundName, self.np)
self.game.playSound3D(soundName)
self.growlTimer = 1.0 + random.uniform(0.0, 1.0)
# print "Growl from %s" % (self.name)
# bite! # player is the only one checking for that now
# res = self.getContacts()
# if "player" in res:
# print "%s took a bite on player at %s!" % (self.name, self.game.globalClock.getFrameTime())
# self.game.onDie()
def destroy(self):
self.alive = False
self.game.world.removeRigidBody(self.node)
self.stopWalkAnim()
self.np.setTransparency(True)
self.np.setColor(1, 1, 1, 1)
self.setGlowOff()
i1 = Wait(random.uniform(0, 1))
i2 = LerpColorInterval(self.np, 1.0, (1, 1, 1, 0))
i3 = Func(self.np.remove)
seq = Sequence()
seq.append(i1)
seq.append(i2)
seq.append(i3)
seq.start()
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 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 BulletNPC(DynamicObject):
def __init__(self, name, game, pos):
self.game = game
self.name = name
self.jumping = 0
self.crouching = 0
# maximum speed when only walking
self.groundSpeed = 4.0
# acceleration used when walking to rise to self.groundSpeed
self.groundAccel = 150.0
self.groundFriction = 0.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(self.name)
self.node.setMass(1.0)
self.node.addShape(self.shape)
self.node.setActive(True, True)
self.node.setDeactivationEnabled(False, True)
self.node.setFriction(self.groundFriction)
#self.node.setGravity(10)
#self.node.setFallSpeed(200)
self.node.setCcdMotionThreshold(1e-7)
# do not use setCcdSweptSphereRadius
# it messes up the bite contact test
#self.node.setCcdSweptSphereRadius(0.5)
self.node.setAngularFactor(Vec3(0, 0, 1))
self.np = self.game.render.attachNewNode(self.node)
self.setPos(pos)
self.setH(0)
#self.np.setCollideMask(BitMask32.allOn())
self.game.world.attachRigidBody(self.node)
self.playerModel = Actor(
"models/monsters/ghoul2.egg", {
"idle": "models/monsters/ghoul2-idle.egg",
"walk": "models/monsters/ghoul2-walk.egg"
})
self.playerModel.setScale(0.15)
self.playerModel.setZ(0.15)
self.playerModel.reparentTo(self.np)
interval = Wait(random.uniform(0, 1))
i2 = Func(self.startWalkAnim)
seq = Sequence()
seq.append(interval)
seq.append(i2)
seq.start()
self.growlTimer = 5.0
self.sound = None
self.alive = True
self.targetNp = NodePath(self.name)
self.brainTimer = 0.0
self.brainDelay = 1.2
self.currentForce = Vec3(0, 0, 0)
def startWalkAnim(self):
self.playerModel.loop("walk")
def stopWalkAnim(self):
self.playerModel.loop("idle")
def checkFront(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
direction = direction * 2.0
p2 = Point3(self.getPos() + direction)
result = self.game.world.rayTestClosest(p1, p2)
#ts1 = TransformState.makePos(p1)
#ts2 = TransformState.makePos(p2)
#result = self.game.world.sweepTestClosest(self.shape, ts1, ts2, 0)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkRight(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
right = direction.cross(Vec3(0, 0, 1))
right = right * 2.0
p2 = Point3(self.getPos() + right)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def checkLeft(self):
p1 = Point3(self.getPos())
direction = self.game.playerNp.getPos() - self.getPos()
direction.normalize()
left = -direction.cross(Vec3(0, 0, 1))
left = left * 2.0
p2 = Point3(self.getPos() + left)
result = self.game.world.rayTestClosest(p1, p2)
if result.hasHit():
pos = result.getHitPos()
n = result.getHitNormal()
frac = result.getHitFraction()
node = result.getNode()
return node.getName()
return None
def updateDirection(self):
direction = self.game.playerNp.getPos() - self.getPos()
direction.setZ(0)
direction.normalize()
direction = direction * self.groundAccel
right = direction.cross(Vec3(0, 0, 1))
left = -right
inFront = self.checkFront()
inRight = self.checkRight()
inLeft = self.checkLeft()
if inFront == "boxpack":
if inRight == "boxpack":
if inLeft == "boxPack":
self.currentForce = -direction
else:
self.currentForce = left
else:
self.currentForce = right
else:
self.currentForce = direction
def update(self, dt=0.0):
if not self.alive:
return
self.brainTimer -= dt
if self.brainTimer <= 0.0:
self.updateDirection()
self.brainTimer = self.brainDelay
self.setForce(self.currentForce)
self.capSpeedXY()
speedVec = self.getSpeedVec()
self.targetNp.setPos(self.getPos() + speedVec)
self.lookAt(self.targetNp)
# growl!
self.growlTimer -= dt
if self.growlTimer <= 0.0:
if self.sound:
self.game.soundDic3D[self.sound].stop()
self.game.detachSound(self.sound)
growlIndex = random.randint(1, len(self.game.zombieGrowlSounds))
soundName = "monster-" + str(growlIndex)
self.sound = soundName
self.game.attachSound(soundName, self.np)
self.game.playSound3D(soundName)
self.growlTimer = 1.0 + random.uniform(0.0, 1.0)
#print "Growl from %s" % (self.name)
# bite! # player is the only one checking for that now
#res = self.getContacts()
#if "player" in res:
#print "%s took a bite on player at %s!" % (self.name, self.game.globalClock.getFrameTime())
#self.game.onDie()
def destroy(self):
self.alive = False
self.game.world.removeRigidBody(self.node)
self.stopWalkAnim()
self.np.setTransparency(True)
self.np.setColor(1, 1, 1, 1)
self.setGlowOff()
i1 = Wait(random.uniform(0, 1))
i2 = LerpColorInterval(self.np, 1.0, (1, 1, 1, 0))
i3 = Func(self.np.remove)
seq = Sequence()
seq.append(i1)
seq.append(i2)
seq.append(i3)
seq.start()
class SMAI():
#This constructs the nods tree needed for an AIChar object.
#Takes in a model as a string, a speed double, a world object, a bullet physics world and n x, y and z positions
def __init__(self, model, speed, world, worldNP, x, y, z):
self.AISpeed = 80000
self.maxSpeed = speed
self.AIX = x
self.AIY = y
self.AIZ = z
self.worldNP = worldNP
bulletWorld = world
self.type = ""
self.AIModel = loader.loadModel(model)
self.AIModel.setScale(0.5)
self.AIModel.reparentTo(render)
AIShape = BulletBoxShape(Vec3(3, 3, 1))
self.AINode = BulletRigidBodyNode('AIChar')
self.AINode.setMass(100)
self.AINode.addShape(AIShape)
self.AINode.setDeactivationEnabled(False)
self.AINode.setAngularFactor(Vec3(0,0,0))
render.attachNewNode(self.AINode)
self.AIChar = self.worldNP.attachNewNode(self.AINode)
self.AIModel.reparentTo(self.AIChar)
self.AIChar.setPos(x, y, z)
bulletWorld.attachRigidBody(self.AIChar.node())
#This method needs to be called on your AIChar object to determine what type of AI you want.
#Takes in a type string; either flee or seek and also a target object.
def setBehavior(self, type, target):
if(type == "flee"):
self.type = type
self.target = target
print("Type set to flee")
elif(type == "seek"):
self.type = type
self.target = target
print("Type set to seek")
else:
print("Error @ Incorrect Type!")
def moveTo(self, target):
self.moveTo = True
self.target = target
def flee(self):
h = 1
hx = 1
hy = 1
if(self.AIChar.getDistance(self.target) < 40.0):
if(self.AINode.getLinearVelocity() > self.maxSpeed):
if(self.AINode.getLinearVelocity() < 100):
self.AIChar.setH(self.target.getH())
h = self.AIChar.getH()
else:
hx = sin(h * DEG_TO_RAD) * 10
hy = cos(h * DEG_TO_RAD) * 10
self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT)
# else:
# self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT)
def seek(self):
if(self.AIChar.getDistance(self.target) > 20.0):
if(self.AINode.getLinearVelocity() > self.maxSpeed):
self.AIChar.lookAt(self.target)
h = self.AIChar.getH()
hx = sin(h * DEG_TO_RAD) * 10
hy = cos(h * DEG_TO_RAD) * 10
self.AINode.applyForce(Vec3(-hx * self.AISpeed * globalClock.getDt(), hy * self.AISpeed * globalClock.getDt(), 0), PNT)
def moveToRun(self):
self.seek()
# if(self.target.getX()+5 <= self.AIChar.getX() and self.target.getX()-5 >= self.AIChar.getX() and self.target.getY()+5 <= self.AIChar.getY() and self.target.getY()-5 >= self.AIChar.getY()):
# print("It's stopped!")
# self.AIChar.clearForces()
# self.AIChar.setLinearVelocity(0)
# else:
# self.AINode.clearForces()
# self.AIChar.lookAt(self.target)
# self.AINode.setLinearVelocity(self.AISpeed)
#Gets called on every AIChar in the world's update method to allow the AI to function at all.
def AIUpdate(self):
if(self.moveTo == True):
self.moveToRun()
if(self.type == "seek"):
self.seek()
elif(self.type == "flee"):
self.flee()
else:
print("Error @ No AI Type")
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 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 Vehicle(object):
def __init__(self, positionHpr, render, world, base):
self.base = base
loader = base.loader
position, hpr = positionHpr
vehicleType = "yugo"
self.vehicleDir = "data/vehicles/" + vehicleType + "/"
# Load vehicle description and specs
with open(self.vehicleDir + "vehicle.json") as vehicleData:
data = json.load(vehicleData)
self.specs = data["specs"]
# Chassis for collisions and mass uses a simple box shape
halfExtents = (0.5 * dim for dim in self.specs["dimensions"])
shape = BulletBoxShape(Vec3(*halfExtents))
ts = TransformState.makePos(Point3(0, 0, 0.5))
self.rigidNode = BulletRigidBodyNode("vehicle")
self.rigidNode.addShape(shape, ts)
self.rigidNode.setMass(self.specs["mass"])
self.rigidNode.setDeactivationEnabled(False)
self.np = render.attachNewNode(self.rigidNode)
self.np.setPos(position)
self.np.setHpr(hpr)
world.attachRigidBody(self.rigidNode)
# Vehicle physics model
self.vehicle = BulletVehicle(world, self.rigidNode)
self.vehicle.setCoordinateSystem(ZUp)
world.attachVehicle(self.vehicle)
# Vehicle graphical model
self.vehicleNP = loader.loadModel(self.vehicleDir + "car.egg")
self.vehicleNP.reparentTo(self.np)
# Create wheels
wheelPos = self.specs["wheelPositions"]
for fb, y in (("F", wheelPos[1]), ("B", -wheelPos[1])):
for side, x in (("R", wheelPos[0]), ("L", -wheelPos[0])):
np = loader.loadModel(self.vehicleDir + "tire%s.egg" % side)
np.reparentTo(render)
isFront = fb == "F"
self.addWheel(Point3(x, y, wheelPos[2]), isFront, np)
def addWheel(self, position, isFront, np):
wheel = self.vehicle.createWheel()
wheelSpecs = self.specs["wheels"]
wheel.setNode(np.node())
wheel.setChassisConnectionPointCs(position)
wheel.setFrontWheel(isFront)
wheel.setWheelDirectionCs(Vec3(0, 0, -1))
wheel.setWheelAxleCs(Vec3(1, 0, 0))
wheel.setWheelRadius(wheelSpecs["radius"])
wheel.setMaxSuspensionTravelCm(wheelSpecs["suspensionTravel"] * 100.0)
wheel.setSuspensionStiffness(wheelSpecs["suspensionStiffness"])
wheel.setWheelsDampingRelaxation(wheelSpecs["dampingRelaxation"])
wheel.setWheelsDampingCompression(wheelSpecs["dampingCompression"])
wheel.setFrictionSlip(wheelSpecs["frictionSlip"])
wheel.setRollInfluence(wheelSpecs["rollInfluence"])
def initialiseSound(self, audioManager):
"""Start the engine sound and set collision sounds"""
# Set sounds to play for collisions
self.collisionSound = CollisionSound(
nodePath=self.np, sounds=["data/sounds/09.wav"], thresholdForce=600.0, maxForce=800000.0
)
self.engineSound = audioManager.loadSfx(self.vehicleDir + "engine.wav")
audioManager.attachSoundToObject(self.engineSound, self.np)
self.engineSound.setLoop(True)
self.engineSound.setPlayRate(0.6)
self.engineSound.play()
self.gearSpacing = self.specs["sound"]["maxExpectedRotationRate"] / self.specs["sound"]["numberOfGears"]
self.reversing = False
def updateSound(self, dt):
"""Use vehicle speed to update sound pitch"""
soundSpecs = self.specs["sound"]
# Use rear wheel rotation speed as some measure of engine revs
wheels = (self.vehicle.getWheel(idx) for idx in (2, 3))
# wheelRate is in degrees per second
wheelRate = 0.5 * abs(sum(w.getDeltaRotation() / dt for w in wheels))
# Calculate which gear we're in, and what the normalised revs are
if self.reversing:
numberOfGears = 1
else:
numberOfGears = self.specs["sound"]["numberOfGears"]
gear = min(int(wheelRate / self.gearSpacing), numberOfGears - 1)
posInGear = (wheelRate - gear * self.gearSpacing) / self.gearSpacing
targetPlayRate = 0.6 + posInGear * (1.5 - 0.6)
currentRate = self.engineSound.getPlayRate()
self.engineSound.setPlayRate(0.8 * currentRate + 0.2 * targetPlayRate)
def updateControl(self, controlState, dt):
"""Updates acceleration, braking and steering
These are all passed in through a controlState dictionary
"""
# Update acceleration and braking
wheelForce = controlState["throttle"] * self.specs["maxWheelForce"]
self.reversing = controlState["reverse"]
if self.reversing:
# Make reversing a bit slower than moving forward
wheelForce *= -0.5
brakeForce = controlState["brake"] * self.specs["brakeForce"]
# Update steering
# Steering control state is from -1 to 1
steering = controlState["steering"] * self.specs["steeringLock"]
# Apply steering to front wheels
self.vehicle.setSteeringValue(steering, 0)
self.vehicle.setSteeringValue(steering, 1)
# Apply engine and brake to rear wheels
self.vehicle.applyEngineForce(wheelForce, 2)
self.vehicle.applyEngineForce(wheelForce, 3)
self.vehicle.setBrake(brakeForce, 2)
self.vehicle.setBrake(brakeForce, 3)
class 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 Player(object):
#global playerNode
#playerNode = NodePath('player')
F_MOVE = 400.0
def __init__(self, btWorld):
self._attachControls()
self._initPhysics(btWorld)
self._loadModel()
taskMgr.add(self.mouseUpdate, 'player-task')
taskMgr.add(self.moveUpdate, 'player-task')
self._vforce = Vec3(0,0,0)
#ItemHandling(self.playerNode)
def _attachControls(self):
#base.accept( "s" , self.__setattr__,["walk",self.STOP] )
base.accept("w", self.goForward)
base.accept("w-up", self.nogoForward)
base.accept("s", self.goBack)
base.accept("s-up", self.nogoBack)
base.accept("a", self.goLeft)
base.accept("a-up", self.nogoLeft)
base.accept("d", self.goRight)
base.accept("d-up", self.nogoRight)
base.accept("space", self.goUp)
base.accept("space-up", self.nogoUp)
def _loadModel(self):
#self._model = loader.loadModel("../data/models/d1_sphere.egg")
#self._model.reparentTo(self.playerNode)
pl = base.cam.node().getLens()
pl.setNear(0.2)
base.cam.node().setLens(pl)
def _initPhysics(self, world):
rad = 1
shape = BulletSphereShape(rad)
self._rb_node = BulletRigidBodyNode('Box')
self._rb_node.setMass(80.0)
self._rb_node.setFriction(1.8)
self._rb_node.addShape(shape)
self._rb_node.setAngularFactor(Vec3(0,0,0))
self._rb_node.setDeactivationEnabled(False, True)
self.playerNode = render.attachNewNode(self._rb_node)
self.playerNode.setPos(0, 0, 4)
self.playerNode.setHpr(0,0,0)
world.attachRigidBody(self._rb_node)
self.camNode = self.playerNode.attachNewNode("cam node")
base.camera.reparentTo(self.camNode)
self.camNode.setPos(self.camNode, 0, 0, 1.75-rad)
#self.camNode.setPos(self.camNode, 0, -5,0.5)
#self.camNode.lookAt(Point3(0,0,0),Vec3(0,1,0))
def mouseUpdate(self,task):
md = base.win.getPointer(0)
dx = md.getX() - base.win.getXSize()/2.0
dy = md.getY() - base.win.getYSize()/2.0
yaw = dx/(base.win.getXSize()/2.0) * 90
self.camNode.setHpr(self.camNode, -yaw, 0, 0)
#base.camera.setHpr(base.camera, yaw, pith, roll)
base.win.movePointer(0, base.win.getXSize() / 2, base.win.getYSize() / 2)
return task.cont
def moveUpdate(self,task):
if (self._vforce.length() > 0):
#self.rbNode.applyCentralForce(self._vforce)
self._rb_node.applyCentralForce(self.playerNode.getRelativeVector(self.camNode, self._vforce))
else:
pass
return task.cont
def goForward(self):
self._vforce.setY( self.F_MOVE)
def nogoForward(self):
self._vforce.setY( 0)
def goBack(self):
self._vforce.setY(-self.F_MOVE)
def nogoBack(self):
self._vforce.setY( 0)
def goLeft(self):
self._vforce.setX(-self.F_MOVE)
def nogoLeft(self):
self._vforce.setX( 0)
def goRight(self):
self._vforce.setX( self.F_MOVE)
def nogoRight(self):
self._vforce.setX( 0)
def goUp(self):
self._vforce.setZ( self.F_MOVE*1.4)
def nogoUp(self):
self._vforce.setZ( 0)
def getRBNode(self):
return self._rb_node
