def formulate_center_problem(f, v, Hv, hv, p=None): nf = f.shape[0] v_lwb = np.asarray([min_func(v[i], v, Hv, hv) for i in range(v.shape[0])]) v_uwb = -np.asarray([min_func(-v[i], v, Hv, hv) for i in range(v.shape[0])]) v_span = np.vstack(v_uwb - v_lwb) norm_Hh = (Hv @ v - hv) / (Hv @ v_span) objective = norm_Hh.T @ norm_Hh g = [] lbg = [] ubg = [] g += [f] lbg += [0] * nf ubg += [0] * nf g += [Hv @ v] lbg += [-inf] * g[-1].shape[0] ubg += [hv] problem = {'f': objective, 'x': v, 'g': vertcat(*g)} if p is None: solver = nlpsol("solver", "ipopt", problem, NLP_OPTS) else: problem["p"] = p solver = nlpsol("solver", "ipopt", problem, NLP_OPTS) return solver, lbg, ubg
def create_nlp(var, par, obj, con, options, name=''): codegen = options['codegen'] if options['verbose'] >= 1: print('Building nlp ... ', end=' ') t0 = time.time() nlp = {'x': var, 'p': par, 'f': obj, 'g': con} slv_opt = options['solver_options'][options['solver']] opt = {} for key, value in slv_opt.items(): opt[key] = value opt.update({'expand': True}) print('opt updated') solver = nlpsol('solver', options['solver'], nlp, opt) print('solver created') name = 'nlp' if name == '' else 'nlp_' + name if codegen['build'] == 'jit': if options['verbose'] >= 1: print(('[jit compilation with flags %s]' % (codegen['flags'])), end=' ') solver.generate_dependencies(name+'.c') compiler = Compiler( name+'.c', 'clang', {'flags': codegen['flags']}) problem = nlpsol('solver', options['solver'], compiler, slv_opt) os.remove(name+'.c') elif codegen['build'] == 'shared': if os.name == 'nt': raise ValueError('Build option is not supported for Windows!') directory = os.path.join(os.getcwd(), 'build') if not os.path.isdir(directory): os.makedirs(directory) path = os.path.join(directory, name) if options['verbose'] >= 1: print(('[compile to .so with flags %s]' % (codegen['flags'])), end=' ') if os.path.isfile(path+'.so'): os.remove(path+'.so') solver.generate_dependencies(name+'.c') shutil.move(name+'.c', path+'.c') os.system('gcc -fPIC -shared %s %s.c -o %s.so' % (codegen['flags'], path, path)) problem = nlpsol('solver', options['solver'], path+'.so', slv_opt) os.remove(path+'.c') elif codegen['build'] == 'existing': if os.name == 'nt': raise ValueError('Build option is not supported for Windows!') directory = os.path.join(os.getcwd(), 'build') path = os.path.join(directory, name) if not os.path.isfile(path+'.so'): raise ValueError('%s.so does not exist!', path) if options['verbose'] >= 1: print(('[using shared object %s.so]' % path), end=' ') problem = nlpsol('solver', options['solver'], path+'.so', slv_opt) elif codegen['build'] is None: problem = solver else: raise ValueError('Invalid build option.') print('nlpsol done.') t1 = time.time() if options['verbose'] >= 1: print('in %5f s' % (t1-t0)) return problem, (t1-t0)
def solve_numerical_nlp(chosen_fun, chosen_data, opt_problem_details, optim_options={}): """solve nlp for determining parameter values numerically""" # formulate nlp nlp = chosen_fun(chosen_data) # NLP solver options if optim_options: solver = optim_options["solver"] opts = optim_options["opts"] else: solver = "ipopt" opts = {"ipopt.tol": 1e-16} # solver = "sqpmethod" # opts = {"qpsol": "qpoases"} # Allocate an NLP solver and buffer solver = casadi.nlpsol("solver", solver, nlp, opts) # Solve the problem res = solver(**opt_problem_details) return res
def solve_nlp(prop_name, thrust): rho0 = 1.225 i0_motor0 = 0.04 V_inf0 = 1 R_motor0 = 0.05 T_desired = thrust #### Thrust constraint prop_data = prop_db[prop_name] key0 = list(prop_data['dynamic'].keys())[0] my_prop = prop_data['dynamic'][key0]['data'] CT_lut = ca.interpolant('CT', 'bspline', [my_prop.index], my_prop.CT) CP_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.CP) eta_lut = ca.interpolant('CP', 'bspline', [my_prop.index], my_prop.eta) eta_prop_lut = ca.interpolant('eta', 'bspline', [my_prop.index], my_prop.eta) Q_prop_lut = ca.substitute(Q_prop, CP, CP_lut(J)) T_prop_lut = ca.substitute(T_prop, CT, CT_lut(J)) states = ca.vertcat(rpm_prop, v_batt, Kv_motor) params = ca.vertcat(rho, r_prop_in, i0_motor, V_inf, R_motor) p0 = [rho0, prop_data['D'] / 2, i0_motor0, V_inf0, R_motor0] f_Q_prop = ca.Function('Q_prop', [states, params], [Q_prop_lut]) f_Q_motor = ca.Function('Q_motor', [states, params], [Q_motor]) f_eta_motor = ca.Function('eta_motor', [states, params], [eta_motor]) f_eta_prop = ca.Function('eta_prop', [states, params], [eta_prop_lut(J)]) f_eta = ca.Function('eta', [states, params], [eta_prop_lut(J) * eta_motor]) f_T_prop = ca.Function('T_prop', [states, params], [T_prop_lut]) #%% nlp = { 'f': -f_eta(states, p0), 'x': states, 'g': ca.substitute(ca.vertcat(Q_prop_lut - Q_motor, T_prop_lut - T_desired), params, p0) } S = ca.nlpsol('eff_max', 'ipopt', nlp, { 'print_time': 0, 'ipopt': { 'sb': 'yes', 'print_level': 0, } }) res = S(x0=[1000, 5, 1000], lbg=[0, 0], ubg=[0, 0]) stats = S.stats() if stats['success']: print('\n') print('propeller -> ' + prop_name, res['x'], res['g'], -res['f']) print('\n') v0 = float(res['x'][1]) kv0 = float(res['x'][2]) if -res['f'] > 0.3: rpm_sweep(prop_name, v0, kv0, i0_motor0, V_inf0, R_motor0) return res, stats
def fitModel(self, u_in, y_out): y_coeff = ca.SX.sym('y_c', self.n_output_lag) u_coeff = ca.SX.sym('u_c', self.n_input_lag + 1) # include current input params = ca.vertcat(u_coeff, y_coeff) obj = self.getObjective(u_in, y_out, u_coeff, y_coeff) # set up solver nlp = {'x': params, 'f': obj} solver_opts = { 'ipopt': { 'print_level': 0, 'linear_solver': 'mumps' }, 'print_time': 0 } solver = ca.nlpsol('solver', 'ipopt', nlp, solver_opts) # solver.generate_dependencies('nlp.c') # os.system("gcc -fPIC -shared nlp.c -o nlp.so") # abspath = os.path.abspath('nlp.so') # solver = ca.nlpsol('solver', 'ipopt', abspath, solver_opts) x0 = np.zeros(params.shape[0]) res = np.array(solver(x0=x0)['x']) self.u_coeff = res[:u_coeff.shape[0]] self.y_coeff = res[u_coeff.shape[0]:] print(res) # # def eval_fct(u_prev, y_prev): # return self.predict(u_prev, y_prev, res[:u_coeff.shape[0]], res[u_coeff.shape[0]:])
def create_solver(self): """ Create a new nonlinear solver. """ problem = { 'f': self.cost_function, 'x': vertcat(*self.var_symbols), 'g': vertcat(*self.cons_exprs)} options = {} if self.solver_name in ['scpgen', 'sqpmethod']: options.update({ 'qpsol': "qpoases", 'qpsol_options': {"printLevel": "none"}, }) elif self.solver_name == 'ipopt': options.update({ 'expand': self.casadi_expand, 'ipopt.fast_step_computation': self.ipopt_fast_step_computation, 'ipopt.fixed_variable_treatment': self.ipopt_fixed_variable_treatment, 'ipopt.linear_solver': self.ipopt_linear_solver, 'ipopt.max_cpu_time': self.ipopt_max_cpu_time, 'ipopt.max_iter': self.ipopt_max_iter, 'ipopt.mu_strategy': self.ipopt_mu_strategy, 'ipopt.nlp_lower_bound_inf': self.ipopt_nlp_lower_bound_inf, 'ipopt.nlp_upper_bound_inf': self.ipopt_nlp_upper_bound_inf, 'ipopt.print_level': self.ipopt_print_level, 'ipopt.warm_start_init_point': self.ipopt_warm_start_init_point, 'verbose': False }) self.solver = nlpsol('solver', self.solver_name, problem, options)
def _initialize_solver(self, **kwargs): nlpsol_args = { "expand", "iteration_callback", "iteration_callback_step", "iteration_callback_ignore_errors", "ignore_check_vec", "warn_initial_bounds", "eval_errors_fatal", "print_time", "verbose_init" } # Initialize NLP object opts = { 'ipopt.max_iter': 10000, # 'linear_solver' : 'ma27' } if kwargs is not None: for key, val in kwargs.items(): if key in nlpsol_args: opts.update({key: val}) else: opts.update({'ipopt.' + key: val}) self._solver_opts = opts constraints = cs.vertcat(*self._constraints_sx) self._solver = cs.nlpsol( "solver", "ipopt", { 'x': self.var.vars_sx, 'p': self.pvar.vars_sx, 'f': self.objective_sx, 'g': constraints }, self._solver_opts) self.col_vars['lbg'] = np.concatenate(self._constraints_lb) self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
def compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef): model = export_chain_mass_model(n_mass, m, D, L) nx = model.x.shape[0] M = int((nx/3 -1)/2) # initial guess for state pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) x0 = np.zeros((nx, 1)) x0[:3*(M+1):3] = pos0_x[1:].reshape((M+1,1)) # decision variables w = [model.x, model.xdot, model.u] # initial guess w0 = ca.vertcat(*[x0, np.zeros(model.xdot.shape), np.zeros(model.u.shape)]) # constraints g = [] g += [model.f_impl_expr] # steady state g += [model.x[3*M:3*(M+1)] - xEndRef] # fix position of last mass g += [model.u] # don't actuate controlled mass # misuse IPOPT as nonlinear equation solver nlp = {'x': ca.vertcat(*w), 'f': 0, 'g': ca.vertcat(*g)} solver = ca.nlpsol('solver', 'ipopt', nlp) sol = solver(x0=w0,lbg=0,ubg=0) wrest = sol['x'].full() xrest = wrest[:nx] return xrest
def setup_solver(self): # Setup relevant functions and expressions self.setup_problem_functions() self.solver = cs.nlpsol("solver", self.options["solver_name"], self.mpc_problem["nlp"], self.options["solver_opts"])
def nlp_solve(prob, obj, p_init, x_init, y_init): """ NLP solver for initial conditions to path-following algorithm """ nx, np, neq, niq, name = prob() if niq > 0: x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj( x_init, y_init, p_init, neq, niq, nx, np) #Formulating NLP to solve #All constraints must be formatted as inequality constraints for this solver nlp = {'x': x, 'p': p, 'f': f, 'g': con} solver = nlpsol('solver', 'ipopt', nlp) sol = solver(x0=x_init, p=p_init, lbg=lbg, ubg=ubg, ubx=ubx, lbx=lbx) x_opt = sol['x'] #Solving for x lagmul = sol['lam_g'] #Determining active constraints #(necessary to determine which multipliers are a lambda and which are a mu) con_vals = conf(x_opt, p_init) tol = 1e-6 for k in range(0, len(con_vals)): if con_vals[k] >= 0 + tol or con_vals[ k] >= 0 - tol: #active constraint lam_opt = lagmul[k] else: #inactive constraint mu_opt = lagmul[k] #print('x_opt:',x_opt,'lambda:',lam_opt,'mu:',mu_opt) return x_opt, lam_opt, mu_opt, con
def export_casadi_problems(self, destination, father, problem): cwd = os.getcwd() filenames = [] # substitutes for child, subst in father.substitutes.items(): for name, fun in subst.items(): filenames.append('subst_'+name+'.c') fun.generate(filenames[-1]) shutil.move(cwd+'/'+filenames[-1], destination+'src/'+filenames[-1]) # updx obj = father.problem_description['obj'] con = father.problem_description['con'] var = father.problem_description['var'] par = father.problem_description['par'] opt = father.problem_description['opt'] nlp = {'x': var, 'p': par, 'f': obj, 'g': con} options = {} for key, value in opt['solver_options']['ipopt'].items(): options[key] = value options.update({'expand': True}) solver = nlpsol('solver', 'ipopt', nlp, options) solver.generate_dependencies('updx.c') # updz, updl, upd_res problem.problem_upd_z.generate('updz.c') problem.problem_upd_l.generate('updl.c') problem.problem_upd_res.generate('updres.c') filenames.extend(['updx.c', 'updz.c', 'updl.c', 'updres.c']) # move files shutil.move(cwd+'/updx.c', destination+'src/updx.c') shutil.move(cwd+'/updz.c', destination+'src/updz.c') shutil.move(cwd+'/updl.c', destination+'src/updl.c') shutil.move(cwd+'/updres.c', destination+'src/updres.c') return filenames
def trim(vt, gamma, m_fuel, z, rhs, p, s0=None): if s0 is None: s0 = [0.1, -np.deg2rad(45), 0, 0, 0, 0] s = ca.SX.sym('s', 6) nlp = { 'x': s, 'f': objective(s, vt=vt, gamma=gamma, m_fuel=m_fuel, rhs=rhs, p=p, z=z), 'g': constraint(s, vt=vt, gamma=gamma, m_fuel=m_fuel, rhs=rhs, p=p, z=z) } S = ca.nlpsol('S', 'ipopt', nlp, { 'print_time': 0, 'ipopt': { 'sb': 'yes', 'print_level': 0, } }) # s = [m_dot, alpha, beta, elev, ail, rdr] m_dot = s[0] alpha = s[1] beta = s[2] ail = s[3] elv = s[4] rdr = s[5] res = S(x0=s0, lbg=[0, 0, 0], ubg=[0, 0, 0], lbx=[ 0, -np.deg2rad(20), -np.deg2rad(20), -np.deg2rad(20), -np.deg2rad(20), -np.deg2rad(20) ], ubx=[ 10, np.deg2rad(20), np.deg2rad(20), np.deg2rad(20), np.deg2rad(20), np.deg2rad(20) ]) stats = S.stats() if not stats['success']: raise ValueError('Trim failed to converge', stats['return_status'], res) s_opt = res['x'] x0, u0 = constrain(s_opt, vt, gamma, m_fuel, z) return { 'x0': np.array(ca.DM(x0)).reshape(-1), 'u0': np.array(ca.DM(u0)).reshape(-1), 's': np.array(ca.DM(s_opt)).reshape(-1), 'f': float(res['f']), 'g': np.array(ca.DM(res['g'])).reshape(-1), 'status': stats['return_status'] }
def run_ipopt_ssmevaluator(ssm, n_s, n_u, linearize_mean): casadi_ssm = CasadiSSMEvaluator(ssm, linearize_mean) x = cas.MX.sym("x", (n_s, 1)) y = cas.MX.sym("y", (n_u, 1)) if linearize_mean: mu, sigma, mu_jac = casadi_ssm(x, y) f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma)) + cas.sum1( cas.sum2(mu_jac)) else: mu, sigma = casadi_ssm(x, y) f = cas.sum1(cas.sum2(mu)) + cas.sum1(cas.sum2(sigma)) x = cas.vertcat(x, y) options = { "ipopt": { "hessian_approximation": "limited-memory", "max_iter": 2, "derivative_test": "first-order" } } solver = cas.nlpsol("solver", "ipopt", {"x": x, "f": f}, options) with capture_stdout() as out: res = solver(x0=np.random.randn(5, 1)) return str(type(ssm)), linearize_mean, out[0]
def smooth_Lagrange_poly(x, y): t = ca.SX.sym('t') d = len(x) # amount of parameters tau = ca.SX.sym('tau', d) # parameter as minimisation variable poly = 0 for j in range(d): # for all data points ... L = tau[j] for r in range(d): if r != j: L *= (t - x[r]) / (x[j] - x[r]) poly += L L_fun = ca.Function('L_fun', [t, tau], [poly]) ddL, _ = ca.hessian(poly, t) ddL_fun = ca.Function('ddL_fun', [t, tau], [ddL]) # ddL_fun = L_fun.hessian(0) # second order derivative to # [ddL,_,_] = ddL_fun([t,tau]) # minimise tau = fct output, incl penalize curvature res = 0.1 * sum([(L_fun(x[k], tau) - y[k])**2 for k in range(d)])[0] res += sum([ddL_fun(x[k], tau)[0]**2 * 1e4 for k in range(d)])[0] Cost = ca.Function('cost', [tau], [res]) nlp = {'x': tau, 'f': res} solver = ca.nlpsol("solver", "ipopt", nlp) sol = solver(**{}) tau_opt = sol['x'] # optimal parameter for polynomial return L_fun, tau_opt
def create_problem(self, nb_of_veh): # Objective is 1 norm of the slacks obj = sum( definite_integral(self.sx_list[i], 0, 1) + definite_integral(self.sy_list[i], 0, 1) for i in range(nb_of_veh)) var = cas.vertcat() par = cas.vertcat() # Setting up the optimization variables (symbolic) for each vehicle. for i in range(nb_of_veh): var = cas.vertcat(var, self.x_cfs_list[i], self.y_cfs_list[i], self.sx_cfs_list[i], self.sy_cfs_list[i]) for i in range(nb_of_veh): var = cas.vertcat(var, self.lmbd_cfs_list[i]) # Setting up the parameter set (symbolic) for each vehicle. These values will be filled in later on, in each solver iteration. for i in range(nb_of_veh): par = cas.vertcat(par, self.corr_start_list[i], self.corr_end_list[i], self.corr_x_min_list[i], self.corr_y_min_list[i], self.corr_x_max_list[i], self.corr_y_max_list[i], self.corr_hp_list[i], self.treshold_list[i]) par = cas.vertcat(par, self.corr_v0_list[i], self.maxVel_list[i], self.maxAcc_list[i], self.radius_list[i], self.overlap_list[i]) con = cas.vertcat(*self.con) nlp = {'x': var, 'f': obj, 'p': par, 'g': con} solver = cas.nlpsol('solver', 'ipopt', nlp, { 'ipopt.tol': 1e-5, 'ipopt.linear_solver': 'ma57' }) self.solver = solver self.nlp = nlp
def solve(self): """ Gives to CasADi states, controls, constraints, sum of all objective functions and theirs bounds. Gives others parameters to control how solver works. """ # NLP nlp = {"x": self.V, "f": self.J, "g": self.g} opts = { "ipopt.tol": 1e-6, "ipopt.max_iter": 1000, "ipopt.hessian_approximation": "exact", # "exact", "limited-memory" "ipopt.limited_memory_max_history": 50, "ipopt.linear_solver": "mumps", # "ma57", "ma86", "mumps" "iteration_callback": self.show_online_optim_callback, } solver = casadi.nlpsol("nlpsol", "ipopt", nlp, opts) # Bounds and initial guess arg = { "lbx": self.V_bounds.min, "ubx": self.V_bounds.max, "lbg": self.g_bounds.min, "ubg": self.g_bounds.max, "x0": self.V_init.init, } # Solve the problem return solver.call(arg)
def J_qi(T, Q, R, P_f, N_pred, states, controls, rhs, func_type=1): # kriegen die Anzahl der Zeiler("shape" ist ein Tuple) n_states = states.shape[0] # p 22. # Eingangsanzahl n_controls = controls.shape[0] # nonlinear mapping function f(x,u) f = Function('f', [states, controls], [rhs]) # Decision variables (controls) u_pwm U = SX.sym('U', n_controls, N_pred) # parameters (which include the !!!initial and the !!!reference state of # the robot!!!reference input) P = SX.sym('P', n_states + n_states + n_controls) # number of x is always N+1 -> 0..N X = SX.sym('X', n_states, (N_pred + 1)) X[:, 0] = P[0:n_states] obj = 0 # compute predicted states and cost function for k in range(N_pred): st = X[:, k] con = U[:, k] f_value = f(st, con) # print(f_value) st_next = st + (T * f_value) X[:, k + 1] = st_next obj = obj + (st - P[n_states:2 * n_states]).T @ Q @ ( st - P[n_states:2 * n_states]) + ( con - P[2 * n_states:2 * n_states + n_controls]).T @ R @ ( con - P[2 * n_states:2 * n_states + n_controls]) # print(obj) st = X[:, N_pred] obj = obj + (st - P[n_states:2 * n_states]).T @ P_f @ ( st - P[n_states:2 * n_states]) # create a dense matrix of state constraints. Size of g: n_states * (N_pred+1) +1 g = SX.zeros(n_states * (N_pred + 1) + 1, 1) # Constrainsvariable spezifizieren for i in range(N_pred + 1): for j in range(n_states): g[n_states * i + j] = X[j, i] g[n_states * (N_pred + 1)] = (X[:, N_pred] - P[n_states:2 * n_states]).T @ P_f @ ( X[:, N_pred] - P[n_states:2 * n_states]) OPT_variables = reshape(U, N_pred, 1) nlp_prob = {'f': obj, 'x': OPT_variables, 'g': g, 'p': P} opts = {} opts['print_time'] = False opts['ipopt'] = { 'max_iter': 100, 'print_level': 0, 'acceptable_tol': 1e-8, 'acceptable_obj_change_tol': 1e-6 } # print(opts) return f, nlpsol("solver", "ipopt", nlp_prob, opts)
def solve(self, problem): prob, bounds, w, x_plot, u_plot = problem # Create an NLP solver solver = ca.nlpsol('solver', 'ipopt', prob, {'verbose': False}) # Function to get x and u trajectories from w trajectories = ca.Function('trajectories', [w], [x_plot, u_plot], ['w'], ['x', 'u']) # Solve the NLP #sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) sol = solver(**bounds) x_opt, u_opt = trajectories(sol['x']) self.x_opt = x_opt.full() # to numpy array self.u_opt = u_opt.full() # to numpy array # Feed the previous state back to the model self.model.set_state_estimate(self.x_opt) self.model.set_control_estimate(self.u_opt) # This ensures the control does not change drastically from the previous # (already-executed) control print("u_opt", [self.u_opt[0, 0], self.u_opt[1, 0]]) self.Uk_prev = [self.u_opt[0, 0], self.u_opt[1, 0]] # we don't need this? we do. return self.x_opt, self.u_opt, solver.stats()
def solve(self): all_J = self.__dispatch_obj_func() all_g, all_g_bounds = self.__dispatch_bounds() self.ipopt_nlp = {"x": self.ocp.V, "f": sum1(all_J), "g": all_g} self.ipopt_limits = { "lbx": self.ocp.V_bounds.min, "ubx": self.ocp.V_bounds.max, "lbg": all_g_bounds.min, "ubg": all_g_bounds.max, "x0": self.ocp.V_init.init, } if self.lam_g is not None: self.ipopt_limits["lam_g0"] = self.lam_g if self.lam_x is not None: self.ipopt_limits["lam_x0"] = self.lam_x solver = nlpsol("nlpsol", "ipopt", self.ipopt_nlp, self.opts) # Solve the problem self.out = {"sol": solver.call(self.ipopt_limits)} self.out["sol"]["time_tot"] = solver.stats()["t_wall_total"] # To match acados convention (0 = success, 1 = error) self.out["sol"]["status"] = int(not solver.stats()["success"]) return self.out
def fit(self, input, output): nsamples = input.shape[0] fobj = 0 for n in range(nsamples): yhat = self.sym['fcn'].call({ 'u': input[n, :], 'w': self.sym['w'] })['yhat'] y = indicator_array(yhat.shape, output[n]) fobj -= cas.sum1(xlogy(y, yhat) + xlogy( (1 - y), 1 - yhat)) # cross entropy loss fobj = fobj / nsamples # scaling if self.alpha: fobj += self.alpha * cas.dot(self.sym['w'], self.sym['w']) # L2 regularization nlp = {'x': self.sym['w'], 'f': fobj, 'g': []} opts = { 'print_time': False, 'ipopt': { 'tol': 1e-3, 'max_iter': 1000, 'hessian_approximation': 'limited-memory', 'print_level': 0 } } solver = cas.nlpsol('nlpsol', 'ipopt', nlp, opts) res = solver(x0=self.weights) self.weights = np.asarray(res['x'])
def solve(self, solver="ipopt", show_online_optim=False, options_ipopt={}): """ Gives to CasADi states, controls, constraints, sum of all objective functions and theirs bounds. Gives others parameters to control how solver works. """ all_J = MX() for j_nodes in self.J: for j in j_nodes: all_J = vertcat(all_J, j) for nlp in self.nlp: for obj_nodes in nlp["J"]: for obj in obj_nodes: all_J = vertcat(all_J, obj) all_g = MX() all_g_bounds = Bounds(interpolation_type=InterpolationType.CONSTANT) for i in range(len(self.g)): for j in range(len(self.g[i])): all_g = vertcat(all_g, self.g[i][j]) all_g_bounds.concatenate(self.g_bounds[i][j]) for nlp in self.nlp: for i in range(len(nlp["g"])): for j in range(len(nlp["g"][i])): all_g = vertcat(all_g, nlp["g"][i][j]) all_g_bounds.concatenate(nlp["g_bounds"][i][j]) nlp = {"x": self.V, "f": sum1(all_J), "g": all_g} options_common = {} if show_online_optim: options_common["iteration_callback"] = OnlineCallback(self) if solver == "ipopt": options_default = { "ipopt.tol": 1e-6, "ipopt.max_iter": 1000, "ipopt.hessian_approximation": "exact", # "exact", "limited-memory" "ipopt.limited_memory_max_history": 50, "ipopt.linear_solver": "mumps", # "ma57", "ma86", "mumps" } for key in options_ipopt: if key[:6] != "ipopt.": options_ipopt[f"ipopt.{key}"] = options_ipopt[key] del options_ipopt[key] opts = {**options_default, **options_common, **options_ipopt} else: raise RuntimeError("Available solvers are: 'ipopt'") solver = casadi.nlpsol("nlpsol", solver, nlp, opts) # Bounds and initial guess arg = { "lbx": self.V_bounds.min, "ubx": self.V_bounds.max, "lbg": all_g_bounds.min, "ubg": all_g_bounds.max, "x0": self.V_init.init, } # Solve the problem return solver.call(arg)
def __init__(self, x, f_exp, g_exp, p): nx = x.size(1) ng = g_exp.size(1) self.x_sol = np.zeros((nx, 1)) # create reference solver # x0 = np.zeros(nx) # lbx = [] # ubx = [] J = f_exp g = g_exp self.lbg = -1e12 * np.ones((ng, 1)) self.ubg = np.zeros((ng, 1)) travis_run = os.getenv('TRAVIS_RUN') if (travis_run != 'true'): print_level = 3 else: print_level = 0 prob = {'f': f_exp, 'x': x, 'g': g_exp, 'p': p} opts = { 'ipopt': { 'print_level': print_level, 'dual_inf_tol': 1e-10, 'constr_viol_tol': 1e-10, 'compl_inf_tol': 1e-10 } } self.ipopt_solver = ca.nlpsol('solver', 'ipopt', prob, opts)
def trim(s0, x: f16.State, p: f16.Parameters, phi_dot: float, theta_dot: float, psi_dot: float, gam: float): def constrain(x, s): u = f16.Control(thtl=s[0], elv_deg=s[1], ail_deg=s[2], rdr_deg=s[3]) alpha = s[4] beta = s[5] x.alpha = alpha x.beta = beta cos = ca.cos sin = ca.sin tan = ca.tan atan = ca.arctan sqrt = ca.sqrt VT = x.VT g = p.g G = psi_dot * VT / g a = 1 - G * tan(alpha) * sin(beta) b = sin(gam) / cos(beta) c = 1 + G**2 * cos(beta)**2 # coordinated turn constraint pg. 188 phi = atan(G * cos(beta) / cos(alpha) * ((a - b**2) + b * tan(alpha) * sqrt(c * (1 - b**2) + G**2 * sin(beta)**2)) / (a**2 - b**2 * (1 + c * tan(alpha)**2))) x.phi = phi # rate of climb constraint pg. 187 a = cos(alpha) * cos(beta) b = sin(phi) * sin(beta) + cos(phi) * sin(alpha) * cos(beta) theta = (a*b + sin(gam)*sqrt(a**2 - sin(gam)**2 + b**2)) \ / (a**2 - sin(gam)**2) x.theta = theta # kinematics pg. 20 x.P = phi_dot - sin(theta) * psi_dot x.Q = cos(phi) * phi_dot + sin(phi) * cos(theta) * psi_dot x.R = -sin(phi) * theta_dot + cos(phi) * cos(theta) * psi_dot # engine power constraint x.power = f16.tables['tgear'](u.thtl) return x, u s = ca.MX.sym('s', 6) x, u = constrain(x, s) f = f16.trim_cost(f16.dynamics(x, u, p)) nlp = {'x': s, 'f': f} S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': { 'print_level': 0, }}) r = S(x0=s0, lbg=0, ubg=0) s_opt = r['x'] x, u = constrain(x, s_opt) return x, u
def quadratic_optimizer_2(eco, payoff_matrix=None, prior_sol=None, current_layered_attack=None): Mx = eco.spectral tot_points = eco.layers res_conc = eco.parameters.efficiency * eco.water.res_counts * eco.parameters.layered_foraging[:, 0] beta = current_layered_attack[:, 1, 0] lam = ca.MX.sym('lam', 2) sigma = ca.MX.sym('sigma', Mx.x.shape[0]) sigma_p = ca.MX.sym('sigma_p', Mx.x.shape[0]) # sigma = eco.heat_kernels[0].T @ sigma_eff # sigma_p = eco.heat_kernels[0].T @ sigma_p_eff inte = np.ones(eco.layers).reshape(1, eco.layers) df1 = eco.parameters.clearance_rate[ 0] * res_conc - eco.parameters.clearance_rate[ 1] * sigma_p * beta - lam[0] * np.ones(tot_points) df2 = eco.parameters.clearance_rate[ 1] * eco.parameters.efficiency * sigma * beta - lam[1] * np.ones( tot_points) # g0 = ca.vertcat(cons_dyn, pred_dyn) g1 = inte @ Mx.M @ (df1 * sigma) + inte @ Mx.M @ (df2 * sigma_p) # g2 = inte @ Mx.M @ sigma_p - 1 g3 = inte @ Mx.M @ sigma - 1 g4 = ca.vertcat(-df1, -df2) g = ca.vertcat(g1, g2, g3, g4) # print(g0.size()) f = 0 sigmas = ca.vertcat(sigma, sigma_p) # sigma_bar x = ca.vertcat(*[sigmas, lam]) lbg = np.zeros(3 + 2 * tot_points) ubg = ca.vertcat(*[np.zeros(3), [ca.inf] * 2 * tot_points]) s_opts = { 'ipopt': { 'print_level': 1, 'linear_solver': 'ma57', 'hessian_approximation': 'limited-memory', 'acceptable_iter': 15 } } # , 'tol':10**-3, 'acceptable_tol': 10**(-2)}} prob = {'x': x, 'f': f, 'g': g} lbx = ca.vertcat(*[np.zeros(x.size()[0] - 2), -ca.inf, -ca.inf]) solver = ca.nlpsol('solver', 'ipopt', prob, s_opts) sol = solver(lbx=lbx, lbg=lbg, ubg=ubg, x0=prior_sol) x_out = np.array(sol['x']).flatten() return x_out
def solveTheProblem(self, gallerys): print('pose:', self.s_x_, self.s_y_, self.s_theta_, self.s_curvative_, self.g_x_, self.g_y_, self.g_theta_, self.g_curvative_) print_time = rospy.Time().now().to_sec() curve_num = 2 * len(self.gallerys_) X = ca.SX.sym('X', 2 * curve_num) #定义优化求解变量 E = ca.SX.sym('E', 4) #定义误差向量 init_X = ca.SX.sym('init_X', 4) #定义初始状态变量 init_X[0] = self.s_x_ init_X[1] = self.s_y_ init_X[2] = self.s_theta_ init_X[3] = self.s_curvative_ g = [] #声明约束方程组 for i in range(curve_num): init_X[0] += self.FC(X[2 * i], init_X[3], init_X[2], X[2 * i + 1]) init_X[1] += self.FS(X[2 * i], init_X[3], init_X[2], X[2 * i + 1]) init_X[2] += 0.5 * X[2 * i] * X[2 * i + 1] * X[ 2 * i + 1] + init_X[3] * X[2 * i + 1] init_X[3] += X[2 * i] * X[2 * i + 1] g.append(init_X[0]) #添加约束方程 g.append(init_X[1]) #添加约束方程 g.append(init_X[3]) #添加约束方程 E[0] = init_X[0] - self.g_x_ E[1] = init_X[1] - self.g_y_ E[2] = init_X[2] - self.g_theta_ E[3] = init_X[3] - self.g_curvative_ obj = ca.mtimes([E.T, self.Q_, E]) #定义目标函数 nlp_prob = {'f': obj, 'x': X, 'g': ca.vertcat(*g)} #加载目标函数、求解变量、约束方程组 #设置求解器 opts_setting = { 'ipopt.max_iter': 100, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.acceptable_tol': 1e-8, 'ipopt.acceptable_obj_change_tol': 1e-6 } solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting) lbx = [] ubx = [] lbg = [] ubg = [] for i in range(curve_num): lbx.append(-10), ubx.append(10) lbx.append(0), ubx.append(5) for i in range(len(gallerys)): lbg.append(gallerys[i].x_min_), ubg.append(gallerys[i].x_max_) lbg.append(gallerys[i].y_min_), ubg.append(gallerys[i].y_max_) lbg.append(-2), ubg.append(2) lbg.append(gallerys[-1].x_min_), ubg.append(gallerys[-1].x_max_) lbg.append(gallerys[-1].y_min_), ubg.append(gallerys[-1].y_max_) lbg.append(-2), ubg.append(2) res = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) #执行求解计算 print(res) x = [] for i in range(2 * curve_num): x.append(res['x'][i]) self.displayClothoid(x)
def test_directCollocationSimple(self): """Test direct collocation on a very simple model """ # Double integrator model x = ce.struct_symMX([ce.entry('q'), ce.entry('v')]) u = cs.MX.sym('u') ode = ce.struct_MX(x) ode['q'] = x['v'] ode['v'] = u quad = x['v']**2 NT = 2 # number of control intervals N = 3 # number of collocation points per interval ts = 1 # time step # DAE model dae = dae_model.SemiExplicitDae(x=x.cat, ode=ode.cat, u=u, quad=quad) # Create direct collocation scheme scheme = cl.CollocationScheme(dae=dae, t=np.arange(NT + 1) * ts, order=N) # Optimization variable w = scheme.combine(['x', 'K', 'Z', 'u']) # Objective f = scheme.q[:, -1] # Constraints g = ce.struct_MX([ ce.entry('eq', expr=scheme.eq), ce.entry('initial', expr=scheme.x[:, 0]), # q0 = 0, v0 = 0 ce.entry('final', expr=scheme.x[:, -1] - np.array([1, 0])) # qf = 1, vf = 0 ]) # Make NLP nlp = {'x': w, 'g': g, 'f': f} # Init NLP solver opts = {'ipopt.linear_solver': 'ma86'} solver = cs.nlpsol('solver', 'ipopt', nlp, opts) # Run NLP solver sol = solver(lbg=0, ubg=0) if np.isnan(float(sol['f'])): raise RuntimeError('Nlp Solver failed') sol_w = w(sol['x']) # Check agains the known solution nptest.assert_allclose(sol_w['u'], [[1, -1]]) nptest.assert_allclose(sol['f'], 2. / 3.)
def CorrThermoConsis(self, dE_start, fix_Ea=[], fix_BE=[], print_screen=False): if self._thermo_constraint_expression is None: self.build_thermo_constraint(thermoTem=298.15) Pnlp = self._Pnlp ini_p = np.hstack([dE_start]) dev = Pnlp - ini_p if py == 2: object_fxn = cas.mul(dev.T, dev) elif py == 3: object_fxn = cas.dot(dev, dev) nlp = dict(f=object_fxn, x=Pnlp, g=self._thermo_constraint_expression) nlpopts = dict() if py == 2: nlpopts['max_iter'] = 500 nlpopts['tol'] = 1e-8 nlpopts['expect_infeasible_problem'] = 'yes' nlpopts['hessian_approximation'] = 'exact' nlpopts['jac_d_constant'] = 'yes' elif py == 3: nlpopts['ipopt.max_iter'] = 500 nlpopts['ipopt.tol'] = 1e-8 nlpopts['ipopt.expect_infeasible_problem'] = 'yes' nlpopts['ipopt.hessian_approximation'] = 'exact' nlpopts['ipopt.jac_d_constant'] = 'yes' if py == 2: solver = cas.NlpSolver('solver', 'ipopt', nlp, nlpopts) elif py == 3: solver = cas.nlpsol('solver', 'ipopt', nlp, nlpopts) # Bounds and initial guess lbP = -np.inf * np.ones(self._Np) ubP = np.inf * np.ones(self._Np) for i in range(len(fix_Ea)): if check_index(fix_Ea[i], self.dEa_index): lbP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)] ubP[i] = dE_start[find_index(fix_Ea[i], self.dEa_index)] for i in range(len(fix_BE)): if check_index(fix_BE[i], self.dBE_index): lbP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)] ubP[i + self._NEa] = dE_start[find_index(fix_BE[i], self.dBE_index) + len(self.dEa_index)] lbG = 0 * np.ones(2 * self.nrxn) ubG = np.inf * np.ones(2 * self.nrxn) if not print_screen: old_stdout = sys.stdout sys.stdout = tempfile.TemporaryFile() solution = solver(x0=ini_p, lbg=lbG, ubg=ubG, lbx=lbP, ubx=ubP) dE_corr = solution['x'].full().T[0].tolist() if not print_screen: sys.stdout = old_stdout return(dE_corr)
def _setup_nlpsolver(self): __dirname__ = os.path.dirname(os.path.abspath(__file__)) path_to_nlp_object = os.path.join(__dirname__, self._PATH_TO_NLP_OBJECT, \ self._NLP_OBJECT_FILENAME) self._nlpsolver = ca.nlpsol(self._solver_name, "ipopt", path_to_nlp_object, self._nlpsolver_options)
def _export_nlp_to_c_code(self): __dirname__ = os.path.dirname(os.path.abspath(__file__)) nlpsolver = ca.nlpsol("nlpsol", "ipopt", self.nlp) nlpsolver.generate_dependencies(self._NLP_SOURCE_FILENAME) os.rename(self._NLP_SOURCE_FILENAME, os.path.join(__dirname__, self._PATH_TO_NLP_SOURCE, self._NLP_SOURCE_FILENAME))
def solve(self): self.__dispatch_bounds() solver = nlpsol("nlpsol", "ipopt", self.ipopt_nlp, self.opts) # Solve the problem self.out = {"sol": solver.call(self.ipopt_limits)} self.out["sol"]["time_tot"] = solver.stats()["t_wall_total"] return self.out
def export_casadi_problems(self, destination, father, problem): filenames = Export.export_casadi_problems(self, destination, father, problem) obj = father.problem_description['obj'] con = father.problem_description['con'] var = father.problem_description['var'] par = father.problem_description['par'] opt = father.problem_description['opt'] nlp = {'x': var, 'p': par, 'f': obj, 'g': con} options = {} for key, value in opt['solver_options']['ipopt'].items(): options[key] = value options.update({'expand': True}) solver = nlpsol('solver', 'ipopt', nlp, options) solver.generate_dependencies('nlp.c') cwd = os.getcwd() shutil.move(cwd+'/nlp.c', destination+'src/nlp.c') filenames.append('nlp.c') return filenames
def _initialize_solver(self, **kwargs): nlpsol_args = {"expand", "iteration_callback", "iteration_callback_step", "iteration_callback_ignore_errors", "ignore_check_vec", "warn_initial_bounds", "eval_errors_fatal", "print_time", "verbose_init"} # Initialize NLP object opts = { 'ipopt.max_iter' : 10000, # 'linear_solver' : 'ma27' } if kwargs is not None: for key, val in kwargs.items(): if key in nlpsol_args: opts.update({key: val }) else: opts.update({'ipopt.' + key: val }) self._solver_opts = opts constraints = cs.vertcat(*self._constraints_sx) self._solver = cs.nlpsol( "solver", "ipopt", {'x': self.var.vars_sx, 'p': self.pvar.vars_sx, 'f': self.objective_sx, 'g': constraints}, self._solver_opts) self.col_vars['lbg'] = np.concatenate(self._constraints_lb) self.col_vars['ubg'] = np.concatenate(self._constraints_ub)
def __init__(self,objective,*args): """ optisolve(objective) optisolve(objective,constraints) """ if len(args)>=1: constraints = args[0] else: constraints = [] options = dict() if len(args)>=2: options = args[1] if not isinstance(constraints,list): raise Exception("Constraints must be given as list: [x>=0,y<=0]") [ gl_pure, gl_equality] = sort_constraints( constraints ) symbols = OptimizationObject.get_primitives([objective]+gl_pure) # helper functions for 'x' X = C.veccat(*symbols["x"]) helper = C.Function('helper',[X],symbols["x"]) helper_inv = C.Function('helper_inv',symbols["x"],[X]) # helper functions for 'p' if applicable if 'p' in symbols: P = C.veccat(*symbols["p"]) self.Phelper_inv = C.Function('Phelper_inv',symbols["p"],[P]) else: P = C.MX.sym('p',0,1) if len(gl_pure)>0: g_helpers = []; for p in gl_pure: g_helpers.append(C.MX.sym('g',p.sparsity())) G_helpers = C.veccat(*g_helpers) self.Ghelper = C.Function('Ghelper',[G_helpers],g_helpers) self.Ghelper_inv = C.Function('Ghelper_inv',g_helpers,[G_helpers]) codegen = False; if 'codegen' in options: codegen = options["codegen"] del options["codegen"] opt = {} if codegen: options["jit"] = True opt["jit"] = True gl_pure_v = C.MX() if len(gl_pure)>0: gl_pure_v = C.veccat(*gl_pure) if objective.is_vector() and objective.numel()>1: F = C.vec(objective) objective = 0.5*C.dot(F,F) FF = C.Function('nlp',[X,P], [F]) JF = FF.jacobian() J_out = JF.call([X,P]) J = J_out[0].T; H = C.mtimes(J,J.T) sigma = C.MX.sym('sigma') Hf = C.Function('H',dict(x=X,p=P,lam_f=sigma,hess_gamma_x_x=sigma*C.triu(H)),['x', 'p', 'lam_f', 'lam_g'],['hess_gamma_x_x'],opt) if "expand" in options and options["expand"]: Hf = Hf.expand() options["hess_lag"] = Hf nlp = {"x":X,"p":P,"f":objective,"g":gl_pure_v} self.solver = C.nlpsol('solver','ipopt', nlp, options) # Save to class properties self.symbols = symbols self.helper = helper self.helper_inv = helper_inv self.gl_equality = gl_equality self.resolve()
ubg.append([0, 0]) # Concatenate vectors w = ca.vertcat(*w) g = ca.vertcat(*g) x_plot = ca.horzcat(*x_plot) u_plot = ca.horzcat(*u_plot) w0 = np.concatenate(w0) lbw = np.concatenate(lbw) ubw = np.concatenate(ubw) lbg = np.concatenate(lbg) ubg = np.concatenate(ubg) # Create an NLP solver prob = {'f': J, 'x': w, 'g': g} solver = ca.nlpsol('solver', 'ipopt', prob); # Function to get x and u trajectories from w trajectories = ca.Function('trajectories', [w], [x_plot, u_plot], ['w'], ['x', 'u']) # Solve the NLP sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg) x_opt, u_opt = trajectories(sol['x']) x_opt = x_opt.full() # to numpy array u_opt = u_opt.full() # to numpy array # Plot the result tgrid = np.linspace(0, T, N+1) plt.figure(1) plt.clf() plt.plot(tgrid, x_opt[0], '--')