def __init__(self,name,syms,expression): # info self.name = name self.sizes = (syms[0].numel(),expression.numel(),sum([s.numel() for s in syms[1:]])) # use a single parameter for the expression (because julia sucks sometimes) self.param_sym = oc.MX.sym("P",self.sizes[2]) self.param_names = [s.name() for s in syms[1:]] expression_ = cs.substitute(expression,cs.vcat(syms[1:]),self.param_sym) # expression evaluation self.eval = cs.Function('eval',[syms[0],self.param_sym],[expression_]).expand() # collect the sx version of the symbols sx_syms = self.eval.sx_in() self.main_sym = sx_syms[0] # expression jacobian jacobian = cs.jacobian(expression_,syms[0]) self.eval_jac = cs.Function('eval_jac',[syms[0],self.param_sym],[jacobian]).expand() # hessian of each of the elements of the expression split_eval = [cs.Function('eval',[syms[0],self.param_sym],[expression_[i]]).expand() for i in range(expression_.shape[0]) ] hessian = [cs.hessian(split_eval[i](*sx_syms),sx_syms[0])[0] for i in range(expression_.shape[0])] self.eval_hes = [cs.Function('eval_hes'+str(i),sx_syms,[hessian[i]]).expand() for i in range(expression.shape[0])] # location of the compiled library self.lib_path = None
def __construct_sensitivities(self): """ Construct NLP sensitivities """ # convenience w = self.__w p = self.__p # cost function self.__f_fun = ca.Function('f_fun', [w, p], [self.__f]) self.__jacf_fun = ca.Function('jacf_fun', [w, p], [ca.jacobian(self.__f, self.__w)]) # constraints self.__g_fun = ca.Function('g_fun', [w, p], [self.__g]) self.__jacg_fun = ca.Function('jacg_fun', [w, p], [ca.jacobian(self.__g, self.__w)]) self.__gzeros = np.zeros((self.__g.shape[0], 1)) # exact hessian lam_g = ca.MX.sym('lam_g', self.__g.shape) lag = self.__f + ct.mtimes(lam_g.T, self.__g) self.__jlag_fun = ca.Function('jLag', [w, p, lam_g], [ca.jacobian(lag, w)]) if self.__options['hessian_approximation'] == 'exact': self.__H_fun = ca.Function('H_fun', [w, p, lam_g], [ca.hessian(lag, w)[0]]) else: self.__H_fun = self.__options['hessian_approximation']
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 __init__(self, ocpVars, lbw, ubw, ocpParams, ocpCosts, ocpConstr, NX, NU, M, w0, Q, R, solvOpts={}): self.var = ocpVars self.lbw = lbw self.ubw = ubw self.params = ocpParams self.costs = sum(ocpCosts) self.g = ca.vertcat(*[c[0] for c in ocpConstr]) self.lbg = ca.vertcat(*[c[1] for c in ocpConstr]) self.ubg = ca.vertcat(*[c[2] for c in ocpConstr]) self.w0 = w0 self.lagrMulConstr = np.zeros((self.g.numel(),)) self.lagrMulOptVars = np.zeros((self.var.cat.numel(),)) self.NX = NX self.NU = NU self.M = M # symbolic variables w = ca.vertcat(self.var['x'][:], self.var['u'] [:]) # primal decision variables # constraint lagrange multipliers (lambda + mu, i.e. dual decision variables) lagrMult = ca.SX.sym('lagrMult', self.g.size()) wGuess = ca.SX.sym('wGuess', w.shape) # guess / linearization point # single weight matrix for all decision variables W = np.diag(np.concatenate([np.tile(Q, M+1), np.tile(R, M)])) L = self.costs + lagrMult.T @ self.g B = ca.Function('B', [w], [ca.hessian(L,w)[0]])(wGuess) wErr = w - wGuess # reference as QP parameters wRef = ca.vertcat(self.params['x_ref'][:], self.params['u_ref'][:]) #B = W # gauss-newton hessian approximation J = W @ (wGuess - wRef) fqp = 1./2. * wErr.T @ B @ wErr + J.T @ wErr gqp = linearize(self.g, w, wGuess) pqp = ca.vertcat(self.params.cat, wGuess, lagrMult) # assemble QP if not solvOpts: solvOpts = {'jit': False, 'print_time': 0, 'printLevel': 'low', 'sparse': True, 'enableEqualities': True} #solvOpts = {'jit' : True, 'print_time' : 0, 'printLevel' : 'high', 'sparse' : True} qp = {'x': self.var, 'f': fqp, 'g': gqp, 'p': pqp} self.solver = ca.qpsol('S', 'qpoases', qp, solvOpts) self.solverResults = [] self.durations = []
def calculate_optimal_control(self): dd_h_dudu, d_h_du = hessian(self.problem.H, self.model.u) if is_equal(dd_h_dudu, DM.zeros(self.model.n_u, self.model.n_u)): # TODO: Implement the case where the controls are linear on the Hamiltonian ("Bang-Bang" control) raise Exception( 'The Hamiltonian "H" is not strictly convex with respect to the control "u". ' + 'The obtained hessian d^2 H/du^2 = 0') # if not ddH_dudu.is_constant(): # raise NotImplementedError('The Hessian of the Hamiltonian with respect to "u" is not constant, # this case has not been implemented') u_opt = -mtimes(inv(dd_h_dudu), substitute(d_h_du, self.model.u, 0)) for i in range(self.model.n_u): if not self.problem.u_min[i] == -inf: u_opt[i] = fmax(u_opt[i], self.problem.u_min[i]) if not self.problem.u_max[i] == inf: u_opt[i] = fmin(u_opt[i], self.problem.u_max[i]) return u_opt
def nllh_casf(self, grad=True, hess=False): """ Creates a casadi function computing the negative log likelihood (without const terms) of the data dependent on hyperparameters v. :param grad: Whether the function should compute the gradient, too :param hess: Whether the function should compute the hessian, too :return: A casadi function taking a value of v as input and returning the neg log likelihood, gradient, and hessian is desired """ vshape = np.atleast_2d(self.v).shape v = cas.MX.sym("v", vshape[0], vshape[1]) phi_x = self.phi_cas(self.x, v) sinv = self.sinv0 + self.beta * cas.mtimes(phi_x.T, phi_x) mean = cas.solve( sinv, cas.mtimes(self.sinv0, self.mean0) + self.beta * cas.mtimes(phi_x.T, self.y)) y_pred = cas.mtimes(mean.T, phi_x.T).T sigma = 1 / self.beta + cas.sum2(phi_x * cas.solve(sinv, phi_x.T).T) y_true = self.y y_diff = y_true - y_pred llht = cas.sum2(y_diff * y_diff) / sigma llh = cas.sum1(llht) if hess: H, llh_grad = cas.hessian(llh, v) elif grad: llh_grad = cas.gradient(llh, v) res = [llh] if grad: res += [llh_grad] if hess: res += [H] f = cas.Function("f_mu", [v], res) return f
def __init__(self,name,syms,expression,type='auto'): # info self.name = name self.sizes = (syms[0].numel(),expression.numel(),sum([s.numel() for s in syms[1:]])) self.main_sym = syms[0] # use a single parameter for the expression (because julia sucks sometimes) self.param_sym = oc.MX.sym("P",self.sizes[2]) self.param_names = [s.name() for s in syms[1:]] expression_ = cs.substitute(expression,cs.vcat(syms[1:]),self.param_sym) # expression evaluation self.eval = cs.Function('eval',[self.main_sym,self.param_sym],[expression_]) # expression jacobian jacobian = cs.jacobian(expression_,self.main_sym) self.jcb_nnz = jacobian.nnz() self.jcb_sparsity = jacobian.sparsity().get_triplet() self.eval_jac = cs.Function('eval_jac',[self.main_sym,self.param_sym],[jacobian]) # hessian of each of the elements of the expression hessian = [cs.hessian(expression_[i],self.main_sym)[0] for i in range(expression_.shape[0])] self.hes_nnz = [hessian[i].nnz() for i in range(expression_.shape[0])] self.hes_sparsity = [hessian[i].sparsity().get_triplet() for i in range(expression_.shape[0])] self.eval_hes = [cs.Function('eval_hes'+str(i),[self.main_sym,self.param_sym],[hessian[i]]) for i in range(expression.shape[0])] # check type self.type = 'Linear' for n in self.hes_nnz: if n > 0: self.type = 'Quadratic' break if self.type == 'Quadratic' and self.type not in ['auto','Auto','Quadratic','quadratic']: raise NameError('LinQuadForJulia: the function is declared '+type+' but it results Quadratic.') if self.type == 'Linear' and self.type not in ['auto','Auto','Linear','linear']: raise NameError('LinQuadForJulia: the function is declared '+type+' but it results Linear.')
# 适用于矩阵或稀疏模式 # y = ca.SX.sym('y',10,1) # print(y.shape) # .size1() => 行数 # .size2() => 列数 # .numel() => 元素个数 == .size1() * .size2() # .sparisity() => 稀疏模式 ''' Part 7: 微分计算 ''' # 微分计算可以从前向和反向分别计算,分别对应得到jacobian vector和jacobian-transposed vector #1. jacobian A = ca.SX.sym('A', 3, 2) x = ca.SX.sym('x', 2) print('A:', A, ' x: ', x, 'Ax: ', ca.mtimes(A, x)) print('J:', ca.jacobian(ca.mtimes(A, x), x)) print(ca.dot(A, A)) print(ca.gradient(ca.dot(A, A), A)) [H, g] = ca.hessian(ca.dot(x, x), x) #hessian()会同时返回梯度和hessian矩阵 print('H:', H) print('g:', g) v = ca.SX.sym('v', 2) f = ca.mtimes(A, x) print(ca.jtimes(f, x, v)) # jtimes = jacobian function * vector
def generate_c_code_external_cost(model, is_terminal): casadi_version = CasadiMeta.version() casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") if casadi_version not in (ALLOWED_CASADI_VERSIONS): casadi_version_warning(casadi_version) x = model.x p = model.p if isinstance(x, MX): symbol = MX.sym else: symbol = SX.sym if is_terminal: suffix_name = "_cost_ext_cost_e_fun" suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_e_fun_jac" u = symbol("u", 0, 0) ext_cost = model.cost_expr_ext_cost_e else: suffix_name = "_cost_ext_cost_fun" suffix_name_hess = "_cost_ext_cost_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_fun_jac" u = model.u ext_cost = model.cost_expr_ext_cost # set up functions to be exported fun_name = model.name + suffix_name fun_name_hess = model.name + suffix_name_hess fun_name_jac = model.name + suffix_name_jac # generate expression for full gradient and Hessian full_hess, grad = hessian(ext_cost, vertcat(u, x)) ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) ext_cost_fun_jac_hess = Function( fun_name_hess, [x, u, p], [ext_cost, grad, full_hess] ) ext_cost_fun_jac = Function( fun_name_jac, [x, u, p], [ext_cost, grad] ) # generate C code if not os.path.exists("c_generated_code"): os.mkdir("c_generated_code") os.chdir("c_generated_code") gen_dir = model.name + '_cost' if not os.path.exists(gen_dir): os.mkdir(gen_dir) gen_dir_location = "./" + gen_dir os.chdir(gen_dir_location) ext_cost_fun.generate(fun_name, casadi_opts) ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) os.chdir("../..") return
def __init__(self, x, u, lc_, lcN_, g_, gN_, fc_, T, M, N, tau, print_level=1, pt=True): """ Define an optimal control formulation Parameters ---------- x : CasADi MX CasADi symbolic variables representing the states u : CasADi MX CasADi symbolic variables representing the inputs lc_ : CasADi expression lc: R^{nx} x R^{nu} -> R (continuous-time Lagrange term) lcN_ : CasADi expression lcN: R^{nx} -> R (Mayer term) g_ : CasADi expression g: R^{n_x} x R^{n_u} -> R^{ng} (constraint function) gN_ : CasADi expression gN: R^{nx} -> R^{ngN} (constraint function at t=T) fc_ : CasADi expression fc: R^{nx} x R^{nu} -> R^{nx} (continuous time dynamics) T : float prediction horizon tau : float tightening factor print_level : int print level pt : bool partial tightening vs standard SQP (default = True) """ self.pt = pt NX = x.shape[0] NU = u.shape[0] if not isinstance(g_, list): NG = g_.shape[0] else: NG = 0 if not isinstance(gN_, list): NGN = gN_.shape[0] else: NGN = 0 dims = OcpDims(NX, NU, NG, NGN, N, M) self.dims = dims self.tau = tau self.print_level = print_level # define CasADi functions lc = ca.Function('lc', [x, u], [lc_]) lcN = ca.Function('lcN', [x], [lcN_]) g = ca.Function('g', [x, u], [g_]) gN = ca.Function('gN', [x], [gN_]) fc = ca.Function('fc', [x, u], [fc_]) self.lc = lc self.lcN = lcN self.g = g self.gN = gN self.fc = fc self.dims = dims NX = dims.nx NU = dims.nu NG = dims.ng NGN = dims.ngN N = dims.N M = dims.M Td = T / N # create integrator integrator = Integrator(x, u, fc_, Td) self.integrator = integrator # build OCP w = [] w0 = [] lbw = [] ubw = [] c = [] lbc = [] ubc = [] Xk = ca.MX.sym('X0', NX, 1) w += [Xk] c += [Xk] lbc += [np.zeros((NX, 1))] ubc += [np.zeros((NX, 1))] lbw += [-np.inf * np.ones((NX, 1))] ubw += [+np.inf * np.ones((NX, 1))] w0 += [np.zeros((NX, 1))] f = 0 # formulate the NLP for k in range(M): # new NLP variable for the control Uk = ca.MX.sym('U_' + str(k), NU, 1) # update variable list w += [Uk] lbw += [-np.inf * np.ones((NU, 1))] ubw += [np.inf * np.ones((NU, 1))] w0 += [np.zeros((NU, 1))] # add cost contribution f = f + Td * lc(Xk, Uk) # add constraints c += [g(Xk, Uk)] lbc += [-np.inf * np.ones((NG, 1))] ubc += [np.zeros((NG, 1))] # integrate till the end of the interval Xk_end = integrator.xplus(Xk, Uk) # new NLP variable for state at end of interval Xk = ca.MX.sym('X_' + str(k + 1), NX, 1) w += [Xk] lbw += [-np.inf * np.ones((NX, 1))] ubw += [np.inf * np.ones((NX, 1))] w0 += [np.zeros((NX, 1))] # add equality constraint c += [Xk_end - Xk] lbc += [np.zeros((NX, 1))] ubc += [np.zeros((NX, 1))] for k in range(M, N): # new NLP variable for the control Uk = ca.MX.sym('U_' + str(k), NU, 1) # compute barrier term barr_term = 0 for i in range(NG): barr_term = barr_term - tau * np.log(-g(Xk, Uk)[i]) f = f + Td * lc(Xk, Uk) + barr_term w += [Uk] lbw += [-np.inf * np.ones((NU, 1))] ubw += [np.inf * np.ones((NU, 1))] w0 += [np.zeros((NU, 1))] # integrate till the end of the interval Xk_end = integrator.xplus(Xk, Uk) # new NLP variable for state at end of interval Xk = ca.MX.sym('X_' + str(k + 1), NX, 1) w += [Xk] lbw += [-np.inf * np.ones((NX, 1))] ubw += [np.inf * np.ones((NX, 1))] w0 += [np.zeros((NX, 1))] # add equality constraint c += [Xk_end - Xk] lbc += [np.zeros((NX, 1))] ubc += [np.zeros((NX, 1))] if M <= N: # compute barrier term barr_term = 0 for i in range(NGN): barr_term = barr_term + -tau * np.log(-gN(Xk)[i]) f = f + lcN(Xk) + barr_term else: f = f + lcN(Xk) # add constraints c += [gN(Xk)] lbc += [np.zeros((NGN, 1))] ubc += [np.zeros((NGN, 1))] c = ca.vertcat(*c) w = ca.vertcat(*w) # convert lists to numpy arrays lbw_a = np.vstack(lbw) self._lbw = np.vstack(lbw) self._ubw = np.vstack(ubw) self._lbc = np.vstack(lbc) self._ubc = np.vstack(ubc) self._w0 = np.vstack(w0) # create an NLP solver prob = {'f': f, 'x': w, 'g': c} # opts = {'ipopt': {'print_level': 2}} opts = {} self.nlp_solver = ca.nlpsol('solver', 'ipopt', prob, opts) #---------------------------------------------------------------------- # partially tightened RTI #---------------------------------------------------------------------- # define CasADi functions for linearization # dynamics jac_x_f = ca.Function('jac_x_f', [integrator.x, integrator.u], \ [ca.jacobian(integrator.xplus_expr, integrator.x)]) self.jac_x_f = jac_x_f jac_u_f = ca.Function('jac_u_f', [integrator.x, integrator.u], \ [ca.jacobian(integrator.xplus_expr, integrator.u)]) self.jac_u_f = jac_u_f # cost jac_x_l = ca.Function('jac_x_l', [x, u], \ [ca.jacobian(Td*lc_, x)]) self.jac_x_l = jac_x_l jac_u_l = ca.Function('jac_u_l', [x, u], \ [ca.jacobian(Td*lc_, u)]) self.jac_u_l = jac_u_l jac_xx_l = ca.Function('jac_xx_l', [x, u], \ [ca.hessian(Td*lc_, x)[0]]) self.jac_xx_l = jac_xx_l jac_uu_l = ca.Function('jac_uu_l', [x, u], \ [ca.hessian(Td*lc_, u)[0]]) self.jac_uu_l = jac_uu_l jac_xu_l = ca.Function('jac_xu_l', [x, u], \ [ca.jacobian(ca.jacobian(Td*lc_, x), u)]) self.jac_xu_l = jac_xu_l jac_xx_lN = ca.Function('jac_xx_lN', [x], \ [ca.hessian(lcN_, x)[0]]) self.jac_xx_lN = jac_xx_lN jac_x_lN = ca.Function('jac_x_lN', [x], \ [ca.jacobian(lcN_, x)]) self.jac_x_lN = jac_x_lN # constraints jac_x_g = ca.Function('jac_x_g', [x, u], \ [ca.jacobian(g_, x)]) self.jac_x_g = jac_x_g jac_u_g = ca.Function('jac_u_g', [x, u], \ [ca.jacobian(g_, u)]) self.jac_u_g = jac_u_g jac_x_gN = ca.Function('jac_x_gN', [x], \ [ca.jacobian(gN_, x)]) self.jac_x_gN = jac_x_gN # these are the primal-dual iterates of the partially tightened RTI self.x = [] self.u = [] self.lam = [] self.t = [] self.nu = [] t_init = 1.00 nu_init = 1.00 if self.pt: for i in range(M): self.x.append(np.zeros((NX, 1))) self.u.append(np.zeros((NU, 1))) self.lam.append(np.zeros((NX, 1))) self.t.append(np.zeros((NG, 1))) self.nu.append(np.zeros((NG, 1))) for i in range(M, N): self.x.append(np.zeros((NX, 1))) self.u.append(np.zeros((NU, 1))) self.lam.append(np.zeros((NX, 1))) self.t.append(t_init * np.ones((NG, 1))) self.nu.append(nu_init * np.ones((NG, 1))) self.x.append(np.zeros((NX, 1))) self.lam.append(np.zeros((NX, 1))) self.t.append(t_init * np.ones((NGN, 1))) self.nu.append(nu_init * np.ones((NGN, 1))) else: for i in range(N): self.x.append(np.zeros((NX, 1))) self.u.append(np.zeros((NU, 1))) self.lam.append(np.zeros((NX, 1))) self.t.append(np.zeros((NG, 1))) self.nu.append(np.zeros((NG, 1))) self.x.append(np.zeros((NX, 1))) self.lam.append(np.zeros((NX, 1))) self.t.append(np.zeros((NGN, 1))) self.nu.append(np.zeros((NGN, 1))) # these are the variables associated with the linearized problem # - matrices self.A = [] self.B = [] self.C = [] self.D = [] self.Hxx = [] self.Huu = [] self.Hxu = [] self.Hxx_t = [] self.Huu_t = [] self.Hxu_t = [] for i in range(N): self.A.append(np.zeros((NX, NX))) self.B.append(np.zeros((NX, NU))) self.C.append(np.zeros((NG, NX))) self.D.append(np.zeros((NG, NU))) self.Hxx.append(np.zeros((NX, NX))) self.Huu.append(np.zeros((NU, NU))) self.Hxu.append(np.zeros((NU, NX))) self.Hxx_t.append(np.zeros((NX, NX))) self.Huu_t.append(np.zeros((NU, NU))) self.Hxu_t.append(np.zeros((NU, NX))) self.C.append(np.zeros((NGN, NX))) self.D.append(np.zeros((NGN, NU))) self.Hxx.append(np.zeros((NX, NX))) self.Hxx_t.append(np.zeros((NX, NX))) # - vectors (residuals) self.r_lam = [] self.r_x = [] self.r_x_t = [] self.r_u = [] self.r_u_t = [] self.r_nu = [] self.e = [] for i in range(N): self.r_lam.append(np.zeros((NX, 1))) self.r_x.append(np.zeros((NX, 1))) self.r_x_t.append(np.zeros((NX, 1))) self.r_u.append(np.zeros((NU, 1))) self.r_u_t.append(np.zeros((NU, 1))) self.r_nu.append(np.zeros((NG, 1))) self.e.append(np.zeros((NG, 1))) self.r_lam.append(np.zeros((NX, 1))) self.r_x.append(np.zeros((NX, 1))) self.r_x_t.append(np.zeros((NX, 1))) self.r_u.append(np.zeros((NU, 1))) self.r_nu.append(np.zeros((NG, 1))) self.e.append(np.zeros((NG, 1))) # - vectors (QP residuals) self.r_lam_qp = [] self.r_x_qp = [] self.r_u_qp = [] self.r_nu_qp = [] self.e_qp = [] for i in range(N): self.r_lam_qp.append(np.zeros((NX, 1))) self.r_x_qp.append(np.zeros((NX, 1))) self.r_u_qp.append(np.zeros((NU, 1))) self.r_nu_qp.append(np.zeros((NG, 1))) self.e_qp.append(np.zeros((NG, 1))) self.r_lam_qp.append(np.zeros((NX, 1))) self.r_x_qp.append(np.zeros((NX, 1))) self.r_u_qp.append(np.zeros((NU, 1))) self.r_nu_qp.append(np.zeros((NG, 1))) self.e_qp.append(np.zeros((NG, 1))) # these are the variables associated with the Riccati recursion self.P = [] self.p = [] self.r_u_t_back = [] self.r_lam_back = [] for i in range(N + 1): self.p.append(np.zeros((NX, 1))) self.P.append(np.zeros((NX, NX))) self.du = [] self.dx = [] self.dlam = [] self.dnu = [] self.dt = [] for i in range(N): self.du.append(np.zeros((NU, 1))) self.dx.append(np.zeros((NX, 1))) self.dt.append(np.zeros((NG, 1))) self.dlam.append(np.zeros((NX, 1))) self.dnu.append(np.zeros((NG, 1))) self.dlam.append(np.zeros((NX, 1))) self.dx.append(np.zeros((NX, 1))) self.dt.append(np.zeros((NGN, 1))) self.dnu.append(np.zeros((NGN, 1))) self.x0 = np.zeros((NX, 1)) if M > 0: # construct reduced QP dW = [] dLam = [] dX = [] dU = [] dNu = [] dT = [] dW0 = [] p = [] Lam_lin = [] X_lin = [] U_lin = [] Nu_lin = [] T_lin = [] # first parameter is initial state X_par = ca.MX.sym('X_par', NX, 1) # define variables for i in range(M): dLamk = ca.MX.sym('dLam_' + str(i), NX, 1) dLam += [dLamk] dW += [dLamk] Lamk_lin = ca.MX.sym('Lam_' + str(i) + '_lin', NX, 1) Lam_lin += [Lamk_lin] dW0 += [np.zeros((NX, 1))] dXk = ca.MX.sym('dX_' + str(i), NX, 1) dX += [dXk] dW += [dXk] Xk_lin = ca.MX.sym('X_' + str(i) + '_lin', NX, 1) X_lin += [Xk_lin] dW0 += [np.zeros((NX, 1))] dUk = ca.MX.sym('dU_' + str(i), NU, 1) dU += [dUk] dW += [dUk] Uk_lin = ca.MX.sym('U_' + str(i) + '_lin', NU, 1) U_lin += [Uk_lin] dW0 += [np.zeros((NU, 1))] dNuk = ca.MX.sym('dNu_' + str(i), NG, 1) dNu += [dNuk] dW += [dNuk] Nuk_lin = ca.MX.sym('dNu_' + str(i) + '_lin', NG, 1) Nu_lin += [Nuk_lin] dW0 += [np.ones((NG, 1))] dTk = ca.MX.sym('dT_' + str(i), NG, 1) dT += [dTk] dW += [dTk] Tk_lin = ca.MX.sym('T_' + str(i) + '_lin', NG, 1) T_lin += [Tk_lin] dW0 += [np.ones((NG, 1))] i = M dLamk = ca.MX.sym('dLam_' + str(i), NX, 1) dLam += [dLamk] dW += [dLamk] Lamk_lin = ca.MX.sym('Lam_' + str(i) + '_lin', NX, 1) Lam_lin += [Lamk_lin] dW0 += [np.zeros((NX, 1))] dXk = ca.MX.sym('dX_' + str(i), NX, 1) dX += [dXk] dW += [dXk] Xk_lin = ca.MX.sym('X_' + str(i) + '_lin', NX, 1) X_lin += [Xk_lin] dW0 += [np.zeros((NX, 1))] if NGN > 0: dNuk = ca.MX.sym('dNu_' + str(i), NGN, 1) dNu += [dNuk] dW += [dNuk] Nuk_lin = ca.MX.sym('dNu_' + str(i) + '_lin', NGN, 1) Nu_lin += [Nuk_lin] dW0 += [np.ones((NG, 1))] dTk = ca.MX.sym('dT_' + str(i), NGN, 1) dT += [dTk] dW += [dTk] Tk_lin = ca.MX.sym('T_' + str(i) + '_lin', NGN, 1) T_lin += [Tk_lin] dW0 += [np.ones((NG, 1))] # form parameter vector p += [X_par] p += Lam_lin[:] p += X_lin[:] p += U_lin[:] p += Nu_lin[:] p += T_lin[:] # last parameters correspond to p_M and P_M P_M_flat = ca.MX.sym('P', NX * NX, 1) p += [P_M_flat] p_M = ca.MX.sym('p_M', NX, 1) p += [p_M] # empty QP c_qp = [] lbc_qp = [] ubc_qp = [] f_qp = 0 # formulate the reduced QP k = 0 dXk = dX[k] r_lam_0 = -X_lin[0] + X_par c_qp += [-dXk + r_lam_0] lbc_qp += [np.zeros((NX, 1))] ubc_qp += [np.zeros((NX, 1))] dUk = dU[k] dXk = dX[k] Lamk_lin = Lam_lin[k] Lamk_lin_next = Lam_lin[k + 1] Xk_lin = X_lin[k] Uk_lin = U_lin[k] Nuk_lin = Nu_lin[k] Tk_lin = T_lin[k] # compute residuals nabla_x_l = ca.transpose(jac_x_l(Xk_lin, Uk_lin)) nabla_u_l = ca.transpose(jac_u_l(Xk_lin, Uk_lin)) nabla_x_f = ca.transpose(jac_x_f(Xk_lin, Uk_lin)) nabla_u_f = ca.transpose(jac_u_f(Xk_lin, Uk_lin)) nabla_x_g = ca.transpose(jac_x_g(Xk_lin, Uk_lin)) nabla_u_g = ca.transpose(jac_u_g(Xk_lin, Uk_lin)) r_x_k = nabla_x_l + ca.mtimes(nabla_x_f, Lamk_lin_next) + \ - Lamk_lin + ca.mtimes(nabla_x_g, Nuk_lin) r_u_k = nabla_u_l + ca.mtimes(nabla_u_f, Lamk_lin_next) + \ ca.mtimes(nabla_u_g, Nuk_lin) r_nu_k = g(Xk_lin, Uk_lin) + Tk_lin # compute Hessian approximation Huu = jac_uu_l(Xk_lin, Uk_lin) Hxx = jac_xx_l(Xk_lin, Uk_lin) Hxu = jac_xu_l(Xk_lin, Uk_lin) Hk = ca.vertcat(ca.horzcat(Hxx, Hxu, r_x_k), \ ca.horzcat(Hxu.T, Huu, r_u_k), \ ca.horzcat(ca.transpose(r_x_k), ca.transpose(r_u_k), 1)) # add cost contribution f_qp = f_qp + ca.mtimes(ca.vertcat(dXk, dUk, 1).T, ca.mtimes(Hk, \ ca.vertcat(ca.vertcat(dXk, dUk, 1)))) # add inequality constraints c_qp += [ca.mtimes(ca.transpose(nabla_x_g), dXk) \ + ca.mtimes(ca.transpose(nabla_u_g), dUk) + r_nu_k] lbc_qp += [-np.inf * np.ones((NG, 1))] ubc_qp += [np.zeros((NG, 1))] for k in range(1, M): dUk = dU[k] dUk_prev = dU[k - 1] dXk = dX[k] dXk_prev = dX[k - 1] Lamk_lin = Lam_lin[k] Lamk_lin_next = Lam_lin[k + 1] Xk_lin = X_lin[k] Xk_lin_prev = X_lin[k - 1] Uk_lin = U_lin[k] Uk_lin_prev = U_lin[k - 1] Nuk_lin = Nu_lin[k] Tk_lin = T_lin[k] # compute residuals nabla_x_l = ca.transpose(jac_x_l(Xk_lin, Uk_lin)) nabla_u_l = ca.transpose(jac_u_l(Xk_lin, Uk_lin)) nabla_x_f = ca.transpose(jac_x_f(Xk_lin, Uk_lin)) nabla_u_f = ca.transpose(jac_u_f(Xk_lin, Uk_lin)) nabla_x_g = ca.transpose(jac_x_g(Xk_lin, Uk_lin)) nabla_u_g = ca.transpose(jac_u_g(Xk_lin, Uk_lin)) nabla_x_f_prev = ca.transpose(jac_x_f(Xk_lin_prev, Uk_lin_prev)) nabla_u_f_prev = ca.transpose(jac_u_f(Xk_lin_prev, Uk_lin_prev)) r_lam_k = -Xk_lin + integrator.eval(Xk_lin_prev, Uk_lin_prev) r_x_k = nabla_x_l + ca.mtimes(nabla_x_f, Lamk_lin_next) \ - Lamk_lin + ca.mtimes(nabla_x_g, Nuk_lin) r_u_k = nabla_u_l + ca.mtimes(nabla_u_f, Lamk_lin_next) \ + ca.mtimes(nabla_u_g, Nuk_lin) r_nu_k = g(Xk_lin, Uk_lin) + Tk_lin # compute Hessian approximation Hxx = jac_uu_l(Xk_lin, Uk_lin) Hxx = jac_xx_l(Xk_lin, Uk_lin) Hxu = jac_xu_l(Xk_lin, Uk_lin) Hk = ca.vertcat(ca.horzcat(Hxx, Hxu, r_x_k), \ ca.horzcat(Hxu.T, Huu, r_u_k), \ ca.horzcat(ca.transpose(r_x_k), ca.transpose(r_u_k), 1)) # add cost contribution f_qp = f_qp + ca.mtimes(ca.vertcat(dXk, dUk, 1).T, ca.mtimes(Hk, \ ca.vertcat(ca.vertcat(dXk, dUk, 1)))) # add equality constraints c_qp += [-dXk + ca.mtimes(ca.transpose(nabla_x_f_prev), dXk_prev) \ + ca.mtimes(ca.transpose(nabla_u_f_prev), dUk_prev) + r_lam_k] lbc_qp += [np.zeros((NG, 1))] ubc_qp += [np.zeros((NG, 1))] # add inequality constraints c_qp += [ca.mtimes(ca.transpose(nabla_x_g), dXk) \ + ca.mtimes(ca.transpose(nabla_u_g), dUk) + r_nu_k] lbc_qp += [-np.inf * np.ones((NG, 1))] ubc_qp += [np.zeros((NG, 1))] k = M Xk_lin_prev = X_lin[k - 1] Uk_lin_prev = U_lin[k - 1] dXk = dX[k] dXk_prev = dX[k - 1] dUk_prev = dU[k - 1] Lamk_lin = Lam_lin[k] Xk_lin = X_lin[k] if M < N: dUk = dU[k - 1] nabla_x_f_prev = ca.transpose(jac_x_f(Xk_lin_prev, Uk_lin_prev)) nabla_u_f_prev = ca.transpose(jac_u_f(Xk_lin_prev, Uk_lin_prev)) # compute residuals if M == N: nabla_x_l = ca.transpose(jac_x_lN(Xk_lin)) else: nabla_x_l = ca.transpose(jac_x_l(Xk_lin, Uk_lin)) r_lam_k = -Xk_lin + integrator.eval(Xk_lin_prev, Uk_lin_prev) # compute Hessian approximation PM = ca.reshape(p[-2], NX, NX) pM = p[-1] HM = ca.vertcat(ca.horzcat(PM, pM), ca.horzcat(pM.T, 1)) # add cost contribution f_qp = f_qp + ca.mtimes(ca.vertcat(dXk, 1).T, ca.mtimes(HM, \ ca.vertcat(dXk, 1))) # add equality constraints c_qp += [-dXk + ca.mtimes(ca.transpose(nabla_x_f_prev), dXk_prev) \ + ca.mtimes(ca.transpose(nabla_u_f_prev), dUk_prev) + r_lam_k] lbc_qp += [np.zeros((NG, 1))] ubc_qp += [np.zeros((NG, 1))] if M == N and NGN > 0: Tk_lin = T_lin[k] nabla_x_g = ca.transpose(jac_x_gN(Xk_lin)) r_nu_k = gN(Xk_lin) + Tk_lin c_qp += [ca.mtimes(ca.transpose(nabla_g_x), dXk) + r_nu_k] lbc_qp += [-np.inf * np.ones((NGN, 1))] ubc_qp += [np.zeros((NGN, 1))] c_qp = ca.vertcat(*c_qp) dW = ca.vertcat(*dW) p = ca.vertcat(*p) # convert lists to numpy arrays self._lbc_qp = np.vstack(lbc_qp) self._ubc_qp = np.vstack(ubc_qp) self._w0 = np.vstack(w0) # create an NLP solver prob = {'f': f_qp, 'x': dW, 'g': c_qp, 'p': p} # opts = {'ipopt': {'print_level': 2}} opts = {} self.qp_solver = ca.nlpsol('solver', 'ipopt', prob, opts)
theta = model.x[1] ocp.model.cost_expr_ext_cost = tanh(theta)**2 + .5 * (model.x[0]**2 + W_u * model.u**2) ocp.model.cost_expr_ext_cost_e = tanh(theta)**2 + .5 * model.x[0]**2 custom_hess_u = W_u J = horzcat(SX.eye(2), SX(2, 2)) print(DM(J.sparsity())) # diagonal matrix with second order terms of outer loss function. D = SX.sym('D', Sparsity.diag(2)) D[0, 0] = 1 [hess_tan, grad_tan] = hessian(tanh(theta)**2, theta) D[1, 1] = if_else(theta == 0, hess_tan, grad_tan / theta) custom_hess_x = J.T @ D @ J zeros = SX(1, nx) cost_expr_ext_cost_custom_hess = blockcat(custom_hess_u, zeros, zeros.T, custom_hess_x) cost_expr_ext_cost_custom_hess_e = custom_hess_x ocp.model.cost_expr_ext_cost_custom_hess = cost_expr_ext_cost_custom_hess ocp.model.cost_expr_ext_cost_custom_hess_e = cost_expr_ext_cost_custom_hess_e # set constraints Fmax = 35 ocp.constraints.lbu = np.array([-Fmax])
def __construct_sensitivity_funcs(self): """ Construct functions for NLP sensitivity evaluations """ # system variables x, u = self.__vars['x'], self.__vars['u'] nx = x.shape[0] wk = ca.vertcat(x, u) if 'us' in self.__vars: us = self.__vars['us'] uhat = ca.vertcat(u, us) wk = ca.vertcat(wk, us) else: uhat = u # dynamics sensitivities x_next = self.__F(x0=x, p=u)['xf'] # symbolic integrator evaluation self.__jac_Fx = ca.Function('jac_Fx', [x, u], [ca.jacobian(x_next, x)]).map(self.__N) self.__jac_Fu = ca.Function('jac_Fu', [x, u], [ca.jacobian(x_next, uhat)]).map(self.__N) self.__hess_F = [] for i in range(nx): self.__hess_F.append( ca.Function('hess_F_' + str(i), [x, u], [ca.hessian(x_next[i], wk)[0]])) # cost sensitivities obj = self.__cost(x, u) # symbolic cost evaluation self.__jac_cost = ca.Function('q', [x, u], [ca.jacobian(obj, wk)]).map(self.__N) hess_cost = ca.Function('hess_cost', [x, u], [ca.hessian(obj, wk)[0]]) self.__hess_cost = hess_cost.map(self.__N) # phase fixing sensitivities alpha = ca.MX.sym('alpha') x0star = ca.MX.sym('x0star', self.__nx, 1) phase_fix = self.__phase_fix_fun(alpha, x0star, x) self.__jac_phase_fix = ca.Function('jac_phase_fix', [alpha, x0star, x], [ca.jacobian(phase_fix, wk)]) self.__hess_phase_fix = ca.Function('hess_phase_fix', [alpha, x0star, x], [ca.hessian(phase_fix, wk)[0]]) # constraints sensitivities if self.__h is not None: if 'us' in self.__vars: constr = self.__h(x, u, us) # symbolic constraint evaluation self.__jac_h = ca.Function('jac_h', [x, u, us], [ca.jacobian(constr, wk)]).map( self.__N) else: constr = self.__h(x, u) self.__jac_h = ca.Function( 'jac_h', [x, u], [ca.jacobian(constr, wk)]).map(self.__N) if self.__gnl is not None: constr = self.__gnl(x, u, us) self.__jac_g = ca.Function('jac_g', [x, u, us], [ca.jacobian(constr, wk)]).map(self.__N) self.__hess_g = [] for i in range(self.__ns): self.__hess_g.append( ca.Function('hess_g_' + str(i), [x, u, us], [ca.hessian(constr[i], wk)[0]])) # hessian of the lagrangian hess_lag = hess_cost(x, u) lam_g = ct.MX.sym('lam_g', nx) hess_lag += sum([lam_g[i] * self.__hess_F[i](x, u) for i in range(nx)]) lag_args = [x, u, lam_g] if 'us' in self.__vars: lam_s = ct.MX.sym('lam_s', self.__ns) hess_lag += sum([ lam_s[i] * self.__hess_g[i](x, u, us) for i in range(self.__ns) ]) lag_args += [us, lam_s] hess_lag += ca.hessian( 0.5 * self.__reg_slack * ct.mtimes(us.T, us), wk)[0] self.__hess_lag = ca.Function('hess_lag_fun', [*lag_args], [hess_lag]).map(self.__N) return None
def generate_solver(self, f, f0, g, lby = [], uby = [], lbg = [], ubg = [], p0 = [], \ y0 = [], lam0 = [], qpoases_root=None, casadi_root=None, eigen_root=None, approximate_hessian=None, optlevel='-O2'): g_shape = g.shape if g_shape[1] != 1: raise Exception( 'g must have shape (<>,1), you have {}.'.format(g_shape)) ni = g_shape[0] self.ni = ni nv = self.nv np = self.np FSQP_INF = 1E12 if lby == []: lby = -FSQP_INF * nmp.ones((nv, 1)) if uby == []: uby = FSQP_INF * nmp.ones((nv, 1)) if lbg == []: lbg = nmp.zeros((ni, 1)) if ubg == []: ubg = nmp.zeros((ni, 1)) if p0 == []: p0 = nmp.zeros((np, 1)) if y0 == []: y0 = nmp.zeros((nv, 1)) if lam0 == []: lam0 = nmp.zeros((2 * (ni + nv), 1)) if not isinstance(lby, nmp.ndarray): raise Exception( 'lby must be of type nmp.array, you have {}'.format(type(lby))) if not isinstance(uby, nmp.ndarray): raise Exception( 'lbu must be of type nmp.array, you have {}'.format(type(uby))) if not isinstance(lbg, nmp.ndarray): raise Exception( 'lbg must be of type nmp.array, you have {}'.format(type(lbg))) if not isinstance(ubg, nmp.ndarray): raise Exception( 'ubg must be of type nmp.array, you have {}'.format(type(ubg))) lby_shape = lby.shape if lby_shape[0] != nv or lby_shape[1] != 1: raise Exception('lby must have shape (nv,1) = ({},1), you have ({},{})'\ .format(nv, lby_shape[0], lby_shape[1])) uby_shape = uby.shape if uby_shape[0] != nv or uby_shape[1] != 1: raise Exception('uby must have shape (nv,1) = ({},1), you have ({},{})'\ .format(nv, uby_shape[0], uby_shape[1])) lbg_shape = lbg.shape if lbg_shape[0] != ni or lbg_shape[1] != 1: raise Exception('lbg must have shape (ni,1) = ({},1), you have ({},{})'\ .format(nv, lbg_shape[0], lbg_shape[1])) ubg_shape = ubg.shape if ubg_shape[0] != ni or ubg_shape[1] != 1: raise Exception('ubg must have shape (ni,1) = ({},1), you have ({},{})'\ .format(ni, ubg_shape[0], ubg_shape[1])) p0_shape = p0.shape if p0_shape[0] != np or p0_shape[1] != 1: raise Exception('p0 must have shape (np,1) = ({},1), you have ({},{})'\ .format(np, p0_shape[0], p0_shape[1])) y0_shape = y0.shape if y0_shape[0] != nv or y0_shape[1] != 1: raise Exception('y0 must have shape (nv,1) = ({},1), you have ({},{})'\ .format(nv, y0_shape[0], y0_shape[1])) opts = dict(with_header=True) lam0_shape = lam0.shape if lam0_shape[0] != 2 * (ni + nv) or lam0_shape[1] != 1: raise Exception('lam0 must have shape (2*(ni+nv),1) = ({},1), you have ({},{})'\ .format(ni, lam0_shape[0], lam0_shape[1])) y = self.y p = self.p lam = ca.SX.sym('lam', 2 * (ni + nv), 1) # multiplier structure: [ubg, lbg, ub, lb] # copy Eigen headers to solver folder cmd = 'mkdir -p {}'.format(self.opts['solver_name']) status = os.system(cmd) if status != 0: raise Exception('{} failed'.format(cmd)) os.chdir(self.opts['solver_name']) ca_f = ca.Function('ca_f', [y, p], [f]) ca_f.generate('ca_f', opts) print('compiling generated code for f...') cmd = 'gcc -fPIC -shared {} ca_f.c -o libca_f.so'.format(optlevel) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared {} ca_f.c -o ca_f.so'.format(optlevel) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) ca_f0 = ca.Function('ca_f0', [y, p], [f0]) ca_f0.generate('ca_f0', opts) print('compiling generated code for f0...') cmd = 'gcc -fPIC -shared {} ca_f0.c -o libca_f0.so'.format(optlevel) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared {} ca_f0.c -o ca_f0.so'.format(optlevel) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) ca_dfdy = ca.Function('ca_dfdy', [y, p], [ca.jacobian(f, y)]) ca_dfdy.generate('ca_dfdy', opts) print('compiling generated code for dfdy...') cmd = 'gcc -fPIC -shared {} ca_dfdy.c -o libca_dfdy.so'.format( optlevel) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared {} ca_dfdy.c -o ca_dfdy.so'.format(optlevel) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) ca_g = ca.Function('ca_g', [y, p], [g]) ca_g.generate('ca_g', opts) print('compiling generated code for g...') cmd = 'gcc -fPIC -shared {} ca_g.c -o libca_g.so'.format(optlevel) os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared {} ca_g.c -o ca_g.so'.format(optlevel) os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) # ca_g.save("ca_g.casadi") print('compiling generated code for dgdy...') ca_dgdy = ca.Function('ca_dgdy', [y, p], [ca.jacobian(g, y)]) ca_dgdy.generate('ca_dgdy', opts) cmd = 'gcc -fPIC -shared {} ca_dgdy.c -o libca_dgdy.so'.format( optlevel) os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared {} ca_dgdy.c -o ca_dgdy.so'.format(optlevel) os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) # Lagrangian L = f + ca.dot(lam[2 * nv:2 * nv + ni], g) - ca.dot( lam[2 * nv + ni:2 * nv + 2 * ni], g) + ca.dot( lam[0:nv], y) - ca.dot(lam[nv:2 * nv], y) #TODO(andrea): bounds first here! print('compiling generated code for dLdyy...') ca_dLdyy = ca.Function('ca_dLdyy', [y, lam, p], [ca.hessian(L, y)[0]]) ca_dLdyy.generate('ca_dLdyy', opts) cmd = 'gcc -fPIC -shared {} ca_dLdyy.c -o libca_dLdyy.so'.format( optlevel) os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared {} ca_dLdyy.c -o ca_dLdyy.so'.format(optlevel) os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) if approximate_hessian is not None: if approximate_hessian.shape != (nv, nv): raise Exception( 'Hessian approximation has wrong dimension! You have {} instead of {}' .format(approximate_hessian.shape, (nv, nv))) print('compiling generated code for approximate Hessian M...') ca_M = ca.Function('ca_M', [y, p], [approximate_hessian]) ca_M.generate('ca_M', opts) cmd = 'gcc -fPIC -shared -O3 ca_M.c -o libca_M.so' os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) cmd = 'gcc -fPIC -shared -O3 ca_M.c -o ca_M.so' os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) use_approximate_hessian = True else: use_approximate_hessian = False print('rendering templated C++ code...') env = Environment(loader=FileSystemLoader( os.path.dirname(os.path.abspath(__file__)))) tmpl = env.get_template("templates/feasibleSQP.in.cpp") code = tmpl.render(solver_opts=self.opts, NI=self.ni, NV=self.nv, NP=self.np, use_approximate_hessian=use_approximate_hessian) with open('{}.cpp'.format(self.opts['solver_name']), "w+") as f: f.write(code) tmpl = env.get_template("templates/feasibleSQP.in.hpp") code = tmpl.render(solver_opts = self.opts, NV = nv, NI = ni, NP = np,\ lby = lby, uby = uby, lbg = lbg, ubg = ubg, p0 = p0, y0 = y0, lam0 = lam0, use_approximate_hessian=use_approximate_hessian) with open('{}.hpp'.format(self.opts['solver_name']), "w+") as f: f.write(code) tmpl = env.get_template("templates/main.in.cpp") code = tmpl.render(solver_opts=self.opts, use_approximate_hessian=use_approximate_hessian) with open('main.cpp', "w+") as f: f.write(code) print('rendering templated Makefile...') tmpl = env.get_template("templates/Makefile.in") build_params = dict() fsqp_root = os.path.dirname(os.path.abspath(__file__)) + '/../' if casadi_root is None: casadi_root = fsqp_root + 'external/casadi' if qpoases_root is None: qpoases_root = fsqp_root + 'external/qpOASES' if eigen_root is None: eigen_root = fsqp_root + 'external/eigen-git-mirror/Eigen' build_params['qpoases_root'] = qpoases_root build_params['casadi_root'] = casadi_root build_params['eigen_root'] = eigen_root build_params['solver_name'] = self.opts['solver_name'] code = tmpl.render(build_params=build_params, use_approximate_hessian=use_approximate_hessian) with open('Makefile', "w+") as f: f.write(code) cmd = 'make {}_shared'.format(self.opts['solver_name']) status = os.system(cmd) if status != 0: raise Exception('Command {} failed'.format(cmd)) print('successfully generated solver!') os.chdir('..') # generate script to set LD_LIBRARY_PATH fsqp_root = os.path.dirname(os.path.abspath(__file__)) + '/../' with open(fsqp_root + '/feasible_sqp/paths.json', 'r') as f: library_paths = json.load(f) paths = '' cwd = os.getcwd() paths = paths + ':' + cwd + '/' + self.opts['solver_name'] for key in library_paths: paths = paths + ':' + library_paths[key] with open('set_LD_LIBRARY_PATH.sh', 'w') as f: f.write('#!/bin/bash\nexport LD_LIBRARY_PATH=$LD_LIBRARY_PATH:{}'. format(paths)) self.init() return
def objective(x,y,p,N,params): nPrimal = x.numel() #number of primal sln nDual = y['lam_g'].numel() #number of dual nParam = p.numel() #number of parameters #Loading initial states and controls data = spio.loadmat('CstrDistXinit.mat', squeeze_me = True) Xinit = data['Xinit'] xf = Xinit[0:84] u_opt = Xinit[84:] #Model parameters NT = params['dist']['NT'] #Stages in column Uf = 0.3 #Feed rate to CSTR F_0 _,state,xdot,inputs = ColCSTR_model(Uf,params) sf = Function('sf',[state,inputs],[xdot]) params['model']['sf'] = sf params['model']['xdot_val_rf_ss'] = xf params['model']['x'] = x params['model']['u_opt'] = u_opt nx = params['prob']['nx'] nu = params['prob']['nu'] nk = params['prob']['nk'] tf = params['prob']['tf'] ns = params['prob']['ns'] h = params['prob']['h'] #Preparing collocation matrices _, C, D, d = collocationSetup() params['prob']['d'] = d colloc = {'C':C,'D':D, 'h':h} params['colloc'] = colloc #NLP variable vector V = MX() #Decision variables (control + state) obj = 0 #Objective function cons = MX() #Nonlinear Constraints delta_time = 1 alpha = 1 beta = 1 gamma = 1 params['weight']['delta_time'] = delta_time params['weight']['alpha'] = alpha params['weight']['beta'] = beta params['weight']['gamma'] = gamma #Initial states and Controls data_init = spio.loadmat('CstrDistXinit.mat', squeeze_me = True) xf = data_init['Xinit'][0:84] u_opt = data_init['Xinit'][84:89] #"Lift" Initial conditions X0 = MX.sym('X0', nx) V = vertcat(V,X0) #Decision variables cons = vertcat(cons, X0 - x[0:nx,0]) #Nonlinear constraints cons_x0 = X0 - x[0:nx,0] #Formulating the NLP Xk = X0 data = spio.loadmat('Qmax.mat', squeeze_me = True) params['Qmax'] = data['Qmax'] ssoftc = 0 for i in range(0,N): obj, cons, V, Xk, params, ssoftc = itPredHorizon_pf(Xk, V, cons, obj, params, i, ssoftc) V = vertcat(V[:]) cons = vertcat(cons[:]) #Objective function and constraint functions f = Function('f', [V], [obj], ['V'], ['objective']) c = Function('c', [V], [cons], ['V'], ['constraint']) cx0 = Function('cx0',[X0], [cons_x0], ['X0'], ['constraint']) #Constructing Lagrangian lag_expr = obj + mtimes(transpose(y['lam_g']),cons) g = Function('g',[V],[jacobian(obj,V),obj]) lagr = Function('lagr', [V], [lag_expr], ['V'], ['lag_expr']) [H,gg] = hessian(lag_expr,V) H = Function('H',[V],[H,gg]) [Hobj,gobj] = hessian(obj,V) Hobj = Function('Hobj', [V], [Hobj,gobj]) J = Function('J',[V],[jacobian(cons,V),cons]) Jp = Function('Jp',[X0],[jacobian(cons_x0,X0),cons_x0]) #Evaluating functions at current point f = f(x) g = g(x) g = g[0] g = transpose(g) H = H(x) H = H[0] Lxp = H[0:nPrimal,0:nParam] J = J(x) J = J[0] Jtemp = DM.zeros((nDual,nParam)) cp = Jp(x[0:nParam]) cp = cp[0] Jtemp[0:nParam, 0:nParam] = cp.full() cp = Jtemp.sparse() cst = c(x) #Evaluation of objective function used for Greshgorin bound Hobj = Hobj(x) Hobj = Hobj[0].sparse() f = f.full() g = g.sparse() Lxp = Lxp.sparse() cst = cst.full() #Equality constraint Jeq = J dpe = cp return f, g, H, Lxp, cst, J, cp, Jeq, dpe, Hobj
def generate_c_code_external_cost(model, stage_type, opts): casadi_version = CasadiMeta.version() casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") if casadi_version not in (ALLOWED_CASADI_VERSIONS): casadi_version_warning(casadi_version) x = model.x p = model.p if isinstance(x, MX): symbol = MX.sym else: symbol = SX.sym if stage_type == 'terminal': suffix_name = "_cost_ext_cost_e_fun" suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_e_fun_jac" u = symbol("u", 0, 0) ext_cost = model.cost_expr_ext_cost_e custom_hess = model.cost_expr_ext_cost_custom_hess_e elif stage_type == 'path': suffix_name = "_cost_ext_cost_fun" suffix_name_hess = "_cost_ext_cost_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_fun_jac" u = model.u ext_cost = model.cost_expr_ext_cost custom_hess = model.cost_expr_ext_cost_custom_hess elif stage_type == 'initial': suffix_name = "_cost_ext_cost_0_fun" suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" suffix_name_jac = "_cost_ext_cost_0_fun_jac" u = model.u ext_cost = model.cost_expr_ext_cost_0 custom_hess = model.cost_expr_ext_cost_custom_hess_0 # set up functions to be exported fun_name = model.name + suffix_name fun_name_hess = model.name + suffix_name_hess fun_name_jac = model.name + suffix_name_jac # generate expression for full gradient and Hessian full_hess, grad = hessian(ext_cost, vertcat(u, x)) if custom_hess is not None: full_hess = custom_hess ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) ext_cost_fun_jac_hess = Function(fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]) ext_cost_fun_jac = Function(fun_name_jac, [x, u, p], [ext_cost, grad]) # generate C code code_export_dir = opts["code_export_directory"] if not os.path.exists(code_export_dir): os.makedirs(code_export_dir) cwd = os.getcwd() os.chdir(code_export_dir) gen_dir = model.name + '_cost' if not os.path.exists(gen_dir): os.mkdir(gen_dir) gen_dir_location = "./" + gen_dir os.chdir(gen_dir_location) ext_cost_fun.generate(fun_name, casadi_opts) ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) os.chdir(cwd) return
def spy_hessian(self, opti): import matplotlib.pylab as plt lag = opti.f + dot(opti.lam_g, opti.g) H = hessian(lag, opti.x)[0].sparsity() plt.spy(H) plt.title("Lagrange Hessian: " + H.dim(True))