def transcription_function(self, weight): """ Returns a function that can be used with Collocation's 'minimize_f', such that total transcription is minimized with constant weight 'weight' """ x = cs.vertcat(self.states) p = cs.vertcat(self.params) txn = [ rxn.rate for rxn in self.reactions if type(rxn) is TranscriptionRxn ] txn = weight * cs.sumAll(cs.vertcat(txn)) t = cs.ssym('t') fn = cs.SXFunction([x, p], [txn]) fn.init() def return_func(x, p): """ Function to return for Collocation """ try: fn.setInput(x, 0) fn.setInput(p, 1) fn.evaluate() return float(fn.output().toArray()) except Exception: return fn.call([x, p])[0] return return_func
def phase_of_point(self, point, error=False, tol=1E-3): """ Finds the phase at which the distance from the point to the limit cycle is minimized. phi=0 corresponds to the definition of y0, returns the phase and the minimum distance to the limit cycle """ point = np.asarray(point) #set up integrator so we only have to once... intr = cs.Integrator('cvodes',self.model) intr.setOption("abstol", self.intoptions['bvp_abstol']) intr.setOption("reltol", self.intoptions['bvp_reltol']) intr.setOption("tf", self.T) intr.setOption("max_num_steps", self.intoptions['transmaxnumsteps']) intr.setOption("disable_internal_warnings", True) intr.init() for i in xrange(100): dist = cs.SX.sym("dist") x = self.model.inputExpr(cs.DAE_X) ode = self.model.outputExpr()[0] dist_ode = cs.sumAll(2.*(x - point)*ode) cat_x = cs.vertcat([x, dist]) cat_ode = cs.vertcat([ode, dist_ode]) dist_model = cs.SXFunction( cs.daeIn(t=self.model.inputExpr(cs.DAE_T), x=cat_x, p=self.model.inputExpr(cs.DAE_P)), cs.daeOut(ode=cat_ode)) dist_model.setOption("name","distance model") dist_0 = ((self.y0 - point)**2).sum() if dist_0 < tol: # catch the case where we start at 0 return 0. cat_y0 = np.hstack([self.y0, dist_0]) roots_class = Oscillator(dist_model, self.param, cat_y0) roots_class.intoptions = self.intoptions #return roots_class roots_class.solve_bvp() roots_class.limit_cycle() roots_class.roots() phases = self._t_to_phi(roots_class.tmin[-1]) distances = roots_class.ymin[-1] distance = np.min(distances) if distance < tol: phase_ind = np.argmin(distances) # for multiple minima return phases[phase_ind]#, roots_class intr.setInput(point, cs.INTEGRATOR_X0) intr.setInput(self.param, cs.INTEGRATOR_P) intr.evaluate() point = intr.output().toArray().flatten() #advance by one cycle raise RuntimeError("Point failed to converge to limit cycle")
def phase_of_point(self, point, error=False, tol=1E-3): """ Finds the phase at which the distance from the point to the limit cycle is minimized. phi=0 corresponds to the definition of y0, returns the phase and the minimum distance to the limit cycle """ point = np.asarray(point) for i in xrange(100): dist = cs.ssym("dist") x = self.model.inputSX(cs.DAE_X) ode = self.model.outputSX() dist_ode = cs.sumAll(2*(x - point)*ode) cat_x = cs.vertcat([x, dist]) cat_ode = cs.vertcat([ode, dist_ode]) dist_model = cs.SXFunction( cs.daeIn(t=self.model.inputSX(cs.DAE_T), x=cat_x, p=self.model.inputSX(cs.DAE_P)), cs.daeOut(ode=cat_ode)) dist_model.setOption("name","distance model") dist_0 = ((self.y0[:-1] - point)**2).sum() cat_y0 = np.hstack([self.y0[:-1], dist_0, self.y0[-1]]) roots_class = pBase(dist_model, self.paramset, cat_y0) # roots_class.solveBVP() roots_class.roots() phase = self._t_to_phi(roots_class.Tmin[-1]) distance = roots_class.Ymin[-1] if distance < tol: return phase, distance intr = cs.CVodesIntegrator(self.model) intr.setOption("abstol", self.intoptions['transabstol']) intr.setOption("reltol", self.intoptions['transreltol']) intr.setOption("tf", self.y0[-1]) intr.setOption("max_num_steps", self.intoptions['transmaxnumsteps']) intr.setOption("disable_internal_warnings", True) intr.init() intr.setInput(point, cs.INTEGRATOR_X0) intr.setInput(self.paramset, cs.INTEGRATOR_P) intr.evaluate() point = intr.output().toArray().flatten() raise RuntimeError("Point failed to converge to limit cycle")
def inverse_parameterization(R, parameterizationFunction): angle1 = ssym('angle1') angle2 = ssym('angle2') angle3 = ssym('angle3') Restimated = parameterizationFunction(angle1,angle2,angle3) E = R - Restimated; e = casadi.sumAll(E*E) f = SXFunction([casadi.vertcat([angle1,angle2,angle3])], [e]) f.init() anglerange = numpy.linspace(-1*numpy.pi,1*numpy.pi,25) best_eTemp = 9999999999999 best_angle1 = numpy.nan best_angle2 = numpy.nan best_angle3 = numpy.nan for i in xrange(len(anglerange)): for j in xrange(len(anglerange)): for k in xrange(len(anglerange)): r = anglerange[i] p = anglerange[j] y = anglerange[k] f.setInput([r,p,y]) f.evaluate() eTemp = f.output(0) if eTemp<best_eTemp: best_eTemp = eTemp best_angle1 = r best_angle2 = p best_angle3 = y if 0: V = msym("V",(3,1)) fval = f.call([V]) fmx = MXFunction([V],fval) nlp_solver = IpoptSolver(fmx) else: nlp_solver = IpoptSolver(f) nlp_solver.setOption('generate_hessian',True) nlp_solver.setOption('expand_f',True) nlp_solver.setOption('expand_g',True) nlp_solver.init() nlp_solver.setInput([best_angle1,best_angle2,best_angle3],NLP_X_INIT) nlp_solver.solve() sol = nlp_solver.output(NLP_X_OPT).toArray() sol = nlp_solver.output(NLP_X_OPT).toArray() return sol
, 0.316039689643 , -0.073559821379 , 0.945889986864 , 0.940484536806 , -0.106993361072 , -0.322554269411 , 0.000000000000 , 0.000000000000 , 0.000000000000 , 0.137035790811 , 3.664945343102 , -1.249768772258 , 3.874600000000 , 0.0 ]) x0=C.veccat([x0,C.sqrt(C.sumAll(x0[0:2]*x0[0:2])),0,0,0]) def setupOcp(dae,conf,nk=50,nicp=1,deg=4): ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg) ocp.setupCollocation(ocp.lookup('endTime')) # constrain invariants def constrainInvariantErrs(): dcm = ocp.lookup('dcm',timestep=0) rawekite.kiteutils.makeOrthonormal(ocp, dcm) ocp.constrain(ocp.lookup('c',timestep=0), '==', 0) ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0) constrainInvariantErrs() # constraint line angle for k in range(0,nk):
def main(): nSteps = 15 print "creating model" dae = models.carousel(zt, rArm, nSteps) print "setting up OCP" ocp = MultipleShootingStage(dae, nSteps) # make the integrator print "setting up dynamics constraints" integratorOptions = [ ("reltol", 1e-7), ("abstol", 1e-9), ("t0", 0), ("tf", 1), ('name', 'integrator'), ("linear_solver_creator", C.CSparse) # , ("linear_solver_creator",C.LapackLUDense) , ("linear_solver", "user_defined") ] ocp.setIdasIntegrator(integratorOptions) # constrain invariants def invariantErrs(): dcm = C.horzcat([ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')]), C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')]), C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')]) ]).trans() err = C.mul(dcm.trans(), dcm) dcmErr = C.veccat([ err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1], err[0, 2], err[1, 2] ]) f = C.SXFunction( [dae.xVec(), dae.uVec(), dae.pVec()], [dae.output('c'), dae.output('cdot'), dcmErr]) f.setOption('name', 'invariant errors') f.init() return f [c0, cdot0, dcmError0] = invariantErrs().call( [ocp.states[:, 0], ocp.actions[:, 0], ocp.params]) ocp.addConstraint(c0, '==', 0) ocp.addConstraint(cdot0, '==', 0) ocp.addConstraint(dcmError0, '==', 0) # bounds ocp.setBound('aileron', (-0.04, 0.04)) ocp.setBound('elevator', (-0.1, 0.1)) ocp.setBound('x', (0, 4)) ocp.setBound('y', (-3, 3)) ocp.setBound('z', (-2, 3)) ocp.setBound('r', (1, 2)) ocp.setBound('dr', (-1, 1)) ocp.setBound('ddr', (0, 0)) ocp.setBound('r', (1.2, 1.2), timestep=0) for e in ['e11', 'e21', 'e31', 'e12', 'e22', 'e32', 'e13', 'e23', 'e33']: ocp.setBound(e, (-1.1, 1.1)) for d in ['dx', 'dy', 'dz']: ocp.setBound(d, (-50, 50)) for w in ['w1', 'w2', 'w3']: ocp.setBound(w, (-8 * pi, 8 * pi)) ocp.setBound('delta', (-0.01, 1.01 * 2 * pi)) ocp.setBound('ddelta', (-pi / 4, 8 * pi)) ocp.setBound('tc', (-200, 600)) # ocp.setBound('tc',(389.970797939731,389.970797939731)) ocp.setBound('endTime', (0.5, 2.0)) # ocp.setBound('endTime',(1.6336935276077966,1.6336935276077966)) ocp.setBound('w0', (10, 10)) # boundary conditions ocp.setBound('delta', (0, 0), timestep=0) ocp.setBound('delta', (2 * pi, 2 * pi), timestep=nSteps - 1) # make it periodic ocp.addConstraint(ocp.states[:18, 0], '==', ocp.states[:18, -1]) ocp.addConstraint(ocp.states[19, 0], '==', ocp.states[19, -1]) ocp.addConstraint(ocp.actions[:, 0], '==', ocp.actions[:, -1]) # make the solver # objective function tc0 = 390 obj = (C.sumAll(ocp.actions[0:2, :] * ocp.actions[0:2, :]) + 1e-10 * C.sumAll((ocp.actions[2, :] - tc0) * (ocp.actions[2, :] - tc0))) * ocp.lookup('endTime') ocp.setObjective(obj) # zero mq setup context = zmq.Context(1) publisher = context.socket(zmq.PUB) publisher.bind("tcp://*:5563") # callback function class MyCallback: def __init__(self): self.iter = 0 def __call__(self, f, *args): self.iter = self.iter + 1 xOpt = f.input(C.NLP_X_OPT) xs, us, p = ocp.getTimestepsFromDvs(xOpt) kiteProtos = [ kiteproto.toKiteProto(xs[k], us[k], p, rArm, zt) for k in range(0, nSteps) ] mc = kite_pb2.KiteOpt() mc.horizon.extend(list(kiteProtos)) xup = ocp.devectorize(xOpt) mc.messages.append("endTime: " + str(xup['endTime'])) mc.messages.append("w0: " + str(xup['w0'])) mc.messages.append("iter: " + str(self.iter)) publisher.send_multipart( ["multi-carousel", mc.SerializeToString()]) def makeCallback(): nd = ocp.getDesignVars().size() nc = ocp._constraints.getG().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_g=C.sp_dense(nc, 1), g=C.sp_dense(nc, 1)), [C.sp_dense(1, 1)]) c.init() return c # solver solverOptions = [("iteration_callback", makeCallback()), ("linear_solver", "ma57") # , ("max_iter",5) ] ocp.setSolver(C.IpoptSolver, solverOptions=solverOptions) #ocp.setBounds() # initial conditions ocp.setXGuess(x0) for k in range(0, nSteps): val = 2 * pi * k / (nSteps - 1) ocp.setGuess('delta', val, timestep=k, quiet=True) ocp.setGuess('aileron', 0) ocp.setGuess('elevator', 0) ocp.setGuess('tc', 389.970797939731) ocp.setGuess('endTime', 1.6336935276077966) ocp.setGuess('ddr', 0) ocp.setGuess('w0', 0) ocp.solve()
def setupOcp(dae,conf,publisher,nk=50,nicp=1,deg=4): ocp = Coll(dae, nk=nk,nicp=nicp,deg=deg) # constrain invariants def invariantErrs(): dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')]) , C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')]) , C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')]) ] ).trans() err = C.mul(dcm.trans(), dcm) dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ]) f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()] , [dae.output('c'),dae.output('cdot'),dcmErr] ) f.setOption('name','invariant errors') f.init() return f [c0,cdot0,dcmError0] = invariantErrs().call([ocp.xVec(0),ocp.uVec(0),ocp.pVec()]) ocp.constrain(c0,'==',0) ocp.constrain(cdot0,'==',0) ocp.constrain(dcmError0,'==',0) # constrain line angle for k in range(0,nk+1): ocp.constrain(kiteutils.getCosLineAngle(ocp,k),'>=',C.cos(55*pi/180)) # constrain airspeed def constrainAirspeedAlphaBeta(): f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()] , [dae.output('airspeed'),dae.output('alpha(deg)'),dae.output('beta(deg)')] ) f.setOption('name','airspeed/alpha/beta') f.init() for k in range(0,nk): [airspeed,alphaDeg,betaDeg] = f.call([ocp.xVec(k),ocp.uVec(k),ocp.pVec()]) ocp.constrainBnds(airspeed,(10,50)) ocp.constrainBnds(alphaDeg,(-5,10)) ocp.constrainBnds(betaDeg,(-10,10)) constrainAirspeedAlphaBeta() # make it periodic for name in [ #"x" # state 0 "y" # state 1 , "z" # state 2 # , "e11" # state 3 # , "e12" # state 4 # , "e13" # state 5 # , "e21" # state 6 # , "e22" # state 7 # , "e23" # state 8 # , "e31" # state 9 # , "e32" # state 10 # , "e33" # state 11 # , "dx" # state 12 , "dy" # state 13 , "dz" # state 14 , "w1" # state 15 , "w2" # state 16 , "w3" # state 17 , "r" # state 20 , "dr" # state 21 ]: ocp.constrain(ocp.lookup(name,timestep=0),'==',ocp.lookup(name,timestep=-1)) # periodic attitude kiteutils.periodicDcm(ocp) # bounds ocp.bound('aileron',(-0.04,0.04)) ocp.bound('elevator',(-0.1,0.1)) ocp.bound('x',(-2,200)) ocp.bound('y',(-200,200)) ocp.bound('z',(0.5,200)) ocp.bound('r',(1,30)) ocp.bound('dr',(-10,10)) ocp.bound('ddr',(-2.5,2.5)) for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']: ocp.bound(e,(-1.1,1.1)) for d in ['dx','dy','dz']: ocp.bound(d,(-70,70)) for w in ['w1','w2','w3']: ocp.bound(w,(-4*pi,4*pi)) ocp.bound('endTime',(0.5,5)) ocp.bound('w0',(10,10)) ocp.bound('energy',(-1e6,1e6)) # boundary conditions ocp.bound('energy',(0,0),timestep=0,quiet=True) ocp.bound('y',(0,0),timestep=0,quiet=True) # objective function obj = 0 for k in range(nk): u = ocp.uVec(k) ddr = ocp.lookup('ddr',timestep=k) aileron = ocp.lookup('aileron',timestep=k) elevator = ocp.lookup('elevator',timestep=k) aileronSigma = 0.1 elevatorSigma = 0.1 ddrSigma = 5.0 ailObj = aileron*aileron / (aileronSigma*aileronSigma) eleObj = elevator*elevator / (elevatorSigma*elevatorSigma) winchObj = ddr*ddr / (ddrSigma*ddrSigma) obj += ailObj + eleObj + winchObj ocp.setObjective( 1e1*C.sumAll(obj)/float(nk) + ocp.lookup('energy',timestep=-1)/ocp.lookup('endTime') ) # callback function class MyCallback: def __init__(self): self.iter = 0 def __call__(self,f,*args): self.iter = self.iter + 1 xOpt = numpy.array(f.input(C.NLP_X_OPT)) opt = ocp.devectorize(xOpt) xup = opt['vardict'] kiteProtos = [] for k in range(0,nk): j = nicp*(deg+1)*k kiteProtos.append( kiteproto.toKiteProto(C.DMatrix(opt['x'][:,j]), C.DMatrix(opt['u'][:,j]), C.DMatrix(opt['p']), conf['kite']['zt'], conf['carousel']['rArm'], zeroDelta=True) ) # kiteProtos = [kiteproto.toKiteProto(C.DMatrix(opt['x'][:,k]),C.DMatrix(opt['u'][:,k]),C.DMatrix(opt['p']), conf['kite']['zt'], conf['carousel']['rArm'], zeroDelta=True) for k in range(opt['x'].shape[1])] mc = kite_pb2.MultiCarousel() mc.css.extend(list(kiteProtos)) mc.messages.append("endTime: "+str(xup['endTime'])) mc.messages.append("w0: "+str(xup['w0'])) mc.messages.append("iter: "+str(self.iter)) # bounds feedback # lbx = ocp.solver.input(C.NLP_LBX) # ubx = ocp.solver.input(C.NLP_UBX) # violations = boundsFeedback(xOpt,lbx,ubx,ocp.bndtags,tolerance=1e-9) # for name in violations: # violmsg = "violation!: "+name+": "+str(violations[name]) # mc.messages.append(violmsg) publisher.send_multipart(["multi-carousel", mc.SerializeToString()]) # solver solverOptions = [ ("expand_f",True) , ("expand_g",True) , ("generate_hessian",True) # ,("qp_solver",C.NLPQPSolver) # ,("qp_solver_options",{'nlp_solver': C.IpoptSolver, "nlp_solver_options":{"linear_solver":"ma57"}}) , ("linear_solver","ma57") , ("max_iter",1000) , ("tol",1e-9) # , ("Timeout", 1e6) # , ("UserHM", True) # , ("ScaleConIter",True) # , ("ScaledFD",True) # , ("ScaledKKT",True) # , ("ScaledObj",True) # , ("ScaledQP",True) ] # initial guess # ocp.guessX(x0) # for k in range(0,nk+1): # val = 2.0*pi*k/nk # ocp.guess('delta',val,timestep=k,quiet=True) # # ocp.guess('aileron',0) # ocp.guess('elevator',0) # ocp.guess('tc',0) ocp.guess('endTime',5.4) # # ocp.guess('ddr',0) ocp.guess('w0',10) print "setting up collocation..." ocp.setupCollocation(ocp.lookup('endTime')) print "setting up solver..." ocp.setupSolver( solverOpts=solverOptions, callback=MyCallback() ) return ocp
def main(): nSteps = 15 print "creating model" dae = models.carousel(zt,rArm,nSteps) print "setting up OCP" ocp = MultipleShootingStage(dae, nSteps) # make the integrator print "setting up dynamics constraints" integratorOptions = [ ("reltol",1e-7) , ("abstol",1e-9) , ("t0",0) , ("tf",1) , ('name','integrator') , ("linear_solver_creator",C.CSparse) # , ("linear_solver_creator",C.LapackLUDense) , ("linear_solver","user_defined") ] ocp.setIdasIntegrator(integratorOptions) # constrain invariants def invariantErrs(): dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')]) , C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')]) , C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')]) ] ).trans() err = C.mul(dcm.trans(), dcm) dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ]) f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()] , [dae.output('c'),dae.output('cdot'),dcmErr] ) f.setOption('name','invariant errors') f.init() return f [c0,cdot0,dcmError0] = invariantErrs().call([ocp.states[:,0],ocp.actions[:,0],ocp.params]) ocp.addConstraint(c0,'==',0) ocp.addConstraint(cdot0,'==',0) ocp.addConstraint(dcmError0,'==',0) # bounds ocp.setBound('aileron',(-0.04,0.04)) ocp.setBound('elevator',(-0.1,0.1)) ocp.setBound('x',(0,4)) ocp.setBound('y',(-3,3)) ocp.setBound('z',(-2,3)) ocp.setBound('r',(1,2)) ocp.setBound('dr',(-1,1)) ocp.setBound('ddr',(0,0)) ocp.setBound('r',(1.2,1.2),timestep=0) for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']: ocp.setBound(e,(-1.1,1.1)) for d in ['dx','dy','dz']: ocp.setBound(d,(-50,50)) for w in ['w1','w2','w3']: ocp.setBound(w,(-8*pi,8*pi)) ocp.setBound('delta',(-0.01,1.01*2*pi)) ocp.setBound('ddelta',(-pi/4,8*pi)) ocp.setBound('tc',(-200,600)) # ocp.setBound('tc',(389.970797939731,389.970797939731)) ocp.setBound('endTime',(0.5,2.0)) # ocp.setBound('endTime',(1.6336935276077966,1.6336935276077966)) ocp.setBound('w0',(10,10)) # boundary conditions ocp.setBound('delta',(0,0),timestep=0) ocp.setBound('delta',(2*pi,2*pi),timestep=nSteps-1) # make it periodic ocp.addConstraint(ocp.states[:18,0],'==',ocp.states[:18,-1]) ocp.addConstraint(ocp.states[19,0],'==',ocp.states[19,-1]) ocp.addConstraint(ocp.actions[:,0],'==',ocp.actions[:,-1]) # make the solver # objective function tc0 = 390 obj = (C.sumAll(ocp.actions[0:2,:]*ocp.actions[0:2,:]) + 1e-10*C.sumAll((ocp.actions[2,:]-tc0)*(ocp.actions[2,:]-tc0)))*ocp.lookup('endTime') ocp.setObjective(obj) # zero mq setup context = zmq.Context(1) publisher = context.socket(zmq.PUB) publisher.bind("tcp://*:5563") # callback function class MyCallback: def __init__(self): self.iter = 0 def __call__(self,f,*args): self.iter = self.iter + 1 xOpt = f.input(C.NLP_X_OPT) xs,us,p = ocp.getTimestepsFromDvs(xOpt) kiteProtos = [kiteproto.toKiteProto(xs[k],us[k],p,rArm,zt) for k in range(0,nSteps)] mc = kite_pb2.KiteOpt() mc.horizon.extend(list(kiteProtos)) xup = ocp.devectorize(xOpt) mc.messages.append("endTime: "+str(xup['endTime'])) mc.messages.append("w0: "+str(xup['w0'])) mc.messages.append("iter: "+str(self.iter)) publisher.send_multipart(["multi-carousel", mc.SerializeToString()]) def makeCallback(): nd = ocp.getDesignVars().size() nc = ocp._constraints.getG().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_g = C.sp_dense(nc,1), g = C.sp_dense(nc,1) ), [C.sp_dense(1,1)] ) c.init() return c # solver solverOptions = [ ("iteration_callback",makeCallback()) , ("linear_solver","ma57") # , ("max_iter",5) ] ocp.setSolver( C.IpoptSolver, solverOptions=solverOptions ) #ocp.setBounds() # initial conditions ocp.setXGuess(x0) for k in range(0,nSteps): val = 2*pi*k/(nSteps-1) ocp.setGuess('delta',val,timestep=k,quiet=True) ocp.setGuess('aileron',0) ocp.setGuess('elevator',0) ocp.setGuess('tc',389.970797939731) ocp.setGuess('endTime',1.6336935276077966) ocp.setGuess('ddr',0) ocp.setGuess('w0',0) ocp.solve()
def _bvp_setup(self): """ Set up the casadi ipopt solver for the bvp solution """ # Define some variables deg = self.deg nk = self.nk NV = nk * (deg + 1) * self.NEQ # NLP variable vector V = cs.msym("V", NV) XD = np.resize(np.array([], dtype=cs.MX), (nk, deg + 1)) P = cs.msym("P", self.NP * self.nk) PK = P.reshape((self.nk, self.NP)) offset = 0 for k in range(nk): for j in range(deg + 1): XD[k][j] = V[offset:offset + self.NEQ] offset += self.NEQ # Constraint function for the NLP g = [] lbg = [] ubg = [] # For all finite elements for k in range(nk): # For all collocation points for j in range(1, deg + 1): # Get an expression for the state derivative at the # collocation point xp_jk = 0 for j2 in range(deg + 1): # get the time derivative of the differential states # (eq 10.19b) xp_jk += self.coll_setup_dict['C'][j2][j] * XD[k][j2] # Generate parameter set, accounting for variable # # Add collocation equations to the NLP [fk] = self.rfmod.call( [0., xp_jk / self.h, XD[k][j], PK[k, :].T]) # impose system dynamics (for the differential states # (eq # 10.19b)) g += [fk[:self.NEQ]] lbg.append(np.zeros(self.NEQ)) # equality constraints ubg.append(np.zeros(self.NEQ)) # equality constraints # Get an expression for the state at the end of the finite # element xf_k = 0 for j in range(deg + 1): xf_k += self.coll_setup_dict['D'][j] * XD[k][j] # Add continuity equation to NLP # End = Beginning of next if k + 1 != nk: g += [XD[k + 1][0] - xf_k] # At the last segment, periodicity constraints else: g += [XD[0][0] - xf_k] lbg.append(np.zeros(self.NEQ)) ubg.append(np.zeros(self.NEQ)) # Nonlinear constraint function gfcn = cs.MXFunction([V, P], [cs.vertcat(g)]) # Objective function (periodicity) ofcn = cs.MXFunction([V, P], [cs.sumAll(g[-1]**2)]) ## ---- ## SOLVE THE NLP ## ---- # Allocate an NLP solver self.solver = cs.IpoptSolver(ofcn, gfcn) # Set options self.solver.setOption("expand_f", True) self.solver.setOption("expand_g", True) self.solver.setOption("generate_hessian", True) self.solver.setOption("max_iter", 1000) self.solver.setOption("tol", self.int_opt['bvp_tol']) self.solver.setOption("constr_viol_tol", self.int_opt['bvp_constr_tol']) self.solver.setOption("linear_solver", self.int_opt['bvp_linear_solver']) self.solver.setOption('parametric', True) self.solver.setOption('print_level', self.int_opt['bvp_print_level']) # initialize the self.solver self.solver.init() self.lbg = lbg self.ubg = ubg self.need_bvp_setup = False
def CollocationSetup(self, warmstart=False): """ Sets up NLP for collocation solution. Constructs initial guess arrays, constructs constraint and objective functions, and otherwise passes arguments to the correct places. This looks really inefficient and is likely unneccessary to run multiple times for repeated runs with new data. Not sure how much time it takes compared to the NLP solution. Run immediately before CollocationSolve. """ # Dimensions of the problem nx = self.NVAR # total number of states ndiff = nx # number of differential states nalg = 0 # number of algebraic states nu = 0 # number of controls # Collocated variables NXD = self.NICP * self.NK * (self.DEG + 1) * ndiff # differential states NXA = self.NICP * self.NK * self.DEG * nalg # algebraic states NU = self.NK * nu # Parametrized controls NV = NXD + NXA + NU + self.NP + self.NMP # Total variables self.NV = NV # NLP variable vector V = cs.msym("V", NV) # All variables with bounds and initial guess vars_lb = np.zeros(NV) vars_ub = np.zeros(NV) vars_init = np.zeros(NV) offset = 0 # # Split NLP vector into useable slices # # Get the parameters P = V[offset:offset + self.NP] vars_init[offset:offset + self.NP] = self.NLPdata['p_init'] vars_lb[offset:offset + self.NP] = self.NLPdata['p_min'] vars_ub[offset:offset + self.NP] = self.NLPdata['p_max'] offset += self.NP # indexing variable # Get collocated states and parametrized control XD = np.resize(np.array([], dtype=cs.MX), (self.NK, self.NICP, self.DEG + 1)) # NB: same name as above XA = np.resize(np.array([], dtype=cs.MX), (self.NK, self.NICP, self.DEG)) # NB: same name as above U = np.resize(np.array([], dtype=cs.MX), self.NK) # Prepare the starting data matrix vars_init, vars_ub, and # vars_lb, by looping over finite elements, states, etc. Also # groups the variables in the large unknown vector V into XD and # XA(unused) for later indexing for k in range(self.NK): # Collocated states for i in range(self.NICP): # for j in range(self.DEG + 1): # Get the expression for the state vector XD[k][i][j] = V[offset:offset + ndiff] if j != 0: XA[k][i][j - 1] = V[offset + ndiff:offset + ndiff + nalg] # Add the initial condition index = (self.DEG + 1) * (self.NICP * k + i) + j if k == 0 and j == 0 and i == 0: vars_init[offset:offset+ndiff] = \ self.NLPdata['xD_init'][index,:] vars_lb[offset:offset+ndiff] = \ self.NLPdata['xDi_min'] vars_ub[offset:offset+ndiff] = \ self.NLPdata['xDi_max'] offset += ndiff else: if j != 0: vars_init[offset:offset+nx] = \ np.append(self.NLPdata['xD_init'][index,:], self.NLPdata['xA_init'][index,:]) vars_lb[offset:offset+nx] = \ np.append(self.NLPdata['xD_min'], self.NLPdata['xA_min']) vars_ub[offset:offset+nx] = \ np.append(self.NLPdata['xD_max'], self.NLPdata['xA_max']) offset += nx else: vars_init[offset:offset+ndiff] = \ self.NLPdata['xD_init'][index,:] vars_lb[offset:offset+ndiff] = \ self.NLPdata['xD_min'] vars_ub[offset:offset+ndiff] = \ self.NLPdata['xD_max'] offset += ndiff # Parametrized controls (unused here) U[k] = V[offset:offset + nu] # Attach these initial conditions to external dictionary self.NLPdata['v_init'] = vars_init self.NLPdata['v_ub'] = vars_ub self.NLPdata['v_lb'] = vars_lb # Setting up the constraint function for the NLP. Over each # collocated state, ensure continuitity and system dynamics g = [] lbg = [] ubg = [] # 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.C[j2][j] * XD[k][i][j2] # Add collocation equations to the NLP [fk] = self.rfmod.call([ 0., xp_jk / self.h, XD[k][i][j], XA[k][i][j - 1], U[k], P ]) # impose system dynamics (for the differential # states (eq 10.19b)) g += [fk[:ndiff]] lbg.append(np.zeros(ndiff)) # equality constraints ubg.append(np.zeros(ndiff)) # equality constraints # impose system dynamics (for the algebraic states # (eq 10.19b)) (unused) g += [fk[ndiff:]] lbg.append(np.zeros(nalg)) # equality constraints ubg.append(np.zeros(nalg)) # equality constraints # 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.D[j] * XD[k][i][j] # if i==self.NICP-1: # Add continuity equation to NLP if k + 1 != self.NK: # End = Beginning of next g += [XD[k + 1][0][0] - xf_k] lbg.append(-self.NLPdata['CONtol'] * np.ones(ndiff)) ubg.append(self.NLPdata['CONtol'] * np.ones(ndiff)) else: # At the last segment # Periodicity constraints (only for NEQ) g += [XD[0][0][0][:self.NEQ] - xf_k[:self.NEQ]] lbg.append(-self.NLPdata['CONtol'] * np.ones(self.NEQ)) ubg.append(self.NLPdata['CONtol'] * np.ones(self.NEQ)) # else: # g += [XD[k][i+1][0] - xf_k] # Flatten contraint arrays for last addition lbg = np.concatenate(lbg).tolist() ubg = np.concatenate(ubg).tolist() # Constraint to protect against fixed point solutions if self.NLPdata['FPgaurd'] is True: fout = self.model.call( cs.daeIn(t=self.tgrid[0], x=XD[0, 0, 0][:self.NEQ], p=V[:self.NP]))[0] g += [cs.MX(cs.sumAll(fout**2))] lbg.append(np.array(self.NLPdata['FPTOL'])) ubg.append(np.array(cs.inf)) elif self.NLPdata['FPgaurd'] is 'all': fout = self.model.call( cs.daeIn(t=self.tgrid[0], x=XD[0, 0, 0][:self.NEQ], p=V[:self.NP]))[0] g += [cs.MX(fout**2)] lbg += [self.NLPdata['FPTOL']] * self.NEQ ubg += [cs.inf] * self.NEQ # Nonlinear constraint function gfcn = cs.MXFunction([V], [cs.vertcat(g)]) # Minimize derivative of first state variable at t=0 xp_0 = 0 for j in range(self.NLPdata['DEG'] + 1): # get the time derivative of the differential # states (eq 10.19b) xp_0 += self.C[j][0] * XD[0][j][0] obj = xp_0 ofcn = cs.MXFunction([V], [obj]) self.CollocationSolver = cs.IpoptSolver(ofcn, gfcn) for opt, val in self.IpoptOpts.iteritems(): self.CollocationSolver.setOption(opt, val) self.CollocationSolver.setOption('obj_scaling_factor', len(vars_init)) if warmstart: self.CollocationSolver.setOption('warm_start_init_point', 'yes') # initialize the self.CollocationSolver self.CollocationSolver.init() # Initial condition self.CollocationSolver.setInput(vars_init, cs.NLP_X_INIT) # Bounds on x self.CollocationSolver.setInput(vars_lb, cs.NLP_LBX) self.CollocationSolver.setInput(vars_ub, cs.NLP_UBX) # Bounds on g self.CollocationSolver.setInput(np.array(lbg), cs.NLP_LBG) self.CollocationSolver.setInput(np.array(ubg), cs.NLP_UBG) if warmstart: self.CollocationSolver.setInput( \ self.WarmStartData['NLP_X_OPT'],cs.NLP_X_INIT) self.CollocationSolver.setInput( \ self.WarmStartData['NLP_LAMBDA_G'],cs.NLP_LAMBDA_INIT) self.CollocationSolver.setOutput( \ self.WarmStartData['NLP_LAMBDA_X'],cs.NLP_LAMBDA_X)
def sum_squares(a): if type(a)==casadi.SXMatrix: return casadi.sumAll(a*a) else: return (a*a).reshape(-1).sum()
def fx_polybasis(u,t): f = casadi.sumAll(casadi.polyval(u,t-.5)) return f
def normalize_casadi(x): n = casadi.sumAll(x*x) n = casadi.sqrt(n) nx = x/n return nx
def collocation_solver_setup(self, warmstart=False): """ Sets up NLP for collocation solution. Constructs initial guess arrays, constructs constraint and objective functions, and otherwise passes arguments to the correct places. This looks really inefficient and is likely unneccessary to run multiple times for repeated runs with new data. Not sure how much time it takes compared to the NLP solution. Run immediately before CollocationSolve. """ # Dimensions of the problem nx = self.NVAR # total number of states ndiff = nx # number of differential states nalg = 0 # number of algebraic states nu = 0 # number of controls # Collocated variables NXD = self.nk*(self.deg+1)*ndiff # differential states NXA = self.nk*self.deg*nalg # algebraic states NU = self.nk*nu # Parametrized controls NV = NXD+NXA+NU+self.ParamCount+self.NMP # Total variables self.NV = NV # NLP variable vector V = cs.msym("V",NV) # All variables with bounds and initial guess vars_lb = np.zeros(NV) vars_ub = np.zeros(NV) vars_init = np.zeros(NV) offset = 0 ## I AM HERE ## # # Split NLP vector into useable slices # # Get the parameters P = V[offset:offset+self.ParamCount] vars_init[offset:offset+self.ParamCount] = self.NLPdata['p_init'] vars_lb[offset:offset+self.ParamCount] = self.NLPdata['p_min'] vars_ub[offset:offset+self.ParamCount] = self.NLPdata['p_max'] # Initial conditions for measurement adjustment MP = V[self.NV-self.NMP:] vars_init[self.NV-self.NMP:] = np.ones(self.NMP) vars_lb[self.NV-self.NMP:] = 0.1*np.ones(self.NMP) vars_ub[self.NV-self.NMP:] = 10*np.ones(self.NMP) pdb.set_trace() offset += self.np # indexing variable # Get collocated states and parametrized control XD = np.resize(np.array([], dtype=cs.MX), (self.nk, self.points_per_interval, self.deg+1)) # NB: same name as above XA = np.resize(np.array([],dtype=cs.MX),(self.nk,self.points_per_interval,self.deg)) # NB: same name as above U = np.resize(np.array([],dtype=cs.MX),self.nk) # Prepare the starting data matrix vars_init, vars_ub, and # vars_lb, by looping over finite elements, states, etc. Also # groups the variables in the large unknown vector V into XD and # XA(unused) for later indexing for k in range(self.nk): # Collocated states for i in range(self.points_per_interval): # for j in range(self.deg+1): # Get the expression for the state vector XD[k][i][j] = V[offset:offset+ndiff] if j !=0: XA[k][i][j-1] = V[offset+ndiff:offset+ndiff+nalg] # Add the initial condition index = (self.deg+1)*(self.points_per_interval*k+i) + j if k==0 and j==0 and i==0: vars_init[offset:offset+ndiff] = \ self.NLPdata['xD_init'][index,:] vars_lb[offset:offset+ndiff] = \ self.NLPdata['xDi_min'] vars_ub[offset:offset+ndiff] = \ self.NLPdata['xDi_max'] offset += ndiff else: if j!=0: vars_init[offset:offset+nx] = \ np.append(self.NLPdata['xD_init'][index,:], self.NLPdata['xA_init'][index,:]) vars_lb[offset:offset+nx] = \ np.append(self.NLPdata['xD_min'], self.NLPdata['xA_min']) vars_ub[offset:offset+nx] = \ np.append(self.NLPdata['xD_max'], self.NLPdata['xA_max']) offset += nx else: vars_init[offset:offset+ndiff] = \ self.NLPdata['xD_init'][index,:] vars_lb[offset:offset+ndiff] = \ self.NLPdata['xD_min'] vars_ub[offset:offset+ndiff] = \ self.NLPdata['xD_max'] offset += ndiff # Parametrized controls (unused here) U[k] = V[offset:offset+nu] # Attach these initial conditions to external dictionary self.NLPdata['v_init'] = vars_init self.NLPdata['v_ub'] = vars_ub self.NLPdata['v_lb'] = vars_lb # Setting up the constraint function for the NLP. Over each # collocated state, ensure continuitity and system dynamics g = [] lbg = [] ubg = [] # For all finite elements for k in range(self.nk): for i in range(self.points_per_interval): # 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.C[j2][j]*XD[k][i][j2] # Add collocation equations to the NLP [fk] = self.rfmod.call([0., xp_jk/self.h, XD[k][i][j], XA[k][i][j-1], U[k], P]) # impose system dynamics (for the differential # states (eq 10.19b)) g += [fk[:ndiff]] lbg.append(np.zeros(ndiff)) # equality constraints ubg.append(np.zeros(ndiff)) # equality constraints # impose system dynamics (for the algebraic states # (eq 10.19b)) (unused) g += [fk[ndiff:]] lbg.append(np.zeros(nalg)) # equality constraints ubg.append(np.zeros(nalg)) # equality constraints # 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.D[j]*XD[k][i][j] # if i==self.points_per_interval-1: # Add continuity equation to NLP if k+1 != self.nk: # End = Beginning of next g += [XD[k+1][0][0] - xf_k] lbg.append(-self.NLPdata['CONtol']*np.ones(ndiff)) ubg.append(self.NLPdata['CONtol']*np.ones(ndiff)) else: # At the last segment # Periodicity constraints (only for NEQ) g += [XD[0][0][0][:self.neq] - xf_k[:self.neq]] lbg.append(-self.NLPdata['CONtol']*np.ones(self.neq)) ubg.append(self.NLPdata['CONtol']*np.ones(self.neq)) # else: # g += [XD[k][i+1][0] - xf_k] # Flatten contraint arrays for last addition lbg = np.concatenate(lbg).tolist() ubg = np.concatenate(ubg).tolist() # Constraint to protect against fixed point solutions if self.NLPdata['FPgaurd'] is True: fout = self.model.call(cs.daeIn(t=self.tgrid[0], x=XD[0,0,0][:self.neq], p=V[:self.np]))[0] g += [cs.MX(cs.sumAll(fout**2))] lbg.append(np.array(self.NLPdata['FPTOL'])) ubg.append(np.array(cs.inf)) elif self.NLPdata['FPgaurd'] is 'all': fout = self.model.call(cs.daeIn(t=self.tgrid[0], x=XD[0,0,0][:self.neq], p=V[:self.np]))[0] g += [cs.MX(fout**2)] lbg += [self.NLPdata['FPTOL']]*self.neq ubg += [cs.inf]*self.neq # Nonlinear constraint function gfcn = cs.MXFunction([V],[cs.vertcat(g)]) # Get Linear Interpolant for YDATA from TDATA objlist = [] # xarr = np.array([V[self.np:][i] for i in \ # xrange(self.neq*self.nk*(self.deg+1))]) # xarr = xarr.reshape([self.nk,self.deg+1,self.neq]) # List of the times when each finite element starts felist = self.tgrid.reshape((self.nk,self.deg+1))[:,0] felist = np.hstack([felist, self.tgrid[-1]]) def z(tau, zs): """ Functon to calculate the interpolated values of z^K at a given tau (0->1). zs is a matrix with the symbolic state values within the finite element """ def l(j,t): """ Intermediate values for polynomial interpolation """ tau = self.NLPdata['collocation_points']\ [self.NLPdata['cp']][self.deg] return np.prod(np.array([ (t - tau[k])/(tau[j] - tau[k]) for k in xrange(0,self.deg+1) if k is not j])) interp_vector = [] for i in xrange(self.neq): # only state variables interp_vector += [np.sum(np.array([l(j, tau)*zs[j][i] for j in xrange(0, self.deg+1)]))] return interp_vector # Set up Objective Function by minimizing distance from # Collocation solution to Measurement Data # Number of measurement functions for i in xrange(self.NM): # Number of sampling points per measurement] for j in xrange(len(self.tdata[i])): # the interpolating polynomial wants a tau value, # where 0 < tau < 1, distance along current element. # Get the index for which finite element of the tdata # values feind = get_ind(self.tdata[i][j],felist)[0] # Get the starting time and tau (0->1) for the tdata taustart = felist[feind] tau = (self.tdata[i][j] - taustart)*(self.nk+1)/self.tf x_interp = z(tau, XD[feind][0]) # Broken in newest numpy version, likely need to redo # this whole file with most recent versions y_model = self.Amatrix[i].dot(x_interp) # Add measurement scaling if self.mpars[i]: y_model *= MP[sum(self.mpars[:i])] # Using relative diff's is probably messing up weights # in Identifiability diff = (y_model - self.ydata[i][j]) if self.NLPdata['ObjMethod'] == 'lsq': dist = (diff**2/self.edata[i][j]**2) elif self.NLPdata['ObjMethod'] == 'laplace': dist = cs.fabs(diff)/np.sqrt(self.edata[i][j]) try: dist *= self.NLPdata['state_weights'][i] except KeyError: pass objlist += [dist] # Minimization Objective if self.minimize_f: # Function integral f_integral = 0 # For each finite element for i in xrange(self.nk): # For each collocation point fvals = [] for j in xrange(self.deg+1): fvals += [self.minimize_f(XD[i][0][j], P)] # integrate with weights f_integral += sum([fvals[k] * self.E[k] for k in xrange(self.deg+1)]) objlist += [self.NLPdata['f_minimize_weight']*f_integral] # Stability Objective (Floquet Multipliers) if self.monodromy: s_final = XD[-1,-1,-1][-self.neq**2:] s_final = s_final.reshape((self.neq,self.neq)) trace = sum([s_final[i,i] for i in xrange(self.neq)]) objlist += [self.NLPdata['stability']*(trace - 1)**2] # Objective function of the NLP obj = cs.sumAll(cs.vertcat(objlist)) ofcn = cs.MXFunction([V], [obj]) self.CollocationSolver = cs.IpoptSolver(ofcn,gfcn) for opt,val in self.IpoptOpts.iteritems(): self.CollocationSolver.setOption(opt,val) self.CollocationSolver.setOption('obj_scaling_factor', len(vars_init)) if warmstart: self.CollocationSolver.setOption('warm_start_init_point','yes') # initialize the self.CollocationSolver self.CollocationSolver.init() # Initial condition self.CollocationSolver.setInput(vars_init,cs.NLP_X_INIT) # Bounds on x self.CollocationSolver.setInput(vars_lb,cs.NLP_LBX) self.CollocationSolver.setInput(vars_ub,cs.NLP_UBX) # Bounds on g self.CollocationSolver.setInput(np.array(lbg),cs.NLP_LBG) self.CollocationSolver.setInput(np.array(ubg),cs.NLP_UBG) if warmstart: self.CollocationSolver.setInput( \ self.WarmStartData['NLP_X_OPT'],cs.NLP_X_INIT) self.CollocationSolver.setInput( \ self.WarmStartData['NLP_LAMBDA_G'],cs.NLP_LAMBDA_INIT) self.CollocationSolver.setOutput( \ self.WarmStartData['NLP_LAMBDA_X'],cs.NLP_LAMBDA_X) pdb.set_trace()
def parameter_estimation(self): """ Again, borrow from PSJ. Gets a decent estimate of the parameters initial values by looking at slopes. I think. Here we will set up an NLP to estimate the initial parameter guess (potentially along with unmeasured state variables) by minimizing the difference between the calculated slope at each shooting node (a function of the parameters) and the measured slope (using a spline interpolant) """ # Here we must interpolate the x_init data using a spline. We # will differentiate this spline to get intial values for # the parameters node_ts = self.tgrid.reshape((self.nk, self.deg+1))[:,0] try: f_init = self.sx(self.tgrid,1).T except AttributeError: # Create interpolation object sx = fnlist([]) def flat_factory(x): """ Protects against nan's resulting from flat trajectories in spline curve """ return lambda t, d: (np.array([x] * len(t)) if d is 0 else np.array([0] * len(t))) for x in np.array(self.x_init).T: tvals = list(node_ts) tvals.append(tvals[0] + self.tf) xvals = list(x.reshape((self.nk, self.deg+1))[:,0]) ## here ## xvals.append(x[0]) if np.linalg.norm(xvals - xvals[0]) < 1E-8: # flat profile sx += [flat_factory(xvals[0])] else: sx += [spline(tvals, xvals, 0)] f_init = sx(self.tgrid,1).T # set initial guess for parameters logmax = np.log10(np.array(self.PARMAX)) logmin = np.log10(np.array(self.PARMIN)) p_init = 10**(logmax - (logmax - logmin)/2) f = self.model V = cs.MX("V", self.ParamCount) par = V # Allocate the initial conditions pvars_init = np.ones(self.ParamCount) pvars_lb = np.array(self.PARMIN) pvars_ub = np.array(self.PARMAX) pvars_init = p_init xin = [] fin = [] # For each state, add the (variable or constant) MX # representation of the measured x and f value to the input # variable list. for state in xrange(self.neq): xin += [cs.MX(self.x_init[:,state])] fin += [cs.MX(f_init[:,state])] xin = cs.horzcat(xin) fin = cs.horzcat(fin) # For all nodes res = [] xmax = self.x_init.max(0) for ii in xrange((self.deg+1)*self.nk): f_out = f.call(cs.daeIn(t=self.tgrid[ii], x=xin[ii,:].T, p=par))[0] res += [cs.sumAll(((f_out - fin[ii,:].T) / xmax)**2)] F = cs.MXFunction([V],[cs.sumAll(cs.vertcat(res))]) F.init() parsolver = cs.IpoptSolver(F) for opt,val in self.IpoptOpts.iteritems(): if not opt == "expand_g": parsolver.setOption(opt,val) parsolver.init() parsolver.setInput(pvars_init,cs.NLP_X_INIT) parsolver.setInput(pvars_lb,cs.NLP_LBX) parsolver.setInput(pvars_ub,cs.NLP_UBX) parsolver.solve() success = parsolver.getStat('return_status') == 'Solve_Succeeded' assert success, "Parameter Estimation Failed" self.pcost = float(parsolver.output(cs.NLP_COST)) pvars_opt = np.array(parsolver.output( \ cs.NLP_X_OPT)).flatten() if success: return pvars_opt else: return False