Пример #1
0
class Chunk(Entity):
    chunkmass = 5.0
    def __init__(self, world, blocks, pos, hpr=Point3(0,0,0), diff = Vec3(0,0,0), angVel = Vec3(0,0,0), linVel = Vec3(0,0,0)):
        super(Chunk, self).__init__()

        self.mass = len(blocks)*Chunk.chunkmass
        self.world = world
        self.blocks = blocks

        self.bnode = BulletRigidBodyNode()

        self.bnode.setMass(self.mass)
        self.bnode.setAngularFactor(Vec3(0,1,0))
        self.bnode.setLinearSleepThreshold(20)
        self.bnode.setAngularSleepThreshold(20)
        self.bnode.setAngularVelocity(angVel)

        self.bnode.setLinearVelocity(linVel)
        self.np = utilities.app.render.attachNewNode(self.bnode)
        self.np.setPos(pos.x,20,pos.y)
        self.np.setHpr(hpr)
        self.np.setPos(self.np, diff)
    
        self.bnode.setPythonTag("entity", self)

        self.inScope = False # culling not done yet

        # centre the blocks around the np and add their shapes in.
        self.minMax()
        cog = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0)
        for block in blocks:
            block.rebuild(self, cog)

        world.bw.attachRigidBody(self.bnode)
        self.hitlist = dict()

    def update(self, timer):
        for index in self.hitlist:
            # returns true if the wall is destroyed by the hit
            if self.blocks[index].hitby(self.hitlist[index]):
                self.blocks[index].destroy()
                self.rebuild(index)
        self.hitlist.clear()    

        if self.playerDistance() > 40.0:
            self.bnode.setAngularSleepThreshold(1)
            self.bnode.setLinearSleepThreshold(1)
        else:    
            self.bnode.setAngularSleepThreshold(0)
            self.bnode.setLinearSleepThreshold(0)
    

    def playerDistance(self):
        sp = self.np.getPos()
        pp = self.world.player.node.getPos()

        distance = hypot(sp.x - pp.x, sp.z - pp.z)
        return distance
        
    # remove an element and rebuild
    # TODO add another method for removing multiple
    # blocks in a single go
    def rebuild(self, index):
        deadblock = self.blocks[index]
        out = list()

        for block in self.blocks:
            for edge in block.edges:
                if edge == deadblock:
                    block.edges.remove(edge)

        for block in deadblock.edges:
            chunk = list()
            self.searchBlock(block, chunk, deadblock)
            out.append(chunk)

        #out is a list of lists of populated blocks
        #remove duplicate chunks
        results = list()
        for chunk in out:
            chunk.sort(compareBlock)
            if not chunk in results and len(chunk) > 0:
                results.append(chunk)
    
        for result in results:
            self.minMax(result)
            #diff = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0)
            diff = Vec3((self.minX+self.maxX) / 2.0, 0, (self.minY+self.maxY) / 2.0)
            p = self.np.getPos()
            pos = Point2(p.x, p.z)
            h = self.np.getHpr()
            self.world.entities.append(Chunk(self.world, result, pos, h, diff, self.bnode.getAngularVelocity(), self.bnode.getLinearVelocity()))

        self.destroyNow()
        self.remove = True
        
    def searchBlock(self, block, lst, deleted):
        if block in lst:
            return
        else:
            lst.append(block)
            for newblock in block.edges:
                self.searchBlock(newblock, lst, deleted)

    def minMax(self, blocks=None):
        if blocks == None:
            blocks = self.blocks

        self.minX = blocks[0].pos.x
        self.minY = blocks[0].pos.y
        self.maxX = blocks[0].pos.x
        self.maxY = blocks[0].pos.y

        for point in blocks:
            self.minX = min(point.pos.x, self.minX)
            self.minY = min(point.pos.y, self.minY)
            self.maxX = max(point.pos.x, self.maxX)
            self.maxY = max(point.pos.y, self.maxY)

    # Need to clear the bulletrigidbody immediately
    # so that the phys object isn't present during next sim phase
    # which occurs before cleanup normally. Otherwise shit will
    # fly everywhere since two things are inside each other
    def destroyNow(self):
        self.world.bw.removeRigidBody(self.bnode)
        
        return

    # since this is handled earlier nothing happens    
    def destroy(self):
        return

    def hitby(self, projectile, index):
        self.hitlist[index] = projectile
Пример #2
0
class Player(Entity):

    walkspeed = 50
    damping = 0.9
    topspeed = 15

    leftMove = False
    rightMove = False
    jumpToggle = False
    crouchToggle = False

    def __init__(self, world):
        super(Player, self).__init__()

        self.obj = utilities.loadObject("player", depth=20)

        self.world = world 
        self.health = 100
        self.inventory = dict()

        self.depth = self.obj.getPos().y

        self.location = Point2(0,0)
        self.velocity = Vec3(0)
        self.pt = 0.0

        self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49))
        self.bnode = BulletRigidBodyNode('Box')
        self.bnode.setMass(1.0)
        self.bnode.setAngularVelocity(Vec3(0))
        self.bnode.setAngularFactor(Vec3(0))
        self.bnode.addShape(self.shape)
        self.bnode.setLinearDamping(0.95)
        self.bnode.setLinearSleepThreshold(0)

        world.bw.attachRigidBody(self.bnode)
        self.bnode.setPythonTag("Entity", self)
        self.bnode.setIntoCollideMask(BitMask32.bit(0))

        self.node = utilities.app.render.attachNewNode(self.bnode)
        self.node.setPos(self.obj.getPos())

        self.obj.setPos(0,0,0)
        self.obj.setScale(1)
        self.obj.reparentTo(self.node)
        self.node.setPos(self.location.x, self.depth, self.location.y)

    def initialise(self):
        self.inventory["LightLaser"] = LightLaser(self.world, self)
        self.inventory["Blowtorch"] = Blowtorch(self.world, self)
        self.inventory["Grenade"] = Grenade(self.world, self)

        for i in self.inventory:
            self.inventory[i].initialise()

        self.currentItem = self.inventory["Blowtorch"]
        self.currentItem.equip()

        self.armNode = self.obj.attachNewNode("arm")
        self.armNode.setPos(0.20,0,0.08)
        self.arm = utilities.loadObject("arm", scaleX = 0.5,scaleY = 0.5, depth = -0.2)
        self.arm.reparentTo(self.armNode)

    def activate(self):
        self.currentItem.activate()

    def update(self, timer):
        self.velocity = self.bnode.getLinearVelocity()

        if (self.leftMove):
            self.bnode.applyCentralForce(Vec3(-Player.walkspeed,0,0))
        if (self.rightMove):
            self.bnode.applyCentralForce(Vec3(Player.walkspeed,0,0))
        if (self.jumpToggle):
            self.bnode.applyCentralForce(Vec3(0,0,Player.walkspeed))
        if (self.crouchToggle):
            self.bnode.applyCentralForce(Vec3(0,0,-Player.walkspeed))
        
        if (self.velocity.x < -self.topspeed):
            self.velocity.x = -self.topspeed
        if (self.velocity.x > self.topspeed):
            self.velocity.x = self.topspeed

        mouse = utilities.app.mousePos
        # extrude test
        near = Point3()
        far = Point3()
        utilities.app.camLens.extrude(mouse, near, far)
        camp = utilities.app.camera.getPos()
        near *= 20 # player depth

        if near.x != 0:
            angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x)
            #angle = atan2(near.z, near.x)
        else:
            angle = 90  

        self.angle = angle

        # set current item to point at cursor   
        self.currentItem.update(timer)

        # move the camera so the player is centred horizontally,
        # but keep the camera steady vertically
        utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z)

        #move arm into correct position.

        gunLength = 2.0

        self.gunVector = Point2(cos(angle)*gunLength - self.armNode.getX()*5, sin(angle)*gunLength - self.armNode.getZ()*2)
        armAngle = atan2(self.gunVector.y, self.gunVector.x)
        self.arm.setHpr(self.armNode, 0, 0, -1 * degrees(armAngle))

    def moveLeft(self, switch):
        self.leftMove = switch 
        #self.bnode.applyCentralForce(Vec3(-500,0,0))

    def moveRight(self, switch):
        self.rightMove = switch 
        #self.bnode.applyCentralForce(Vec3(500,0,0))

    def jump(self, switch):
        self.jumpToggle = switch

    def crouch(self, switch):
        self.crouchToggle = switch
class DroneEnv(gym.Env):
    def __init__(self):

        self.visualize = False

        if self.visualize == False:
            from pandac.PandaModules import loadPrcFileData
            loadPrcFileData("", "window-type none")

        import direct.directbase.DirectStart

        self.ep = 0
        self.ep_rew = 0
        self.t = 0

        self.prevDis = 0

        self.action_space = Box(-1, 1, shape=(3, ))
        self.observation_space = Box(-50, 50, shape=(9, ))

        self.target = 8 * np.random.rand(3)
        self.construct()

        self.percentages = []
        self.percentMean = []
        self.percentStd = []

        taskMgr.add(self.stepTask, 'update')
        taskMgr.add(self.lightTask, 'lights')

        self.rotorForce = np.array([0, 0, 9.81], dtype=np.float)

    def construct(self):

        if self.visualize:

            base.cam.setPos(17, 17, 1)
            base.cam.lookAt(0, 0, 0)

            wp = WindowProperties()
            wp.setSize(1200, 500)
            base.win.requestProperties(wp)

        # World
        self.world = BulletWorld()
        self.world.setGravity(Vec3(0, 0, -9.81))

        #skybox
        skybox = loader.loadModel('../models/skybox.gltf')
        skybox.setTexGen(TextureStage.getDefault(),
                         TexGenAttrib.MWorldPosition)
        skybox.setTexProjector(TextureStage.getDefault(), render, skybox)
        skybox.setTexScale(TextureStage.getDefault(), 1)
        skybox.setScale(100)
        skybox.setHpr(0, -90, 0)

        tex = loader.loadCubeMap('../textures/s_#.jpg')
        skybox.setTexture(tex)
        skybox.reparentTo(render)

        render.setTwoSided(True)

        #Light
        plight = PointLight('plight')
        plight.setColor((1.0, 1.0, 1.0, 1))
        plnp = render.attachNewNode(plight)
        plnp.setPos(0, 0, 0)
        render.setLight(plnp)

        # Create Ambient Light
        ambientLight = AmbientLight('ambientLight')
        ambientLight.setColor((0.15, 0.05, 0.05, 1))
        ambientLightNP = render.attachNewNode(ambientLight)
        render.setLight(ambientLightNP)

        # Drone
        shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
        self.drone = BulletRigidBodyNode('Box')
        self.drone.setMass(1.0)
        self.drone.addShape(shape)
        self.droneN = render.attachNewNode(self.drone)
        self.droneN.setPos(0, 0, 3)
        self.world.attachRigidBody(self.drone)
        model = loader.loadModel('../models/drone.gltf')
        model.setHpr(0, 90, 0)
        model.flattenLight()
        model.reparentTo(self.droneN)

        blade = loader.loadModel("../models/blade.gltf")
        blade.reparentTo(self.droneN)
        blade.setHpr(0, 90, 0)
        blade.setPos(Vec3(0.3, -3.0, 1.1))
        rotation_interval = blade.hprInterval(0.2, Vec3(180, 90, 0))
        rotation_interval.loop()

        placeholder = self.droneN.attachNewNode("blade-placeholder")
        placeholder.setPos(Vec3(0, 6.1, 0))
        blade.instanceTo(placeholder)

        placeholder = self.droneN.attachNewNode("blade-placeholder")
        placeholder.setPos(Vec3(3.05, 3.0, 0))
        blade.instanceTo(placeholder)

        placeholder = self.droneN.attachNewNode("blade-placeholder")
        placeholder.setPos(Vec3(-3.05, 3.0, 0))
        blade.instanceTo(placeholder)

        #drone ligths
        self.plight2 = PointLight('plight')
        self.plight2.setColor((0.9, 0.1, 0.1, 1))
        plnp = self.droneN.attachNewNode(self.plight2)
        plnp.setPos(0, 0, -1)
        self.droneN.setLight(plnp)

        #over light
        plight3 = PointLight('plight')
        plight3.setColor((0.9, 0.8, 0.8, 1))
        plnp = self.droneN.attachNewNode(plight3)
        plnp.setPos(0, 0, 2)
        self.droneN.setLight(plnp)

        #target object
        self.targetObj = loader.loadModel("../models/target.gltf")
        self.targetObj.reparentTo(render)
        self.targetObj.setPos(
            Vec3(self.target[0], self.target[1], self.target[2]))

    def lightTask(self, task):

        count = globalClock.getFrameCount()

        rest = count % 100
        if rest < 10:
            self.plight2.setColor((0.1, 0.9, 0.1, 1))
        elif rest > 30 and rest < 40:
            self.plight2.setColor((0.9, 0.1, 0.1, 1))
        elif rest > 65 and rest < 70:
            self.plight2.setColor((0.9, 0.9, 0.9, 1))
        elif rest > 75 and rest < 80:
            self.plight2.setColor((0.9, 0.9, 0.9, 1))
        else:
            self.plight2.setColor((0.1, 0.1, 0.1, 1))

        return task.cont

    def getState(self):

        #vel = self.drone.get_linear_velocity()
        po = self.drone.transform.pos
        ang = self.droneN.getHpr()

        #velocity = np.nan_to_num(np.array([vel[0], vel[1], vel[2]]))
        position = np.array([po[0], po[1], po[2]])

        state = np.array([position, self.target]).reshape(6, )
        state = np.around(state, decimals=3)

        return state

    def getReward(self):

        reward = 0

        s = self.getState()
        d = np.linalg.norm(s[0:3] - s[3:6])

        if d < 20:
            reward = 20 - d
            reward = reward / 40  #/4000

        #if d < self.prevDis :
        #    reward *= 1.2

        #self.prevDis = np.copy(d)

        return reward

    def reset(self):

        #log
        self.percentages.append(self.ep_rew)
        me = np.mean(self.percentages[-500:])
        self.percentMean.append(me)
        self.percentStd.append(np.std(self.percentages[-500:]))

        s = self.getState()
        d = np.linalg.norm(np.abs(s[:3] - self.target))
        ds = np.linalg.norm(s[:3] - np.array([0, 0, 4]))

        if self.ep % 50 == 0:
            self.PlotReward()

        print(
            f'{self.ep}   {self.t}    {self.ep_rew:+8.2f}    {me:+6.2f}    {d:6.2f}    {ds:6.2f}    {s}'
        )  #{s[:6]}

        #physics reset
        self.droneN.setPos(0, 0, 4)
        self.droneN.setHpr(0, 0, 0)
        self.drone.set_linear_velocity(Vec3(0, 0, 0))
        self.drone.setAngularVelocity(Vec3(0, 0, 0))

        self.rotorForce = np.array([0, 0, 9.81], dtype=np.float)

        #define new target:
        self.target = 8 * (2 * np.random.rand(3) - 1)
        #self.target = np.zeros((3))
        self.target[2] = np.abs(self.target[2])
        self.target[1] = np.abs(self.target[1])
        self.targetObj.setPos(
            Vec3(self.target[0], self.target[1], self.target[2]))

        self.ep += 1
        self.t = 0
        self.ep_rew = 0

        state = self.getState()

        return state

    def stepTask(self, task):

        dt = globalClock.getDt()

        if self.visualize:
            self.world.doPhysics(dt)
        else:
            self.world.doPhysics(0.1)

        self.drone.setDeactivationEnabled(False)

        #application of force
        force = Vec3(self.rotorForce[0], self.rotorForce[1],
                     self.rotorForce[2])
        self.drone.applyCentralForce(force)  #should be action

        po = self.drone.transform.pos
        position = np.array([po[0], po[1], po[2]])

        return task.cont

    def step(self, action):

        done = False
        reward = 0

        self.t += 1
        reward = self.getReward()
        self.ep_rew += reward
        state = self.getState()

        basis = np.array([0, 0, 9.81], dtype=np.float)
        self.rotorForce = basis + 0.2 * action  #0.1 *action

        #10 sub steps in each step
        for i in range(5):
            c = taskMgr.step()
            self.rotorForce -= 0.05 * (self.rotorForce - basis)

        #time constraint
        if self.t > 150:
            done = True

        return state, reward, done, {}

    def PlotReward(self):

        c = range(len(self.percentages))
        plt.plot(self.percentMean, c='b', alpha=0.8)
        plt.fill_between(
            c,
            np.array(self.percentMean) + np.array(self.percentStd),
            np.array(self.percentMean) - np.array(self.percentStd),
            color='g',
            alpha=0.3,
            label='Objective 1')

        plt.grid()
        plt.savefig('rews.png')
        plt.close()
Пример #4
0
class Chunk(Entity):
    chunkmass = 5.0

    def __init__(self,
                 world,
                 blocks,
                 pos,
                 hpr=Point3(0, 0, 0),
                 diff=Vec3(0, 0, 0),
                 angVel=Vec3(0, 0, 0),
                 linVel=Vec3(0, 0, 0)):
        super(Chunk, self).__init__()

        self.mass = len(blocks) * Chunk.chunkmass
        self.world = world
        self.blocks = blocks

        self.bnode = BulletRigidBodyNode()

        self.bnode.setMass(self.mass)
        self.bnode.setAngularFactor(Vec3(0, 1, 0))
        self.bnode.setLinearSleepThreshold(20)
        self.bnode.setAngularSleepThreshold(20)
        self.bnode.setAngularVelocity(angVel)

        self.bnode.setLinearVelocity(linVel)
        self.np = utilities.app.render.attachNewNode(self.bnode)
        self.np.setPos(pos.x, 20, pos.y)
        self.np.setHpr(hpr)
        self.np.setPos(self.np, diff)

        self.bnode.setPythonTag("entity", self)

        self.inScope = False  # culling not done yet

        # centre the blocks around the np and add their shapes in.
        self.minMax()
        cog = Point2((self.minX + self.maxX) / 2.0,
                     (self.minY + self.maxY) / 2.0)
        for block in blocks:
            block.rebuild(self, cog)

        world.bw.attachRigidBody(self.bnode)
        self.hitlist = dict()

    def update(self, timer):
        for index in self.hitlist:
            # returns true if the wall is destroyed by the hit
            if self.blocks[index].hitby(self.hitlist[index]):
                self.blocks[index].destroy()
                self.rebuild(index)
        self.hitlist.clear()

        if self.playerDistance() > 40.0:
            self.bnode.setAngularSleepThreshold(1)
            self.bnode.setLinearSleepThreshold(1)
        else:
            self.bnode.setAngularSleepThreshold(0)
            self.bnode.setLinearSleepThreshold(0)

    def playerDistance(self):
        sp = self.np.getPos()
        pp = self.world.player.node.getPos()

        distance = hypot(sp.x - pp.x, sp.z - pp.z)
        return distance

    # remove an element and rebuild
    # TODO add another method for removing multiple
    # blocks in a single go
    def rebuild(self, index):
        deadblock = self.blocks[index]
        out = list()

        for block in self.blocks:
            for edge in block.edges:
                if edge == deadblock:
                    block.edges.remove(edge)

        for block in deadblock.edges:
            chunk = list()
            self.searchBlock(block, chunk, deadblock)
            out.append(chunk)

        #out is a list of lists of populated blocks
        #remove duplicate chunks
        results = list()
        for chunk in out:
            chunk.sort(compareBlock)
            if not chunk in results and len(chunk) > 0:
                results.append(chunk)

        for result in results:
            self.minMax(result)
            #diff = Point2((self.minX+self.maxX) / 2.0, (self.minY+self.maxY) / 2.0)
            diff = Vec3((self.minX + self.maxX) / 2.0, 0,
                        (self.minY + self.maxY) / 2.0)
            p = self.np.getPos()
            pos = Point2(p.x, p.z)
            h = self.np.getHpr()
            self.world.entities.append(
                Chunk(self.world, result, pos, h, diff,
                      self.bnode.getAngularVelocity(),
                      self.bnode.getLinearVelocity()))

        self.destroyNow()
        self.remove = True

    def searchBlock(self, block, lst, deleted):
        if block in lst:
            return
        else:
            lst.append(block)
            for newblock in block.edges:
                self.searchBlock(newblock, lst, deleted)

    def minMax(self, blocks=None):
        if blocks == None:
            blocks = self.blocks

        self.minX = blocks[0].pos.x
        self.minY = blocks[0].pos.y
        self.maxX = blocks[0].pos.x
        self.maxY = blocks[0].pos.y

        for point in blocks:
            self.minX = min(point.pos.x, self.minX)
            self.minY = min(point.pos.y, self.minY)
            self.maxX = max(point.pos.x, self.maxX)
            self.maxY = max(point.pos.y, self.maxY)

    # Need to clear the bulletrigidbody immediately
    # so that the phys object isn't present during next sim phase
    # which occurs before cleanup normally. Otherwise shit will
    # fly everywhere since two things are inside each other
    def destroyNow(self):
        self.world.bw.removeRigidBody(self.bnode)

        return

    # since this is handled earlier nothing happens
    def destroy(self):
        return

    def hitby(self, projectile, index):
        self.hitlist[index] = projectile
Пример #5
0
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
Пример #6
0
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
Пример #7
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)
		
		'''
Пример #8
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)
        '''
Пример #9
0
class Window3D:
    def __init__(self, window_id, rect):
        print("Creating window ID={}".format(window_id))
        self.rect = rect
        self.window_id = window_id
        self.box = WindowBox(globals.front_texture, globals.back_texture)
        self.box.update_vertices(rect, globals.desk_rect)
        x0 = int(rect[0] * 1.1) - 2000 + int(0.7 * rect[2])
        y0 = 1980
        z0 = int(rect[1] * 1.1) - 500
        self.default_position = Point3(x0, y0, z0)
        self.box.node.setTag('wid', str(window_id))

        size = [0.5 * (a - b) for a, b in zip(self.box.mx, self.box.mn)]
        self.shape = BulletBoxShape(Vec3(size[0], size[1], size[2] + 10))
        self.phy_node = BulletRigidBodyNode('window')
        volume = size[0] * size[1] * size[2]
        self.phy_node.setMass(0.001 * volume)
        self.phy_node.addShape(self.shape)
        self.node = globals.gui.render.attachNewNode(self.phy_node)
        self.phy_node.setLinearDamping(0.6)
        self.phy_node.setAngularDamping(0.6)
        globals.gui.world.attachRigidBody(self.phy_node)
        self.node.setCollideMask(BitMask32.bit(1))
        self.box.node.reparentTo(self.node)
        self.box.node.setPos(0, 0, size[2] - 10)
        self.node.setPos(x0, y0, z0)
        self.set_physics(False)

    def reset_position(self):
        self.node.setPos(self.default_position)
        self.node.setHpr(0, 0, 0)
        self.set_physics(False)

    def get_hook_pos(self):
        return self.node.getPos() + self.box.node.getPos()

    def update_rectangle(self, rect):
        self.rect = rect
        self.box.update_vertices(rect, globals.desk_rect)

    def set_physics(self, state):
        if state:
            self.phy_node.setMass(1.0)
        else:
            self.phy_node.setMass(0.0)
            self.phy_node.setLinearVelocity(LVector3(0, 0, 0))
            self.phy_node.setAngularVelocity(LVector3(0, 0, 0))

    def rotate(self, dx, dy):
        dx = norm_rot(dx)
        dy = norm_rot(dy)
        v = self.box.node.getHpr()
        v = v + LVecBase3(dx, dy, 0)
        self.box.node.setHpr(v)

    def move(self, delta):
        v = self.node.getPos()
        for i in range(len(delta)):
            v[i] = v[i] + delta[i]
        self.node.setPos(v)
        # self.phy_node.setAngularVelocity(LVector3(0,0,0))

    def close(self):
        self.box.close()

    def calc_desktop_coords(self, v):
        if v[2] > 0:
            return None
        x = int(self.rect[0] + (0.5 * self.rect[2] + v[0]))
        y = int(self.rect[1] - v[2])
        return x, y
Пример #10
0
class Player(Entity):

    walkspeed = 5
    damping = 0.9
    topspeed = 15

    leftMove = False
    rightMove = False
    jumpToggle = False
    crouchToggle = False

    def __init__(self, world):
        super(Player, self).__init__()

        self.obj = utilities.loadObject("tdplayer", depth=20)

        self.world = world 
        self.health = 100
        self.inventory = list()

        self.depth = self.obj.getPos().y

        self.location = Point2(10,0)
        self.velocity = Vec3(0)

        self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49))
        self.bnode = BulletRigidBodyNode('Box')
        self.bnode.setMass(0.1)
        self.bnode.setAngularVelocity(Vec3(0))
        self.bnode.setAngularFactor(Vec3(0))
        self.bnode.addShape(self.shape)
        self.bnode.setLinearDamping(0.95)
        self.bnode.setLinearSleepThreshold(0)

        world.bw.attachRigidBody(self.bnode)
        self.bnode.setPythonTag("entity", self)
        self.bnode.setIntoCollideMask(BitMask32.bit(0))

        self.node = utilities.app.render.attachNewNode(self.bnode)
        self.node.setPos(self.obj.getPos())

        self.obj.setPos(0,0,0)
        self.obj.setScale(1)
        self.obj.reparentTo(self.node)
        self.node.setPos(self.location.x, self.depth, self.location.y)

    def initialise(self):
        self.inventory.append(LightLaser(self.world, self))
        self.inventory.append(Blowtorch(self.world, self))
        #self.inventory["Grenade"] = Grenade(self.world, self)

        for item in self.inventory:
            item.initialise()

        self.currentItemIndex = 1
        self.currentItem = self.inventory[self.currentItemIndex]
        self.currentItem.equip()

    def activate(self):
        self.currentItem.activate()

    def update(self, timer):
        self.velocity = self.bnode.getLinearVelocity()

        if (self.leftMove):
            self.bnode.applyCentralForce(Vec3(-Player.walkspeed,0,0))
        if (self.rightMove):
            self.bnode.applyCentralForce(Vec3(Player.walkspeed,0,0))
        if (self.jumpToggle):
            self.bnode.applyCentralForce(Vec3(0,0,Player.walkspeed))
        if (self.crouchToggle):
            self.bnode.applyCentralForce(Vec3(0,0,-Player.walkspeed))
        
        if (self.velocity.x < -self.topspeed):
            self.velocity.x = -self.topspeed
        if (self.velocity.x > self.topspeed):
            self.velocity.x = self.topspeed

        mouse = utilities.app.mousePos
        # extrude test
        near = Point3()
        far = Point3()
        utilities.app.camLens.extrude(mouse, near, far)
        camp = utilities.app.camera.getPos()
        near *= 20 # player depth

        if near.x != 0:
            angle = atan2(near.z + camp.z - self.node.getPos().z, near.x + camp.x - self.node.getPos().x)
            #angle = atan2(near.z, near.x)
        else:
            angle = 90  

        self.angle = angle

        # set current item to point at cursor   
        self.currentItem.update(timer)

        # move the camera so the player is centred horizontally,
        # but keep the camera steady vertically
        utilities.app.camera.setPos(self.node.getPos().x, 0, self.node.getPos().z)

        self.obj.setHpr(0, 0, -1 * degrees(angle))
        self.location.x = self.node.getPos().x
        self.location.y = self.node.getPos().z

    def moveLeft(self, switch):
        self.leftMove = switch 

    def moveRight(self, switch):
        self.rightMove = switch 

    def jump(self, switch):
        self.jumpToggle = switch

    def crouch(self, switch):
        self.crouchToggle = switch

    def scrollItem(self, number):
        self.currentItem.unequip()
        self.currentItemIndex = self.currentItemIndex + number
        if self.currentItemIndex < 0:
            self.currentItemIndex = len(self.inventory) - 1 
        if self.currentItemIndex > len(self.inventory) - 1:
            self.currentItemIndex = 0

        self.currentItem = self.inventory[self.currentItemIndex]
        self.currentItem.equip()

    def selectItem(self, number):
        if (number - 1 < len(self.inventory) and number - 1 >= 0):
            self.currentItem.unequip()
            self.currentItemIndex = number - 1
            self.currentItem = self.inventory[self.currentItemIndex]
            self.currentItem.equip()

    def hitby(self, projectile, index):
        self.health -= projectile.damage 
        if (self.health < 0):
            self.obj.setColor(1,0,0,1)
            return True
        greyscale  = 0.3 + (0.01 * self.health)
        self.obj.setColor(greyscale, greyscale,greyscale,greyscale)
        return False
Пример #11
0
class Catcher(Enemy):
    damage = 25

    def __init__(self, location, player, cmap, world):
        super(Catcher, self).__init__(location)
        self.player = player
        self.cmap = cmap

        self.obj = utilities.loadObject("robot", depth=20)

        self.world = world 
        self.health = 100

        self.depth = self.obj.getPos().y

        self.location = location 
        self.velocity = Vec3(0)

        self.shape = BulletBoxShape(Vec3(0.5, 1.0, 0.5))
        self.bnode = BulletRigidBodyNode('Box')
        self.bnode.setMass(0.1)
        self.bnode.setAngularVelocity(Vec3(0))
        self.bnode.setAngularFactor(Vec3(0))
        self.bnode.addShape(self.shape)
        self.bnode.setLinearDamping(0.75)
        self.bnode.setLinearSleepThreshold(0)

        world.bw.attachRigidBody(self.bnode)
        self.bnode.setPythonTag("entity", self)
        self.bnode.setIntoCollideMask(BitMask32.bit(0))

        self.node = utilities.app.render.attachNewNode(self.bnode)
        self.node.setPos(self.obj.getPos())

        self.obj.setPos(0,0,0)
        self.obj.setScale(1)
        self.obj.reparentTo(self.node)
        self.node.setPos(self.location.x, self.depth, self.location.y)

    def update(self, time):
         self.location = Point2(self.node.getPos().x - self.player.location.x, self.node.getPos().z - self.player.location.y)

         if self.location.x > 20 or self.location.x < -20:
             return
         if self.location.y > 20 or self.location.y < -20:
             return
         path = findPath(Point2(self.location + Point2(20,20)), Point2(20,20), self.world.cmap)
         if len(path) > 1:
            move = path[1] - self.location
            self.bnode.applyCentralForce(Vec3(move.x-20,0,move.y-20))
    
    def hitby(self, projectile, index):
        self.health -= projectile.damage 
        if (self.health < 0):
            self.remove = True
        greyscale  = 0.3 + (0.01 * self.health)
        self.obj.setColor(1, greyscale,greyscale,1)
        return False

    def removeOnHit(self):
        return

    def destroy(self):
        self.obj.hide()
        self.world.bw.removeRigidBody(self.bnode)
Пример #12
0
class Catcher(Enemy):
    damage = 25

    def __init__(self, location, player, cmap, world):
        super(Catcher, self).__init__(location)
        self.player = player
        self.cmap = cmap

        self.obj = utilities.loadObject("robot", depth=20)

        self.world = world
        self.health = 100

        self.depth = self.obj.getPos().y

        self.location = location
        self.velocity = Vec3(0)

        self.shape = BulletBoxShape(Vec3(0.5, 1.0, 0.5))
        self.bnode = BulletRigidBodyNode('Box')
        self.bnode.setMass(0.1)
        self.bnode.setAngularVelocity(Vec3(0))
        self.bnode.setAngularFactor(Vec3(0))
        self.bnode.addShape(self.shape)
        self.bnode.setLinearDamping(0.75)
        self.bnode.setLinearSleepThreshold(0)

        world.bw.attachRigidBody(self.bnode)
        self.bnode.setPythonTag("entity", self)
        self.bnode.setIntoCollideMask(BitMask32.bit(0))

        self.node = utilities.app.render.attachNewNode(self.bnode)
        self.node.setPos(self.obj.getPos())

        self.obj.setPos(0, 0, 0)
        self.obj.setScale(1)
        self.obj.reparentTo(self.node)
        self.node.setPos(self.location.x, self.depth, self.location.y)

    def update(self, time):
        self.location = Point2(self.node.getPos().x - self.player.location.x,
                               self.node.getPos().z - self.player.location.y)

        if self.location.x > 20 or self.location.x < -20:
            return
        if self.location.y > 20 or self.location.y < -20:
            return
        path = findPath(Point2(self.location + Point2(20, 20)), Point2(20, 20),
                        self.world.cmap)
        if len(path) > 1:
            move = path[1] - self.location
            self.bnode.applyCentralForce(Vec3(move.x - 20, 0, move.y - 20))

    def hitby(self, projectile, index):
        self.health -= projectile.damage
        if (self.health < 0):
            self.remove = True
        greyscale = 0.3 + (0.01 * self.health)
        self.obj.setColor(1, greyscale, greyscale, 1)
        return False

    def removeOnHit(self):
        return

    def destroy(self):
        self.obj.hide()
        self.world.bw.removeRigidBody(self.bnode)
Пример #13
0
class Player(Entity):

    walkspeed = 5
    damping = 0.9
    topspeed = 15

    leftMove = False
    rightMove = False
    jumpToggle = False
    crouchToggle = False

    def __init__(self, world):
        super(Player, self).__init__()

        self.obj = utilities.loadObject("tdplayer", depth=20)

        self.world = world
        self.health = 100
        self.inventory = list()

        self.depth = self.obj.getPos().y

        self.location = Point2(10, 0)
        self.velocity = Vec3(0)

        self.shape = BulletBoxShape(Vec3(0.3, 1.0, 0.49))
        self.bnode = BulletRigidBodyNode('Box')
        self.bnode.setMass(0.1)
        self.bnode.setAngularVelocity(Vec3(0))
        self.bnode.setAngularFactor(Vec3(0))
        self.bnode.addShape(self.shape)
        self.bnode.setLinearDamping(0.95)
        self.bnode.setLinearSleepThreshold(0)

        world.bw.attachRigidBody(self.bnode)
        self.bnode.setPythonTag("entity", self)
        self.bnode.setIntoCollideMask(BitMask32.bit(0))

        self.node = utilities.app.render.attachNewNode(self.bnode)
        self.node.setPos(self.obj.getPos())

        self.obj.setPos(0, 0, 0)
        self.obj.setScale(1)
        self.obj.reparentTo(self.node)
        self.node.setPos(self.location.x, self.depth, self.location.y)

    def initialise(self):
        self.inventory.append(LightLaser(self.world, self))
        self.inventory.append(Blowtorch(self.world, self))
        #self.inventory["Grenade"] = Grenade(self.world, self)

        for item in self.inventory:
            item.initialise()

        self.currentItemIndex = 1
        self.currentItem = self.inventory[self.currentItemIndex]
        self.currentItem.equip()

    def activate(self):
        self.currentItem.activate()

    def update(self, timer):
        self.velocity = self.bnode.getLinearVelocity()

        if (self.leftMove):
            self.bnode.applyCentralForce(Vec3(-Player.walkspeed, 0, 0))
        if (self.rightMove):
            self.bnode.applyCentralForce(Vec3(Player.walkspeed, 0, 0))
        if (self.jumpToggle):
            self.bnode.applyCentralForce(Vec3(0, 0, Player.walkspeed))
        if (self.crouchToggle):
            self.bnode.applyCentralForce(Vec3(0, 0, -Player.walkspeed))

        if (self.velocity.x < -self.topspeed):
            self.velocity.x = -self.topspeed
        if (self.velocity.x > self.topspeed):
            self.velocity.x = self.topspeed

        mouse = utilities.app.mousePos
        # extrude test
        near = Point3()
        far = Point3()
        utilities.app.camLens.extrude(mouse, near, far)
        camp = utilities.app.camera.getPos()
        near *= 20  # player depth

        if near.x != 0:
            angle = atan2(near.z + camp.z - self.node.getPos().z,
                          near.x + camp.x - self.node.getPos().x)
            #angle = atan2(near.z, near.x)
        else:
            angle = 90

        self.angle = angle

        # set current item to point at cursor
        self.currentItem.update(timer)

        # move the camera so the player is centred horizontally,
        # but keep the camera steady vertically
        utilities.app.camera.setPos(self.node.getPos().x, 0,
                                    self.node.getPos().z)

        self.obj.setHpr(0, 0, -1 * degrees(angle))
        self.location.x = self.node.getPos().x
        self.location.y = self.node.getPos().z

    def moveLeft(self, switch):
        self.leftMove = switch

    def moveRight(self, switch):
        self.rightMove = switch

    def jump(self, switch):
        self.jumpToggle = switch

    def crouch(self, switch):
        self.crouchToggle = switch

    def scrollItem(self, number):
        self.currentItem.unequip()
        self.currentItemIndex = self.currentItemIndex + number
        if self.currentItemIndex < 0:
            self.currentItemIndex = len(self.inventory) - 1
        if self.currentItemIndex > len(self.inventory) - 1:
            self.currentItemIndex = 0

        self.currentItem = self.inventory[self.currentItemIndex]
        self.currentItem.equip()

    def selectItem(self, number):
        if (number - 1 < len(self.inventory) and number - 1 >= 0):
            self.currentItem.unequip()
            self.currentItemIndex = number - 1
            self.currentItem = self.inventory[self.currentItemIndex]
            self.currentItem.equip()

    def hitby(self, projectile, index):
        self.health -= projectile.damage
        if (self.health < 0):
            self.obj.setColor(1, 0, 0, 1)
            return True
        greyscale = 0.3 + (0.01 * self.health)
        self.obj.setColor(greyscale, greyscale, greyscale, greyscale)
        return False