def create_adjoint_states(self): lamb = SX.sym(self.name + '_lamb', self.model.n_x) nu = SX.sym(self.name + '_nu', self.model.n_y) self.eta = SX.sym(self.name + '_eta', self.n_h_final) self.H = self.L + dot(lamb, self.model.ode) + dot(nu, self.model.alg) l_dot = -gradient(self.H, self.model.x) alg_eq = gradient(self.H, self.model.y) self.include_state(lamb, l_dot, suppress=True) self.model.has_adjoint_variables = True self.include_algebraic(nu, alg_eq) self.h_final = vertcat( self.h_final, self.model.lamb_sym - gradient(self.V, self.model.x_sys_sym) - mtimes(jacobian(self.h_final, self.model.x_sys_sym).T, self.eta))
def build_moment_functions(n, mgf): """ Builds a dictionary of functions Args: n ([type]): [description] mgf ([type]): [description] Returns: [type]: [description] """ moment_functions = dict() for i in range(n): moment_fun = casadi.gradient(moment_fun, t) moment_functions[i + 1] = casadi.Function('g' + str(i + 1), [t], [moment_fun]) return moment_functions
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 _convert(self, symbol, t, y, y_dot, inputs): """ See :meth:`CasadiConverter.convert()`. """ if isinstance( symbol, ( pybamm.Scalar, pybamm.Array, pybamm.Time, pybamm.InputParameter, pybamm.ExternalVariable, ), ): return casadi.MX(symbol.evaluate(t, y, y_dot, inputs)) elif isinstance(symbol, pybamm.StateVector): if y is None: raise ValueError("Must provide a 'y' for converting state vectors") return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices]) elif isinstance(symbol, pybamm.StateVectorDot): if y_dot is None: raise ValueError("Must provide a 'y_dot' for converting state vectors") return casadi.vertcat(*[y_dot[y_slice] for y_slice in symbol.y_slices]) elif isinstance(symbol, pybamm.BinaryOperator): left, right = symbol.children # process children converted_left = self.convert(left, t, y, y_dot, inputs) converted_right = self.convert(right, t, y, y_dot, inputs) if isinstance(symbol, pybamm.Minimum): return casadi.fmin(converted_left, converted_right) if isinstance(symbol, pybamm.Maximum): return casadi.fmax(converted_left, converted_right) # _binary_evaluate defined in derived classes for specific rules return symbol._binary_evaluate(converted_left, converted_right) elif isinstance(symbol, pybamm.UnaryOperator): converted_child = self.convert(symbol.child, t, y, y_dot, inputs) if isinstance(symbol, pybamm.AbsoluteValue): return casadi.fabs(converted_child) return symbol._unary_evaluate(converted_child) elif isinstance(symbol, pybamm.Function): converted_children = [ self.convert(child, t, y, y_dot, inputs) for child in symbol.children ] # Special functions if symbol.function == np.min: return casadi.mmin(*converted_children) elif symbol.function == np.max: return casadi.mmax(*converted_children) elif symbol.function == np.abs: return casadi.fabs(*converted_children) elif symbol.function == np.sqrt: return casadi.sqrt(*converted_children) elif symbol.function == np.sin: return casadi.sin(*converted_children) elif symbol.function == np.arcsinh: return casadi.arcsinh(*converted_children) elif symbol.function == np.arccosh: return casadi.arccosh(*converted_children) elif symbol.function == np.tanh: return casadi.tanh(*converted_children) elif symbol.function == np.cosh: return casadi.cosh(*converted_children) elif symbol.function == np.sinh: return casadi.sinh(*converted_children) elif symbol.function == np.cos: return casadi.cos(*converted_children) elif symbol.function == np.exp: return casadi.exp(*converted_children) elif symbol.function == np.log: return casadi.log(*converted_children) elif symbol.function == np.sign: return casadi.sign(*converted_children) elif isinstance(symbol.function, (PchipInterpolator, CubicSpline)): return casadi.interpolant("LUT", "bspline", [symbol.x], symbol.y)( *converted_children ) elif symbol.function.__name__.startswith("elementwise_grad_of_"): differentiating_child_idx = int(symbol.function.__name__[-1]) # Create dummy symbolic variables in order to differentiate using CasADi dummy_vars = [ casadi.MX.sym("y_" + str(i)) for i in range(len(converted_children)) ] func_diff = casadi.gradient( symbol.differentiated_function(*dummy_vars), dummy_vars[differentiating_child_idx], ) # Create function and evaluate it using the children casadi_func_diff = casadi.Function("func_diff", dummy_vars, [func_diff]) return casadi_func_diff(*converted_children) # Other functions else: return symbol._function_evaluate(converted_children) elif isinstance(symbol, pybamm.Concatenation): converted_children = [ self.convert(child, t, y, y_dot, inputs) for child in symbol.children ] if isinstance(symbol, (pybamm.NumpyConcatenation, pybamm.SparseStack)): return casadi.vertcat(*converted_children) # DomainConcatenation specifies a particular ordering for the concatenation, # which we must follow elif isinstance(symbol, pybamm.DomainConcatenation): slice_starts = [] all_child_vectors = [] for i in range(symbol.secondary_dimensions_npts): child_vectors = [] for child_var, slices in zip( converted_children, symbol._children_slices ): for child_dom, child_slice in slices.items(): slice_starts.append(symbol._slices[child_dom][i].start) child_vectors.append( child_var[child_slice[i].start : child_slice[i].stop] ) all_child_vectors.extend( [v for _, v in sorted(zip(slice_starts, child_vectors))] ) return casadi.vertcat(*all_child_vectors) else: raise TypeError( """ Cannot convert symbol of type '{}' to CasADi. Symbols must all be 'linear algebra' at this stage. """.format( type(symbol) ) )
# 适用于矩阵或稀疏模式 # 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
con.T - 157.29) @ R @ (con - 157.29) g[(k + 1) * 3:(k + 2) * 3] = Z[:, k + 1] - (f(st, con) * T + Z[:, k]) st = Z[:, N_pred] print(st) obj = obj + (st - P[3:6]).T @ P_f @ (st - P[3:6]) # create a dense matrix of state constraints #g = SX.zeros(3 * (N_pred + 1), 1) for i in range(Lambda.size(2)): L = L + Lambda[:, i].T @ g[3 * i:3 * (i + 1)] L = L + obj print(L) #hinkreigen phi und Solution S phi = SX.sym('phi') S = SX.sym('S') phi = gradient(L, Lambda[:, 0]) S = Lambda[:, 0] print("phi", phi) for i in range(N_pred): phi = vertcat(phi, gradient(L, Z[:, i])) phi = vertcat(phi, gradient(L, V[:, i])) phi = vertcat(phi, gradient(L, Lambda[:, i + 1])) S = vertcat(S, Z[:, i]) S = vertcat(S, V[:, i]) S = vertcat(S, Lambda[:, i + 1]) phi = vertcat(phi, gradient(L, Z[:, N_pred])) S = vertcat(S, Z[:, N_pred]) phi_func = Function("phi_func", [P, S], [phi]) print(phi_func)
def makeSolver(self): # make sure all bounds are set (xuMissing,pMissing) = self._boundMap.getMissing() msg = [] for name in xuMissing: msg.append("you forgot to set a bound on \""+name+"\" at timesteps: "+str(xuMissing[name])) for name in pMissing: msg.append("you forgot to set a bound on \""+name+"\"") if len(msg)>0: raise ValueError('\n'.join(msg)) # constraints: constraints = self._constraints._g constraintLbgs = self._constraints._glb constraintUbgs = self._constraints._gub g = [self._setupDynamicsConstraints()] g = [] h = [] hlbs = [] hubs = [] for k in range(len(constraints)): lb = constraintLbgs[k] ub = constraintUbgs[k] if all(lb==ub): g.append(constraints[k]-lb) # constrain to be zero else: h.append(constraints[k]) hlbs.append(lb) hubs.append(ub) g = C.veccat(g) h = C.veccat(h) hlbs = C.veccat(hlbs) hubs = C.veccat(hubs) # design vars V = self._dvMap.vectorize() # gradient of arbitraryObj if hasattr(self,'_obj'): arbitraryObj = self._obj else: arbitraryObj = 0 gradF = C.gradient(arbitraryObj,V) # hessian of lagrangian: J = 0 for gnf in self._gaussNewtonObjF: J += C.jacobian(gnf,V) hessL = C.mul(J.T,J) + C.jacobian(gradF,V) # equality constraint jacobian jacobG = C.jacobian(g,V) # inequality constraint jacobian jacobH = C.jacobian(h,V) # function which generates everything needed masterFun = C.MXFunction([V],[hessL, gradF, g, jacobG, h, jacobH]) masterFun.init() class JorisError(Exception): pass raise JorisError('JORIS, please read the following comment')
def makeSolver(self,endTime,traj=None): # make sure all bounds are set (xMissing,pMissing) = self._boundMap.getMissing() msg = [] for name in xMissing: msg.append("you forgot to set a bound on \""+name+"\" at timesteps: "+str(xMissing[name])) for name in pMissing: msg.append("you forgot to set a bound on \""+name+"\"") if len(msg)>0: raise ValueError('\n'.join(msg)) # constraints: g = self._constraints.getG() glb = self._constraints.getLb() gub = self._constraints.getUb() gDyn = self._setupDynamicsConstraints(endTime,traj) gDynLb = gDynUb = [C.DMatrix.zeros(gg.shape) for gg in gDyn] g = C.veccat([g]+gDyn) glb = C.veccat([glb]+gDynLb) gub = C.veccat([gub]+gDynUb) self.glb = glb self.gub = gub # design vars V = self._dvMap.vectorize() # gradient of arbitraryObj if hasattr(self,'_obj'): arbitraryObj = self._obj else: arbitraryObj = 0 gradF = C.gradient(arbitraryObj,V) # hessian of lagrangian: Js = [C.jacobian(gnf,V) for gnf in self._gaussNewtonObjF] gradFgns = [C.mul(J.T,F) for (F,J) in zip(self._gaussNewtonObjF, Js)] gaussNewtonHess = sum([C.mul(J.T,J) for J in Js]) hessL = gaussNewtonHess + C.jacobian(gradF,V) gradF += sum(gradFgns) # equality/inequality constraint jacobian gfcn = C.MXFunction([V,self._U],[g]) gfcn.init() jacobG = gfcn.jacobian(0,0) jacobG.init() # function which generates everything needed f = sum([f_*f_ for f_ in self._gaussNewtonObjF]) if hasattr(self,'_obj'): f += self._obj self.masterFun = C.MXFunction([V,self._U],[hessL, gradF, g, jacobG.call([V,self._U])[0], f]) self.masterFun.init() # self.qp = C.CplexSolver(hessL.sparsity(),jacobG.output(0).sparsity()) self.qp = C.NLPQPSolver(hessL.sparsity(),jacobG.output(0).sparsity()) self.qp.setOption('nlp_solver',C.IpoptSolver) self.qp.setOption('nlp_solver_options',{'print_level':0,'print_time':False}) self.qp.init()
delta_Q = SX([[5, 0, 0], [0, 5, 0], [0, 0, 5]]) K = reshape(K, 1, 3) X = SX.sym('X', 3, 1) psi = f(X, -K @ (X - state_r) + u_r) - ((phi_c) @ (X - state_r) + state_r) V_inf = (X - state_r).T @ P_f @ (X - state_r) Dis = norm_2(-K @ (X - state_r) + u_r[0] - 255) / sqrt(K[0]**2 + K[1]**2 + K[2]**2) print("dis", Dis) print("K", K, K.shape) obj = Dis OPT_variables = reshape(X, 3, 1) g = SX.zeros(2, 1) g[0] = K @ gradient(V_inf, X) / (norm_2(K) * norm_2(gradient(V_inf, X))) g[1] = -K @ (X - state_r) + u_r[0] nlp_prob = {'f': obj, 'x': OPT_variables, 'g': g} opts = {} opts['print_time'] = False opts['ipopt'] = { 'max_iter': 1000, 'print_level': 3, #'acceptable_tol': 1e-8, #'acceptable_obj_change_tol': 1e-6 } # print(opts) solver = nlpsol("solver", "ipopt", nlp_prob, opts) nl = {} nl['lbx'] = [0 for i in range(3)] nl['ubx'] = [0 for i in range(3)]
def build_moment_functions(n, mgf): """ Builds a dictionary of functions Args: n ([type]): [description] mgf ([type]): [description] Returns: [type]: [description] """ moment_functions = dict() for i in range(n): moment_fun = casadi.gradient(moment_fun, t) moment_functions[i + 1] = casadi.Function('g' + str(i + 1), [t], [moment_fun]) return moment_functions t = casadi.MX.sym('t') mu = casadi.MX.sym('mu') sigmasq = casadi.MX.sym('sigmasq') a = casadi.MX.sym('a') b = casadi.MX.sym('b') foo = truncated_normal_mgf(t, mu, sigmasq, a, b) for i in range(8): foo = casadi.gradient(foo, t) fun = casadi.Function('g' + str(i + 1), [t, mu, sigmasq, a, b], [foo])
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case): """ QP solver for path-following algorithm inputs: prob - problem description obj - problem equations p_init - initial parameter x_init - initial primal variable y_init - initial dual variable lam_opt - Lagrange multipliers of equality and active constraints mu_opt - Lagrange multipliers of inequality constraints outputs: y - solution primal variable qp_val - objective function value qp_exit - return status of QP solver deriv - derivatives of the problem k_zero_tilde - active set index k_plus_tilde - inactive set index grad - gradient of objective function """ print 'Current point x:', x_init #Importing problem to be solved nx, np, neq, niq, name = prob() x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj( x_init, y_init, p_init, neq, niq, nx, np) #Deteriming constraint types eq_con_ind = array([]) #indices of equality constraints iq_con_ind = array([]) #indices of inequality constraints eq_con = array([]) #equality constraints iq_con = array([]) #inequality constraints for i in range(0, len(lbg[0])): if lbg[0, i] == 0: eq_con = vertcat(eq_con, con[i]) eq_con_ind = append(eq_con_ind, i) elif lbg[0, i] < 0: iq_con = vertcat(iq_con, con[i]) iq_con_ind = append(iq_con_ind, i) # print 'Equality Constraint:', eq_con # print 'Inequality Constraint:', iq_con # if case == 'pure-predictor': # return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt if case == 'predictor-corrector': #Evaluating constraints at current iteration point con_vals = conf(x_init, p_init) #Determining which inequality constraints are active k_plus_tilde = array([]) #active constraint k_zero_tilde = array([]) #inactive constraint tol = 10e-5 #tolerance for i in range(0, len(iq_con_ind)): if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0, i] + tol: k_plus_tilde = append(k_plus_tilde, i) else: k_zero_tilde = append(k_zero_tilde, i) # print 'Active constraints:', k_plus_tilde # print 'Inactive constraints:', k_zero_tilde # print 'Constraint values:', con_vals nk_pt = len(k_plus_tilde) #number of active constraints nk_zt = len(k_zero_tilde) #number of inactive constraints #Calculating Lagrangian lam = SX.sym('lam', neq) #Lagrangian multiplier equality constraints mu = SX.sym('mu', niq) #Lagrangian multiplier inequality constraints lag_f = f + mtimes(lam.T, eq_con) + mtimes( mu.T, iq_con) #Lagrangian equation #Calculating derivatives g = gradient(f, x) #Derivative of objective function g_fun = Function('g_fun', [x, p], [gradient(f, x)]) H = 2 * jacobian(gradient(lag_f, x), x) #Second derivative of the Lagrangian H_fun = Function('H_fun', [x, p, lam, mu], [jacobian(jacobian(lag_f, x), x)]) if len(eq_con_ind) > 0: deq = jacobian(eq_con, x) #Derivative of equality constraints else: deq = array([]) if len(iq_con_ind) > 0: diq = jacobian(iq_con, x) #Derivative of inequality constraints else: diq = array([]) #Creating constraint matrices nc = niq + neq #Total number of constraints if (niq > 0) and (neq > 0): #Equality and inequality constraints #this part needs to be tested if (nk_zt > 0): #Inactive constraints exist A = SX.zeros((nc, nx)) print deq A[0, :] = deq #A matrix lba = -1e16 * SX.zeros((nc, 1)) lba[0, :] = -eq_con #lower bound of A uba = 1e16 * SX.zeros((nc, 1)) uba[0, :] = -eq_con #upper bound of A for j in range(0, nk_pt): #adding active constraints A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :] lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])] uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])] for i in range(0, nk_zt): #adding inactive constraints A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :] uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])] #inactive constraints don't have lower bounds else: #Active constraints only A = vertcat(deq, diq) lba = vertcat(-eq_con, -iq_con) uba = vertcat(-eq_con, -iq_con) elif (niq > 0) and (neq == 0): #Inquality constraints if (nk_zt > 0): #Inactive constraints exist A = SX.zeros((nc, nx)) lba = -1e16 * SX.ones((nc, 1)) uba = 1e16 * SX.ones((nc, 1)) for j in range(0, nk_pt): #adding active constraints A[j, :] = diq[int(k_plus_tilde[j]), :] lba[j] = -iq_con[int(k_plus_tilde[j])] uba[j] = -iq_con[int(k_plus_tilde[j])] for i in range(0, nk_zt): #adding inactive constraints A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :] uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])] #inactive constraints don't have lower bounds else: raw_input() A = vertcat(deq, diq) lba = -iq_con uba = -iq_con elif (niq == 0) and (neq > 0): #Equality constriants A = deq lba = -eq_con uba = -eq_con A_fun = Function('A_fun', [x, p], [A]) lba_fun = Function('lba_fun', [x, p], [lba]) uba_fun = Function('uba_fun', [x, p], [uba]) #Checking that matrices are correct sizes and types if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'): #H matrix should be a sparse (nxn) and symmetrical print( 'WARNING: H matrix is not the correct dimensions or matrix type' ) if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True': #g matrix should be a dense (nx1) print( 'WARNING: g matrix is not the correct dimensions or matrix type' ) if (A.size1() != (neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'): #A should be a sparse (nc x n) print( 'WARNING: A matrix is not the correct dimensions or matrix type' ) if lba.size1() != (neq + niq) or (lba.size2() != 1) or lba.is_dense() == 'False': print( 'WARNING: lba matrix is not the correct dimensions or matrix type' ) if uba.size1() != (neq + niq) or (uba.size2() != 1) or uba.is_dense() == 'False': print( 'WARNING: uba matrix is not the correct dimensions or matrix type' ) #Evaluating QP matrices at optimal points H_opt = H_fun(x_init, p_init, lam_opt, mu_opt) g_opt = g_fun(x_init, p_init) A_opt = A_fun(x_init, p_init) lba_opt = lba_fun(x_init, p_init) uba_opt = uba_fun(x_init, p_init) # print 'Lower bounds', lba_opt # print 'Upper bounds', uba_opt # print 'Bound matrix', A_opt #Defining QP structure qp = {} qp['h'] = H_opt.sparsity() qp['a'] = A_opt.sparsity() optimize = conic('optimize', 'qpoases', qp) optimal = optimize(h=H_opt, g=g_opt, a=A_opt, lba=lba_opt, uba=uba_opt, x0=x_init) x_qpopt = optimal['x'] if x_qpopt.shape == x_init.shape: qp_exit = 'optimal' else: qp_exit = '' lag_qpopt = optimal['lam_a'] #Determing Lagrangian multipliers (lambda and mu) lam_qpopt = zeros( (nk_pt, 1)) #Lagrange multiplier of active constraints mu_qpopt = zeros( (nk_zt, 1)) #Lagrange multiplier of inactive constraints if nk_pt > 0: for j in range(0, len(k_plus_tilde)): lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])] if nk_zt > 0: for k in range(0, len(k_zero_tilde)): print lag_qpopt[int(k_zero_tilde[k])] return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
def makeSolver(self): # make sure all bounds are set (xuMissing, pMissing) = self._boundMap.getMissing() msg = [] for name in xuMissing: msg.append("you forgot to set a bound on \"" + name + "\" at timesteps: " + str(xuMissing[name])) for name in pMissing: msg.append("you forgot to set a bound on \"" + name + "\"") if len(msg) > 0: raise ValueError('\n'.join(msg)) # constraints: constraints = self._constraints._g constraintLbgs = self._constraints._glb constraintUbgs = self._constraints._gub g = [self._setupDynamicsConstraints()] g = [] h = [] hlbs = [] hubs = [] for k in range(len(constraints)): lb = constraintLbgs[k] ub = constraintUbgs[k] if all(lb == ub): g.append(constraints[k] - lb) # constrain to be zero else: h.append(constraints[k]) hlbs.append(lb) hubs.append(ub) g = C.veccat(g) h = C.veccat(h) hlbs = C.veccat(hlbs) hubs = C.veccat(hubs) # design vars V = self._dvMap.vectorize() # gradient of arbitraryObj if hasattr(self, '_obj'): arbitraryObj = self._obj else: arbitraryObj = 0 gradF = C.gradient(arbitraryObj, V) # hessian of lagrangian: J = 0 for gnf in self._gaussNewtonObjF: J += C.jacobian(gnf, V) hessL = C.mul(J.T, J) + C.jacobian(gradF, V) # equality constraint jacobian jacobG = C.jacobian(g, V) # inequality constraint jacobian jacobH = C.jacobian(h, V) # function which generates everything needed masterFun = C.MXFunction([V], [hessL, gradF, g, jacobG, h, jacobH]) masterFun.init() class JorisError(Exception): pass raise JorisError('JORIS, please read the following comment')
def solve_hanging_chain_qp(num_masses, use_contraints): m_i = 40.0 / num_masses D_i = 70.0 * num_masses g0 = 9.81 zmin = 0.5 # ground x = [] f = 0 g = [] lbx = [] ubx = [] lbg = [] ubg = [] y_start, z_start = -2, 1 y_end, z_end = 2, 1 # Loop over all chain elements y_prev = z_prev = None for i in range(0, num_masses): # Create variables for the (y_i, z_i) coordinates y_i = casadi.SX.sym('y_' + str(i)) z_i = casadi.SX.sym('z_' + str(i)) # Add to the list of variables x += [y_i, z_i] lbx += [-numpy.inf, zmin] ubx += [numpy.inf, numpy.inf] # Spring potential if i == 0: f += D_i / 2 * ((y_start - y_i)**2 + (z_start - z_i)**2) else: f += D_i / 2 * ((y_prev - y_i)**2 + (z_prev - z_i)**2) # Graviational potential f += g0 * m_i * z_i # Slanted ground constraints if use_contraints: g.append(z_i - 0.1 * y_i) lbg.append(0.5) ubg.append(numpy.inf) # Prepare for the next iteration y_prev = y_i z_prev = z_i f += D_i / 2 * ((y_i - y_end)**2 + (z_i - z_end)**2) num_variables = len(x) num_constraints = len(g) x = casadi.vertcat(*x) g = casadi.vertcat(*g) qp = {'x': x, 'f': f, 'g': g} solver = casadi.qpsol('solver', 'qpoases', qp) gradient_f = casadi.gradient(f, x) h = casadi.jacobian(gradient_f, x, {'symmetric': True}) c = casadi.substitute(gradient_f, x, casadi.SX.zeros(x.sparsity())) a = casadi.jacobian(g, x) prob = casadi.Function("qp_eval", [x], [h, c, a]) qp_items = prob(casadi.SX.sym('eval_args', x.shape)) h_flat = numpy.asarray(casadi.DM(qp_items[0])) c_flat = numpy.asarray(casadi.DM(qp_items[1])) a_flat = numpy.asarray(casadi.DM(qp_items[2])) sol = solver(lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg) x_opt = numpy.asarray(sol['x']).flatten() y_opt = numpy.asarray(-casadi.vertcat(sol['lam_x'], sol['lam_g'])) f_opt = sol['f'] return (num_variables, num_constraints, h_flat, c_flat, a_flat, numpy.asarray(lbx), numpy.asarray(ubx), numpy.asarray(lbg), numpy.asarray(ubg), x_opt, y_opt, f_opt)
def makeSolver(self, endTime, traj=None): # make sure all bounds are set (xMissing, pMissing) = self._boundMap.getMissing() msg = [] for name in xMissing: msg.append("you forgot to set a bound on \"" + name + "\" at timesteps: " + str(xMissing[name])) for name in pMissing: msg.append("you forgot to set a bound on \"" + name + "\"") if len(msg) > 0: raise ValueError('\n'.join(msg)) # constraints: g = self._constraints.getG() glb = self._constraints.getLb() gub = self._constraints.getUb() gDyn = self._setupDynamicsConstraints(endTime, traj) gDynLb = gDynUb = [C.DMatrix.zeros(gg.shape) for gg in gDyn] g = C.veccat([g] + gDyn) glb = C.veccat([glb] + gDynLb) gub = C.veccat([gub] + gDynUb) self.glb = glb self.gub = gub # design vars V = self._dvMap.vectorize() # gradient of arbitraryObj if hasattr(self, '_obj'): arbitraryObj = self._obj else: arbitraryObj = 0 gradF = C.gradient(arbitraryObj, V) # hessian of lagrangian: Js = [C.jacobian(gnf, V) for gnf in self._gaussNewtonObjF] gradFgns = [C.mul(J.T, F) for (F, J) in zip(self._gaussNewtonObjF, Js)] gaussNewtonHess = sum([C.mul(J.T, J) for J in Js]) hessL = gaussNewtonHess + C.jacobian(gradF, V) gradF += sum(gradFgns) # equality/inequality constraint jacobian gfcn = C.MXFunction([V, self._U], [g]) gfcn.init() jacobG = gfcn.jacobian(0, 0) jacobG.init() # function which generates everything needed f = sum([f_ * f_ for f_ in self._gaussNewtonObjF]) if hasattr(self, '_obj'): f += self._obj self.masterFun = C.MXFunction( [V, self._U], [hessL, gradF, g, jacobG.call([V, self._U])[0], f]) self.masterFun.init() # self.qp = C.CplexSolver(hessL.sparsity(),jacobG.output(0).sparsity()) self.qp = C.NLPQPSolver(hessL.sparsity(), jacobG.output(0).sparsity()) self.qp.setOption('nlp_solver', C.IpoptSolver) self.qp.setOption('nlp_solver_options', { 'print_level': 0, 'print_time': False }) self.qp.init()
c = (Q + F) / 2 dC = jtimes(C, q, dq) dQ = jtimes(Q, q, dq) dc = jtimes(c, q, dq) # Modeling with Lagrange mechanics E_kin = 0.5 * dtau**2 * (m * l**2 / 12) + 0.5 * ddelta**2 * ( M * H**2 / 12) + 0.5 * M * sumsqr(dC) + 0.5 * m * sumsqr(dc) E_pot = M * g * C[1] + m * g * c[1] Lag = E_kin - E_pot E_tot = E_kin + E_pot Lag_q = gradient(Lag, q) Lag_dq = gradient(Lag, dq) rhs = solve(jacobian(Lag_dq, dq), vertcat(0, R, T) + Lag_q - jtimes(Lag_dq, q, dq), "symbolicqr") ocp.set_der(alpha, dalpha) ocp.set_der(beta, dbeta) ocp.set_der(gamma, dgamma) ocp.set_der(dalpha, rhs[0]) ocp.set_der(dbeta, rhs[1]) ocp.set_der(dgamma, rhs[2]) ocp.set_initial(gamma, pi / 2) ocp.set_initial(R, Rn)