コード例 #1
0
    def __init__(self, dae, nk=None, nicp=1, deg=4, collPoly='RADAU'):
        assert nk is not None
        assert isinstance(dae, Dae)

        self.dae = dae

        self.nk = nk
        self.nicp = nicp
        self.deg = deg
        self.collPoly = collPoly

        self._bounds = BoundsMap(self,"bounds")
        self._guess = collmaps.WriteableCollMap(self,"guess")

        self._constraints = Constraints()

        # setup NLP variables
        self._dvMap = collmaps.VectorizedReadOnlyCollMap(self,"design var map",CS.msym("V",self.getNV()))

        # quadratures
        self._quadratureManager = collmaps.QuadratureManager(self)
コード例 #2
0
def getSteadyState(dae,conf,omega0,r0,ref_dict=None):
    if ref_dict is None:
        ref_dict = {}
    # make steady state model
    g = Constraints()
    g.add(dae.getResidual(),'==',0,tag=('dae residual',None))
    def constrainInvariantErrs():
        R_c2b = dae['R_c2b']
        makeOrthonormal(g, R_c2b)
        g.add(dae['c'], '==', 0, tag=('c(0)==0',None))
        g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0',None))
    constrainInvariantErrs()

    # Rotational velocity time derivative
    g.add(C.mul(dae['R_c2b'].T,dae['w_bn_b']) - C.veccat([0,0,omega0]) , '==', 0, tag=
                       ("Rotational velocities",None))

    for name in ['alpha_deg','beta_deg','cL']:
        if name in ref_dict:
            g.addBnds(dae[name], ref_dict[name], tag=(name,None))

    dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
    obj = 0
    for name in ['aileron','elevator','rudder','flaps']:
        if name in dae:
            obj += dae[name]**2
    ffcn = C.SXFunction([dvs],[obj])
    gfcn = C.SXFunction([dvs],[g.getG()])
    ffcn.init()
    gfcn.init()

    guess = {'x':r0,'y':0,'z':0,
             'r':r0,'dr':0,
             'e11':0, 'e12':-1, 'e13':0,
             'e21':0, 'e22':0, 'e23':1,
             'e31':-1, 'e32':0, 'e33':0,
             'dx':0,'dy':0,'dz':0,
             'w_bn_b_x':0,'w_bn_b_y':omega0,'w_bn_b_z':0,
             'ddelta':omega0,
             'cos_delta':1,'sin_delta':0,
             'aileron':0,'elevator':0,'rudder':0,'flaps':0,
             'daileron':0,'delevator':0,'drudder':0,'dflaps':0,
             'nu':100,'motor_torque':0,
             'dmotor_torque':0,'ddr':0,
             'dddr':0.0,'w0':0.0}
    dotGuess = {'x':0,'y':0,'z':0,'dx':0,'dy':0,'dz':0,
                'r':0,'dr':0,
                'e11':0,'e12':0,'e13':0,
                'e21':0,'e22':0,'e23':0,
                'e31':0,'e32':0,'e33':0,
                'w_bn_b_x':0,'w_bn_b_y':0,'w_bn_b_z':0,
                'ddelta':0,
                'cos_delta':0,'sin_delta':omega0,
                'aileron':0,'elevator':0,'rudder':0,'flaps':0,
                'motor_torque':0,'ddr':0}

    guessVec = C.DMatrix([guess[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+
                         [dotGuess[n] for n in dae.xNames()])

    bounds = {'x':(-0.1*r0,r0*2),'y':(-1.1*r0,1.1*r0),'z':(-1.1*r0,1.1*r0),
             'dx':(0,0),'dy':(0,0),'dz':(0,0),
             'r':(r0,r0),'dr':(-100,100),
             'e11':(-2,2),'e12':(-2,2),'e13':(-2,2),
             'e21':(-2,2),'e22':(-2,2),'e23':(-2,2),
             'e31':(-2,2),'e32':(-2,2),'e33':(-2,2),
             'w_bn_b_x':(-50,50),'w_bn_b_y':(-50,50),'w_bn_b_z':(-50,50),
             'ddelta':(omega0,omega0),
             'cos_delta':(1,1),'sin_delta':(0,0),
             'aileron':(-0.2,0.2),'elevator':(-0.2,0.2),'rudder':(-0.2,0.2),'flaps':(-0.2,0.2),
             'daileron':(0,0),'delevator':(0,0),'drudder':(0,0),'dflaps':(0,0),
             'nu':(0,3000),'motor_torque':(-1000,1000),
             'ddr':(0,0),
             'dmotor_torque':(0,0),'dddr':(0,0),'w0':(0,0)}

    if ref_dict is not None:
        for name in ref_dict: bounds[name] = ref_dict[name]
    dotBounds = {'x':(-1,1),'y':(-1,1),'z':(-1,1),
                 'dx':(0,0),'dy':(0,0),'dz':(0,0),
                 'r':(-100,100),'dr':(-1,1),
                 'e11':(-50,50),'e12':(-50,50),'e13':(-50,50),
                 'e21':(-50,50),'e22':(-50,50),'e23':(-50,50),
                 'e31':(-50,50),'e32':(-50,50),'e33':(-50,50),
                 'w_bn_b_x':(0,0),'w_bn_b_y':(0,0),'w_bn_b_z':(0,0),
                 'ddelta':(0,0),
                 'cos_delta':(-1,1),'sin_delta':(omega0-1,omega0+1),
                 'aileron':(-1,1),'elevator':(-1,1),'rudder':(-1,1),'flaps':(-1,1),
                 'motor_torque':(-1000,1000),'ddr':(-100,100)}
    boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \
                [dotBounds[n] for n in dae.xNames()]


#    gfcn.setInput(guessVec)
#    gfcn.evaluate()
#    ret = gfcn.output()
#    for k,v in enumerate(ret):
#        if math.isnan(v):
#            print 'index ',k,' is nan: ',g._tags[k]
#    import sys; sys.exit()

#    context   = zmq.Context(1)
#    publisher = context.socket(zmq.PUB)
#    publisher.bind("tcp://*:5563")
#    class MyCallback:
#        def __init__(self):
#            import rawekite.kiteproto as kiteproto
#            import rawekite.kite_pb2 as kite_pb2
#            self.kiteproto = kiteproto
#            self.kite_pb2 = kite_pb2
#            self.iter = 0
#        def __call__(self,f,*args):
#            x = C.DMatrix(f.input('x'))
#            sol = {}
#            for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
#                sol[name] = x[k].at(0)
#            lookup = lambda name: sol[name]
#            kp = self.kiteproto.toKiteProto(lookup,
#                                            lineAlpha=0.2)
#            mc = self.kite_pb2.MultiCarousel()
#            mc.horizon.extend([kp])
#            mc.messages.append("iter: "+str(self.iter))
#            self.iter += 1
#            publisher.send_multipart(["multi-carousel", mc.SerializeToString()])


    solver = C.IpoptSolver(ffcn,gfcn)
    def addCallback():
        nd = len(boundsVec)
        nc = g.getLb().size()
        c = C.PyFunction( MyCallback(), C.nlpsolverOut(x=C.sp_dense(nd,1), f=C.sp_dense(1,1), lam_x=C.sp_dense(nd,1), lam_p = C.sp_dense(0,1), lam_g = C.sp_dense(nc,1), g = C.sp_dense(nc,1) ), [C.sp_dense(1,1)] )
        c.init()
        solver.setOption("iteration_callback", c)
#    addCallback()
    solver.setOption('max_iter',10000)
#    solver.setOption('tol',1e-14)
#    solver.setOption('suppress_all_output','yes')
#    solver.setOption('print_time',False)
    solver.init()

    solver.setInput(g.getLb(),'lbg')
    solver.setInput(g.getUb(),'ubg')
    solver.setInput(guessVec,'x0')
    lb,ub = zip(*boundsVec)
    solver.setInput(C.DMatrix(lb), 'lbx')
    solver.setInput(C.DMatrix(ub), 'ubx')

    solver.solve()
    ret = solver.getStat('return_status')
    assert ret in ['Solve_Succeeded','Solved_To_Acceptable_Level'], 'Solver failed: '+ret

#    publisher.close()
#    context.destroy()
    xOpt = solver.output('x')
    k = 0
    sol = {}
    for name in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames():
        sol[name] = xOpt[k].at(0)
        k += 1
#        print name+':\t',sol[name]
    dotSol = {}
    for name in dae.xNames():
        dotSol[name] = xOpt[k].at(0)
        k += 1
#        print 'DDT('+name+'):\t',dotSol[name]
    return sol, dotSol
コード例 #3
0
def getSteadyState(dae, r0, v0):
    # make steady state model
    g = Constraints()
    g.add(dae.getResidual(), '==', 0, tag=('dae residual', None))

    def constrainInvariantErrs():
        R_n2b = dae['R_n2b']
        makeOrthonormal(g, R_n2b)
        g.add(dae['c'], '==', 0, tag=('c(0)==0', None))
        g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0', None))

    constrainInvariantErrs()

    # constrain airspeed
    g.add(dae['airspeed'], '>=', v0, tag=('airspeed fixed', None))
    g.addBnds(dae['alpha_deg'], (4, 10), tag=('alpha', None))
    g.addBnds(dae['beta_deg'], (-10, 10), tag=('beta', None))

    dvs = C.veccat(
        [dae.xVec(),
         dae.zVec(),
         dae.uVec(),
         dae.pVec(),
         dae.xDotVec()])
    #    ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])])
    obj = 0
    obj += (dae['cL'] - 0.5)**2
    obj += dae.ddt('w_bn_b_x')**2
    obj += dae.ddt('w_bn_b_y')**2
    obj += dae.ddt('w_bn_b_z')**2
    ffcn = C.SXFunction([dvs], [obj])
    gfcn = C.SXFunction([dvs], [g.getG()])
    ffcn.init()
    gfcn.init()

    guess = {
        'x': r0,
        'y': 0,
        'z': -1,
        'r': r0,
        'dr': 0,
        'e11': 0,
        'e12': -1,
        'e13': 0,
        'e21': 0,
        'e22': 0,
        'e23': 1,
        'e31': -1,
        'e32': 0,
        'e33': 0,
        'dx': 0,
        'dy': -20,
        'dz': 0,
        'w_bn_b_x': 0,
        'w_bn_b_y': 0,
        'w_bn_b_z': 0,
        'aileron': 0,
        'elevator': 0,
        'rudder': 0,
        'daileron': 0,
        'delevator': 0,
        'drudder': 0,
        'nu': 300,
        'motor_torque': 10,
        'dmotor_torque': 0,
        'ddr': 0,
        'dddr': 0.0,
        'w0': 10.0
    }
    dotGuess = {
        'x': 0,
        'y': -20,
        'z': 0,
        'dx': 0,
        'dy': 0,
        'dz': 0,
        'r': 0,
        'dr': 0,
        'e11': 0,
        'e12': 0,
        'e13': 0,
        'e21': 0,
        'e22': 0,
        'e23': 0,
        'e31': 0,
        'e32': 0,
        'e33': 0,
        'w_bn_b_x': 0,
        'w_bn_b_y': 0,
        'w_bn_b_z': 0,
        'aileron': 0,
        'elevator': 0,
        'rudder': 0,
        'ddr': 0
    }

    guessVec = C.DMatrix([
        guess[n]
        for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()
    ] + [dotGuess[n] for n in dae.xNames()])

    bounds = {
        'x': (0.01, r0 * 2),
        'y': (0, 0),
        'z': (-r0 * 0.2, -r0 * 0.2),
        'dx': (0, 0),
        'dy': (-50, 0),
        'dz': (0, 0),
        'r': (r0, r0),
        'dr': (0, 0),
        'ddr': (0, 0),
        'e11': (-0.5, 0.5),
        'e12': (-1.5, -0.5),
        'e13': (-0.5, 0.5),
        'e21': (-0.5, 0.5),
        'e22': (-0.5, 0.5),
        'e23': (0.5, 1.5),
        'e31': (-1.5, -0.5),
        'e32': (-0.5, 0.5),
        'e33': (-0.5, 0.5),
        'w_bn_b_x': (0, 0),
        'w_bn_b_y': (0, 0),
        'w_bn_b_z': (0, 0),
        #              'aileron':(-0.2,0.2),'elevator':(-0.2,0.2),'rudder':(-0.2,0.2),
        'aileron': (0, 0),
        'elevator': (0, 0),
        'rudder': (0, 0),
        'daileron': (0, 0),
        'delevator': (0, 0),
        'drudder': (0, 0),
        'nu': (0, 3000),
        'dddr': (0, 0),
        'w0': (10, 10)
    }
    dotBounds = {
        'dz': (-C.inf, 0)
    }  #'dx':(-500,-500),'dy':(-500,500),'dz':(-500,500),
    #                 'w_bn_b_x':(0,0),'w_bn_b_y':(0,0),'w_bn_b_z':(0,0),

    for name in dae.xNames():
        if name not in dotBounds:
            dotBounds[name] = (-C.inf, C.inf)
    boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \
                [dotBounds[n] for n in dae.xNames()]

    #    gfcn.setInput(guessVec)
    #    gfcn.evaluate()
    #    ret = gfcn.output()
    #    for k,v in enumerate(ret):
    #        if math.isnan(v):
    #            print 'index ',k,' is nan: ',g._tags[k]
    #    import sys; sys.exit()

    #    context   = zmq.Context(1)
    #    publisher = context.socket(zmq.PUB)
    #    publisher.bind("tcp://*:5563")
    #    class MyCallback:
    #        def __init__(self):
    #            import rawekite.kiteproto as kiteproto
    #            import rawekite.kite_pb2 as kite_pb2
    #            self.kiteproto = kiteproto
    #            self.kite_pb2 = kite_pb2
    #            self.iter = 0
    #        def __call__(self,f,*args):
    #            x = C.DMatrix(f.input('x'))
    #            sol = {}
    #            for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
    #                sol[name] = x[k].at(0)
    #            lookup = lambda name: sol[name]
    #            kp = self.kiteproto.toKiteProto(lookup,
    #                                            lineAlpha=0.2)
    #            mc = self.kite_pb2.MultiCarousel()
    #            mc.horizon.extend([kp])
    #            mc.messages.append("iter: "+str(self.iter))
    #            self.iter += 1
    #            publisher.send_multipart(["multi-carousel", mc.SerializeToString()])

    solver = C.IpoptSolver(ffcn, gfcn)

    def addCallback():
        nd = len(boundsVec)
        nc = g.getLb().size()
        c = C.PyFunction(
            MyCallback(),
            C.nlpsolverOut(x=C.sp_dense(nd, 1),
                           f=C.sp_dense(1, 1),
                           lam_x=C.sp_dense(nd, 1),
                           lam_p=C.sp_dense(0, 1),
                           lam_g=C.sp_dense(nc, 1),
                           g=C.sp_dense(nc, 1)), [C.sp_dense(1, 1)])
        c.init()
        solver.setOption("iteration_callback", c)
#    addCallback()

    solver.setOption('max_iter', 10000)
    solver.setOption('expand', True)
    #    solver.setOption('tol',1e-14)
    #    solver.setOption('suppress_all_output','yes')
    #    solver.setOption('print_time',False)
    solver.init()

    solver.setInput(g.getLb(), 'lbg')
    solver.setInput(g.getUb(), 'ubg')
    #guessVec = numpy.load('steady_state_guess.npy')
    solver.setInput(guessVec, 'x0')
    lb, ub = zip(*boundsVec)
    solver.setInput(C.DMatrix(lb), 'lbx')
    solver.setInput(C.DMatrix(ub), 'ubx')

    solver.solve()
    ret = solver.getStat('return_status')
    assert ret in ['Solve_Succeeded',
                   'Solved_To_Acceptable_Level'], 'Solver failed: ' + ret

    #    publisher.close()
    #    context.destroy()
    xOpt = solver.output('x')
    #numpy.save('steady_state_guess',numpy.squeeze(numpy.array(xOpt)))
    k = 0
    sol = {}
    for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames():
        sol[name] = xOpt[k].at(0)
        k += 1
#        print name+':\t',sol[name]
    dotSol = {}
    for name in dae.xNames():
        dotSol[name] = xOpt[k].at(0)
        k += 1
#        print 'DDT('+name+'):\t',dotSol[name]
    return sol, dotSol
コード例 #4
0
class Coll():
    collocationIsSetup = False
    def __init__(self, dae, nk=None, nicp=1, deg=4, collPoly='RADAU'):
        assert nk is not None
        assert isinstance(dae, Dae)

        self.dae = dae

        self.nk = nk
        self.nicp = nicp
        self.deg = deg
        self.collPoly = collPoly

        self._bounds = BoundsMap(self,"bounds")
        self._guess = collmaps.WriteableCollMap(self,"guess")

        self._constraints = Constraints()

        # setup NLP variables
        self._dvMap = collmaps.VectorizedReadOnlyCollMap(self,"design var map",CS.msym("V",self.getNV()))

        # quadratures
        self._quadratureManager = collmaps.QuadratureManager(self)


    def setQuadratureDdt(self,quadratureStateName,quadratureStateDotName):
        ''' Add a new quadrature state to the collocation problem by specifying the name of its derivative
        '''
        # run some checks and pass it to the less safe CollMapPlus.setQuadratureDdt
        if not self.collocationIsSetup:
            raise ValueError("Can't add quadratures until you call setupCollocation")

        # make sure this is a unique name (quadratureManager also checks)
        self.dae.assertUniqueName(quadratureStateName)

        # make sure nobody adds an output named the same as quadratureStateName
        self.dae._illegalNames.append(quadratureStateName)

        # setup the quadrature state
        self._quadratureManager.setQuadratureDdt(quadratureStateName,quadratureStateDotName,
                                                 self.lookup,self.lagrangePoly,self.h,self._dvMap.vectorize())

    def setupCollocation(self,tf):
        if self.collocationIsSetup:
            raise ValueError("you can't setup collocation twice")
        self.collocationIsSetup = True

        ## -----------------------------------------------------------------------------
        ## Collocation setup
        ## -----------------------------------------------------------------------------
        # Size of the finite elements
        self.h = tf/float(self.nk*self.nicp)

        # make coefficients for collocation/continuity equations
        self.lagrangePoly = LagrangePoly(deg=self.deg,collPoly=self.collPoly)

        # function to get h out
        self.hfun = CS.MXFunction([self._dvMap.vectorize()],[self.h])
        self.hfun.init()

        # add collocation constraints
        ffcn = self._makeResidualFun()

        ndiff = self.xSize()
        nalg = self.zSize()

        self._xDot = np.resize(np.array([None]),(self.nk,self.nicp,self.deg+1))

        # For all finite elements
        for k in range(self.nk):
            for i in range(self.nicp):
                # For all collocation points
                for j in range(1,self.deg+1):
                    # Get an expression for the state derivative at the collocation point
                    xp_jk = 0
                    for j2 in range (self.deg+1):
                        # get the time derivative of the differential states (eq 10.19b)
                        xp_jk += self.lagrangePoly.lDotAtTauRoot[j,j2]*self.xVec(k,nicpIdx=i,degIdx=j2)
                    self._xDot[k,i,j] = xp_jk/self.h
                    # Add collocation equations to the NLP
                    [fk] = ffcn.call([self._xDot[k,i,j],
                                      self.xVec(k,nicpIdx=i,degIdx=j),
                                      self.zVec(k,nicpIdx=i,degIdx=j),
                                      self.uVec(k),
                                      self.pVec()])

                    # impose system dynamics (for the differential states (eq 10.19b))
                    self.constrain(fk,'==',0,tag=("implicit dynamic equation",(k,i,j)))

                # Get an expression for the state at the end of the finite element
                xf_k = 0
                for j in range(self.deg+1):
                    xf_k += self.lagrangePoly.lAtOne[j]*self.xVec(k,nicpIdx=i,degIdx=j)
#                    print "self.lagrangePoly.lAtOne["+str(j)+"]:" +str(self.lagrangePoly.lAtOne[j])

#                mxfun = CS.MXFunction([self._V],[xf_k])
#                mxfun.init()
#                sxfun = CS.SXFunction(mxfun)
#                sxfun.init()
#                print ""
#                print sxfun.outputSX()

                # Add continuity equation to NLP
                if i==self.nicp-1:
                    self.constrain(self.xVec(k+1,nicpIdx=0,degIdx=0), '==', xf_k, tag=("continuity",(k,i)))
                else:
                    self.constrain(self.xVec(k,nicpIdx=i+1,degIdx=0), '==', xf_k, tag=("continuity",(k,i)))

        # add outputs
        self._outputMapGenerator = collmaps.OutputMapGenerator(self, self._xDot)
        self._outputMap = collmaps.OutputMap(self._outputMapGenerator, self._dvMap.vectorize())

    def xVec(self,*args,**kwargs):
        return self._dvMap.xVec(*args,**kwargs)
    def zVec(self,*args,**kwargs):
        return self._dvMap.zVec(*args,**kwargs)
    def uVec(self,*args,**kwargs):
        return self._dvMap.uVec(*args,**kwargs)
    def pVec(self,*args,**kwargs):
        return self._dvMap.pVec(*args,**kwargs)

    def constrain(self,lhs,comparison,rhs,tag=('unnamed_constraint',None)):
        self._constraints.add(lhs,comparison,rhs,tag=tag)

    def constrainBnds(self,g,(lbg,ubg),tag=('unnamed_constraint',None)):
コード例 #5
0
def getSteadyState(dae, r0, v0):
    # make steady state model
    g = Constraints()
    g.add(dae.getResidual(), "==", 0, tag=("dae residual", None))

    def constrainInvariantErrs():
        R_n2b = dae["R_n2b"]
        makeOrthonormal(g, R_n2b)
        g.add(dae["c"], "==", 0, tag=("c(0)==0", None))
        g.add(dae["cdot"], "==", 0, tag=("cdot(0)==0", None))

    constrainInvariantErrs()

    # constrain airspeed
    g.add(dae["airspeed"], ">=", v0, tag=("airspeed fixed", None))
    g.addBnds(dae["alpha_deg"], (4, 10), tag=("alpha", None))
    g.addBnds(dae["beta_deg"], (-10, 10), tag=("beta", None))

    dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
    #    ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])])
    obj = 0
    obj += (dae["cL"] - 0.5) ** 2
    obj += dae.ddt("w_bn_b_x") ** 2
    obj += dae.ddt("w_bn_b_y") ** 2
    obj += dae.ddt("w_bn_b_z") ** 2
    ffcn = C.SXFunction([dvs], [obj])
    gfcn = C.SXFunction([dvs], [g.getG()])
    ffcn.init()
    gfcn.init()

    guess = {
        "x": r0,
        "y": 0,
        "z": -1,
        "r": r0,
        "dr": 0,
        "e11": 0,
        "e12": -1,
        "e13": 0,
        "e21": 0,
        "e22": 0,
        "e23": 1,
        "e31": -1,
        "e32": 0,
        "e33": 0,
        "dx": 0,
        "dy": -20,
        "dz": 0,
        "w_bn_b_x": 0,
        "w_bn_b_y": 0,
        "w_bn_b_z": 0,
        "aileron": 0,
        "elevator": 0,
        "rudder": 0,
        "daileron": 0,
        "delevator": 0,
        "drudder": 0,
        "nu": 300,
        "motor_torque": 10,
        "dmotor_torque": 0,
        "ddr": 0,
        "dddr": 0.0,
        "w0": 10.0,
    }
    dotGuess = {
        "x": 0,
        "y": -20,
        "z": 0,
        "dx": 0,
        "dy": 0,
        "dz": 0,
        "r": 0,
        "dr": 0,
        "e11": 0,
        "e12": 0,
        "e13": 0,
        "e21": 0,
        "e22": 0,
        "e23": 0,
        "e31": 0,
        "e32": 0,
        "e33": 0,
        "w_bn_b_x": 0,
        "w_bn_b_y": 0,
        "w_bn_b_z": 0,
        "aileron": 0,
        "elevator": 0,
        "rudder": 0,
        "ddr": 0,
    }

    guessVec = C.DMatrix(
        [guess[n] for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()]
        + [dotGuess[n] for n in dae.xNames()]
    )

    bounds = {
        "x": (0.01, r0 * 2),
        "y": (0, 0),
        "z": (-r0 * 0.2, -r0 * 0.2),
        "dx": (0, 0),
        "dy": (-50, 0),
        "dz": (0, 0),
        "r": (r0, r0),
        "dr": (0, 0),
        "ddr": (0, 0),
        "e11": (-0.5, 0.5),
        "e12": (-1.5, -0.5),
        "e13": (-0.5, 0.5),
        "e21": (-0.5, 0.5),
        "e22": (-0.5, 0.5),
        "e23": (0.5, 1.5),
        "e31": (-1.5, -0.5),
        "e32": (-0.5, 0.5),
        "e33": (-0.5, 0.5),
        "w_bn_b_x": (0, 0),
        "w_bn_b_y": (0, 0),
        "w_bn_b_z": (0, 0),
        #              'aileron':(-0.2,0.2),'elevator':(-0.2,0.2),'rudder':(-0.2,0.2),
        "aileron": (0, 0),
        "elevator": (0, 0),
        "rudder": (0, 0),
        "daileron": (0, 0),
        "delevator": (0, 0),
        "drudder": (0, 0),
        "nu": (0, 3000),
        "dddr": (0, 0),
        "w0": (10, 10),
    }
    dotBounds = {"dz": (-C.inf, 0)}  #'dx':(-500,-500),'dy':(-500,500),'dz':(-500,500),
    #                 'w_bn_b_x':(0,0),'w_bn_b_y':(0,0),'w_bn_b_z':(0,0),

    for name in dae.xNames():
        if name not in dotBounds:
            dotBounds[name] = (-C.inf, C.inf)
    boundsVec = [bounds[n] for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames()] + [
        dotBounds[n] for n in dae.xNames()
    ]

    #    gfcn.setInput(guessVec)
    #    gfcn.evaluate()
    #    ret = gfcn.output()
    #    for k,v in enumerate(ret):
    #        if math.isnan(v):
    #            print 'index ',k,' is nan: ',g._tags[k]
    #    import sys; sys.exit()

    #    context   = zmq.Context(1)
    #    publisher = context.socket(zmq.PUB)
    #    publisher.bind("tcp://*:5563")
    #    class MyCallback:
    #        def __init__(self):
    #            import rawekite.kiteproto as kiteproto
    #            import rawekite.kite_pb2 as kite_pb2
    #            self.kiteproto = kiteproto
    #            self.kite_pb2 = kite_pb2
    #            self.iter = 0
    #        def __call__(self,f,*args):
    #            x = C.DMatrix(f.input('x'))
    #            sol = {}
    #            for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
    #                sol[name] = x[k].at(0)
    #            lookup = lambda name: sol[name]
    #            kp = self.kiteproto.toKiteProto(lookup,
    #                                            lineAlpha=0.2)
    #            mc = self.kite_pb2.MultiCarousel()
    #            mc.horizon.extend([kp])
    #            mc.messages.append("iter: "+str(self.iter))
    #            self.iter += 1
    #            publisher.send_multipart(["multi-carousel", mc.SerializeToString()])

    solver = C.IpoptSolver(ffcn, gfcn)

    def addCallback():
        nd = len(boundsVec)
        nc = g.getLb().size()
        c = C.PyFunction(
            MyCallback(),
            C.nlpsolverOut(
                x=C.sp_dense(nd, 1),
                f=C.sp_dense(1, 1),
                lam_x=C.sp_dense(nd, 1),
                lam_p=C.sp_dense(0, 1),
                lam_g=C.sp_dense(nc, 1),
                g=C.sp_dense(nc, 1),
            ),
            [C.sp_dense(1, 1)],
        )
        c.init()
        solver.setOption("iteration_callback", c)

    #    addCallback()
    solver.setOption("max_iter", 10000)
    solver.setOption("expand", True)
    #    solver.setOption('tol',1e-14)
    #    solver.setOption('suppress_all_output','yes')
    #    solver.setOption('print_time',False)
    solver.init()

    solver.setInput(g.getLb(), "lbg")
    solver.setInput(g.getUb(), "ubg")
    # guessVec = numpy.load('steady_state_guess.npy')
    solver.setInput(guessVec, "x0")
    lb, ub = zip(*boundsVec)
    solver.setInput(C.DMatrix(lb), "lbx")
    solver.setInput(C.DMatrix(ub), "ubx")

    solver.solve()
    ret = solver.getStat("return_status")
    assert ret in ["Solve_Succeeded", "Solved_To_Acceptable_Level"], "Solver failed: " + ret

    #    publisher.close()
    #    context.destroy()
    xOpt = solver.output("x")
    # numpy.save('steady_state_guess',numpy.squeeze(numpy.array(xOpt)))
    k = 0
    sol = {}
    for name in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames():
        sol[name] = xOpt[k].at(0)
        k += 1
    #        print name+':\t',sol[name]
    dotSol = {}
    for name in dae.xNames():
        dotSol[name] = xOpt[k].at(0)
        k += 1
    #        print 'DDT('+name+'):\t',dotSol[name]
    return sol, dotSol
コード例 #6
0
 def getRobustSteadyStateNlpFunctions(self, dae, ref_dict = {}):
     xDotSol, zSol = dae.solveForXDotAndZ()
 
     ginv = Constraints()
     def constrainInvariantErrs():
         R_c2b = dae['R_c2b']
         self.makeOrthonormal(ginv, R_c2b)
         ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
         ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
         di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1
         ginv.add(di, '==', 0, tag = ('delta invariant', None))
         
         ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
                        ("Rotational velocities", None))
         
     constrainInvariantErrs()
     invariants = ginv.getG()
     
     J = C.jacobian(invariants,dae.xVec())
 
     # make steady state model
     g = Constraints()
     
     xds = C.vertcat([xDotSol[ name ] for name in dae.xNames()])
     jInv = C.mul(J.T,C.solve(C.mul(J,J.T),invariants))
     
     g.add(xds - jInv - dae.xDotVec(), '==', 0, tag = ('dae residual', None))            
 
     for name in ['alpha_deg', 'beta_deg', 'cL']:
         if name in ref_dict:
             g.addBnds(dae[name], ref_dict[name], tag = (name, None))
 
     dvs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
     
     obj = 0
     for name in dae.uNames() + ['aileron', 'elevator']:
         if name in dae:
             obj += dae[ name ] ** 2
     
     return dvs, obj, g.getG(), g.getLb(), g.getUb(), zSol
コード例 #7
0
 def getSteadyStateNlpFunctions(self, dae, ref_dict = {}):
     # make steady state model
     g = Constraints()
     g.add(dae.getResidual(), '==', 0, tag = ('dae residual', None))
     def constrainInvariantErrs():
         R_c2b = dae['R_c2b']
         self.makeOrthonormal(g, R_c2b)
         g.add(dae['c'], '==', 0, tag = ('c(0) == 0', None))
         g.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None))
     constrainInvariantErrs()
 
     # Rotational velocity time derivative
     # NOTE: In the following constraint omega0 was hard-coded.
     g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag =
                        ("Rotational velocities", None))
 
     for name in ['alpha_deg', 'beta_deg', 'cL']:
         if name in ref_dict:
             g.addBnds(dae[name], ref_dict[name], tag = (name, None))
 
     dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
     obj = 0
 #     for name in ['aileron', 'elevator', 'rudder', 'flaps']:
     for name in dae.uNames():
         if name in dae:
             obj += dae[ name ] ** 2
     
     return dvs, obj, g.getG(), g.getLb(), g.getUb()
コード例 #8
0
def getSteadyState(dae,conf,omega0,r0):
    # make steady state model
    g = Constraints()
    g.add(dae.getResidual(),'==',0,tag=('dae residual',None))
    def constrainInvariantErrs():
        dcm = dae['dcm']
        err = C.mul(dcm.T,dcm)
        g.add( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag=
                       ("initial dcm orthonormal",None))
        g.add(dae['c'], '==', 0, tag=('c(0)==0',None))
        g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0',None))
    constrainInvariantErrs()

    dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()])
    ffcn = C.SXFunction([dvs],[sum([dae[n]**2 for n in ['aileron','elevator','y','z']])])
    gfcn = C.SXFunction([dvs],[g.getG()])
    ffcn.init()
    gfcn.init()

    guess = {'x':r0,'y':0,'z':0,
             'r':r0,'dr':0,
             'e11':0, 'e12':1, 'e13':0,
             'e21':0, 'e22':0, 'e23':1,
             'e31':1, 'e32':0, 'e33':0,
             'dx':0,'dy':0,'dz':0,
             'w1':0,'w2':omega0,'w3':0,
             'delta':0,'ddelta':omega0,
             'cos_delta':1,'sin_delta':0,
             'aileron':0,'elevator':0,
             'daileron':0,'delevator':0,
             'nu':300,'motor_torque':10,
             'ddr':0.0,'w0':0.0}
    dotGuess = {'x':0,'y':0,'z':0,'dx':0,'dy':0,'dz':0,
                'r':0,'dr':0,
                'e11':omega0,'e12':0,'e13':0,
                'e21':0,'e22':-omega0,'e23':0,
                'e31':0,'e32':0,'e33':0,
                'w1':0,'w2':0,'w3':0,
                'delta':omega0,'ddelta':0,
                'cos_delta':0,'sin_delta':omega0,
                'aileron':0,'elevator':0}

    guessVec = C.DMatrix([guess[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+
                         [dotGuess[n] for n in dae.xNames()])

    bounds = {'x':(0.01,r0+1),'y':(-1,0),'z':(-1,0),
             'r':(r0,r0),'dr':(0,0),
             'e11':(-2,2),'e12':(-2,2),'e13':(-2,2),
             'e21':(-2,2),'e22':(-2,2),'e23':(-2,2),
             'e31':(-2,2),'e32':(-2,2),'e33':(-2,2),
             'dx':(-50,50),'dy':(-50,50),'dz':(0,0),
             'w1':(-50,50),'w2':(-50,50),'w3':(-50,50),
             'delta':(0,0),'ddelta':(omega0,omega0),
             'cos_delta':(1,1),'sin_delta':(0,0),
             'aileron':(-0.1,0.1),'elevator':(-0.1,0.1),
             'daileron':(0,0),'delevator':(0,0),
             'nu':(0,3000),'motor_torque':(0,1000),
             'ddr':(0,0),'w0':(0,0)}
    dotBounds = {'x':(-50,50),'y':(-50,50),'z':(-50,50)
                 ,'dx':(0,0),'dy':(0,0),'dz':(0,0),
                 'r':(-1,1),'dr':(-1,1),
                 'e11':(-50,50),'e12':(-50,50),'e13':(-50,50),
                 'e21':(-50,50),'e22':(-50,50),'e23':(-50,50),
                 'e31':(-50,50),'e32':(-50,50),'e33':(-50,50),
                 'w1':(0,0),'w2':(0,0),'w3':(0,0),
                 'delta':(omega0-1,omega0+1),'ddelta':(0,0),
                 'cos_delta':(0,0),'sin_delta':(omega0,omega0),
                 'aileron':(-1,1),'elevator':(-1,1)}
    boundsVec = [bounds[n] for n in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()]+ \
                [dotBounds[n] for n in dae.xNames()]
    

#    gfcn.setInput(guessVec)
#    gfcn.evaluate()
#    ret = gfcn.output()
#    for k,v in enumerate(ret):
#        if math.isnan(v):
#            print 'index ',k,' is nan: ',g._tags[k]
#    import sys; sys.exit()
    context   = zmq.Context(1)
    publisher = context.socket(zmq.PUB)
    publisher.bind("tcp://*:5563")
    class MyCallback:
        def __init__(self):
            self.iter = 0
        def __call__(self,f,*args):
            x = C.DMatrix(f.input(C.NLP_X_OPT))
            sol = {}
            for k,name in enumerate(dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames()):
                sol[name] = x[k].at(0)
            lookup = lambda name: sol[name]
            kp = kiteproto.toKiteProto(lookup,
                                       lineAlpha=0.2)
            mc = kite_pb2.MultiCarousel()
            mc.horizon.extend([kp])
            mc.messages.append("iter: "+str(self.iter))
            self.iter += 1
            publisher.send_multipart(["multi-carousel", mc.SerializeToString()])

    
    solver = C.IpoptSolver(ffcn,gfcn)
    def addCallback():
        nd = len(boundsVec)
        nc = g.getLb().size()
        c = C.PyFunction( MyCallback(), C.nlpsolverOut(x_opt=C.sp_dense(nd,1), cost=C.sp_dense(1,1), lambda_x=C.sp_dense(nd,1), lambda_p = C.sp_dense(0,1), lambda_g = C.sp_dense(nc,1), g = C.sp_dense(nc,1) ), [C.sp_dense(1,1)] )
        c.init()
        solver.setOption("iteration_callback", c)
#    addCallback()
    solver.setOption('max_iter',10000)
    solver.init()

    solver.setInput(g.getLb(),C.NLP_LBG)
    solver.setInput(g.getUb(),C.NLP_UBG)
    solver.setInput(guessVec,C.NLP_X_INIT)
    lb,ub = zip(*boundsVec)
    solver.setInput(C.DMatrix(lb), C.NLP_LBX)
    solver.setInput(C.DMatrix(ub), C.NLP_UBX)

    solver.solve()
    publisher.close()
    context.destroy()
    xOpt = solver.output(C.NLP_X_OPT)
    k = 0
    sol = {}
    for name in dae.xNames()+dae.zNames()+dae.uNames()+dae.pNames():
        sol[name] = xOpt[k].at(0)
        k += 1
#        print name+':\t',sol[name]
    dotSol = {}
    for name in dae.xNames():
        dotSol[name] = xOpt[k].at(0)
        k += 1
#        print 'DDT('+name+'):\t',dotSol[name]
    return sol