Ejemplo n.º 1
0
    def setup(self):
        self.worldNP = render.attach_new_node('World')

        # World
        self.debugNP = self.worldNP.attach_new_node(BulletDebugNode('Debug'))
        self.debugNP.show()
        self.debugNP.node().show_wireframe(True)
        self.debugNP.node().show_constraints(True)
        self.debugNP.node().show_bounding_boxes(False)
        self.debugNP.node().show_normals(False)

        self.world = BulletWorld()
        self.world.set_gravity(LVector3(0, 0, -9.81))
        self.world.set_debug_node(self.debugNP.node())

        # Box A
        shape = BulletBoxShape(LVector3(0.5, 0.5, 0.5))

        bodyA = BulletRigidBodyNode('Box A')
        bodyNP = self.worldNP.attach_new_node(bodyA)
        bodyNP.node().add_shape(shape)
        bodyNP.set_collide_mask(BitMask32.all_on())
        bodyNP.set_pos(-3, 0, 4)

        visNP = loader.load_model('models/box.egg')
        visNP.clear_model_nodes()
        visNP.reparent_to(bodyNP)

        self.world.attach(bodyA)

        # Box B
        shape = BulletBoxShape(LVector3(0.5, 0.5, 0.5))

        bodyB = BulletRigidBodyNode('Box B')
        bodyNP = self.worldNP.attach_new_node(bodyB)
        bodyNP.node().add_shape(shape)
        bodyNP.node().set_mass(1.0)
        bodyNP.node().set_deactivation_enabled(False)
        bodyNP.set_collide_mask(BitMask32.all_on())
        bodyNP.set_pos(0, 0, 0)

        visNP = loader.load_model('models/box.egg')
        visNP.clear_model_nodes()
        visNP.reparent_to(bodyNP)

        self.world.attach(bodyB)

        # Slider
        frameA = TransformState.make_pos_hpr(LPoint3(2, 0, 0),
                                             LVector3(0, 0, 45))
        frameB = TransformState.make_pos_hpr(LPoint3(0, -3, 0),
                                             LVector3(0, 0, 0))

        slider = BulletSliderConstraint(bodyA, bodyB, frameA, frameB, True)
        slider.set_debug_draw_size(2.0)
        slider.set_lower_linear_limit(0)
        slider.set_upper_linear_limit(6)
        slider.set_lower_angular_limit(-60)
        slider.set_upper_angular_limit(60)
        self.world.attach(slider)
Ejemplo n.º 2
0
class Simulation(ShowBase):
    scale = 1
    height = 500
    lateralError = 200
    dt = 0.1
    steps = 0
    #Test Controllers
    fuelPID = PID(10, 0.5, 10, 0, 100)
    heightPID = PID(0.08, 0, 0.3, 0.1, 1)
    pitchPID = PID(10, 0, 1000, -10, 10)
    rollPID = PID(10, 0, 1000, -10, 10)
    XPID = PID(0.2, 0, 0.8, -10, 10)
    YPID = PID(0.2, 0, 0.8, -10, 10)
    vulcain = NeuralNetwork()
    tau = 0.5
    Valves=[]

    CONTROL = False

    EMPTY = False
    LANDED = False
    DONE = False

    gimbalX = 0
    gimbalY = 0
    targetAlt = 150

    R = RE(200 * 9.806, 250 * 9.806, 7607000 / 9 * scale, 0.4)
    throttle = 0.0
    fuelMass_full = 417000 * scale
    fuelMass_init = 0.10

    radius = 1.8542 * scale
    length = 47 * scale
    Cd = 1.5


    def __init__(self, VISUALIZE=False):

        self.VISUALIZE = VISUALIZE


        if VISUALIZE is True:
            ShowBase.__init__(self)
            self.setBackgroundColor(0.2, 0.2, 0.2, 1)
            self.setFrameRateMeter(True)
            self.render.setShaderAuto()
            self.cam.setPos(-120 * self.scale, -120 * self.scale, 120 * self.scale)
            self.cam.lookAt(0, 0, 10 * self.scale)
            alight = AmbientLight('ambientLight')
            alight.setColor(Vec4(0.5, 0.5, 0.5, 1))
            alightNP = self.render.attachNewNode(alight)

            dlight = DirectionalLight('directionalLight')
            dlight.setDirection(Vec3(1, -1, -1))
            dlight.setColor(Vec4(0.7, 0.7, 0.7, 1))
            dlightNP = self.render.attachNewNode(dlight)

            self.render.clearLight()
            self.render.setLight(alightNP)
            self.render.setLight(dlightNP)

            self.accept('escape', self.doExit)
            self.accept('r', self.doReset)
            self.accept('f1', self.toggleWireframe)
            self.accept('f2', self.toggleTexture)
            self.accept('f3', self.toggleDebug)
            self.accept('f5', self.doScreenshot)

            inputState.watchWithModifiers('forward', 'w')
            inputState.watchWithModifiers('left', 'a')
            inputState.watchWithModifiers('reverse', 's')
            inputState.watchWithModifiers('right', 'd')
            inputState.watchWithModifiers('turnLeft', 'q')
            inputState.watchWithModifiers('turnRight', 'e')
            self.ostData = OnscreenText(text='ready', pos=(-1.3, 0.9), scale=0.07, fg=Vec4(1, 1, 1, 1),
            align=TextNode.ALeft)

        self.fuelMass = self.fuelMass_full * self.fuelMass_init
        self.vulcain.load_existing_model(path="LandingRockets/",model_name="140k_samples_1024neurons_3layers_l2-0.000001")



        # Physics
        self.setup()

    # _____HANDLER_____

    def doExit(self):
        self.cleanup()
        sys.exit(1)

    def doReset(self):
        self.cleanup()
        self.setup()

    def toggleWireframe(self):
        ...#self.toggleWireframe()

    def toggleTexture(self):
        ...#self.toggleTexture()

    def toggleDebug(self):
        if self.debugNP.isHidden():
            self.debugNP.show()
        else:
            self.debugNP.hide()

    def doScreenshot(self):
        self.screenshot('Bullet')

    # ____TASK___

    def processInput(self):
        throttleChange = 0.0

        if inputState.isSet('forward'): self.gimbalY = 10
        if inputState.isSet('reverse'): self.gimbalY = -10
        if inputState.isSet('left'):    self.gimbalX = 10
        if inputState.isSet('right'):   self.gimbalX = -10
        if inputState.isSet('turnLeft'):  throttleChange = -1.0
        if inputState.isSet('turnRight'): throttleChange = 1.0

        self.throttle += throttleChange / 100.0
        self.throttle = min(max(self.throttle, 0), 1)

    def processContacts(self):
        result = self.world.contactTestPair(self.rocketNP.node(), self.groundNP.node())
        #print(result.getNumContacts())
        if result.getNumContacts() != 0:
            self.LANDED = True
            #self.DONE = True

    def update(self,task):

        #self.control(0.1,0.1,0.1)
        #self.processInput()
        #self.processContacts()
        # self.terrain.update()
        return task.cont

    def cleanup(self):
        self.world.removeRigidBody(self.groundNP.node())
        self.world.removeRigidBody(self.rocketNP.node())
        self.world = None

        self.debugNP = None
        self.groundNP = None
        self.rocketNP = None

        self.worldNP.removeNode()

        self.LANDED = False
        self.EMPTY = False
        self.DONE = False
        self.steps = 0
        self.fuelMass = self.fuelMass_full*self.fuelMass_init

    def setup(self):

        self.targetAlt = r.randrange(100,300)
        self.Valves = np.array([0.15,0.2,0.15])
        self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1))

        if self.VISUALIZE is True:
            self.worldNP = self.render.attachNewNode('World')

        else:
            self.root = NodePath(PandaNode("world root"))
            self.worldNP = self.root.attachNewNode('World')






        self.world = BulletWorld()
        self.world.setGravity(Vec3(0, 0, -9.80665))

        # World
        self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug'))
        self.debugNP.node().showWireframe(True)
        self.debugNP.node().showConstraints(True)
        self.debugNP.node().showBoundingBoxes(False)
        self.debugNP.node().showNormals(True)
        self.debugNP.show()
        self.world.setDebugNode(self.debugNP.node())

        # self.debugNP.showTightBounds()
        # self.debugNP.showBounds()


        # Ground (static)
        shape = BulletPlaneShape(Vec3(0, 0, 1), 1)

        self.groundNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Ground'))
        self.groundNP.node().addShape(shape)
        self.groundNP.setPos(0, 0, 0)
        self.groundNP.setCollideMask(BitMask32.allOn())

        self.world.attachRigidBody(self.groundNP.node())

        # Rocket
        shape = BulletCylinderShape(self.radius, self.length, ZUp)

        self.rocketNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cylinder'))
        self.rocketNP.node().setMass(27200 * self.scale)
        self.rocketNP.node().addShape(shape)
        #self.rocketNP.setPos(20,20,250)
        self.rocketNP.setPos(20, 20, r.randrange(100, 300)+200)
        #self.rocketNP.setPos(r.randrange(-self.lateralError, self.lateralError, 1), r.randrange(-self.lateralError, self.lateralError, 1), self.height)
        # self.rocketNP.setPos(0, 0, self.length*10)
        self.rocketNP.setCollideMask(BitMask32.allOn())
        # self.rocketNP.node().setCollisionResponse(0)
        self.rocketNP.node().notifyCollisions(True)

        self.world.attachRigidBody(self.rocketNP.node())

        for i in range(4):
            leg = BulletCylinderShape(0.1 * self.radius, 0.5 * self.length, XUp)
            self.rocketNP.node().addShape(leg, TransformState.makePosHpr(
                Vec3(6 * self.radius * math.cos(i * math.pi / 2), 6 * self.radius * math.sin(i * math.pi / 2),
                     -0.6 * self.length), Vec3(i * 90, 0, 30)))

        shape = BulletConeShape(0.75 * self.radius, 0.5 * self.radius, ZUp)
        self.rocketNP.node().addShape(shape, TransformState.makePosHpr(Vec3(0, 0, -1 / 2 * self.length), Vec3(0, 0, 0)))

        # Fuel
        self.fuelRadius = 0.9 * self.radius
        shape = BulletCylinderShape(self.fuelRadius, 0.01 * self.length, ZUp)
        self.fuelNP = self.worldNP.attachNewNode(BulletRigidBodyNode('Cone'))
        self.fuelNP.node().setMass(self.fuelMass_full * self.fuelMass_init)
        self.fuelNP.node().addShape(shape)
        self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.5 * (1 - self.fuelMass_init))
        self.fuelNP.setCollideMask(BitMask32.allOn())
        self.fuelNP.node().setCollisionResponse(0)

        self.world.attachRigidBody(self.fuelNP.node())

        frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90))
        frameB = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90))

        self.fuelSlider = BulletSliderConstraint(self.rocketNP.node(), self.fuelNP.node(), frameA, frameB, 1)
        self.fuelSlider.setTargetLinearMotorVelocity(0)
        self.fuelSlider.setDebugDrawSize(2.0)
        self.fuelSlider.set_lower_linear_limit(0)
        self.fuelSlider.set_upper_linear_limit(0)
        self.world.attachConstraint(self.fuelSlider)

        self.npThrustForce = LineNodePath(self.rocketNP, 'Thrust', thickness=4, colorVec=Vec4(1, 0.5, 0, 1))
        self.npDragForce = LineNodePath(self.rocketNP, 'Drag', thickness=4, colorVec=Vec4(1, 0, 0, 1))
        self.npLiftForce = LineNodePath(self.rocketNP, 'Lift', thickness=4, colorVec=Vec4(0, 0, 1, 1))
        self.npFuelState = LineNodePath(self.fuelNP, 'Fuel', thickness=20, colorVec=Vec4(0, 1, 0, 1))

        self.rocketCSLon = self.radius ** 2 * math.pi
        self.rocketCSLat = self.length * 2 * self.radius

        if self.VISUALIZE is True:
            self.terrain = loader.loadModel("LZGrid2.egg")
            self.terrain.setScale(10)
            self.terrain.reparentTo(self.render)
            self.terrain.setColor(Vec4(0.1, 0.2, 0.1, 1))
            self.toggleTexture()

        #self.fuelNP.setPos(0, 0, self.rocketNP.getPos().getZ() - self.length * 0.4 * (1 - self.fuelMass_init))

        for i in range(5):
            self.world.doPhysics(self.dt, 5, 1.0 / 180.0)

        self.fuelSlider.set_lower_linear_limit(-self.length * 0.5 * (1 - self.fuelMass_init))
        self.fuelSlider.set_upper_linear_limit(self.length * 0.5 * (1 - self.fuelMass_init))

        for i in range(100):
            self.world.doPhysics(self.dt, 5, 1.0 / 180.0)
            self.rocketNP.node().applyForce(Vec3(0,0,300000), Vec3(0, 0, 0))








    def updateRocket(self, mdot, dt):

        # Fuel Update
        self.fuelMass = self.fuelNP.node().getMass() - dt * mdot
        if self.fuelMass <= 0:
            self.EMPTY is True
        fuel_percent = self.fuelMass / self.fuelMass_full
        self.fuelNP.node().setMass(self.fuelMass)
        fuelHeight = self.length * fuel_percent
        I1 = 1 / 2 * self.fuelMass * self.fuelRadius ** 2
        I2 = 1 / 4 * self.fuelMass * self.fuelRadius ** 2 + 1 / 12 * self.fuelMass * fuelHeight * fuelHeight
        self.fuelNP.node().setInertia(Vec3(I2, I2, I1))

        # Shift fuel along slider constraint
        fuelTargetPos = 0.5 * (self.length - fuelHeight)
        fuelPos = self.fuelSlider.getLinearPos()
        self.fuelSlider.set_upper_linear_limit(fuelTargetPos)
        self.fuelSlider.set_lower_linear_limit(-fuelTargetPos)

        self.npFuelState.reset()
        self.npFuelState.drawArrow2d(Vec3(0, 0, -0.5 * fuelHeight), Vec3(0, 0, 0.5 * fuelHeight), 45, 2)
        self.npFuelState.create()

    def observe(self):
        pos = self.rocketNP.getPos()
        vel = self.rocketNP.node().getLinearVelocity()
        quat = self.rocketNP.getTransform().getQuat()
        Roll, Pitch, Yaw = quat.getHpr()
        rotVel = self.rocketNP.node().getAngularVelocity()
        offset = self.targetAlt-pos.getZ()

        return pos, vel, Roll, Pitch, Yaw, rotVel, self.fuelMass / self.fuelMass_full, self.EMPTY, self.DONE, self.LANDED, offset, self.EngObs[0], self.Valves

    def control(self,ValveCommands):

        self.gimbalX = 0
        self.gimbalY = 0

        self.Valves = ValveCommands-(ValveCommands-self.Valves)*np.exp(-self.dt/self.tau)

        self.EngObs = self.vulcain.predict_data_point(np.array(self.Valves).reshape(1,-1 ))
        #Brennkammerdruck, Gaskammertemp, H2Massenstrom, LOX MAssentrom, Schub
        #Bar,K,Kg/s,Kg/s,kN
        #self.dt = globalClock.getDt()

        pos = self.rocketNP.getPos()
        vel = self.rocketNP.node().getLinearVelocity()
        quat = self.rocketNP.getTransform().getQuat()
        Roll, Pitch, Yaw = quat.getHpr()
        rotVel = self.rocketNP.node().getAngularVelocity()

        # CHECK STATE
        if self.fuelMass <= 0:
            self.EMPTY = True
        #if pos.getZ() <= 36:
        #    self.LANDED = True
        self.LANDED = False
        self.processContacts()

        P, T, rho = air_dens(pos[2])
        rocketZWorld = quat.xform(Vec3(0, 0, 1))

        AoA = math.acos(min(max(dot(norm(vel), norm(-rocketZWorld)), -1), 1))

        dynPress = 0.5 * dot(vel, vel) * rho

        dragArea = self.rocketCSLon + (self.rocketCSLat - self.rocketCSLon) * math.sin(AoA)

        drag = norm(-vel) * dynPress * self.Cd * dragArea

        time = globalClock.getFrameTime()

        liftVec = norm(vel.project(rocketZWorld) - vel)
        if AoA > 0.5 * math.pi:
            liftVec = -liftVec
        lift = liftVec * (math.sin(AoA * 2) * self.rocketCSLat * dynPress)

        if self.CONTROL:
            self.throttle = self.heightPID.control(pos.getZ(), vel.getZ(), 33)

            pitchTgt = self.XPID.control(pos.getX(), vel.getX(), 0)
            self.gimbalX = -self.pitchPID.control(Yaw, rotVel.getY(), pitchTgt)

            rollTgt = self.YPID.control(pos.getY(), vel.getY(), 0)
            self.gimbalY = -self.rollPID.control(Pitch, rotVel.getX(), -rollTgt)



        self.thrust = self.EngObs[0][4]*1000
        #print(self.EngObs)
        quat = self.rocketNP.getTransform().getQuat()
        quatGimbal = TransformState.makeHpr(Vec3(0, self.gimbalY, self.gimbalX)).getQuat()
        thrust = quatGimbal.xform(Vec3(0, 0, self.thrust))
        thrustWorld = quat.xform(thrust)

        #print(thrustWorld)

        self.npDragForce.reset()
        self.npDragForce.drawArrow2d(Vec3(0, 0, 0), quat.conjugate().xform(drag) / 1000, 45, 2)
        self.npDragForce.create()

        self.npLiftForce.reset()
        self.npLiftForce.drawArrow2d(Vec3(0, 0, 0), quat.conjugate().xform(lift) / 1000, 45, 2)
        self.npLiftForce.create()

        self.npThrustForce.reset()
        if self.EMPTY is False & self.LANDED is False:
            self.npThrustForce.drawArrow2d(Vec3(0, 0, -0.5 * self.length),
                                           Vec3(0, 0, -0.5 * self.length) - thrust / 30000, 45, 2)
            self.npThrustForce.create()

        self.rocketNP.node().applyForce(drag, Vec3(0, 0, 0))
        self.rocketNP.node().applyForce(lift, Vec3(0, 0, 0))
        #print(self.EMPTY,self.LANDED)
        if self.EMPTY is False & self.LANDED is False:
            self.rocketNP.node().applyForce(thrustWorld, quat.xform(Vec3(0, 0, -1 / 2 * self.length)))
            self.updateRocket(self.EngObs[0][2]+self.EngObs[0][3], self.dt)
        self.rocketNP.node().setActive(True)
        self.fuelNP.node().setActive(True)

        self.processInput()

        self.world.doPhysics(self.dt)
        self.steps+=1


        if self.steps > 1000:
            self.DONE = True

        telemetry = []

        telemetry.append('Thrust: {}'.format(int(self.EngObs[0][4])))
        telemetry.append('Fuel: {}%'.format(int(self.fuelMass / self.fuelMass_full * 100.0)))
        telemetry.append('Gimbal: {}'.format(int(self.gimbalX)) + ',{}'.format(int(self.gimbalY)))
        telemetry.append('AoA: {}'.format(int(AoA / math.pi * 180.0)))
        telemetry.append('\nPos: {},{}'.format(int(pos.getX()), int(pos.getY())))
        telemetry.append('Height: {}'.format(int(pos.getZ())))
        telemetry.append('RPY: {},{},{}'.format(int(Roll), int(Pitch), int(Yaw)))
        telemetry.append('Speed: {}'.format(int(np.linalg.norm(vel))))
        telemetry.append('Vel: {},{},{}'.format(int(vel.getX()), int(vel.getY()), int(vel.getZ())))
        telemetry.append('Rot: {},{},{}'.format(int(rotVel.getX()), int(rotVel.getY()), int(rotVel.getZ())))
        telemetry.append('LANDED: {}'.format(self.LANDED))
        telemetry.append('Time: {}'.format(self.steps*self.dt))
        telemetry.append('TARGET: {}'.format(self.targetAlt))
        #print(pos)
        if self.VISUALIZE is True:
            self.ostData.setText('\n'.join(telemetry))
            self.cam.setPos(pos[0] - 120 * self.scale, pos[1] - 120 * self.scale, pos[2] + 80 * self.scale)
            self.cam.lookAt(pos[0], pos[1], pos[2])
            self.taskMgr.step()


    """