def make_output_structure(outputs): outputs_vec = [] full_list = [] outputs_dict = {} for output_type in list(outputs.keys()): local_list = [] for name in list(outputs[output_type].keys()): # prepare empty entry list to generate substruct local_list += [ cas.entry(name, shape=outputs[output_type][name].shape) ] # generate vector with outputs - SX expressions outputs_vec = cas.vertcat(outputs_vec, outputs[output_type][name]) # generate dict with sub-structs outputs_dict[output_type] = cas.struct_symMX(local_list) # prepare list with sub-structs to generate struct full_list += [cas.entry(output_type, struct=outputs_dict[output_type])] # generate "empty" structure out_struct = cas.struct_symMX(full_list) # generate structure with SX expressions outputs_struct = out_struct(outputs_vec) return [outputs_struct, outputs_dict]
def vars(): """ System states and controls """ x = ct.struct_symMX(['z', 'y', 'ez', 'ey']) u = ct.struct_symMX(['u']) return x, u
def vars(): """ System states and controls """ x = ct.struct_symMX(['X2', 'P2']) u = ct.struct_symMX(['P100', 'F200']) return x, u
def vars(): """ System states and controls """ x = ct.struct_symMX(['cA', 'cB', 'theta', 'thetaK']) u = ct.struct_symMX(['Vdot', 'QdotK']) return x, u
def construct_upd_z(self, problem=None, lineq_updz=True): if problem is not None: self.problem_upd_z = problem self._lineq_updz = lineq_updz return 0. # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: raise ValueError('For now, only equality constrained QP ' + 'z-updates are allowed!') x_i = struct_symMX(self.q_i_struct) x_j = struct_symMX(self.q_ij_struct) l_i = struct_symMX(self.q_i_struct) l_ij = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') par = struct_symMX(self.par_global_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t / T # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A(par.cat) b = b(par.cat) # build KKT system and solve it via schur complement method l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat) f = -(l + rho * x) G = -(1 / rho) * mtimes(A, A.T) h = b + (1 / rho) * mtimes(A, f) mu = solve(G, h) z = -(1 / rho) * (mtimes(A.T, mu) + f) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi + l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, buildtime = create_function('upd_z_' + str(self._index), inp, out, self.options) self.problem_upd_z = prob return buildtime
def construct_upd_z(self, problem=None, lineq_updz=True): if problem is not None: self.problem_upd_z = problem self._lineq_updz = lineq_updz return 0. # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: raise ValueError('For now, only equality constrained QP ' + 'z-updates are allowed!') x_i = struct_symMX(self.q_i_struct) x_j = struct_symMX(self.q_ij_struct) l_i = struct_symMX(self.q_i_struct) l_ij = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') par = struct_symMX(self.par_global_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t/T # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A(par.cat) b = b(par.cat) # build KKT system and solve it via schur complement method l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat) f = -(l + rho*x) G = -(1/rho)*mtimes(A, A.T) h = b + (1/rho)*mtimes(A, f) mu = solve(G, h) z = -(1/rho)*(mtimes(A.T, mu)+f) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options) self.problem_upd_z = prob return buildtime
def construct_Xdot_struct(nlp_options, variables_dict): ''' Construct a symbolic structure for the discretized state derivatives. @param nlp_options - discretization options @param model - awebox model @return Vdot - discretized state derivatives sym. struct. ''' # extract information nk = nlp_options['n_k'] xd = variables_dict['xd'] # derivatives at interval nodes entry_tuple = (cas.entry('xd', repeat=[nk], struct=xd), ) # add derivatives on collocation nodes if nlp_options['discretization'] == 'direct_collocation': d = nlp_options['collocation']['d'] entry_tuple += (cas.entry('coll_xd', repeat=[nk, d], struct=xd), ) # make new symbolic structure Xdot = cas.struct_symMX([entry_tuple]) return Xdot
def construct_parameters(self): entries = [] for label, child in self.children.items(): entries_child = [] for name, par in child._parameters.items(): entries_child.append(entry(name, shape=par.shape)) entries.append(entry(label, struct=struct(entries_child))) self._par_struct = struct(entries) return struct_symMX(self._par_struct)
def get_component_cost_structure(component_costs): list_of_entries = [] for name in list(component_costs.keys()): list_of_entries += [cas.entry(name)] component_cost_struct = cas.struct_symMX(list_of_entries) return component_cost_struct
def construct_upd_l(self, problem=None): if problem is not None: self.problem_upd_l = problem return 0. # create parameters z_ij = struct_symMX(self.q_ij_struct) l_ij = struct_symMX(self.q_ij_struct) x_j = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') inp = [x_j, z_ij, l_ij, t, T, rho] # update lambda l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat)) out = [l_ij_new] # create problem prob, buildtime = create_function('upd_l_'+str(self._index), inp, out, self.options) self.problem_upd_l = prob return buildtime
def setup_nlp_p(V, model): cost = setup_nlp_cost() p_fix = setup_nlp_p_fix(V, model) P = cas.struct_symMX([ cas.entry('p', struct=p_fix), cas.entry('cost', struct=cost), cas.entry('theta0', struct=model.parameters_dict['theta0']) ]) return P
def setup_nlp_p(V, model): cost = setup_nlp_cost() p_fix = setup_nlp_p_fix(V, model) use_vortex_linearization = 'lin' in model.parameters_dict.keys() if use_vortex_linearization: P = cas.struct_symMX([ cas.entry('p', struct=p_fix), cas.entry('cost', struct=cost), cas.entry('theta0', struct=model.parameters_dict['theta0']), cas.entry('lin', struct=V) ]) else: P = cas.struct_symMX([ cas.entry('p', struct=p_fix), cas.entry('cost', struct=cost), cas.entry('theta0', struct=model.parameters_dict['theta0']) ]) return P
def generate_parameterization_settings(self, options): [ periodic, initial_conditions, param_initial_conditions, param_terminal_conditions, terminal_inequalities, integral_constraints ] = operation.get_operation_conditions(options) xi = cas.struct_symMX([(cas.entry('xi_0'), cas.entry('xi_f'))]) xi_bounds = {} xi_bounds['xi_0'] = [0.0, 0.0] xi_bounds['xi_f'] = [0.0, 0.0] V_pickle_initial = None plot_dict_pickle_initial = None V_pickle_terminal = None plot_dict_pickle_terminal = None if param_initial_conditions: xi_bounds['xi_0'] = [0.0, 1.0] if options['trajectory']['type'] == 'compromised_landing': xi_0 = options['landing']['xi_0_initial'] xi_bounds['xi_0'] = [xi_0, xi_0] V_pickle_initial, plot_dict_pickle_initial = self.__get_V_pickle( options, 'initial') if param_terminal_conditions: xi_bounds['xi_f'] = [0.0, 1.0] V_pickle_terminal, plot_dict_pickle_terminal = self.__get_V_pickle( options, 'terminal') if param_terminal_conditions and param_initial_conditions: for theta in struct_op.subkeys(V_pickle_initial, 'theta'): diff = V_pickle_terminal['theta', theta] - V_pickle_initial['theta', theta] if theta != 't_f': if (float(diff) != 0.0): raise ValueError( 'Parameters of initial and terminal trajectory are not identical.' ) xi_dict = {} xi_dict['V_pickle_initial'] = V_pickle_initial xi_dict['plot_dict_pickle_initial'] = plot_dict_pickle_initial xi_dict['V_pickle_terminal'] = V_pickle_terminal xi_dict['plot_dict_pickle_terminal'] = plot_dict_pickle_terminal xi_dict['xi'] = xi xi_dict['xi_bounds'] = xi_bounds self.__xi_dict = xi_dict return None
def _check_for_lineq(self): g = [] for con in self.global_constraints: lb, ub = con[1], con[2] g = vertcat(g, con[0] - lb) if not isinstance(lb, np.ndarray): lb, ub = [lb], [ub] for k, _ in enumerate(lb): if lb[k] != ub[k]: return False, None, None sym, jac = [], [] for child, q_i in self.q_i.items(): for name, ind in q_i.items(): var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False) jj = jacobian(g, var) jac = horzcat(jac, jj[:, ind]) sym.append(var) for nghb in self.q_ij.keys(): for child, q_ij in self.q_ij[nghb].items(): for name, ind in q_ij.items(): var = self.distr_problem.father.get_variables( child, name, spline=False, symbolic=True, substitute=False) jj = jacobian(g, var) jac = horzcat(jac, jj[:, ind]) sym.append(var) for sym in symvar(jac): if sym not in self.par_global.values(): return False, None, None par = struct_symMX(self.par_global_struct) A, b = jac, -g for s in sym: A = substitute(A, s, np.zeros(s.shape)) b = substitute(b, s, np.zeros(s.shape)) dep_b = [s.name() for s in symvar(b)] dep_A = [s.name() for s in symvar(b)] for name, sym in self.par_global.items(): if sym.name() in dep_b: b = substitute(b, sym, par[name]) if sym.name() in dep_A: A = substitute(A, sym, par[name]) A = Function('A', [par], [A]).expand() b = Function('b', [par], [b]).expand() return True, A, b
def get_collocation_variables_struct(self, variables_dict, u_param): entry_list = [ cas.entry('xd', struct=variables_dict['xd']), cas.entry('xa', struct=variables_dict['xa']) ] if 'xl' in list(variables_dict.keys()): entry_list += [cas.entry('xl', struct=variables_dict['xl'])] if u_param == 'poly': entry_list += [cas.entry('u', struct=variables_dict['u'])] return cas.struct_symMX(entry_list)
def _check_for_lineq(self): g = [] for con in self.global_constraints: lb, ub = con[1], con[2] g = vertcat(g, con[0] - lb) if not isinstance(lb, np.ndarray): lb, ub = [lb], [ub] for k, _ in enumerate(lb): if lb[k] != ub[k]: return False, None, None sym, jac = [], [] for child, q_i in self.q_i.items(): for name, ind in q_i.items(): var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False) jj = jacobian(g, var) jac = horzcat(jac, jj[:, ind]) sym.append(var) for nghb in self.q_ij.keys(): for child, q_ij in self.q_ij[nghb].items(): for name, ind in q_ij.items(): var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False) jj = jacobian(g, var) jac = horzcat(jac, jj[:, ind]) sym.append(var) for sym in symvar(jac): if sym not in self.par_global.values(): return False, None, None par = struct_symMX(self.par_global_struct) A, b = jac, -g for s in sym: A = substitute(A, s, np.zeros(s.shape)) b = substitute(b, s, np.zeros(s.shape)) dep_b = [s.name() for s in symvar(b)] dep_A = [s.name() for s in symvar(b)] for name, sym in self.par_global.items(): if sym.name() in dep_b: b = substitute(b, sym, par[name]) if sym.name() in dep_A: A = substitute(A, sym, par[name]) A = Function('A', [par], [A]).expand() b = Function('b', [par], [b]).expand() return True, A, b
def _check_for_lineq(self): g = [] for con in self.constraints: lb, ub = con[1], con[2] g = vertcat([g, con[0] - lb]) if not isinstance(lb, np.ndarray): lb, ub = [lb], [ub] for k in range(len(lb)): if lb[k] != ub[k]: return False, None, None sym, jac = [], [] for child, q_i in self.q_i.items(): for name, ind in q_i.items(): var = child.get_variable(name, spline=False) jj = jacobian(g, var) jac = horzcat([jac, jj[:, ind]]) sym.append(var) for nghb in self.q_ij.keys(): for child, q_ij in self.q_ij[nghb].items(): for name, ind in q_ij.items(): var = child.get_variable(name, spline=False) jj = jacobian(g, var) jac = horzcat([jac, jj[:, ind]]) sym.append(var) for sym in symvar(jac): if sym not in self.par_i.values(): return False, None, None par = struct_symMX(self.par_struct) A, b = jac, -g for s in sym: A = substitute(A, s, np.zeros(s.shape)) b = substitute(b, s, np.zeros(s.shape)) dep_b = [s.getName() for s in symvar(b)] dep_A = [s.getName() for s in symvar(b)] for name, sym in self.par_i.items(): if sym.getName() in dep_b: b = substitute(b, sym, par[name]) if sym.getName() in dep_A: A = substitute(A, sym, par[name]) A = MXFunction('A', [par], [A]).expand() b = MXFunction('b', [par], [b]).expand() return True, A, b
def setup_integral_output_structure(nlp_numerics_options, integral_outputs): nk = nlp_numerics_options['n_k'] entry_tuple = () # interval outputs entry_tuple += (cas.entry('int_out', repeat=[nk + 1], struct=integral_outputs), ) if nlp_numerics_options['discretization'] == 'direct_collocation': d = nlp_numerics_options['collocation']['d'] entry_tuple += (cas.entry('coll_int_out', repeat=[nk, d], struct=integral_outputs), ) Integral_outputs_struct = cas.struct_symMX([entry_tuple]) return Integral_outputs_struct
def construct_upd_res(self, problem=None): if problem is not None: self.problem_upd_res = problem return 0. # create parameters x_i = struct_symMX(self.q_i_struct) z_i = struct_symMX(self.q_i_struct) z_i_p = struct_symMX(self.q_i_struct) z_ij = struct_symMX(self.q_ij_struct) z_ij_p = struct_symMX(self.q_ij_struct) x_j = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') t0 = t / T rho = MX.sym('rho') inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho] # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) z_i = self.q_i_struct(z_i) z_i_p = self.q_i_struct(z_i_p) z_ij = self.q_ij_struct(z_ij) z_ij_p = self.q_ij_struct(z_ij_p) x_j = self.q_ij_struct(x_j) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i) self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij) # compute residuals pr = mtimes((x_i.cat - z_i.cat).T, (x_i.cat - z_i.cat)) pr += mtimes((x_j.cat - z_ij.cat).T, (x_j.cat - z_ij.cat)) dr = rho * mtimes((z_i.cat - z_i_p.cat).T, (z_i.cat - z_i_p.cat)) dr += rho * mtimes((z_ij.cat - z_ij_p.cat).T, (z_ij.cat - z_ij_p.cat)) cr = rho * pr + dr out = [pr, dr, cr] # create problem prob, buildtime = create_function('upd_res_' + str(self._index), inp, out, self.options) self.problem_upd_res = prob return buildtime
def construct_upd_res(self, problem=None): if problem is not None: self.problem_upd_res = problem return 0. # create parameters x_i = struct_symMX(self.q_i_struct) z_i = struct_symMX(self.q_i_struct) z_i_p = struct_symMX(self.q_i_struct) z_ij = struct_symMX(self.q_ij_struct) z_ij_p = struct_symMX(self.q_ij_struct) x_j = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') t0 = t/T rho = MX.sym('rho') inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho] # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) z_i = self.q_i_struct(z_i) z_i_p = self.q_i_struct(z_i_p) z_ij = self.q_ij_struct(z_ij) z_ij_p = self.q_ij_struct(z_ij_p) x_j = self.q_ij_struct(x_j) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i) self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij) # compute residuals pr = mtimes((x_i.cat-z_i.cat).T, (x_i.cat-z_i.cat)) pr += mtimes((x_j.cat-z_ij.cat).T, (x_j.cat-z_ij.cat)) dr = rho*mtimes((z_i.cat-z_i_p.cat).T, (z_i.cat-z_i_p.cat)) dr += rho*mtimes((z_ij.cat-z_ij_p.cat).T, (z_ij.cat-z_ij_p.cat)) cr = rho*pr + dr out = [pr, dr, cr] # create problem prob, buildtime = create_function('upd_res_'+str(self._index), inp, out, self.options) self.problem_upd_res = prob return buildtime
def __init__( self, dynamics, obstacles=[], h=0.05, horizon=10, Q=None, P=None, R=None, S=None, Sl=None, ulb=None, uub=None, xlb=None, xub=None, terminal_constraint=None, solver_opts=None, x_d=[0] * 12, ): """ Initialize and build the MPC solver # Arguments: horizon: Prediction horizon in seconds model: System model # Optional Argumants: Q: State penalty matrix, default=diag(1,...,1) P: Termial penalty matrix, default=diag(1,...,1) R: Input penalty matrix, default=diag(1,...,1)*0.01 ulb: Lower boundry input uub: Upper boundry input xlb: Lower boundry state xub: Upper boundry state terminal_constraint: Terminal condition on the state * if None: No terminal constraint is used * if zero: Terminal state is equal to zero * if nonzero: Terminal state is bounded within +/- the constraint solver_opts: Additional options to pass to the NLP solver e.g.: solver_opts['print_time'] = False solver_opts['ipopt.tol'] = 1e-8 """ self.horizon = horizon build_solver_time = -time.time() self.dt = h self.Nx, self.Nu = 12, 4 # TODO Nopt = self.Nu + self.Nx self.Nt = int(horizon) self.dynamics = dynamics # Initialize variables self.set_cost_functions() # Cost function weights if P is None: P = np.eye(self.Nx) * 10 if Q is None: Q = np.eye(self.Nx) if R is None: R = np.eye(self.Nu) * 0.01 if S is None: S = np.eye(self.Nx) * 1000 if Sl is None: Sl = np.full((self.Nx, ), 1000) self.Q = ca.MX(Q) self.P = ca.MX(P) self.R = ca.MX(R) self.S = ca.MX(S) self.Sl = ca.MX(Sl) if xub is None: xub = np.full((self.Nx), np.inf) if xlb is None: xlb = np.full((self.Nx), -np.inf) if uub is None: uub = np.full((self.Nu), np.inf) if ulb is None: ulb = np.full((self.Nu), -np.inf) self.ulb = ulb self.uub = uub self.terminal_constraint = terminal_constraint # Starting state parameters - add slack here x0 = ca.MX.sym('x0', self.Nx) x_ref = ca.MX.sym('x_ref', self.Nx * (self.Nt + 1)) u0 = ca.MX.sym('u0', self.Nu) param_s = ca.vertcat(x0, x_ref, u0) # Create optimization variables opt_var = ctools.struct_symMX([(ctools.entry('u', shape=(self.Nu, ), repeat=self.Nt), ctools.entry('x', shape=(self.Nx, ), repeat=self.Nt + 1), ctools.entry('s', shape=(self.Nx, ), repeat=1))]) self.opt_var = opt_var self.num_var = opt_var.size # Decision variable boundries self.optvar_lb = opt_var(-np.inf) self.optvar_ub = opt_var(np.inf) """ Set initial values """ obj = ca.MX(0) con_eq = [] con_ineq = [] con_ineq_lb = [] con_ineq_ub = [] con_eq.append(opt_var['x', 0] - x0) # Generate MPC Problem for t in range(self.Nt): # Get variables x_t = opt_var['x', t] u_t = opt_var['u', t] # Dynamics constraint x_t_next = self.dynamics(x_t, u_t) con_eq.append(x_t_next - opt_var['x', t + 1]) # Input constraints if uub is not None: con_ineq.append(u_t) con_ineq_ub.append(uub) con_ineq_lb.append(np.full((self.Nu, ), -ca.inf)) if ulb is not None: con_ineq.append(u_t) con_ineq_ub.append(np.full((self.Nu, ), ca.inf)) con_ineq_lb.append(ulb) # State constraints if xub is not None: con_ineq.append(x_t) con_ineq_ub.append(xub) con_ineq_lb.append(np.full((self.Nx, ), -ca.inf)) if xlb is not None: con_ineq.append(x_t) con_ineq_ub.append(np.full((self.Nx, ), ca.inf)) con_ineq_lb.append(xlb) if obstacles is not None: for obs in obstacles: con_ineq.append(self.distance_obs(x_t, obs)) con_ineq_ub.append(ca.inf) con_ineq_lb.append(ca.DM((obs.eps + obs.radius)**2)) obj += self.running_cost((x_t - x_ref[t * 12:12 + t * 12]), self.Q, u_t, self.R) # Terminal Cost obj += self.terminal_cost( (opt_var['x', self.Nt] - x_ref[(self.Nt) * 12:12 + (self.Nt) * 12]), self.P) # Terminal contraint s_t = opt_var['s', 0] con_ineq.append((opt_var['x', self.Nt] - x_ref[(self.Nt) * 12:12 + (self.Nt) * 12])**2 - s_t) con_ineq_lb.append(np.full((self.Nx, ), 0.0)) con_ineq_ub.append(np.full((self.Nx, ), terminal_constraint**2)) obj += self.slack_cost(s_t, self.S, self.Sl) # Equality constraints bounds are 0 (they are equality constraints), # -> Refer to CasADi documentation num_eq_con = ca.vertcat(*con_eq).size1() num_ineq_con = ca.vertcat(*con_ineq).size1() con_eq_lb = np.zeros((num_eq_con, 1)) con_eq_ub = np.zeros((num_eq_con, 1)) # Set constraints con = ca.vertcat(*(con_eq + con_ineq)) self.con_lb = ca.vertcat(con_eq_lb, *con_ineq_lb) self.con_ub = ca.vertcat(con_eq_ub, *con_ineq_ub) # Build NLP Solver (can also solve QP) nlp = dict(x=opt_var, f=obj, g=con, p=param_s) options = { 'ipopt.print_level': 0, 'ipopt.mu_init': 0.01, 'ipopt.tol': 1e-4, 'ipopt.sb': 'yes', 'ipopt.warm_start_init_point': 'yes', 'ipopt.warm_start_bound_push': 1e-3, 'ipopt.warm_start_bound_frac': 1e-3, 'ipopt.warm_start_slack_bound_frac': 1e-3, 'ipopt.warm_start_slack_bound_push': 1e-3, 'ipopt.warm_start_mult_bound_push': 1e-3, 'ipopt.mu_strategy': 'adaptive', 'print_time': False, 'verbose': False, 'expand': True, 'ipopt.max_iter': 100 } if solver_opts is not None: options.update(solver_opts) self.solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options) # self.solver = ca.nlpsol('mpc_solver', 'sqpmethod', nlp, self.sol_options_sqp) build_solver_time += time.time() print('\n________________________________________') print('# Time to build mpc solver: %f sec' % build_solver_time) print('# Number of variables: %d' % self.num_var) print('# Number of equality constraints: %d' % num_eq_con) print('# Number of inequality constraints: %d' % num_ineq_con) print('----------------------------------------') pass
def __init__(self, model, dynamics, horizon=10, Q=None, P=None, R=None, ulb=None, uub=None, xlb=None, xub=None, terminal_constraint=None, solver_opts=None): """ Initialize and build the MPC solver # Arguments: horizon: Prediction horizon in seconds model: System model # Optional Argumants: Q: State penalty matrix P: Termial penalty matrix R: Input penalty matrix ulb: Lower boundry input uub: Upper boundry input xlb: Lower boundry state xub: Upper boundry state terminal_constraint: Terminal condition on the state * if None: No terminal constraint is used * if zero: Terminal state is equal to zero * if nonzero: Terminal state is bounded within +/- the constraint solver_opts: Additional options to pass to the NLP solver e.g.: solver_opts['print_time'] = False solver_opts['ipopt.tol'] = 1e-8 """ build_solver_time = -time.time() self.dt = model.dt self.Nx, self.Nu = len(model.x_eq), 1 Nopt = self.Nu + self.Nx self.Nt = int(horizon / self.dt) self.dynamics = dynamics # Initialize variables self.set_cost_functions() self.x_sp = None # Cost function weights if P is None: P = np.eye(self.Nx) * 10 if Q is None: Q = np.eye(self.Nx) if R is None: R = np.eye(self.Nu) * 0.01 self.Q = ca.MX(Q) self.P = ca.MX(P) self.R = ca.MX(R) if xub is None: xub = np.full((self.Nx), np.inf) if xlb is None: xlb = np.full((self.Nx), -np.inf) if uub is None: uub = np.full((self.Nu), np.inf) if ulb is None: ulb = np.full((self.Nu), -np.inf) # Starting state parameters - add slack here x0 = ca.MX.sym('x0', self.Nx) x0_ref = ca.MX.sym('x0_ref', self.Nx) u0 = ca.MX.sym('u0', self.Nu) param_s = ca.vertcat(x0, x0_ref, u0) # Create optimization variables opt_var = ctools.struct_symMX([( ctools.entry('u', shape=(self.Nu, ), repeat=self.Nt), ctools.entry('x', shape=(self.Nx, ), repeat=self.Nt + 1), )]) self.opt_var = opt_var self.num_var = opt_var.size # Decision variable boundries self.optvar_lb = opt_var(-np.inf) self.optvar_ub = opt_var(np.inf) """ Set initial values """ obj = ca.MX(0) con_eq = [] con_ineq = [] con_ineq_lb = [] con_ineq_ub = [] con_eq.append(opt_var['x', 0] - x0) # Generate MPC Problem for t in range(self.Nt): # Get variables x_t = opt_var['x', t] u_t = opt_var['u', t] # Dynamics constraint x_t_next = self.dynamics(x_t, u_t) con_eq.append(x_t_next - opt_var['x', t + 1]) # Input constraints if uub is not None: con_ineq.append(u_t) con_ineq_ub.append(uub) con_ineq_lb.append(np.full((self.Nu, ), -ca.inf)) if ulb is not None: con_ineq.append(u_t) con_ineq_ub.append(np.full((self.Nu, ), ca.inf)) con_ineq_lb.append(ulb) # State constraints if xub is not None: con_ineq.append(x_t) con_ineq_ub.append(xub) con_ineq_lb.append(np.full((self.Nx, ), -ca.inf)) if xlb is not None: con_ineq.append(x_t) con_ineq_ub.append(np.full((self.Nx, ), ca.inf)) con_ineq_lb.append(xlb) # Objective Function / Cost Function obj += self.running_cost((x_t - x0_ref), self.Q, u_t, self.R) # Terminal Cost obj += self.terminal_cost(opt_var['x', self.Nt] - x0_ref, self.P) # Terminal contraint if terminal_constraint is not None: con_ineq.append(opt_var['x', self.Nt] - x0_ref) con_ineq_lb.append(np.full((self.Nx, ), -terminal_constraint)) con_ineq_ub.append(np.full((self.Nx, ), terminal_constraint)) # Equality constraints bounds are 0 (they are equality constraints), # -> Refer to CasADi documentation num_eq_con = ca.vertcat(*con_eq).size1() num_ineq_con = ca.vertcat(*con_ineq).size1() con_eq_lb = np.zeros((num_eq_con, )) con_eq_ub = np.zeros((num_eq_con, )) # Set constraints con = ca.vertcat(*con_eq, *con_ineq) self.con_lb = ca.vertcat(con_eq_lb, *con_ineq_lb) self.con_ub = ca.vertcat(con_eq_ub, *con_ineq_ub) # Build NLP Solver (can also solve QP) nlp = dict(x=opt_var, f=obj, g=con, p=param_s) options = { 'ipopt.print_level': 0, 'ipopt.mu_init': 0.01, 'ipopt.tol': 1e-8, 'ipopt.warm_start_init_point': 'yes', 'ipopt.warm_start_bound_push': 1e-9, 'ipopt.warm_start_bound_frac': 1e-9, 'ipopt.warm_start_slack_bound_frac': 1e-9, 'ipopt.warm_start_slack_bound_push': 1e-9, 'ipopt.warm_start_mult_bound_push': 1e-9, 'ipopt.mu_strategy': 'adaptive', 'print_time': False, 'verbose': False, 'expand': False } if solver_opts is not None: options.update(solver_opts) self.solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options) build_solver_time += time.time() print('\n________________________________________') print('# Time to build mpc solver: %f sec' % build_solver_time) print('# Number of variables: %d' % self.num_var) print('# Number of equality constraints: %d' % num_eq_con) print('# Number of inequality constraints: %d' % num_ineq_con) print('----------------------------------------') pass
def get_xi_struct(): xi = cas.struct_symMX([(cas.entry('xi_0'), cas.entry('xi_f'))]) return xi
def __init__(self, horizon, model, gp=None, Q=None, P=None, R=None, S=None, lam=None, lam_state=None, ulb=None, uub=None, xlb=None, xub=None, terminal_constraint=None, feedback=True, percentile=None, gp_method='TA', costFunc='quad', solver_opts=None, discrete_method='gp', inequality_constraints=None, num_con_par=0, hybrid=None, Bd=None, Bf=None): """ Initialize and build the MPC solver # Arguments: horizon: Prediction horizon with control inputs model: System model # Optional Argumants: gp: GP model Q: State penalty matrix, default=diag(1,...,1) P: Termial penalty matrix, default=diag(1,...,1) if feedback is True, then P is the solution of the DARE, discarding this option. R: Input penalty matrix, default=diag(1,...,1)*0.01 S: Input rate of change penalty matrix, default=diag(1,...,1)*0.1 lam: Slack variable penalty for constraints, defalt=1000 lam_state: Slack variable penalty for violation of upper/lower state boundy, defalt=None ulb: Lower boundry input uub: Upper boundry input xlb: Lower boundry state xub: Upper boundry state terminal_constraint: Terminal condition on the state * if None: No terminal constraint is used * if zero: Terminal state is equal to zero * if nonzero: Terminal state is bounded within +/- the constraint * if not None and feedback is True, then the expected value of the Lyapunov function E{x^TPx} < terminal_constraint is used as a terminal constraint. feedback: If true, use an LQR feedback function u= Kx + v percentile: Measure how far from the contrain that is allowed, P(X in constrained set) > percentile, percentile= 1 - probability of violation, default=0.95 gp_method: Method of propagating the uncertainty Possible options: 'TA': Second order Taylor approximation 'ME': Mean equivalent approximation costFunc: Cost function to use in the objective 'quad': Expected valaue of Quadratic Cost 'sat': Expected value of Saturating cost solver_opts: Additional options to pass to the NLP solver e.g.: solver_opts['print_time'] = False solver_opts['ipopt.tol'] = 1e-8 discrete_method: 'gp' - Gaussian process model 'rk4' - Runga-Kutta 4 Integrator 'exact' - CVODES or IDEAS (for ODEs or DEAs) 'hybrid' - GP model for dynamic equations, and RK4 for kinematic equations 'd_hybrid' - Same as above, without uncertainty 'f_hybrid' - GP estimating modelling errors, with RK4 computing the the actual model num_con_par: Number of parameters to pass to the inequality function inequality_constraints: Additional inequality constraints Use a function with inputs (x, covar, u, eps) and that returns a dictionary with inequality constraints and limits. e.g. cons = dict(con_ineq=con_ineq_array, con_ineq_lb=con_ineq_lb_array, con_ineq_ub=con_ineq_ub_array ) # NOTES: * Differentiation of Sundails integrators is not supported with SX graph, meaning that the solver option 'extend_graph' must be set to False to use MX graph instead when using the 'exact' discrete method. * At the moment the f_hybrid option is not finished implemented... """ build_solver_time = -time.time() dt = model.sampling_time() Ny, Nu, Np = model.size() Nx = Nu + Ny Nt = int(horizon / dt) self.__dt = dt self.__Nt = Nt self.__Ny = Ny self.__Nx = Nx self.__Nu = Nu self.__num_con_par = num_con_par self.__model = model self.__hybrid = hybrid self.__gp = gp self.__feedback = feedback self.__discrete_method = discrete_method """ Default penalty values """ if P is None: P = np.eye(Ny) if Q is None: Q = np.eye(Ny) if R is None: R = np.eye(Nu) * 0.01 if S is None: S = np.eye(Nu) * 0.1 if lam is None: lam = 1000 self.__Q = Q self.__P = P self.__R = R self.__S = S self.__Bd = Bd self.__Bf = Bf if xub is None: xub = np.full((Ny), np.inf) if xlb is None: xlb = np.full((Ny), -np.inf) if uub is None: uub = np.full((Nu), np.inf) if ulb is None: ulb = np.full((Nu), -np.inf) """ Default percentile probability """ if percentile is None: percentile = 0.95 quantile_x = np.ones(Ny) * norm.ppf(percentile) quantile_u = np.ones(Nu) * norm.ppf(percentile) Hx = ca.MX.eye(Ny) Hu = ca.MX.eye(Nu) """ Create parameter symbols """ mean_0_s = ca.MX.sym('mean_0', Ny) mean_ref_s = ca.MX.sym('mean_ref', Ny) u_0_s = ca.MX.sym('u_0', Nu) covariance_0_s = ca.MX.sym('covariance_0', Ny * Ny) K_s = ca.MX.sym('K', Nu * Ny) P_s = ca.MX.sym('P', Ny * Ny) con_par = ca.MX.sym('con_par', num_con_par) param_s = ca.vertcat(mean_0_s, mean_ref_s, covariance_0_s, u_0_s, K_s, P_s, con_par) """ Select wich GP function to use """ if discrete_method is 'gp': self.__gp.set_method(gp_method) #TODO:Fix if solver_opts['expand'] is not False and discrete_method is 'exact': raise TypeError( "Can't use exact discrete system with expanded graph") """ Initialize state variance with the GP noise variance """ if gp is not None: #TODO: Cannot use gp variance with hybrid model self.__variance_0 = np.full((Ny), 1e-10) #gp.noise_variance() else: self.__variance_0 = np.full((Ny), 1e-10) """ Define which cost function to use """ self.__set_cost_function(costFunc, mean_ref_s, P_s.reshape((Ny, Ny))) """ Feedback function """ mean_s = ca.MX.sym('mean', Ny) v_s = ca.MX.sym('v', Nu) if feedback: u_func = ca.Function( 'u', [mean_s, mean_ref_s, v_s, K_s], [v_s + ca.mtimes(K_s.reshape((Nu, Ny)), mean_s - mean_ref_s)]) else: u_func = ca.Function('u', [mean_s, mean_ref_s, v_s, K_s], [v_s]) self.__u_func = u_func """ Create variables struct """ var = ctools.struct_symMX([( ctools.entry('mean', shape=(Ny, ), repeat=Nt + 1), ctools.entry('L', shape=(int((Ny**2 - Ny) / 2 + Ny), ), repeat=Nt + 1), ctools.entry('v', shape=(Nu, ), repeat=Nt), ctools.entry('eps', shape=(3, ), repeat=Nt + 1), ctools.entry('eps_state', shape=(Ny, ), repeat=Nt + 1), )]) num_slack = 3 #TODO: Make this a little more dynamic... num_state_slack = Ny self.__var = var self.__num_var = var.size # Decision variable boundries self.__varlb = var(-np.inf) self.__varub = var(np.inf) """ Adjust hard boundries """ for t in range(Nt + 1): j = Ny k = 0 for i in range(Ny): # Lower boundry of diagonal self.__varlb['L', t, k] = 0 k += j j -= 1 self.__varlb['eps', t] = 0 self.__varlb['eps_state', t] = 0 if xub is not None: self.__varub['mean', t] = xub if xlb is not None: self.__varlb['mean', t] = xlb if lam_state is None: self.__varub['eps_state'] = 0 """ Input covariance matrix """ if discrete_method is 'hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() Nz_gp = Ny_gp + Nu_gp covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp) K_sx = ca.SX.sym('K', Nu, Ny) covar_u_func = ca.Function( 'cov_u', [covar_d_sx, K_sx], # [K_sx @ covar_d_sx @ K_sx.T]) [ca.SX(Nu, Nu)]) covar_s = ca.SX(Nz_gp, Nz_gp) covar_s[:Ny_gp, :Ny_gp] = covar_d_sx # covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u) covar_func = ca.Function('covar', [covar_d_sx], [covar_s]) elif discrete_method is 'f_hybrid': #TODO: Fix this... N_gp, Ny_gp, Nu_gp = self.__gp.get_size() Nz_gp = Ny_gp + Nu_gp # covar_x_s = ca.MX.sym('covar_x', Ny_gp, Ny_gp) covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp) K_sx = ca.SX.sym('K', Nu, Ny) # covar_u_func = ca.Function( 'cov_u', [covar_d_sx, K_sx], # [K_sx @ covar_d_sx @ K_sx.T]) [ca.SX(Nu, Nu)]) # cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx], # [covar_x_sx @ K_sx.T]) # cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny))) # cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny))) covar_s = ca.SX(Nz_gp, Nz_gp) covar_s[:Ny_gp, :Ny_gp] = covar_d_sx # covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u) covar_func = ca.Function('covar', [covar_d_sx], [covar_s]) else: covar_x_s = ca.MX.sym('covar_x', Ny, Ny) covar_x_sx = ca.SX.sym('cov_x', Ny, Ny) K_sx = ca.SX.sym('K', Nu, Ny) covar_u_func = ca.Function('cov_u', [covar_x_sx, K_sx], [K_sx @ covar_x_sx @ K_sx.T]) cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx], [covar_x_sx @ K_sx.T]) cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny))) cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny))) covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u) covar_func = ca.Function('covar', [covar_x_s], [covar_s]) """ Hybrid output covariance matrix """ if discrete_method is 'hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp) covar_x_sx = ca.SX.sym('covar_x', Ny, Ny) u_s = ca.SX.sym('u', Nu) cov_x_next_s = ca.SX(Ny, Ny) cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx #TODO: Missing kinematic states covar_x_next_func = ca.Function( 'cov', #[mean_s, u_s, covar_d_sx, covar_x_sx], [covar_d_sx], [cov_x_next_s]) """ f_hybrid output covariance matrix """ elif discrete_method is 'f_hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() # Nz_gp = Ny_gp + Nu_gp covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp) covar_x_sx = ca.SX.sym('covar_x', Ny, Ny) # L_x = ca.SX.sym('L', ca.Sparsity.lower(Ny)) # L_d = ca.SX.sym('L', ca.Sparsity.lower(3)) mean_s = ca.SX.sym('mean', Ny) u_s = ca.SX.sym('u', Nu) # A_f = hybrid.rk4_jacobian_x(mean_s[Ny_gp:], mean_s[:Ny_gp]) # B_f = hybrid.rk4_jacobian_u(mean_s[Ny_gp:], mean_s[:Ny_gp]) # C = ca.horzcat(A_f, B_f) # cov = ca.blocksplit(covar_x_s, Ny_gp, Ny_gp) # cov[-1][-1] = covar_d_sx # cov_i = ca.blockcat(cov) # cov_f = C @ cov_i @ C.T # cov[0][0] = cov_f cov_x_next_s = ca.SX(Ny, Ny) cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx # cov_x_next_s[Ny_gp:, Ny_gp:] = #TODO: Pre-solve the GP jacobian using the initial condition in the solve iteration # jac_mean = ca.SX(Ny_gp, Ny) # jac_mean = self.__gp.jacobian(mean_s[:Ny_gp], u_s, 0) # A = ca.horzcat(jac_f, Bd) # jac = Bf @ jac_f @ Bf.T + Bd @ jac_mean @ Bd.T # temp = jac_mean @ covar_x_s # temp = jac_mean @ L_s # cov_i = ca.SX(Ny + 3, Ny + 3) # cov_i[:Ny,:Ny] = covar_x_s # cov_i[Ny:, Ny:] = covar_d_s # cov_i[Ny:, :Ny] = temp # cov_i[:Ny, Ny:] = temp.T #TODO: This is just a new TA implementation... CLEAN UP... covar_x_next_func = ca.Function( 'cov', [mean_s, u_s, covar_d_sx, covar_x_sx], #TODO: Clean up # [A @ cov_i @ A.T]) # [Bd @ covar_d_s @ Bd.T + jac @ covar_x_s @ jac.T]) # [ca.blockcat(cov)]) [cov_x_next_s]) # Cholesky factorization of covariance function # S_x_next_func = ca.Function( 'S_x', [mean_s, u_s, covar_d_s, covar_x_s], # [Bd @ covar_d_s + jac @ covar_x_s]) L_s = ca.SX.sym('L', ca.Sparsity.lower(Ny)) L_to_cov_func = ca.Function('cov', [L_s], [L_s @ L_s.T]) covar_x_sx = ca.SX.sym('cov_x', Ny, Ny) cholesky = ca.Function('cholesky', [covar_x_sx], [ca.chol(covar_x_sx).T]) """ Set initial values """ obj = ca.MX(0) con_eq = [] con_ineq = [] con_ineq_lb = [] con_ineq_ub = [] con_eq.append(var['mean', 0] - mean_0_s) L_0_s = ca.MX(ca.Sparsity.lower(Ny), var['L', 0]) L_init = cholesky(covariance_0_s.reshape((Ny, Ny))) con_eq.append(L_0_s.nz[:] - L_init.nz[:]) u_past = u_0_s """ Build constraints """ for t in range(Nt): # Input to GP mean_t = var['mean', t] u_t = u_func(mean_t, mean_ref_s, var['v', t], K_s) L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', t]) covar_x_t = L_to_cov_func(L_x) if discrete_method is 'hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() covar_t = covar_func(covar_x_t[:Ny_gp, :Ny_gp]) elif discrete_method is 'd_hybrid': N_gp, Ny_gp, Nu_gp = self.__gp.get_size() covar_t = ca.MX(Ny_gp + Nu_gp, Ny_gp + Nu_gp) elif discrete_method is 'gp': covar_t = covar_func(covar_x_t) else: covar_t = ca.MX(Nx, Nx) """ Select the chosen integrator """ if discrete_method is 'rk4': mean_next_pred = model.rk4(mean_t, u_t, []) covar_x_next_pred = ca.MX(Ny, Ny) elif discrete_method is 'exact': mean_next_pred = model.Integrator(x0=mean_t, p=u_t)['xf'] covar_x_next_pred = ca.MX(Ny, Ny) elif discrete_method is 'd_hybrid': # Deterministic hybrid GP model N_gp, Ny_gp, Nu_gp = self.__gp.get_size() mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t, covar_t) mean_next_pred = ca.vertcat( mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], [])) covar_x_next_pred = ca.MX(Ny, Ny) elif discrete_method is 'hybrid': # Hybrid GP model N_gp, Ny_gp, Nu_gp = self.__gp.get_size() mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t, covar_t) mean_next_pred = ca.vertcat( mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], [])) #covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d, # covar_x_t) covar_x_next_pred = covar_x_next_func(covar_d) elif discrete_method is 'f_hybrid': #TODO: Hybrid GP model estimating model error N_gp, Ny_gp, Nu_gp = self.__gp.get_size() mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t, covar_t) mean_next_pred = ca.vertcat( mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], [])) covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d, covar_x_t) else: # Use GP as default mean_next_pred, covar_x_next_pred = self.__gp.predict( mean_t, u_t, covar_t) """ Continuity constraints """ mean_next = var['mean', t + 1] con_eq.append(mean_next_pred - mean_next) L_x_next = ca.MX(ca.Sparsity.lower(Ny), var['L', t + 1]) covar_x_next = L_to_cov_func(L_x_next).reshape((Ny * Ny, 1)) L_x_next_pred = cholesky(covar_x_next_pred) con_eq.append(L_x_next_pred.nz[:] - L_x_next.nz[:]) """ Chance state constraints """ cons = self.__constraint(mean_next, L_x_next, Hx, quantile_x, xub, xlb, var['eps_state', t]) con_ineq.extend(cons['con']) con_ineq_lb.extend(cons['con_lb']) con_ineq_ub.extend(cons['con_ub']) """ Input constraints """ # cov_u = covar_u_func(covar_x_t, K_s.reshape((Nu, Ny))) cov_u = ca.MX(Nu, Nu) # cons = self.__constraint(u_t, cov_u, Hu, quantile_u, uub, ulb) # con_ineq.extend(cons['con']) # con_ineq_lb.extend(cons['con_lb']) # con_ineq_ub.extend(cons['con_ub']) if uub is not None: con_ineq.append(u_t) con_ineq_ub.extend(uub) con_ineq_lb.append(np.full((Nu, ), -ca.inf)) if ulb is not None: con_ineq.append(u_t) con_ineq_ub.append(np.full((Nu, ), ca.inf)) con_ineq_lb.append(ulb) """ Add extra constraints """ if inequality_constraints is not None: cons = inequality_constraints(var['mean', t + 1], covar_x_next, u_t, var['eps', t], con_par) con_ineq.extend(cons['con_ineq']) con_ineq_lb.extend(cons['con_ineq_lb']) con_ineq_ub.extend(cons['con_ineq_ub']) """ Objective function """ u_delta = u_t - u_past obj += self.__l_func(var['mean', t], covar_x_t, u_t, cov_u, u_delta) \ + np.full((1, num_slack),lam) @ var['eps', t] if lam_state is not None: obj += np.full( (1, num_state_slack), lam_state) @ var['eps_state', t] u_t = u_past L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', Nt]) covar_x_t = L_to_cov_func(L_x) obj += self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape((Ny, Ny))) \ + np.full((1, num_slack),lam) @ var['eps', Nt] if lam_state is not None: obj += np.full( (1, num_state_slack), lam_state) @ var['eps_state', Nt] num_eq_con = ca.vertcat(*con_eq).size1() num_ineq_con = ca.vertcat(*con_ineq).size1() con_eq_lb = np.zeros((num_eq_con, )) con_eq_ub = np.zeros((num_eq_con, )) """ Terminal contraint """ if terminal_constraint is not None and not feedback: con_ineq.append(var['mean', Nt] - mean_ref_s) num_ineq_con += Ny con_ineq_lb.append(np.full((Ny, ), -terminal_constraint)) con_ineq_ub.append(np.full((Ny, ), terminal_constraint)) elif terminal_constraint is not None and feedback: con_ineq.append( self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape( (Ny, Ny)))) num_ineq_con += 1 con_ineq_lb.append(0) con_ineq_ub.append(terminal_constraint) con = ca.vertcat(*con_eq, *con_ineq) self.__conlb = ca.vertcat(con_eq_lb, *con_ineq_lb) self.__conub = ca.vertcat(con_eq_ub, *con_ineq_ub) """ Build solver object """ nlp = dict(x=var, f=obj, g=con, p=param_s) options = { 'ipopt.print_level': 0, 'ipopt.mu_init': 0.01, 'ipopt.tol': 1e-8, 'ipopt.warm_start_init_point': 'yes', 'ipopt.warm_start_bound_push': 1e-9, 'ipopt.warm_start_bound_frac': 1e-9, 'ipopt.warm_start_slack_bound_frac': 1e-9, 'ipopt.warm_start_slack_bound_push': 1e-9, 'ipopt.warm_start_mult_bound_push': 1e-9, 'ipopt.mu_strategy': 'adaptive', 'print_time': False, 'verbose': False, 'expand': True } if solver_opts is not None: options.update(solver_opts) self.__solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options) # First prediction used in the NLP, used in plot later self.__var_prediction = np.zeros((Nt + 1, Ny)) self.__mean_prediction = np.zeros((Nt + 1, Ny)) self.__mean = None build_solver_time += time.time() print('\n________________________________________') print('# Time to build mpc solver: %f sec' % build_solver_time) print('# Number of variables: %d' % self.__num_var) print('# Number of equality constraints: %d' % num_eq_con) print('# Number of inequality constraints: %d' % num_ineq_con) print('----------------------------------------')
def _construct_upd_z_nlp(self): # construct variables self._var_struct_updz = struct([entry('z_i', struct=self.q_i_struct), entry('z_ij', struct=self.q_ij_struct)]) var = struct_symMX(self._var_struct_updz) z_i = self.q_i_struct(var['z_i']) z_ij = self.q_ij_struct(var['z_ij']) # construct parameters self._par_struct_updz = struct([entry('x_i', struct=self.q_i_struct), entry('x_j', struct=self.q_ij_struct), entry('l_i', struct=self.q_i_struct), entry('l_ij', struct=self.q_ij_struct), entry('t'), entry('T'), entry('rho'), entry('par', struct=self.par_struct)]) par = struct_symMX(self._par_struct_updz) x_i, x_j = self.q_i_struct(par['x_i']), self.q_ij_struct(par['x_j']) l_i, l_ij = self.q_i_struct(par['l_i']), self.q_ij_struct(par['l_ij']) t, T, rho = par['t'], par['T'], par['rho'] t0 = t/T # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, l_i], tf, self.q_i) self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij) # construct constraints constraints, lb, ub = [], [], [] for con in self.constraints: c = con[0] for sym in symvar(c): for label, child in self.group.items(): if sym.getName() in child.symbol_dict: name = child.symbol_dict[sym.getName()][1] v = z_i[label, name] ind = self.q_i[child][name] sym2 = MX.zeros(sym.size()) sym2[ind] = v sym2 = reshape(sym2, sym.shape) c = substitute(c, sym, sym2) break for nghb in self.q_ij.keys(): for label, child in nghb.group.items(): if sym.getName() in child.symbol_dict: name = child.symbol_dict[sym.getName()][1] v = z_ij[nghb.label, label, name] ind = self.q_ij[nghb][child][name] sym2 = MX.zeros(sym.size()) sym2[ind] = v sym2 = reshape(sym2, sym.shape) c = substitute(c, sym, sym2) break for name, s in self.par_i.items(): if s.getName() == sym.getName(): c = substitute(c, sym, par['par', name]) constraints.append(c) lb.append(con[1]) ub.append(con[2]) self.lb_updz, self.ub_updz = lb, ub # construct objective obj = 0. for child, q_i in self.q_i.items(): for name in q_i.keys(): x = x_i[child.label, name] z = z_i[child.label, name] l = l_i[child.label, name] obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z)) for nghb in self.q_ij.keys(): for child, q_ij in self.q_ij[nghb].items(): for name in q_ij.keys(): x = x_j[str(nghb), child.label, name] z = z_ij[str(nghb), child.label, name] l = l_ij[str(nghb), child.label, name] obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z)) # construct problem prob, compile_time = self.father.create_nlp(var, par, obj, constraints, self.options) self.problem_upd_z = prob
def setup_nlp_v(nlp_numerics_options, model, formulation, Collocation): # extract necessary inputs variables_dict = model.variables_dict nk = nlp_numerics_options['n_k'] # check if phase fix and adjust theta accordingly if nlp_numerics_options['phase_fix']: theta = get_phase_fix_theta(variables_dict) else: theta = variables_dict['theta'] # define interval struct entries for controls and states entry_tuple = ( cas.entry('xd', repeat=[nk + 1], struct=variables_dict['xd']), cas.entry('u', repeat=[nk], struct=variables_dict['u']), ) # add additional variables according to provided options if nlp_numerics_options['discretization'] == 'direct_collocation': # add algebraic variables at interval except for radau case if nlp_numerics_options['collocation']['scheme'] != 'radau': if nlp_numerics_options['lift_xddot']: entry_tuple += ( cas.entry( 'xddot', repeat=[nk], struct=variables_dict['xddot'] ), # depends on implementation (e.g. not present for radau collocation) ) if nlp_numerics_options['lift_xa']: entry_tuple += ( cas.entry('xa', repeat=[nk], struct=variables_dict['xa']), ) # depends on implementation (e.g. not present for radau collocation) if 'xl' in list(variables_dict.keys()): entry_tuple += ( cas.entry('xl', repeat=[nk], struct=variables_dict['xl']), ) # depends on implementation (e.g. not present for radau collocation) # add collocation node variables d = nlp_numerics_options['collocation'][ 'd'] # interpolating polynomial order coll_var = Collocation.get_collocation_variables_struct(variables_dict) entry_tuple += (cas.entry('coll_var', struct=coll_var, repeat=[nk, d]), ) elif nlp_numerics_options['discretization'] == 'multiple_shooting': # add slack variables for inequalities if nlp_numerics_options[ 'slack_constraints'] == True and model.constraints_dict[ 'inequality']: entry_tuple += (cas.entry( 'us', repeat=[nk], struct=model.constraints_dict['inequality']), ) # multiple shooting: add algebraic variables at interval if lifted if nlp_numerics_options['lift_xddot']: entry_tuple += ( cas.entry( 'xddot', repeat=[nk], struct=variables_dict['xddot'] ), # depends on implementation (e.g. not present for radau collocation) ) if nlp_numerics_options['lift_xa']: entry_tuple += ( cas.entry('xa', repeat=[nk], struct=variables_dict['xa']), ) # depends on implementation (e.g. not present for radau collocation) if 'xl' in list(variables_dict.keys()): entry_tuple += ( cas.entry('xl', repeat=[nk], struct=variables_dict['xl']), ) # depends on implementation (e.g. not present for radau collocation) # add global entries entry_list = [entry_tuple] entry_list += [ cas.entry('theta', struct=theta), cas.entry('phi', struct=model.parameters_dict['phi']), cas.entry('xi', struct=formulation.xi_dict['xi']) ] # generate structure V = cas.struct_symMX(entry_list) return V
def run(self, x0, u_sim, theta_sim, phi_sim): # check consistency of initial conditions: # to do: check values of g, gdot... consistency = True if consistency == False: raise ValueError('provided initial conditions are not consistent!') else: # horizon length N = len(u_sim) # V_sim / simulation output structure V_sim = cas.struct_symMX([ ( cas.entry('xd', repeat=[N, self.__N_sim], struct=self.__variables_dict['xd']), cas.entry('xa', repeat=[N, self.__N_sim-1], struct=self.__variables_dict['xa']), cas.entry('xl', repeat=[N, self.__N_sim-1], struct=self.__variables_dict['xl']), cas.entry('u', repeat=[N], struct=self.__variables_dict['u']), ), cas.entry('theta', struct=self.__variables_dict['theta']), cas.entry('phi', struct=self.__phi) ]) # initialize solution vector V0 = V_sim(0.) # fill in controls and parameters for i in range(N): for name in list(self.__variables_dict['u'].keys()): V0['u',i,name] = self.__variables_dict['u'](u_sim[i])[name] V0['theta'] = theta_sim # adjust time-scaling factor for integrator step size V0['theta','t_f'] = V0['theta','t_f'] / N V0['phi'] = phi_sim # integrate / fill in states and alg vars # initial state x_sim = self.__x(x0)['xd'] z_sim = self.__z(0.0) for i in range(N): # dae parameters for this time step p_sim = self.__p(0.) p_sim['u'] = u_sim[i] p_sim['theta'] = V0['theta'] p_sim['phi'] = V0['phi'] # state on initial time of shooting node for name in list(self.__variables_dict['xd'].keys()): V0['xd',i,0,name] = self.__variables_dict['xd'](x_sim)[name] # integrate up to (including) the final time of the shooting node for j in range(self.__N_sim-1): print(j) ### TESTING # [ode_test, alg_test] = self.__f(x_sim,0.,p_sim.cat) # z_test = self.__rootfinder(0.,x_sim,p_sim.cat) # xddot_test = self.__variables_dict['xddot'](self.__z(z_test)['xddot']) # perform integration res = self.__integrator(x0= x_sim, p = p_sim.cat, z0 = z_sim.cat) # set new initial guess for algebraic variable z_sim = self.__z(res['zf']) # set algebraic variables for name in list(self.__variables_dict['xa'].keys()): V0['xa',i,j,name] = self.__variables_dict['xa'](z_sim['xa'])[name] # set lifted variables for name in list(self.__variables_dict['xl'].keys()): V0['xl',i,j,name] = self.__variables_dict['xl'](z_sim['xl'])[name] # set-up next state x_sim = self.__x(res['xf'])['xd'] # set differential states for name in list(self.__variables_dict['xd'].keys()): V0['xd',i,j+1,name] = self.__variables_dict['xd'](x_sim)[name] self.__status = 'I am a simulation.' self.__V0 = V0 print('Simulation solved.') return None
def __construct_solver(self): """ Construct periodic NLP and solver. """ # system variables and dimensions x = self.__vars['x'] u = self.__vars['u'] variables_entry = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N), ct.entry('u', shape=(self.__nu, ), repeat=self.__N)) if 'us' in self.__vars: variables_entry += (ct.entry('us', shape=(self.__ns, ), repeat=self.__N), ) # nlp variables + bounds w = ct.struct_symMX([variables_entry]) self.__lbw = w(-np.inf) self.__ubw = w(np.inf) # prepare dynamics and path constraints entry constraints_entry = (ct.entry('dyn', shape=(self.__nx, ), repeat=self.__N), ) if self.__h is not None: constraints_entry += (ct.entry('h', shape=self.__h.size1_out(0), repeat=self.__N), ) if self.__gnl is not None: constraints_entry += (ct.entry('g', shape=self.__gnl.size1_out(0), repeat=self.__N), ) # create general constraints structure g_struct = ct.struct_symMX([ constraints_entry, ]) # create symbolic constraint expressions map_args = collections.OrderedDict() map_args['x0'] = ct.horzcat(*w['x']) map_args['p'] = ct.horzcat(*w['u']) # evaluate function dynamics F_constr = ct.horzsplit( self.__F.map(self.__N, self.__parallelization)(**map_args)['xf']) # generate constraints constr = collections.OrderedDict() constr['dyn'] = [ a - b for a, b in zip(F_constr, w['x', 1:] + [w['x', 0]]) ] if 'us' in self.__vars: map_args['us'] = ct.horzcat(*w['us']) if self.__h is not None: constr['h'] = ct.horzsplit( self.__h.map(self.__N, self.__parallelization)(*map_args.values())) if self.__gnl is not None: constr['g'] = ct.horzsplit( self.__gnl.map(self.__N, self.__parallelization)(*map_args.values())) # interleaving of constraints repeated_constr = list( itertools.chain.from_iterable(zip(*constr.values()))) # fill in constraint structure self.__g = g_struct(ca.vertcat(*repeated_constr)) # constraint bounds self.__lbg = g_struct(np.zeros(self.__g.shape)) self.__ubg = g_struct(np.zeros(self.__g.shape)) if self.__h is not None: self.__ubg['h', :] = np.inf # nlp cost cost_map_fun = self.__cost.map(self.__N, self.__parallelization) f = ca.sum2(cost_map_fun(map_args['x0'], map_args['p'])) # add phase fixing cost self.__construct_phase_fixing_cost() alpha = ca.MX.sym('alpha') x0star = ca.MX.sym('x0star', self.__nx, 1) f += self.__phase_fix_fun(alpha, x0star, w['x', 0]) # add slack regularization # if 'us' in self.__vars: # f += self.__reg_slack*ct.mtimes(ct.vertcat(*w['us']).T,ct.vertcat(*w['us'])) # NLP parameters p = ca.vertcat(alpha, x0star) self.__w = w self.__g_fun = ca.Function('g_fun', [w, p], [self.__g]) # create IP-solver prob = {'f': f, 'g': self.__g, 'x': w, 'p': p} opts = {'ipopt': {'linear_solver': 'ma57'}, 'expand': False} if Logger.logger.getEffectiveLevel() > 10: opts['ipopt']['print_level'] = 0 opts['print_time'] = 0 opts['ipopt']['sb'] = 'yes' self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts) # create SQP-solver prob['lbg'] = self.__lbg prob['ubg'] = self.__ubg self.__sqp_solver = sqp_method.Sqp(prob) return None
def __construct_solver(self): """ Construct periodic MPC solver """ # system variables and dimensions x = self.__vars['x'] u = self.__vars['u'] # NLP parameters if self.__type == 'economic': # parameters self.__p = ct.struct_symMX([ ct.entry('x0', shape=(self.__nx, 1)), ct.entry('xN', shape=(self.__nx, 1)) ]) # reassign for brevity x0 = self.__p['x0'] xN = self.__p['xN'] if self.__type == 'tracking': ref_vars = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N + 1), ct.entry('u', shape=(self.__nu, ), repeat=self.__N)) if 'us' in self.__vars: ref_vars += (ct.entry('us', shape=(self.__ns, ), repeat=self.__N), ) # reference trajectory wref = ct.struct_symMX([ref_vars]) nw = self.__nx + self.__nu + self.__ns tuning = ct.struct_symMX([ # tracking tuning ct.entry('H', shape=(nw, nw), repeat=self.__N), ct.entry('q', shape=(nw, 1), repeat=self.__N) ]) # parameters self.__p = ct.struct_symMX([ ct.entry('x0', shape=(self.__nx, )), ct.entry('wref', struct=wref), ct.entry('tuning', struct=tuning) ]) # reassign for brevity x0 = self.__p['x0'] wref = self.__p.prefix['wref'] tuning = self.__p.prefix['tuning'] xN = wref['x', -1] # NLP variables variables_entry = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N + 1), ct.entry('u', shape=(self.__nu, ), repeat=self.__N)) if 'us' in self.__vars: variables_entry += (ct.entry('us', shape=(self.__ns, ), repeat=self.__N), ) self.__wref = ct.struct_symMX([variables_entry ]) # structure of reference if 'usc' in self.__vars: variables_entry += (ct.entry('usc', shape=(self.__nsc, ), repeat=self.__N), ) # nlp variables + bounds w = ct.struct_symMX([variables_entry]) # variable bounds are implemented as inequalities self.__lbw = w(-np.inf) self.__ubw = w(np.inf) # prepare dynamics and path constraints entry constraints_entry = (ct.entry('dyn', shape=(self.__nx, ), repeat=self.__N), ) if self.__gnl is not None: constraints_entry += (ct.entry('g', shape=self.__gnl.size1_out(0), repeat=self.__N), ) if self.__h is not None: constraints_entry += (ct.entry('h', shape=self.__h.size1_out(0), repeat=self.__N), ) # terminal constraint self.__nx_term = self.__p_operator.size1_out(0) # create general constraints structure g_struct = ct.struct_symMX([ ct.entry('init', shape=(self.__nx, 1)), constraints_entry, ct.entry('term', shape=(self.__nx_term, 1)) ]) # create symbolic constraint expressions map_args = collections.OrderedDict() map_args['x0'] = ct.horzcat(*w['x', :-1]) map_args['p'] = ct.horzcat(*w['u']) F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf']) # generate constraints constr = collections.OrderedDict() constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])] if 'us' in self.__vars: map_args['us'] = ct.horzcat(*w['us']) if self.__gnl is not None: constr['g'] = ct.horzsplit( self.__gnl.map(self.__N)(*map_args.values())) if 'usc' in self.__vars: map_args['usc'] = ct.horzcat(*w['usc']) if self.__h is not None: constr['h'] = ct.horzsplit( self.__h.map(self.__N)(*map_args.values())) repeated_constr = list( itertools.chain.from_iterable(zip(*constr.values()))) term_constraint = self.__p_operator(w['x', -1] - xN) self.__g = g_struct( ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint)) self.__lbg = g_struct(np.zeros(self.__g.shape)) self.__ubg = g_struct(np.zeros(self.__g.shape)) if self.__h is not None: self.__ubg['h', :] = np.inf for i in self.__h_us_idx + self.__h_x_idx: # rm constraints the only depend on x at k = 0 self.__lbg['h', 0, i] = -np.inf # nlp cost cost_map = self.__cost.map(self.__N) if self.__type == 'economic': cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])] elif self.__type == 'tracking': if self.__ns != 0: cost_args_w = ct.horzcat(*[ ct.vertcat(w['x', k], w['u', k], w['us', k]) for k in range(self.__N) ]) cost_args_w_ref = ct.horzcat(*[ ct.vertcat(wref['x', k], wref['u', k], wref['us', k]) for k in range(self.__N) ]) else: cost_args_w = ct.horzcat(*[ ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N) ]) cost_args_w_ref = ct.horzcat(*[ ct.vertcat(wref['x', k], wref['u', k]) for k in range(self.__N) ]) cost_args = [ cost_args_w, cost_args_w_ref, ct.horzcat(*tuning['H']), ct.horzcat(*tuning['q']) ] if self.__options['hessian_approximation'] == 'gauss_newton': if 'usc' not in self.__vars: hess_gn = ct.diagcat(*tuning['H'], ca.DM.zeros(self.__nx, self.__nx)) else: hess_block = list( itertools.chain.from_iterable( zip(tuning['H'], [ca.DM.zeros(self.__nsc, self.__nsc)] * self.__N))) hess_gn = ct.diagcat(*hess_block, ca.DM.zeros(self.__nx, self.__nx)) J = ca.sum2(cost_map(*cost_args)) # add cost on slacks if 'usc' in self.__vars: J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc']))) # create solver prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p} self.__w = w self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g]) # create IPOPT-solver instance if needed if self.__options['ipopt_presolve']: opts = { 'ipopt': { 'linear_solver': 'ma57', 'print_level': 0 }, 'expand': False } if Logger.logger.getEffectiveLevel() > 10: opts['ipopt']['print_level'] = 0 opts['print_time'] = 0 opts['ipopt']['sb'] = 'yes' self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts) # create hessian approximation function if self.__options['hessian_approximation'] == 'gauss_newton': lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used hess_approx = ca.Function('hess_approx', [self.__w, self.__p, lam_g], [hess_gn]) elif self.__options['hessian_approximation'] == 'exact': hess_approx = 'exact' # create sqp solver prob['lbg'] = self.__lbg prob['ubg'] = self.__ubg sqp_opts = { 'hessian_approximation': hess_approx, 'max_iter': self.__options['max_iter'] } self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts)
def __construct_solver(self): """ Construct casadi.nlpsol Object based on MPC trial information. """ awelogger.logger.info("Constructing MPC solver object...") if self.__cost_type == 'economic': # parameters self.__p = ct.struct_symMX([ ct.entry('x0', shape = (self.__nx,1)) ]) if self.__cost_type == 'tracking': # parameters self.__p = ct.struct_symMX([ ct.entry('x0', shape = (self.__nx,)), ct.entry('ref', struct = self.__trial.nlp.V) ]) # create P evaluator for use in NLP arguments self.__create_P_fun() # generate mpc constraints g = self.__trial.nlp.g_fun(self.__trial.nlp.V, self.__P_fun(self.__p)) # generate cost function f = self.__generate_objective() # fill in nlp dict nlp = {'x': self.__trial.nlp.V, 'p': self.__p, 'f': f, 'g': g} # store nlp bounds self.__trial.nlp.V_bounds['ub']['phi'] = 0.0 self.__trial.nlp.V_bounds['lb']['xi'] = 0.0 self.__trial.nlp.V_bounds['ub']['xi'] = 0.0 for name in list(self.__trial.model.variables_dict['u'].keys()): if 'fict' in name: self.__trial.nlp.V_bounds['lb']['coll_var',:,:,'u',name] = 0.0 self.__trial.nlp.V_bounds['ub']['coll_var',:,:,'u',name] = 0.0 self.__lbw = self.__trial.nlp.V_bounds['lb'] self.__ubw = self.__trial.nlp.V_bounds['ub'] self.__lbg = self.__trial.nlp.g_bounds['lb'] self.__ubg = self.__trial.nlp.g_bounds['ub'] awelogger.logger.level = awelogger.logger.getLogger().getEffectiveLevel() opts = {} opts['expand'] = self.__mpc_options['expand'] opts['ipopt.linear_solver'] = self.__mpc_options['linear_solver'] opts['ipopt.max_iter'] = self.__mpc_options['max_iter'] opts['ipopt.max_cpu_time'] = self.__mpc_options['max_cpu_time'] opts['jit'] = self.__mpc_options['jit'] if awelogger.logger.level > 10: opts['ipopt.print_level'] = 0 opts['print_time'] = 0 self.__solver = ct.nlpsol('solver', 'ipopt', nlp, opts) return None