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 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
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
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)):
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
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
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()
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