def create_nlp(self, var, par, obj, con, options): jit = options['codegen'] if options['verbose'] >= 2: print 'Building nlp ... ', if jit['jit']: print('[jit compilation with flags %s]' % (','.join(jit['jit_options']['flags']))), t0 = time.time() nlp = MXFunction('nlp', nlpIn(x=var, p=par), nlpOut(f=obj, g=con)) solver = NlpSolver('solver', 'ipopt', nlp, options['solver']) grad_f, jac_g = nlp.gradient('x', 'f'), nlp.jacobian('x', 'g') hess_lag = solver.hessLag() var, par = struct_symSX(var), struct_symSX(par) nlp = SXFunction('nlp', [var, par], nlp([var, par]), jit) grad_f = SXFunction('grad_f', [var, par], grad_f([var, par]), jit) jac_g = SXFunction('jac_g', [var, par], jac_g([var, par]), jit) lam_f, lam_g = SX.sym('lam_f'), SX.sym('lam_g', con.size) hess_lag = SXFunction('hess_lag', [var, par, lam_f, lam_g], hess_lag([var, par, lam_f, lam_g]), jit) options['solver'].update({'grad_f': grad_f, 'jac_g': jac_g, 'hess_lag': hess_lag}) problem = NlpSolver('solver', 'ipopt', nlp, options['solver']) t1 = time.time() if options['verbose'] >= 2: print 'in %5f s' % (t1-t0) return problem, (t1-t0)
def setUp(self): self.x = ca.MX.sym("x") self.f = ca.MX.sym("f") self.g = ca.MX.sym("g") self.nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.x), ca.nlpOut(f=self.f, g=self.g))
def create_plan_fc(b0, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=belief), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Objective function m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']] m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']] dm_bc = m_bN - m_cN J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0 # Regularize controls J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs g = [] for k in range(N_sim): # Multiple shooting [x_next] = BF([V['X',k], V['U',k]]) g.append(x_next - V['X',k+1]) # Penalize uncertainty J += 1e-1 * ca.trace(V['X',k,'S']) * dt g = ca.vertcat(g) # log-probability, doesn't work with full collocation #Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']] #J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \ # ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # 0 <= v <= v_max lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # m(t=0) = m0 lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m'] # S(t=0) = S0 lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S'] # Solve the NLP sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) return V(sol['x'])
def create_belief_plan(cls, model, warm_start=False, x0=0, lam_x0=0, lam_g0=0): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X', repeat=model.n+1, struct=model.x), cat.entry('U', repeat=model.n, struct=model.u) ) ]) # Box constraints [lbx, ubx] = cls._create_box_constraints(model, V) # Non-linear constraints [g, lbg, ubg] = cls._create_belief_nonlinear_constraints(model, V) # Objective function J = cls._create_belief_objective_function(model, V) # Formulate non-linear problem nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g)) op = {# Linear solver #'linear_solver': 'ma57', # Warm start # 'warm_start_init_point': 'yes', # Termination 'max_iter': 1500, 'tol': 1e-6, 'constr_viol_tol': 1e-5, 'compl_inf_tol': 1e-4, # Acceptable termination 'acceptable_tol': 1e-3, 'acceptable_iter': 5, 'acceptable_obj_change_tol': 1e-2, # NLP # 'fixed_variable_treatment': 'make_constraint', # Quasi-Newton 'hessian_approximation': 'limited-memory', 'limited_memory_max_history': 5, 'limited_memory_max_skipping': 1} if warm_start: op['warm_start_init_point'] = 'yes' op['fixed_variable_treatment'] = 'make_constraint' # Initialize solver solver = ca.NlpSolver('solver', 'ipopt', nlp, op) # Solve if warm_start: sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0) else: sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) return V(sol['x']), sol['lam_x'], sol['lam_g']
def setupSolver(self, solverOpts=[], constraintFunOpts=[], callback=None): if not self.collocationIsSetup: raise ValueError("you forgot to call setupCollocation") g = self._constraints.getG() lbg = self._constraints.getLb() ubg = self._constraints.getUb() # Objective function/constraints of the NLP if not hasattr(self, '_objective'): raise ValueError('need to set objective function') nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()), CS.nlpOut(f=self._objective, g=g)) setFXOptions(nlp, constraintFunOpts) nlp.init() # solver callback (optional) if callback is not None: nd = self._dvMap.vectorize().size() nc = self._constraints.getG().size() c = CS.PyFunction( callback, CS.nlpSolverOut(x=CS.sp_dense(nd, 1), f=CS.sp_dense(1, 1), lam_x=CS.sp_dense(nd, 1), lam_g=CS.sp_dense(nc, 1), lam_p=CS.sp_dense(0, 1), g=CS.sp_dense(nc, 1)), [CS.sp_dense(1, 1)]) c.init() solverOpts.append(("iteration_callback", c)) # Allocate an NLP solver self.solver = CS.IpoptSolver(nlp) # self.solver = CS.WorhpSolver(nlp) # self.solver = CS.SQPMethod(nlp) # Set options setFXOptions(self.solver, solverOpts) # initialize the solver self.solver.init() # Bounds on g self.solver.setInput(lbg, 'lbg') self.solver.setInput(ubg, 'ubg') ## Nonlinear constraint function, for debugging gfcn = CS.MXFunction([self._dvMap.vectorize()], [g]) gfcn.init() setFXOptions(gfcn, constraintFunOpts) self._gfcn = gfcn
def setupSolver(self,solverOpts=[],constraintFunOpts=[],callback=None): if not self.collocationIsSetup: raise ValueError("you forgot to call setupCollocation") g = self._constraints.getG() lbg = self._constraints.getLb() ubg = self._constraints.getUb() # Objective function/constraints of the NLP if not hasattr(self,'_objective'): raise ValueError('need to set objective function') nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()),CS.nlpOut(f=self._objective, g=g)) setFXOptions(nlp,constraintFunOpts) nlp.init() # solver callback (optional) if callback is not None: nd = self._dvMap.vectorize().size() nc = self._constraints.getG().size() c = CS.PyFunction( callback, CS.nlpSolverOut(x = CS.sp_dense(nd,1), f = CS.sp_dense(1,1), lam_x = CS.sp_dense(nd,1), lam_g = CS.sp_dense(nc,1), lam_p = CS.sp_dense(0,1), g = CS.sp_dense(nc,1) ), [CS.sp_dense(1,1)] ) c.init() solverOpts.append( ("iteration_callback", c) ) # Allocate an NLP solver self.solver = CS.IpoptSolver(nlp) # self.solver = CS.WorhpSolver(nlp) # self.solver = CS.SQPMethod(nlp) # Set options setFXOptions(self.solver, solverOpts) # initialize the solver self.solver.init() # Bounds on g self.solver.setInput(lbg,'lbg') self.solver.setInput(ubg,'ubg') ## Nonlinear constraint function, for debugging gfcn = CS.MXFunction([self._dvMap.vectorize()],[g]) gfcn.init() setFXOptions(gfcn,constraintFunOpts) self._gfcn = gfcn
def create_simple_plan(x0, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=state), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Objective function x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']] x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']] dx_bc = x_bN - x_cN J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN # Regularize controls J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and non-linear control constraints g = [] for k in range(N_sim): # Multiple shooting [x_next] = F([V['X',k], V['U',k], dt]) g.append(x_next - V['X',k+1]) g = ca.vertcat(g) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) opts = {} #opts['hessian_approximation'] = 'limited-memory' opts['linear_solver'] = 'ma57' # Create a solver solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # 0 <= v <= v_max lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # m(t=0) = m0 lbx['X',0] = ubx['X',0] = x0 # Solve the NLP sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) return V(sol['x'])
def findZ(self, u, x0=None, z0=None): if z0 is None: z0 = self.z0 else: z0 = z0 / self.algStateScaling if x0 is None: x0 = self.x0 else: x0 = x0 / self.stateScaling u = u / self.controlScaling ocp = self.ocp stateScaling = self.stateScaling algStateScaling = self.algStateScaling controlScaling = self.controlScaling algS = C.substitute( ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) nlp = C.SXFunction(C.nlpIn(x=ocp.z, p=C.vertcat([ocp.x, ocp.u])), C.nlpOut(f=C.SX(0), g=algS)) C.NlpSolver.loadPlugin('ipopt') solver = C.NlpSolver('ipopt', nlp) solver.setOption('print_user_options', 'no') solver.setOption('tol', 1e-10) solver.setOption('print_level', 0) solver.setOption('file_print_level', 0) solver.setOption("max_iter", 200) # IPOPT maximum iterations solver.init() solver.setInput(NP.zeros(ocp.z.size()), 'lbg') # g_L solver.setInput(NP.zeros(ocp.z.size()), 'ubg') # g_U solver.setInput(z0, 'x0') solver.setInput(C.vertcat([x0, u]), 'p') solver.evaluate() self.z0 = solver.output('x') return solver.output('x') * self.algStateScaling
def _initialize_solver(self, **kwargs): # Initialize NLP object self._nlp = cs.SXFunction( 'nlp', cs.nlpIn(x = self.var.vars_sx, p = self.col_vars['alpha']), cs.nlpOut(f = self.objective_sx, g = cs.vertcat(self.constraints_sx))) opts = { 'max_iter' : 10000, 'linear_solver' : 'ma27' } if kwargs is not None: opts.update(kwargs) self._solver = cs.NlpSolver("solver", "ipopt", self._nlp, opts)
def create_plan(cls, model, warm_start=False, x0=0, lam_x0=0, lam_g0=0): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X', repeat=model.n+1, struct=model.x), cat.entry('U', repeat=model.n, struct=model.u) ) ]) # Box constraints [lbx, ubx] = cls._create_box_constraints(model, V) # Force the catcher to always look forward # lbx['U', :, 'theta'] = ubx['U', :, 'theta'] = 0 # Non-linear constraints [g, lbg, ubg] = cls._create_nonlinear_constraints(model, V) # Objective function J = cls._create_objective_function(model, V, warm_start) # Formulate non-linear problem nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g)) op = {# Linear solver #'linear_solver': 'ma57', # Acceptable termination 'acceptable_iter': 5} if warm_start: op['warm_start_init_point'] = 'yes' op['fixed_variable_treatment'] = 'make_constraint' # Initialize solver solver = ca.NlpSolver('solver', 'ipopt', nlp, op) # Solve if warm_start: sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg, lam_x0=lam_x0, lam_g0=lam_g0) else: sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) return V(sol['x']), sol['lam_x'], sol['lam_g']
def set_data(self, data): """ Attach experimental measurement data. data : a pd.DataFrame object Data should have columns corresponding to the state labels in self.state_names, with an index corresponding to the measurement times. """ # Should raise an error if no state name is present df = data.loc[:, self.state_names] # Rename columns with state indicies df.columns = np.arange(self.NEQ) # Remove empty (nonmeasured) states self.data = df.loc[:, ~pd.isnull(df).all(0)] obj_list = [] for ((ti, state), xi) in self.data.stack().iteritems(): obj_list += [ (self._get_interp(ti, [state]) - xi) / self.data[state].max() ] obj_resid = cs.sum_square(cs.vertcat(obj_list)) # ts = data.index # Define objective function # obj_resid = cs.sum_square(cs.vertcat( # [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0) # for ti, xi in zip(ts, xs)])) alpha = cs.SX.sym('alpha') obj_lasso = alpha * cs.sumRows(cs.fabs(self._P)) self._obj = obj_resid + obj_lasso # Create the solver object self._nlp = cs.SXFunction('nlp', cs.nlpIn(x=self._V, p=alpha), cs.nlpOut(f=self._obj, g=self._g))
def estimate_BtoAng(theta_0,phal,joint,s,b): theta_x = cs.SX.sym('theta',3) print "theta_0", theta_0 print "phal",phal measB = [float(i) for i in b.tolist()] print "b",measB f = cs.norm_2(angToB(theta_x,phal,joint,s)-measB) # f = cs.norm_2(testFun(theta_x)-b) nlp = cs.SXFunction( cs.nlpIn(x=theta_x), cs.nlpOut(f=f)) solver = cs.NlpSolver("ipopt", nlp) solver.init() solver.setInput(theta_0,'x0') solver.setInput(0.,'lbx') solver.setInput([np.pi/2,np.pi/(180/110),np.pi/2],'ubx') solver.evaluate() x = solver.getOutput('x') # val = solver.getOutput('f') return x
def setupSolver(self,solverOpts=[],constraintFunOpts=[],callback=None,solver='ipopt'): if not self.collocationIsSetup: raise ValueError("you forgot to call setupCollocation") g = self._constraints.getG() lbg = self._constraints.getLb() ubg = self._constraints.getUb() # Objective function/constraints of the NLP if not hasattr(self,'_objective'): raise ValueError('need to set objective function') nlp = CS.MXFunction(CS.nlpIn(x=self._dvMap.vectorize()),CS.nlpOut(f=self._objective, g=g)) setFXOptions(nlp,constraintFunOpts) nlp.init() # solver callback (optional) if callback is not None: @pycallback def c(f): return callback(f) solverOpts.append( ("iteration_callback", c) ) # Allocate an NLP solver self.solver = CS.NlpSolver(solver,nlp) # Set options setFXOptions(self.solver, solverOpts) # initialize the solver self.solver.init() # Bounds on g self.solver.setInput(lbg,'lbg') self.solver.setInput(ubg,'ubg') ## Nonlinear constraint function, for debugging gfcn = CS.MXFunction([self._dvMap.vectorize()],[g]) gfcn.init() setFXOptions(gfcn,constraintFunOpts) self._gfcn = gfcn
def set_data(self, data): """ Attach experimental measurement data. data : a pd.DataFrame object Data should have columns corresponding to the state labels in self.state_names, with an index corresponding to the measurement times. """ # Should raise an error if no state name is present df = data.loc[:, self.state_names] # Rename columns with state indicies df.columns = np.arange(self.NEQ) # Remove empty (nonmeasured) states self.data = df.loc[:, ~pd.isnull(df).all(0)] obj_list = [] for ((ti, state), xi) in self.data.stack().iteritems(): obj_list += [(self._get_interp(ti, [state]) - xi) / self.data[state].max()] obj_resid = cs.sum_square(cs.vertcat(obj_list)) # ts = data.index # Define objective function # obj_resid = cs.sum_square(cs.vertcat( # [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0) # for ti, xi in zip(ts, xs)])) alpha = cs.SX.sym("alpha") obj_lasso = alpha * cs.sumRows(cs.fabs(self._P)) self._obj = obj_resid + obj_lasso # Create the solver object self._nlp = cs.SXFunction("nlp", cs.nlpIn(x=self._V, p=alpha), cs.nlpOut(f=self._obj, g=self._g))
def _initialize_solver(self, **kwargs): # Initialize NLP object self._nlp = cs.SXFunction( 'nlp', cs.nlpIn(x = self.var.vars_sx, p = self.pvar.vars_sx), cs.nlpOut(f = self.objective_sx, g = cs.vertcat(self._constraints_sx))) opts = { 'max_iter' : 10000, 'linear_solver' : 'ma27' } if kwargs is not None: opts.update(kwargs) self._solver_opts = opts self._solver = cs.NlpSolver("solver", "ipopt", self._nlp, self._solver_opts) self.col_vars['lbg'] = np.concatenate(self._constraints_lb) self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
def create_plan_pc(b0, BF, dt, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=state), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Final coordinate x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']] x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']] dx_bc = x_bN - x_cN # Final velocity v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']] v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']] dv_bc = v_bN - v_cN # Angle between gaze and ball velocity theta = V['X',N_sim,'theta'] phi = V['X',N_sim,'phi'] d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ]) r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']] r_unit = r / (ca.norm_2(r) + 1e-2) d_gaze = d - r_unit # Regularize controls J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs bk = cat.struct_SX(belief) bk['S'] = b0['S'] g, lbg, ubg = [], [], [] for k in range(N_sim): # Belief propagation bk['m'] = V['X',k] [bk_next] = BF([ bk, V['U',k] ]) bk_next = belief(bk_next) # simplify indexing # Multiple shooting g.append(bk_next['m'] - V['X',k+1]) lbg.append(ca.DMatrix.zeros(nx)) ubg.append(ca.DMatrix.zeros(nx)) # Control constraints g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi'])) lbg.append(-ca.inf) ubg.append(ca.DMatrix([0])) # Advance time bk = bk_next g = ca.vertcat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) # Simple cost J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity #J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity J += 1e1 * ca.trace(bk_next['S']) # uncertainty # log-probability cost #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']] #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # State constraints # catcher can look within the upper hemisphere lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max # Control constraints # 0 <= F lbx['U',:,'F'] = 0 # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # -pi <= psi <= pi lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi # m(t=0) = m0 lbx['X',0] = ubx['X',0] = b0['m'] # Take care of the time #lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf # Simulation ends when the ball touches the ground #lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0 # Solve the NLP sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) return V(sol['x'])
Xk_end = integrator({"x0": Xk, "p": U[k]})["xf"] # append continuity constraints g.append(Xk_next - Xk_end) g_min.append(np.zeros(Xk.nnz())) g_max.append(np.zeros(Xk.nnz())) # Objective function: L(T) f = X2[nk] # Continuity constraints: 0<= x(T(k+1)) - X(T(k)) <=0 g = ca.vertcat(g) # Create NLP solver instance opts = {"linear_solver": "ma27"} nlp = ca.MXFunction("nlp", ca.nlpIn(x=V), ca.nlpOut(f=f, g=g)) solver = ca.NlpSolver("solver", "ipopt", nlp, opts) # Solve the problem sol = solver({"lbx": VMIN, "ubx": VMAX, "x0": VINIT, "lbg": np.concatenate(g_min), "ubg": np.concatenate(g_max)}) # Retrieve the solution v_opt = sol["x"] u_opt = np.array(v_opt[0:nk]) x0_opt = np.array(v_opt[nk + 0 :: 5]) x1_opt = np.array(v_opt[nk + 1 :: 5]) x2_opt = np.array(v_opt[nk + 2 :: 5]) x3_opt = np.array(v_opt[nk + 3 :: 5]) # Get values at the beginning of each finite element tgrid_x = np.linspace(0, tf, nk + 1)
X = ca.MX([0, 1]) # Objective function f = 0 # Build a graph of integrator calls for k in range(nk): X, QF = itemgetter('xf', 'qf')(integrator({'x0': X, 'p': U[k]})) f += QF # Terminal constraints: x_0(T)=x_1(T)=0 g = X # Allocate an NLP solver opts = {'linear_solver': 'ma27'} nlp = ca.MXFunction("nlp", ca.nlpIn(x=x), ca.nlpOut(f=f, g=g)) solver = ca.NlpSolver("solver", "ipopt", nlp, opts) # Solve the problem sol = solver({"lbx": -0.75, "ubx": 1, "x0": 0, "lbg": 0, "ubg": 0}) # Retrieve the solution u_opt = NP.array(sol["x"]) print(sol) # Time grid tgrid_x = NP.linspace(0, 10, nk + 1) tgrid_u = NP.linspace(0, 10, nk) # Plot the results plt.figure(1)
# Control constraints g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi'])) lbg.append(-ca.inf) ubg.append(ca.DMatrix([0])) g = ca.vertcat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) # Objective function J += 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN J += 1e0 * ca.mul(dv_bc.T, dv_bc) # v_cN -> v_bN #J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) opts = {} #opts['hessian_approximation'] = 'limited-memory' opts['linear_solver'] = 'ma57' # Create a solver solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # State constraints # catcher can look within the upper hemisphere lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
geom = geometry.Geometry(thetaLoc, chordLoc, yLoc, aIncGeometricLoc, clPolyLoc, bref, n) (makeMeZero, alphaiLoc, CL, CDi) = setupImplicitFunction(dvs['operAlpha'], dvs['An'], geom) outputsFcn = C.SXFunction([dvs.cat], [alphaiLoc, CL, CDi, geom.sref]) outputsFcn.init() g = CT.struct_SX([ CT.entry("makeMeZero",expr=makeMeZero), CT.entry('alphaiLoc',expr=alphaiLoc), CT.entry("CL",expr=CL), CT.entry("CDi",expr=CDi), CT.entry("sref",expr=geom.sref)]) obj = -CL / (CDi + 0.01) nlp = C.SXFunction(C.nlpIn(x=dvs),C.nlpOut(f=obj,g=g)) # callback class MyCallback: def __init__(self): self.iters = [] def __call__(self,f,*args): self.iters.append(numpy.array(f.getInput("x"))) mycallback = MyCallback() pyfun = C.PyFunction( mycallback, C.nlpSolverOut(x=C.sp_dense(dvs.size,1), f=C.sp_dense(1,1), lam_x=C.sp_dense(dvs.size,1), lam_g = C.sp_dense(g.size,1), lam_p = C.sp_dense(0,1), g = C.sp_dense(g.size,1) ), [C.sp_dense(1,1)] )
def defineOCP(self, ocp, DT=20, controlCost=0, xOpt=[], uOpt=[], finalStateCost=1, deltaUCons=[]): self.ocp = ocp ocp = self.ocp self.DT = DT self.n_k = int(self.ocp.tf / self.DT) self.controlCost = controlCost stateScaling = C.vertcat([ ocp.variable(ocp.x[k].getName()).nominal for k in range(ocp.x.size()) ]) algStateScaling = C.vertcat([ ocp.variable(ocp.z[k].getName()).nominal for k in range(ocp.z.size()) ]) controlScaling = C.vertcat([ ocp.variable(ocp.u[k].getName()).nominal for k in range(ocp.u.size()) ]) xOpt = xOpt / stateScaling uOpt = uOpt / controlScaling self.xOpt = xOpt self.uOpt = uOpt self.stateScaling = C.vertcat([ ocp.variable(ocp.x[k].getName()).nominal for k in range(ocp.x.size()) ]) self.algStateScaling = C.vertcat([ ocp.variable(ocp.z[k].getName()).nominal for k in range(ocp.z.size()) ]) self.controlScaling = C.vertcat([ ocp.variable(ocp.u[k].getName()).nominal for k in range(ocp.u.size()) ]) odeS = C.substitute( ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / stateScaling algS = C.substitute( ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) ltermS = C.substitute( ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) sysIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t) sysOut = C.daeOut(ode=odeS, alg=algS, quad=ltermS) odeF = C.SXFunction(sysIn, sysOut) odeF.init() C.Integrator.loadPlugin("idas") G = C.Integrator("idas", odeF) G.setOption("reltol", self.INTG_REL_TOL) #for CVODES and IDAS G.setOption("abstol", self.INTG_ABS_TOL) #for CVODES and IDAS G.setOption("max_multistep_order", 5) #for CVODES and IDAS G.setOption("max_step_size", self.IDAS_MAX_STEP_SIZE) #for IDAS only G.setOption("tf", self.DT) self.G = G #============================================================================== # G.setOption('verbose',True) # G.addMonitor('res') # G.addMonitor('inputs') # G.addMonitor('outputs') #G.addMonitor('djacB') # G.addMonitor('bjacB') # G.addMonitor('jtimesB') # G.addMonitor('psetup') # G.addMonitor('psetupB') # G.addMonitor('psolveB') # G.addMonitor('resB') # G.addMonitor('resS') # G.addMonitor('rhsQB') #============================================================================== G.init() self.n_u = self.ocp.u.size() self.n_x = self.ocp.x.size() self.n_v = self.n_u * self.n_k + self.n_x * self.n_k self.V = C.MX.sym("V", int(self.n_v), 1) self.U, self.X = self.splitVariables(self.V) uMin = C.vertcat([ self.ocp.variable(self.ocp.u[i].getName()).min.getValue() for i in range(self.n_u) ]) / controlScaling uMax = C.vertcat([ self.ocp.variable(self.ocp.u[i].getName()).max.getValue() for i in range(self.n_u) ]) / controlScaling UMIN = C.vertcat([uMin for k in range(self.n_k)]) UMAX = C.vertcat([uMax for k in range(self.n_k)]) xMin = C.vertcat([ self.ocp.variable(self.ocp.x[i].getName()).min.getValue() for i in range(self.n_x) ]) / stateScaling xMax = C.vertcat([ self.ocp.variable(self.ocp.x[i].getName()).max.getValue() for i in range(self.n_x) ]) / stateScaling XMIN = C.vertcat([xMin for k in range(self.n_k)]) XMAX = C.vertcat([xMax for k in range(self.n_k)]) if len(deltaUCons) > 0: addDeltaUCons = True deltaUCons = deltaUCons / self.controlScaling else: addDeltaUCons = False pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t) pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)] pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames]) pathS = C.substitute( ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / pathScaling pathConstraints = C.SXFunction(pathIn, [pathS]) pathMax = C.vertcat([ ocp.variable(pathVarNames[i]).max.getValue() for i in range(ocp.path.size()) ]) / pathScaling pathMin = C.vertcat([ ocp.variable(pathVarNames[i]).min.getValue() for i in range(ocp.path.size()) ]) / pathScaling pathConstraints.setOption("name", "PATH") pathConstraints.init() pathConstraints.setInput(xOpt, 'x') pathConstraints.setInput([], 'z') pathConstraints.setInput(uOpt, 'p') pathConstraints.setInput(0, 't') pathConstraints.evaluate() pathOpt = pathConstraints.getOutput() optimalValues = {} print 'min <= (name,optimal,nominal) <= max' for i in range(self.n_x): print ocp.variable( ocp.x[i].getName()).min.getValue(), ' <= (', ocp.x[i].getName( ), ',', xOpt[i] * stateScaling[i], ',', stateScaling[ i], ') <= ', ocp.variable( ocp.x[i].getName()).max.getValue() optimalValues[ocp.x[i].getName()] = xOpt[i] * stateScaling[i] for i in range(self.n_u): print ocp.variable( ocp.u[i].getName()).min.getValue(), ' <= (', ocp.u[i].getName( ), ',', uOpt[i] * controlScaling[i], ',', controlScaling[ i], ') <= ', ocp.variable( ocp.u[i].getName()).max.getValue() if addDeltaUCons: print -deltaUCons[i] * controlScaling[i], ' <= (Delta(', ocp.u[ i].getName(), ')/DTMPC,', 0, ',', controlScaling[ i], ') <= ', deltaUCons[i] * controlScaling[i] optimalValues[ocp.u[i].getName()] = uOpt[i] * controlScaling[i] for i in range(len(pathVarNames)): print ocp.variable(pathVarNames[i]).min.getValue( ), ' <= (', pathVarNames[i], ',', pathOpt[i] * pathScaling[ i], ',', pathScaling[i], ') <= ', ocp.variable( pathVarNames[i]).max.getValue() optimalValues[pathVarNames[i]] = pathOpt[i] * pathScaling[i] plotTags = [ocp.x[i].getName() for i in range(ocp.x.size())] plotTags = plotTags + [ocp.u[i].getName() for i in range(ocp.u.size())] plotTags = plotTags + [sv.getName() for sv in ocp.beq(ocp.path)] self.plotTags = plotTags self.optimalValues = optimalValues # Constraint functions g = [] g_min = [] g_max = [] self.XU0 = C.MX.sym("XU0", self.n_x + self.n_u, 1) Z = self.XU0[0:self.n_x] U0 = self.XU0[self.n_x:self.n_x + self.n_u] # Build up a graph of integrator calls obj = 0 zf = C.vertcat([ ocp.variable(ocp.z[k].getName()).start for k in range(ocp.z.size()) ]) / algStateScaling for k in range(self.n_k): Z, QF, zf = C.integratorOut( G(C.integratorIn(x0=Z, p=self.U[k], z0=zf)), "xf", "qf", "zf") errU = self.U[k] - U0 obj = obj + QF + C.mul(C.mul(errU.T, controlCost), errU) U0 = self.U[k] # include MS constraints! g.append(Z - self.X[k]) g_min.append(NP.zeros(self.n_x)) g_max.append(NP.zeros(self.n_x)) Z = self.X[k] [pathCons] = pathConstraints.call( C.daeIn(t=[], x=self.X[k], z=zf, p=self.U[k])) g.append(pathCons) ## be carefull on giving all inputs g_max.append(pathMax) g_min.append(pathMin) if addDeltaUCons: g.append(errU) g_max.append(deltaUCons * DT) g_min.append(-deltaUCons * DT) #errU = (self.U[-1]-uOpt) #errX = self.X[-1]-xOpt #obj = obj + finalStateCost*C.mul((errX).trans(),(errX))+C.mul(C.mul(errU.T,controlCost),errU) self.obj = obj ### Constrains g = C.vertcat(g) nlp = C.MXFunction(C.nlpIn(x=self.V, p=self.XU0), C.nlpOut(f=obj, g=g)) nlp.init() self.odeF = odeF self.nlp = nlp solver = C.NlpSolver('ipopt', nlp) # remove the comment to implement the hessian solver.setOption('hessian_approximation', 'limited-memory') # comment for exact hessian solver.setOption('print_user_options', 'no') solver.setOption("tol", self.IPOPT_tol) # IPOPT tolerance solver.setOption("dual_inf_tol", self.IPOPT_dual_inf_tol) # dual infeasibility solver.setOption("constr_viol_tol", self.IPOPT_constr_viol_tol) # primal infeasibility solver.setOption("compl_inf_tol", self.IPOPT_compl_inf_tol) # complementarity # solver.setOption("acceptable_tol",0.01) # solver.setOption("acceptable_obj_change_tol",1e-6) # solver.setOption("acceptable_constr_viol_tol",1e-6) solver.setOption("max_iter", self.IPOPT_max_iter) # IPOPT maximum iterations solver.setOption("print_level", self.IPOPT_print_level) solver.setOption("max_cpu_time", self.IPOPT_max_cpu_time) # IPOPT maximum iterations solver.init() ### Variable Bounds and initial guess solver.setInput(C.vertcat([UMIN, XMIN]), 'lbx') # u_L solver.setInput(C.vertcat([UMAX, XMAX]), 'ubx') # u_U solver.setInput(C.vertcat(g_min), 'lbg') # g_L solver.setInput(C.vertcat(g_max), 'ubg') # g_U self.solver = solver u0N = C.vertcat([ self.ocp.variable(self.ocp.u[i].getName()).initialGuess.getValue() for i in range(self.n_u) ]) / controlScaling x0N = C.vertcat([ self.ocp.variable(self.ocp.x[i].getName()).initialGuess.getValue() for i in range(self.n_x) ]) / stateScaling USOL, XSOL = self.forwardSimulation(x0N, u0N) self.USOL = USOL self.XSOL = XSOL
def getSteadyState(dae, conf, omega0, r0, ref_dict={}, bounds=None, dotBounds=None, guess=None, dotGuess=None, verbose=True): dvs, ffcn, gfcn, lbg, ubg = getSteadyStateNlpFunctions(dae, ref_dict) if guess is None: 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, 'dt1_disturbance': 0.0, 'dt2_disturbance': 0.0, 'dt3_disturbance': 0.0, 't1_disturbance': 0.0, 't2_disturbance': 0.0, 't3_disturbance': 0.0, 'df1_disturbance': 0.0, 'df2_disturbance': 0.0, 'df3_disturbance': 0.0, 'f1_disturbance': 0.0, 'f2_disturbance': 0.0, 'f3_disturbance': 0.0, 'wind_x': 0.0, 'wind_y': 0.0, 'wind_z': 0.0, 'delta_wind_x': 0.0, 'delta_wind_y': 0.0, 'delta_wind_z': 0.0, 'alpha0': 0.0 } if dotGuess is None: 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, 'dt1_disturbance': 0.0, 'dt2_disturbance': 0.0, 'dt3_disturbance': 0.0, 't1_disturbance': 0.0, 't2_disturbance': 0.0, 't3_disturbance': 0.0, 'df1_disturbance': 0.0, 'df2_disturbance': 0.0, 'df3_disturbance': 0.0, 'f1_disturbance': 0.0, 'f2_disturbance': 0.0, 'f3_disturbance': 0.0, 'wind_x': 0.0, 'wind_y': 0.0, 'wind_z': 0.0, 'delta_wind_x': 0.0, 'delta_wind_y': 0.0, 'delta_wind_z': 0.0, 'alpha0': 0.0 } guessVec = C.DMatrix([ guess[n] for n in dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames() ] + [dotGuess[n] for n in dae.xNames()]) if bounds is None: 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.17, 0.17), 'elevator': (-0.17, 0.17), '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), 'dt1_disturbance': (0, 0), 'dt2_disturbance': (0, 0), 'dt3_disturbance': (0, 0), 't1_disturbance': (0, 0), 't2_disturbance': (0, 0), 't3_disturbance': (0, 0), 'df1_disturbance': (0, 0), 'df2_disturbance': (0, 0), 'df3_disturbance': (0, 0), 'f1_disturbance': (0, 0), 'f2_disturbance': (0, 0), 'f3_disturbance': (0, 0), 'wind_x': (0, 0), 'wind_y': (0, 0), 'wind_z': (0, 0), 'delta_wind_x': (0, 0), 'delta_wind_y': (0, 0), 'delta_wind_z': (0, 0), 'alpha0': (0.0, 0.0) } if ref_dict is not None: for name in ref_dict: bounds[name] = ref_dict[name] # print bounds if dotBounds is None: 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), 'dt1_disturbance': (0, 0), 'dt2_disturbance': (0, 0), 'dt3_disturbance': (0, 0), 't1_disturbance': (0, 0), 't2_disturbance': (0, 0), 't3_disturbance': (0, 0), 'df1_disturbance': (0, 0), 'df2_disturbance': (0, 0), 'df3_disturbance': (0, 0), 'f1_disturbance': (0, 0), 'f2_disturbance': (0, 0), 'f3_disturbance': (0, 0), 'wind_x': (0, 0), 'wind_y': (0, 0), 'wind_z': (0, 0), 'delta_wind_x': (0, 0), 'delta_wind_y': (0, 0), 'delta_wind_z': (0, 0), 'alpha0': (0.0, 0.0) } 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()]) nlp = C.SXFunction(C.nlpIn(x=dvs), C.nlpOut(f=ffcn, g=gfcn)) solver = C.NlpSolver("ipopt", nlp) # 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) if verbose is False: solver.setOption('suppress_all_output', 'yes') solver.setOption('print_time', False) solver.init() solver.setInput(lbg, 'lbg') solver.setInput(ubg, 'ubg') solver.setInput(guessVec, 'x0') lb, ub = zip(*boundsVec) solver.setInput(C.DMatrix(lb), 'lbx') solver.setInput(C.DMatrix(ub), 'ubx') solver.evaluate() ret = solver.getStat('return_status') assert ret in ['Solve_Succeeded', 'Solved_To_Acceptable_Level', 1], 'Solver failed: ' + str(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
for k in range(N): x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"] obj.append(x_end - ydata_noise[k+1, :].T) r = ca.vertcat([ca.vertcat(obj), EPS_U]) Sigma_y_inv = ca.diag(ca.vec(wv)) Sigma_u_inv = ca.diag(weps_u) Z = ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))) Sigma = ca.blockcat(Sigma_y_inv, Z, Z.T, Sigma_u_inv) nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), \ ca.nlpOut(f = 0.5 * ca.mul([r.T, Sigma, r]))) nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp) V0 = ca.vertcat([ pl.ones(3), \ pl.zeros(N), \ ydata[0,:].T ]) sol = nlpsolver(x0 = V0) p_est_single_shooting = sol["x"][:3]
x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"] obj.append(x_end - ydata_noise[k+1, :].T) r = ca.vertcat([ca.vertcat(obj), EPS_U]) wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape) weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape) Sigma_y_inv = ca.diag(ca.vec(wv)) Sigma_u_inv = ca.diag(weps_u) Sigma = ca.blockcat(Sigma_y_inv, ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\ ca.DMatrix(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv) nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), ca.nlpOut(f = ca.mul([r.T, Sigma, r]))) nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp) V0 = ca.vertcat([ pl.ones(3), \ pl.zeros(N), \ ydata_noise[0,:].T ]) sol = nlpsolver(x0 = V0) p_est_single_shooting = sol["x"][:3]
def run_parameter_estimation(self, hessian="gauss-newton"): r''' :param hessian: Method of hessian calculation/approximation; possible values are `gauss-newton` and `exact-hessian` :type hessian: str This functions will run a least squares parameter estimation for the given problem and data set. For this, an NLP of the following structure is set up with a direct collocation approach and solved using IPOPT: .. math:: \begin{aligned} & \text{arg}\,\underset{x, p, v, \epsilon_e, \epsilon_u}{\text{min}} & & \frac{1}{2} \| R(w, v, \epsilon_e, \epsilon_u) \|_2^2 \\ & \text{subject to:} & & R(w, v, \epsilon_e, \epsilon_u) = w^{^\mathbb{1}/_\mathbb{2}} \begin{pmatrix} {v} \\ {\epsilon_e} \\ {\epsilon_u} \end{pmatrix} \\ & & & w = \begin{pmatrix} {w_{v}}^T & {w_{\epsilon_{e}}}^T & {w_{\epsilon_{u}}}^T \end{pmatrix} \\ & & & v_{l} + y_{l} - \phi(t_{l}, u_{l}, x_{l}, p) = 0 \\ & & & (t_{k+1} - t_{k}) f(t_{k,j}, u_{k,j}, x_{k,j}, p, \epsilon_{e,k,j}, \epsilon_{u,k,j}) - \sum_{r=0}^{d} \dot{L}_r(\tau_j) x_{k,r} = 0 \\ & & & x_{k+1,0} - \sum_{r=0}^{d} L_r(1) x_{k,r} = 0 \\ & & & t_{k,j} = t_k + (t_{k+1} - t_{k}) \tau_j \\ & & & L_r(\tau) = \prod_{r=0,r\neq j}^{d} \frac{\tau - \tau_r}{\tau_j - \tau_r}\\ & \text{for:} & & k = 1, \dots, N, ~~~ l = 1, \dots, M, ~~~ j = 1, \dots, d, ~~~ r = 1, \dots, d \\ & & & \tau_j = \text{time points w. r. t. scheme and order} \end{aligned} The status of IPOPT provides information whether the computation could be finished sucessfully. The optimal values for all optimization variables :math:`\hat{x}` can be accessed via the class variable ``LSq.Xhat``, while the estimated parameters :math:`\hat{p}` can also be accessed separately via the class attribute ``LSq.phat``. **Please be aware:** IPOPT finishing sucessfully does not necessarly mean that the estimation results for the unknown parameters are useful for your purposes, it just means that IPOPT was able to solve the given optimization problem. You have in any case to verify your results, e. g. by simulation using the class function :func:`run_simulation`. ''' intro.pecas_intro() print('\n' + 18 * '-' + \ ' PECas least squares parameter estimation ' + 18 * '-') print(''' Starting least squares parameter estimation using IPOPT, this might take some time ... ''') self.tstart_estimation = time.time() g = ca.vertcat([ca.vec(self.pesetup.phiN) - self.yN + \ ca.vec(self.pesetup.V)]) self.R = ca.sqrt(self.w) * \ ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]) if self.pesetup.g.size(): g = ca.vertcat([g, self.pesetup.g]) self.g = g self.Vars = ca.veccat([ self.pesetup.P, \ self.pesetup.X, \ self.pesetup.XF, \ self.pesetup.V, \ self.pesetup.EPS_E, \ self.pesetup.EPS_U, \ ]) nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.Vars), \ ca.nlpOut(f=(0.5 * ca.mul([self.R.T, self.R])), g=self.g)) options = {} options["tol"] = 1e-10 options["linear_solver"] = self.linear_solver if hessian == "gauss-newton": # ipdb.set_trace() gradF = nlp.gradient() jacG = nlp.jacobian("x", "g") # Can't the following be implemented more efficiently?! # gradF.derivative(0, 1) J = ca.jacobian(self.R, self.Vars) sigma = ca.MX.sym("sigma") hessLag = ca.MXFunction("H", \ ca.hessLagIn(x = self.Vars, lam_f = sigma), \ ca.hessLagOut(hess = sigma * ca.mul(J.T, J))) options["hess_lag"] = hessLag options["grad_f"] = gradF options["jac_g"] = jacG elif hessian == "exact-hessian": # let NlpSolver-class compute everything pass else: raise NotImplementedError( \ "Requested method is not implemented. Availabe methods " + \ "are 'gauss-newton' (default) and 'exact-hessian'.") # Initialize the solver, solve the optimization problem solver = ca.NlpSolver("solver", "ipopt", nlp, options) # Store the results of the computation Varsinit = ca.veccat([ self.pesetup.Pinit, \ self.pesetup.Xinit, \ self.pesetup.XFinit, \ self.pesetup.Vinit, \ self.pesetup.EPS_Einit, \ self.pesetup.EPS_Uinit, \ ]) sol = solver(x0=Varsinit, lbg=0, ubg=0) self.Varshat = sol["x"] R_squared_fcn = ca.MXFunction("R_squared_fcn", [self.Vars], [ca.mul([ \ ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]).T, ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])])]) [self.R_squared] = R_squared_fcn([self.Varshat]) self.tend_estimation = time.time() self.duration_estimation = self.tend_estimation - \ self.tstart_estimation print(''' Parameter estimation finished. Check IPOPT output for status information.''')
def findConstrainedSteadyState(self, altUTags, altUVals, u0, x0=None, z0=None, consList=[]): if z0 is not None: z0 = z0 / self.algStateScaling else: z0 = self.z0 if x0 is not None: x0 = x0 / self.stateScaling else: x0 = self.x0 u0 = u0 / self.controlScaling ocp = self.ocp measurementScaling = self.measurementScaling stateScaling = self.stateScaling algStateScaling = self.algStateScaling controlScaling = self.controlScaling consScaling = C.vertcat([ocp.variable(k).nominal for k in consList]) altUScaling = C.vertcat( [ocp.variable(altUTags[k]).nominal for k in range(len(altUTags))]) odeS = C.substitute( ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / stateScaling algS = C.substitute( ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) altU = C.vertcat( [ocp.variable(altUTags[k]).beq for k in range(len(altUTags))]) altU = C.substitute( altU, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / altUScaling mSX = C.vertcat( [ocp.variable(consList[k]).beq for k in range(len(consList))]) mSX = C.substitute( mSX, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / consScaling split = C.SXFunction([C.vertcat([ocp.x, ocp.z, ocp.u])], [ocp.x, ocp.z, ocp.u]) split.init() nlp = C.SXFunction( C.nlpIn(x=C.vertcat([ocp.x, ocp.z, ocp.u])), C.nlpOut(f=C.SX(0), g=C.vertcat([odeS, algS, altU, mSX]))) C.NlpSolver.loadPlugin('ipopt') solver = C.NlpSolver('ipopt', nlp) solver.setOption('tol', 1e-10) solver.setOption('print_user_options', 'yes') solver.setOption("max_iter", 200) # IPOPT maximum iterations solver.init() xMin = C.vertcat([ ocp.variable(ocp.x[i].getName()).min.getValue() for i in range(ocp.x.size()) ]) / stateScaling xMax = C.vertcat([ ocp.variable(ocp.x[i].getName()).max.getValue() for i in range(ocp.x.size()) ]) / stateScaling zMin = C.vertcat([ ocp.variable(ocp.z[i].getName()).min.getValue() for i in range(ocp.z.size()) ]) / algStateScaling zMax = C.vertcat([ ocp.variable(ocp.z[i].getName()).max.getValue() for i in range(ocp.z.size()) ]) / algStateScaling uMin = C.vertcat([ ocp.variable(ocp.u[i].getName()).min.getValue() for i in range(ocp.u.size()) ]) / controlScaling uMax = C.vertcat([ ocp.variable(ocp.u[i].getName()).max.getValue() for i in range(ocp.u.size()) ]) / controlScaling cMin = C.vertcat([ ocp.variable(consList[i]).min.getValue() for i in range(len(consList)) ]) / consScaling cMax = C.vertcat([ ocp.variable(consList[i]).max.getValue() for i in range(len(consList)) ]) / consScaling solver.setInput( C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), altUVals, cMin]), 'lbg') # g_L solver.setInput( C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), altUVals, cMax]), 'ubg') # g_U solver.setInput(C.vertcat([xMin, zMin, uMin]), 'lbx') # u_L solver.setInput(C.vertcat([xMax, zMax, uMax]), 'ubx') # u_U solver.setInput(C.vertcat([x0, z0, u0]), 'x0') solver.evaluate() xzu = solver.output('x') split.setInput(xzu) split.evaluate() x0 = split.getOutput(0) z0 = split.getOutput(1) u0 = split.getOutput(2) self.mSXF.setInput(x0, 'x') self.mSXF.setInput(z0, 'z') self.mSXF.setInput(u0, 'p') self.mSXF.evaluate() y0 = self.mSXF.getOutput() return x0 * stateScaling, u0 * controlScaling, z0 * algStateScaling, y0 * measurementScaling
def steadyState(self, u, T=0): ocp = self.ocp xMin = C.vertcat([ ocp.variable(ocp.x[i].getName()).min.getValue() for i in range(ocp.x.size()) ]) xMax = C.vertcat([ ocp.variable(ocp.x[i].getName()).max.getValue() for i in range(ocp.x.size()) ]) x0 = C.vertcat([ ocp.variable(ocp.x[i].getName()).initialGuess.getValue() for i in range(ocp.x.size()) ]) zMin = C.vertcat([ ocp.variable(ocp.z[i].getName()).min.getValue() for i in range(ocp.z.size()) ]) zMax = C.vertcat([ ocp.variable(ocp.z[i].getName()).max.getValue() for i in range(ocp.z.size()) ]) z0 = C.vertcat([ ocp.variable(ocp.z[i].getName()).initialGuess.getValue() for i in range(ocp.z.size()) ]) if T > 0: tout, xh, zh, outs = self.simulatePlot(x0, [u for k in range(T)], outputVars=None) x0 = xh[-1] z0 = zh[-1] varList = [ocp.x, ocp.z] varMax = [xMax, zMax] varMin = [xMin, zMin] var0 = [x0, z0] cons = [ocp.ode(ocp.x), ocp.alg] consMax = [NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size())] consMin = [NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size())] C.NlpSolver.loadPlugin('ipopt') nlp = C.SXFunction(C.nlpIn(x=C.vertcat(varList), p=ocp.u), C.nlpOut(f=C.SX(0), g=C.vertcat(cons))) nlp.init() solver = C.NlpSolver('ipopt', nlp) solver.setOption("max_iter", 100) # IPOPT maximum iterations solver.init() ### Variable Bounds and initial guess solver.setInput(C.vertcat(varMin), 'lbx') solver.setInput(C.vertcat(varMax), 'ubx') solver.setInput(C.vertcat(consMin), 'lbg') # g_L solver.setInput(C.vertcat(consMax), 'ubg') # g_U solver.setInput(C.vertcat(var0), 'x0') solver.setInput(u, 'p') solver.evaluate() outX = solver.getOutput() splitF = C.SXFunction([C.vertcat(varList)], varList) splitF.init() splitF.setInput(outX) splitF.evaluate() xss = splitF.getOutput(0) zss = splitF.getOutput(1) return xss, zss
g = [] for k in range(n_sim): g.append(V['X', k + 1] - F([V['X', k], V['U', k], dt])[0]) g = ca.vertcat(g) # Objective [final_cost] = lf([V['X', n_sim]]) J = final_cost # Regularize controls for k in range(n_sim): [stage_cost] = l([V['X', k], V['U', k], dt]) J += stage_cost # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g)) # Create solver opts = {'linear_solver': 'ma57'} solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Constraints lbx = V(-ca.inf) ubx = V(ca.inf) # x(t=0) = x0 lbx['X', 0] = ubx['X', 0] = x0 # Solve nlp sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) sol = V(sol['x'])
def create_plan_pc(b0, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=state), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Final means m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']] m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']] dm_bc = m_bN - m_cN # Regularize controls J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs bk = cat.struct_SX(belief) bk['S'] = b0['S'] g = [] lbg = [] ubg = [] for k in range(N_sim): # Belief propagation bk['m'] = V['X',k] [bk_next] = BF([ bk, V['U',k] ]) bk_next = belief(bk_next) # simplify indexing # Multiple shooting g.append(bk_next['m'] - V['X',k+1]) lbg.append(ca.DMatrix.zeros(nx)) ubg.append(ca.DMatrix.zeros(nx)) # Control constraints g.append(V['U',k,'v'] - a - b * ca.cos(V['U',k,'psi'])) lbg.append(-ca.inf) ubg.append(ca.DMatrix([0])) # Advance time bk = bk_next g = ca.vertcat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) # Simple cost J += 1e1 * (ca.mul(dm_bc.T, dm_bc) + ca.trace(bk_next['S'])) # log-probability cost #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']] #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # 0 <= v lbx['U',:,'v'] = 0 # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # -pi <= psi <= pi lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi # m(t=0) = m0 lbx['X',0] = ubx['X',0] = b0['m'] # Solve the NLP sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) return V(sol['x'])
def __init__(self, dae, conf, omega0, r0, ref_dict = {}, bounds = None, dotBounds = None, guess = None, dotGuess = None, robustVersion = False, verbose = True): self.dae, self.conf = dae, conf self.omega0 = omega0 self.r0 = r0 self.ref_dict = ref_dict self.robustVersion = robustVersion if robustVersion == True: self.dvs, self.ffcn, self.gfcn, self.lbg, self.ubg, zSol = self.getRobustSteadyStateNlpFunctions(dae, ref_dict) self.dvsNames = dae.xNames() + dae.uNames() + dae.pNames() zIn = C.veccat([dae[ name ] for name in self.dvsNames]) zOut = C.veccat([zSol[ el ] for el in dae.zNames()]) self.zFcn = C.SXFunction([ zIn ], [ zOut ]) self.zFcn.init() else: self.dvs, self.ffcn, self.gfcn, self.lbg, self.ubg = self.getSteadyStateNlpFunctions(dae, ref_dict) self.dvsNames = dae.xNames() + dae.zNames() + dae.uNames() + dae.pNames() self.nlp = C.SXFunction(C.nlpIn(x = self.dvs), C.nlpOut(f = self.ffcn, g = self.gfcn)) self.solver = C.IpoptSolver( self.nlp ) self.solver.setOption('max_iter', 10000) self.solver.setOption('tol', 1e-9) if verbose is False: self.solver.setOption('suppress_all_output', 'yes') self.solver.setOption('print_time', False) self.solver.init() self.guess = guess if self.guess is None: self.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, 'dt1_disturbance':0.0, 'dt2_disturbance':0.0, 'dt3_disturbance':0.0, 't1_disturbance':0.0, 't2_disturbance':0.0, 't3_disturbance':0.0, 'df1_disturbance':0.0, 'df2_disturbance':0.0, 'df3_disturbance':0.0, 'f1_disturbance':0.0, 'f2_disturbance':0.0, 'f3_disturbance':0.0, 'wind_x':0.0, 'wind_y':0.0, 'wind_z':0.0, 'delta_wind_x':0.0, 'delta_wind_y':0.0, 'delta_wind_z':0.0, 'alpha0': 0.0} self.dotGuess = dotGuess if self.dotGuess is None: self.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, 'dt1_disturbance':0.0, 'dt2_disturbance':0.0, 'dt3_disturbance':0.0, 't1_disturbance':0.0, 't2_disturbance':0.0, 't3_disturbance':0.0, 'df1_disturbance':0.0, 'df2_disturbance':0.0, 'df3_disturbance':0.0, 'f1_disturbance':0.0, 'f2_disturbance':0.0, 'f3_disturbance':0.0, 'wind_x':0.0, 'wind_y':0.0, 'wind_z':0.0, 'delta_wind_x':0.0, 'delta_wind_y':0.0, 'delta_wind_z':0.0, 'alpha0': 0.0} self.bounds = bounds if self.bounds is None: self.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), 'dt1_disturbance':(0, 0), 'dt2_disturbance':(0, 0), 'dt3_disturbance':(0, 0), 't1_disturbance':(0, 0), 't2_disturbance':(0, 0), 't3_disturbance':(0, 0), 'df1_disturbance':(0, 0), 'df2_disturbance':(0, 0), 'df3_disturbance':(0, 0), 'f1_disturbance':(0, 0), 'f2_disturbance':(0, 0), 'f3_disturbance':(0, 0), 'wind_x':(0, 0), 'wind_y':(0, 0), 'wind_z':(0, 0), 'delta_wind_x':(0, 0), 'delta_wind_y':(0, 0), 'delta_wind_z':(0, 0), 'alpha0': (0.0, 0.0)} if ref_dict is not None: for name in ref_dict: self.bounds[name] = ref_dict[name] self.dotBounds = dotBounds if self.dotBounds is None: self.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), 'dt1_disturbance':(0, 0), 'dt2_disturbance':(0, 0), 'dt3_disturbance':(0, 0), 't1_disturbance':(0, 0), 't2_disturbance':(0, 0), 't3_disturbance':(0, 0), 'df1_disturbance':(0, 0), 'df2_disturbance':(0, 0), 'df3_disturbance':(0, 0), 'f1_disturbance':(0, 0), 'f2_disturbance':(0, 0), 'f3_disturbance':(0, 0), 'wind_x':(0, 0), 'wind_y':(0, 0), 'wind_z':(0, 0), 'delta_wind_x':(0, 0), 'delta_wind_y':(0, 0), 'delta_wind_z':(0, 0), 'alpha0': (0.0, 0.0)} if self.robustVersion == True: # Joris's recommendation for name in self.dae.xNames(): if name in ["r", "ddelta"] or "disturbance" in name: continue # Override bounds if self.bounds[name][0] == self.bounds[name][1]: self.bounds[name] = (-1e12, 1e12) # Override dotBounds if self.dotBounds[name][0] == self.dotBounds[name][1]: self.dotBounds[name] = (-1e12, 1e12) if name not in ["aileron", "elevator", "motor_torque", "ddr", "sin_delta"]: self.dotBounds[ name ] = (0, 0)
geom = geometry.Geometry(thetaLoc, chordLoc, yLoc, aIncGeometricLoc, clPolyLoc, bref, n) (makeMeZero, alphaiLoc, CL, CDi) = setupImplicitFunction(dvs['operAlpha'], dvs['An'], geom) outputsFcn = C.SXFunction([dvs.cat], [alphaiLoc, CL, CDi, geom.sref]) outputsFcn.init() g = CT.struct_SX([ CT.entry("makeMeZero",expr=makeMeZero), CT.entry('alphaiLoc',expr=alphaiLoc), CT.entry("CL",expr=CL), CT.entry("CDi",expr=CDi), CT.entry("sref",expr=geom.sref)]) obj = -CL / (CDi + 0.01) nlp = C.SXFunction(C.nlpIn(x=dvs),C.nlpOut(f=obj,g=g)) solver = C.IpoptSolver(nlp) solver.setOption('tol',1e-11) solver.setOption('linear_solver','ma27') #solver.setOption('linear_solver','ma57') solver.init() lbg = g(solver.input("lbg")) ubg = g(solver.input("ubg")) lbx = dvs(solver.input("lbx")) ubx = dvs(solver.input("ubx")) x0 = dvs(solver.input("x0")) lbg["makeMeZero"] = 0.0 ubg["makeMeZero"] = 0.0
def run_parameter_estimation(self, hessian = "gauss-newton"): r''' :param hessian: Method of hessian calculation/approximation; possible values are `gauss-newton` and `exact-hessian` :type hessian: str This functions will run a least squares parameter estimation for the given problem and data set. For this, an NLP of the following structure is set up with a direct collocation approach and solved using IPOPT: .. math:: \begin{aligned} & \text{arg}\,\underset{x, p, v, \epsilon_e, \epsilon_u}{\text{min}} & & \frac{1}{2} \| R(w, v, \epsilon_e, \epsilon_u) \|_2^2 \\ & \text{subject to:} & & R(w, v, \epsilon_e, \epsilon_u) = w^{^\mathbb{1}/_\mathbb{2}} \begin{pmatrix} {v} \\ {\epsilon_e} \\ {\epsilon_u} \end{pmatrix} \\ & & & w = \begin{pmatrix} {w_{v}}^T & {w_{\epsilon_{e}}}^T & {w_{\epsilon_{u}}}^T \end{pmatrix} \\ & & & v_{l} + y_{l} - \phi(t_{l}, u_{l}, x_{l}, p) = 0 \\ & & & (t_{k+1} - t_{k}) f(t_{k,j}, u_{k,j}, x_{k,j}, p, \epsilon_{e,k,j}, \epsilon_{u,k,j}) - \sum_{r=0}^{d} \dot{L}_r(\tau_j) x_{k,r} = 0 \\ & & & x_{k+1,0} - \sum_{r=0}^{d} L_r(1) x_{k,r} = 0 \\ & & & t_{k,j} = t_k + (t_{k+1} - t_{k}) \tau_j \\ & & & L_r(\tau) = \prod_{r=0,r\neq j}^{d} \frac{\tau - \tau_r}{\tau_j - \tau_r}\\ & \text{for:} & & k = 1, \dots, N, ~~~ l = 1, \dots, M, ~~~ j = 1, \dots, d, ~~~ r = 1, \dots, d \\ & & & \tau_j = \text{time points w. r. t. scheme and order} \end{aligned} The status of IPOPT provides information whether the computation could be finished sucessfully. The optimal values for all optimization variables :math:`\hat{x}` can be accessed via the class variable ``LSq.Xhat``, while the estimated parameters :math:`\hat{p}` can also be accessed separately via the class attribute ``LSq.phat``. **Please be aware:** IPOPT finishing sucessfully does not necessarly mean that the estimation results for the unknown parameters are useful for your purposes, it just means that IPOPT was able to solve the given optimization problem. You have in any case to verify your results, e. g. by simulation using the class function :func:`run_simulation`. ''' intro.pecas_intro() print('\n' + 18 * '-' + \ ' PECas least squares parameter estimation ' + 18 * '-') print(''' Starting least squares parameter estimation using IPOPT, this might take some time ... ''') self.tstart_estimation = time.time() g = ca.vertcat([ca.vec(self.pesetup.phiN) - self.yN + \ ca.vec(self.pesetup.V)]) self.R = ca.sqrt(self.w) * \ ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]) if self.pesetup.g.size(): g = ca.vertcat([g, self.pesetup.g]) self.g = g self.Vars = ca.veccat([ self.pesetup.P, \ self.pesetup.X, \ self.pesetup.XF, \ self.pesetup.V, \ self.pesetup.EPS_E, \ self.pesetup.EPS_U, \ ]) nlp = ca.MXFunction("nlp", ca.nlpIn(x=self.Vars), \ ca.nlpOut(f=(0.5 * ca.mul([self.R.T, self.R])), g=self.g)) options = {} options["tol"] = 1e-10 options["linear_solver"] = self.linear_solver if hessian == "gauss-newton": # ipdb.set_trace() gradF = nlp.gradient() jacG = nlp.jacobian("x", "g") # Can't the following be implemented more efficiently?! # gradF.derivative(0, 1) J = ca.jacobian(self.R, self.Vars) sigma = ca.MX.sym("sigma") hessLag = ca.MXFunction("H", \ ca.hessLagIn(x = self.Vars, lam_f = sigma), \ ca.hessLagOut(hess = sigma * ca.mul(J.T, J))) options["hess_lag"] = hessLag options["grad_f"] = gradF options["jac_g"] = jacG elif hessian == "exact-hessian": # let NlpSolver-class compute everything pass else: raise NotImplementedError( \ "Requested method is not implemented. Availabe methods " + \ "are 'gauss-newton' (default) and 'exact-hessian'.") # Initialize the solver, solve the optimization problem solver = ca.NlpSolver("solver", "ipopt", nlp, options) # Store the results of the computation Varsinit = ca.veccat([ self.pesetup.Pinit, \ self.pesetup.Xinit, \ self.pesetup.XFinit, \ self.pesetup.Vinit, \ self.pesetup.EPS_Einit, \ self.pesetup.EPS_Uinit, \ ]) sol = solver(x0 = Varsinit, lbg = 0, ubg = 0) self.Varshat = sol["x"] R_squared_fcn = ca.MXFunction("R_squared_fcn", [self.Vars], [ca.mul([ \ ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U]).T, ca.veccat([self.pesetup.V, self.pesetup.EPS_E, self.pesetup.EPS_U])])]) [self.R_squared] = R_squared_fcn([self.Varshat]) self.tend_estimation = time.time() self.duration_estimation = self.tend_estimation - \ self.tstart_estimation print(''' Parameter estimation finished. Check IPOPT output for status information.''')
def findSteadyState(self, u, x0, z0=None, simCount=0, consList=[]): if z0 is not None: z0 = z0 / self.algStateScaling else: z0 = self.z0 if x0 is not None: x0 = x0 / self.stateScaling else: x0 = self.x0 ocp = self.ocp u = u / self.controlScaling measurementScaling = self.measurementScaling stateScaling = self.stateScaling algStateScaling = self.algStateScaling controlScaling = self.controlScaling consScaling = C.vertcat([ocp.variable(k).nominal for k in consList]) for k in range(simCount): x0, z0, y = self.oneStep(x0 * stateScaling, u * controlScaling, z0 * algStateScaling) x0 = x0 / stateScaling z0 = z0 / algStateScaling y = y / measurementScaling odeS = C.substitute( ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / stateScaling algS = C.substitute( ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) mSX = C.vertcat( [ocp.variable(consList[k]).beq for k in range(len(consList))]) mSX = C.substitute( mSX, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / consScaling split = C.SXFunction([C.vertcat([ocp.x, ocp.z])], [ocp.x, ocp.z]) split.init() nlp = C.SXFunction(C.nlpIn(x=C.vertcat([ocp.x, ocp.z]), p=ocp.u), C.nlpOut(f=C.SX(0), g=C.vertcat([odeS, algS, mSX]))) C.NlpSolver.loadPlugin('ipopt') solver = C.NlpSolver('ipopt', nlp) solver.setOption('print_user_options', 'no') solver.setOption('tol', 1e-10) solver.setOption('print_level', 5) solver.setOption("max_iter", 2000) # IPOPT maximum iterations solver.init() xMin = C.vertcat([ ocp.variable(ocp.x[i].getName()).min.getValue() for i in range(ocp.x.size()) ]) / stateScaling xMax = C.vertcat([ ocp.variable(ocp.x[i].getName()).max.getValue() for i in range(ocp.x.size()) ]) / stateScaling zMin = C.vertcat([ ocp.variable(ocp.z[i].getName()).min.getValue() for i in range(ocp.z.size()) ]) / algStateScaling zMax = C.vertcat([ ocp.variable(ocp.z[i].getName()).max.getValue() for i in range(ocp.z.size()) ]) / algStateScaling cMin = C.vertcat([ ocp.variable(consList[i]).min.getValue() for i in range(len(consList)) ]) / consScaling cMax = C.vertcat([ ocp.variable(consList[i]).max.getValue() for i in range(len(consList)) ]) / consScaling solver.setInput( C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMin]), 'lbg') # g_L solver.setInput( C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMax]), 'ubg') # g_U solver.setInput(C.vertcat([xMin, zMin]), 'lbx') # u_L solver.setInput(C.vertcat([xMax, zMax]), 'ubx') # u_U solver.setInput(C.vertcat([x0, z0]), 'x0') solver.setInput(u, 'p') solver.evaluate() xz = solver.output('x') split.setInput(xz) split.evaluate() x0 = split.getOutput(0) z0 = split.getOutput(1) self.mSXF.setInput(x0, 'x') self.mSXF.setInput(z0, 'z') self.mSXF.setInput(u, 'p') self.mSXF.evaluate() y0 = self.mSXF.getOutput() return x0 * stateScaling, z0 * algStateScaling, y0 * measurementScaling
def findOptimalPoint(self, u0, x0=None, z0=None, simCount=0, consList=[]): if z0 is not None: z0 = z0 / self.algStateScaling else: z0 = self.z0 if x0 is not None: x0 = x0 / self.stateScaling else: x0 = self.x0 ocp = self.ocp u0 = u0 / self.controlScaling measurementScaling = self.measurementScaling stateScaling = self.stateScaling algStateScaling = self.algStateScaling controlScaling = self.controlScaling consScaling = C.vertcat([ocp.variable(k).nominal for k in consList]) for k in range(simCount): x0, z0, y = self.oneStep(x0 * stateScaling, u * controlScaling, z0 * algStateScaling) x0 = x0 / stateScaling z0 = z0 / algStateScaling y = y / measurementScaling odeS = C.substitute( ocp.ode(ocp.x), C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / stateScaling algS = C.substitute( ocp.alg, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) ltermS = C.substitute( ocp.lterm, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) mSX = C.vertcat( [ocp.variable(consList[k]).beq for k in range(len(consList))]) mSX = C.substitute( mSX, C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / consScaling pathVarNames = [sv.getName() for sv in ocp.beq(ocp.path)] pathScaling = C.vertcat([ocp.nominal(pv) for pv in pathVarNames]) pathS = C.substitute( ocp.beq(ocp.beq(ocp.path)), C.vertcat([ocp.x, ocp.z, ocp.u]), C.vertcat([ stateScaling * ocp.x, algStateScaling * ocp.z, controlScaling * ocp.u ])) / pathScaling split = C.SXFunction([C.vertcat([ocp.u, ocp.x, ocp.z])], [ocp.u, ocp.x, ocp.z]) split.init() nlp = C.SXFunction( C.nlpIn(x=C.vertcat([ocp.u, ocp.x, ocp.z])), C.nlpOut(f=ltermS, g=C.vertcat([odeS, algS, mSX, pathS]))) C.NlpSolver.loadPlugin('ipopt') solver = C.NlpSolver('ipopt', nlp) solver.setOption('print_user_options', 'no') solver.setOption('print_level', 5) solver.setOption("tol", 1e-14) solver.setOption("max_iter", 300) # IPOPT maximum iterations solver.init() uMin = C.vertcat([ ocp.variable(ocp.u[i].getName()).min.getValue() for i in range(ocp.u.size()) ]) / controlScaling uMax = C.vertcat([ ocp.variable(ocp.u[i].getName()).max.getValue() for i in range(ocp.u.size()) ]) / controlScaling xMin = C.vertcat([ ocp.variable(ocp.x[i].getName()).min.getValue() for i in range(ocp.x.size()) ]) / stateScaling xMax = C.vertcat([ ocp.variable(ocp.x[i].getName()).max.getValue() for i in range(ocp.x.size()) ]) / stateScaling zMin = C.vertcat([ ocp.variable(ocp.z[i].getName()).min.getValue() for i in range(ocp.z.size()) ]) / algStateScaling zMax = C.vertcat([ ocp.variable(ocp.z[i].getName()).max.getValue() for i in range(ocp.z.size()) ]) / algStateScaling cMin = C.vertcat([ ocp.variable(consList[i]).min.getValue() for i in range(len(consList)) ]) / consScaling cMax = C.vertcat([ ocp.variable(consList[i]).max.getValue() for i in range(len(consList)) ]) / consScaling pathMax = C.vertcat([ ocp.variable(pathVarNames[i]).max.getValue() for i in range(ocp.path.size()) ]) / pathScaling pathMin = C.vertcat([ ocp.variable(pathVarNames[i]).min.getValue() for i in range(ocp.path.size()) ]) / pathScaling solver.setInput( C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMin, pathMin]), 'lbg') # g_L solver.setInput( C.vertcat([NP.zeros(ocp.z.size() + ocp.x.size()), cMax, pathMax]), 'ubg') # g_U solver.setInput(C.vertcat([uMin, xMin, zMin]), 'lbx') # u_L solver.setInput(C.vertcat([uMax, xMax, zMax]), 'ubx') # u_U solver.setInput(C.vertcat([u0, x0, z0]), 'x0') solver.evaluate() xz = solver.output('x') split.setInput(xz) split.evaluate() u0 = split.getOutput(0) x0 = split.getOutput(1) z0 = split.getOutput(2) self.mSXF.setInput(x0, 'x') self.mSXF.setInput(z0, 'z') self.mSXF.setInput(u0, 'p') self.mSXF.evaluate() y0 = self.mSXF.getOutput() return u0 * controlScaling, x0 * stateScaling, z0 * algStateScaling, y0 * measurementScaling
def nlpOut(f = None, g = None): return ca.nlpOut(f = f, g = g)
def steadyStateOptimal(self, x0=None, u0=None, z0=None): ocp = self.ocp uMin = C.vertcat([ ocp.variable(ocp.u[i].getName()).min.getValue() for i in range(ocp.u.size()) ]) uMax = C.vertcat([ ocp.variable(ocp.u[i].getName()).max.getValue() for i in range(ocp.u.size()) ]) if u0 is None: u0 = C.vertcat([ ocp.variable(ocp.u[i].getName()).initialGuess.getValue() for i in range(ocp.u.size()) ]) xMin = C.vertcat([ ocp.variable(ocp.x[i].getName()).min.getValue() for i in range(ocp.x.size()) ]) xMax = C.vertcat([ ocp.variable(ocp.x[i].getName()).max.getValue() for i in range(ocp.x.size()) ]) if x0 is None: x0 = C.vertcat([ ocp.variable(ocp.x[i].getName()).initialGuess.getValue() for i in range(ocp.x.size()) ]) zMin = C.vertcat([ ocp.variable(ocp.z[i].getName()).min.getValue() for i in range(ocp.z.size()) ]) zMax = C.vertcat([ ocp.variable(ocp.z[i].getName()).max.getValue() for i in range(ocp.z.size()) ]) if z0 is None: z0 = C.vertcat([ ocp.variable(ocp.z[i].getName()).initialGuess.getValue() for i in range(ocp.z.size()) ]) ## PATH CONSTRAINTS pathIn = C.daeIn(x=ocp.x, z=ocp.z, p=ocp.u, t=ocp.t) pathConstraints = C.SXFunction(pathIn, [ocp.beq(ocp.path)]) pathMax = [ ocp.variable(ocp.path[i].getName()).max.getValue() for i in range(ocp.path.size()) ] pathMin = [ ocp.variable(ocp.path[i].getName()).min.getValue() for i in range(ocp.path.size()) ] pathConstraints.setOption("name", "PATH") pathConstraints.init() varList = [ocp.x, ocp.z, ocp.u] varMax = [xMax, zMax, uMax] varMin = [xMin, zMin, uMin] var0 = [x0, z0, u0] cons = [ocp.ode(ocp.x), ocp.alg, ocp.beq(ocp.path)] consMax = [ NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size()), C.vertcat(pathMax) ] consMin = [ NP.zeros(ocp.x.size()), NP.zeros(ocp.z.size()), C.vertcat(pathMin) ] C.NlpSolver.loadPlugin('ipopt') nlp = C.SXFunction(C.nlpIn(x=C.vertcat(varList)), C.nlpOut(f=ocp.lterm, g=C.vertcat(cons))) nlp.init() solver = C.NlpSolver('ipopt', nlp) solver.setOption("max_iter", 100) # IPOPT maximum iterations solver.init() ### Variable Bounds and initial guess solver.setInput(C.vertcat(varMin), 'lbx') solver.setInput(C.vertcat(varMax), 'ubx') solver.setInput(C.vertcat(consMin), 'lbg') # g_L solver.setInput(C.vertcat(consMax), 'ubg') # g_U solver.setInput(C.vertcat(var0), 'x0') # g_U solver.evaluate() outX = solver.getOutput() splitF = C.SXFunction([C.vertcat(varList)], varList) splitF.init() splitF.setInput(outX) splitF.evaluate() xOpt = splitF.getOutput(0) zOpt = splitF.getOutput(1) uOpt = splitF.getOutput(2) return uOpt, xOpt, zOpt