def idr(y, X, groups=None, orders=dict({"1": "comp"}), verbose=False, max_iter=10000, eps_rel=0.00001, eps_abs=0.00001): """ Fits isotonic distirbutional regression (IDR) to a training dataset. Parameters ---------- y : np.array one dimensional array (response variable) X : pd.DataFrame data frame of variables (regression covariates) groups : dictionary denoting groups of variables that are to be ordered with the same order. Only relevant if X contains more than one variable The default uses one group for all variables orders : dictionary denotes the order that is applied to a group. Only relevant if X contains mode than one variable The default is dict({"1":"comp"}). (OSQP Solver Setting) verbose : boolean print output of OSQP solver. The default is False max_iter : maximum number of iterations eps_rel : relative tolerance epc_abs : absolute tolerance Returns ------- An object of class idrobject containing the components: X: data frame of distinct covariate combinations used for fit y: list of responses for given covariate combination cdf: estimated CDF thresholds: where CDF is evaluated groups: groups used for estimation orders: orders used for estimation indices: indices of covariates in original dataset constraints: in multivariate IDR is None, otherwise the order constraints for optimization """ if not isinstance(X, pd.DataFrame): raise ValueError("X must be a pandas data frame") if groups is None: groups = dict(zip(X.columns, np.ones(X.shape[1]))) y = np.asarray(y) if y.ndim > 1: raise ValueError('idr only handles 1-D arrays of observations') if isinstance(X, pd.DataFrame) == False: raise ValueError("data must be a pandas data frame") if X.shape[0] <= 1: raise ValueError('X must have more than 1 row') if np.isnan(np.sum(y)) == True: raise ValueError("y contains nan values") if X.isnull().values.any() == True: raise ValueError("X contains nan values") if y.size != X.shape[0]: raise ValueError("length of y must match number of rows in X") if all(item in ['comp', 'icx', 'sd'] for item in orders.values()) == False: raise ValueError("orders must be in comp, icx, sd") M = all(elem in groups.keys() for elem in X.columns) if M == False: raise ValueError("some variables must be used in groups and in X") thresholds = np.sort(np.unique(y)) nThr = len(thresholds) if nThr == 1: raise ValueError("y must contain more than 1 distinct value") Xp = X.copy() Xp = prepareData(Xp, groups, orders) nVar = Xp.shape[1] oldNames = Xp.columns Xp['y'] = y Xp['ind'] = np.arange(len(y)) tt = list(oldNames) X_grouped = Xp.groupby(tt).agg({'y': list, 'ind': list}) X_grouped = X_grouped.sort_values(by=list(oldNames)[::-1]) X_grouped = X_grouped.reset_index() cpY = X_grouped["y"] indices = X_grouped["ind"] Xp = X_grouped[tt] lenlist = np.vectorize(len) weights = lenlist(indices) N = Xp.shape[0] if nVar == 1: constr = None rep_time = [len(x) for x in indices] flat_indices = [item for sublist in cpY for item in sublist] posY = np.repeat(np.arange(len(indices)), rep_time)[np.argsort(flat_indices, kind="mergesort")] cdf_vec = isocdf_seq(np.array(weights), np.ones(y.shape[0]), np.sort(y), posY, thresholds) cdf1 = np.reshape(cdf_vec, (N, nThr), order="F") else: constr = comp_ord(Xp) cdf = np.zeros((N, nThr - 1)) A = tr_reduc(constr[0], N) nConstr = A.shape[1] l = np.zeros(nConstr) A = sparse.csc_matrix((np.repeat([1, -1], nConstr), (np.tile(np.arange(nConstr), 2), A.flatten())), shape=(nConstr, N)) P = sparse.csc_matrix((weights, (np.arange(N), np.arange(N)))) i = 0 I = nThr - 1 #conv = np.full(I, False, dtype=bool) q = -weights * np.array( cpY.apply(lambda x: np.mean(np.array(x) <= thresholds[i]))) m = osqp.OSQP() m.setup(P=P, q=q, A=A, l=l, verbose=verbose, max_iter=max_iter, eps_rel=eps_rel, eps_abs=eps_abs) sol = m.solve() pmax = np.where(sol.x > 0, sol.x, 0) cdf[:, 0] = np.where(pmax < 1, pmax, 1) if I > 1: for i in tqdm(range(1, I)): m.warm_start(x=cdf[:, i - 1]) q = -weights * np.array( cpY.apply(lambda x: np.mean(np.array(x) <= thresholds[i]))) m.update(q=q) sol = m.solve() pmax = np.where(sol.x > 0, sol.x, 0) cdf[:, i] = np.where(pmax < 1, pmax, 1) if nVar > 1: cdf = pavaCorrect(cdf) cdf1 = np.ones((N, nThr)) cdf1[:, :-1] = cdf idr_object = idrobject(ecdf=cdf1, thresholds=thresholds, indices=indices, X=Xp, y=cpY, groups=groups, orders=orders, constraints=constr) return (idr_object)
def _quadprog(cls, Q, y, C, solver='osqp'): """solve min_x x^TPx + q^Tx, s.t. Gx<=h, Ax=b Parameters ---------- Q : [type] [description] y : [type] [description] C : [type] [description] solver : str, optional [description], by default 'osqp' Returns ------- [type] [description] """ # dual nl = y.shape[0] q = -1 * np.ones((nl, 1)) if solver == 'cvxopt': G = np.zeros((2 * nl, nl)) G[:nl, :] = -1 * np.eye(nl) G[nl:, :] = np.eye(nl) h = np.zeros((2 * nl, 1)) h[nl:, :] = C / nl # convert numpy matrix to cvxopt matrix P = matrix(Q) q = matrix(q) G = matrix(G) h = matrix(h) A = matrix(y.reshape(1, -1).astype('float64')) b = matrix(np.zeros(1).astype('float64')) solvers.options['show_progress'] = False sol = solvers.qp(P, q, G, h, A, b) alpha = np.array(sol['x']).reshape(nl) elif solver == 'osqp': warnings.simplefilter('ignore', sparse.SparseEfficiencyWarning) P = sparse.csc_matrix((nl, nl)) P[:nl, :nl] = Q[:nl, :nl] G = sparse.vstack([sparse.eye(nl), y.reshape(1, -1)]).tocsc() l_ = np.zeros((nl + 1, 1)) u = np.zeros(l_.shape) u[:nl, 0] = C prob = osqp.OSQP() prob.setup(P, q, G, l_, u, verbose=False) res = prob.solve() alpha = res.x else: raise ValueError('Invalid QP solver') return alpha
def mpc_solver(p_0, max_v, min_v, max_a, min_a, max_j, min_j, Target_p, step_n): v_0 = 0 a_0 = 0 j_0 = 0 K = 20 dt = 0.2 w1 = 10 w2 = 1 w3 = 1 w4 = 5 w5 = 1e8 count = 0 log_p = [] log_v = [] log_a = [] log_j = [] conc_with_identity = lambda x, y: np.concatenate([x, y * np.ones([K, K])], axis=1) for i in range(step_n-1): Tp, Tv, Ta, Bp, Bv, Ba = getPredictionMatrix(K, dt, p_0, v_0, a_0) Target_p_K = np.asarray(Target_p[count:count + K]).reshape(1, -1) H = scipy.linalg.block_diag(w4 * np.ones([K, K]) + w1 * (np.matmul(Tp.transpose(), Tp)), w5 * np.ones([K, K])) F = np.concatenate([2 * w1 * (np.matmul(Bp.transpose(), Tp) - np.matmul(Target_p_K, Tp)), np.zeros([1, K])], axis=1) A = np.concatenate([conc_with_identity(Tv, 0), conc_with_identity(-Tv, -1), conc_with_identity(Ta, 0), conc_with_identity(-Ta, -1), conc_with_identity(np.ones([K, K]), 0), conc_with_identity(-np.ones([K, K]), -1), conc_with_identity(np.zeros_like(Ta), -1)], axis=0) b = np.concatenate([np.ones([K, 1]) * max_v - Bv, -np.ones([K, 1]) * min_v + Bv, np.ones([K, 1]) * max_a - Ba, -np.ones([K, 1]) * min_a + Ba, np.ones([K, 1]) * max_j, -np.ones([K, 1]) * min_j, np.zeros([K, 1])], axis=0) P = sparse.csc_matrix(H) q = np.array(F.T) A_ = sparse.csc_matrix(A) l = np.array(-np.inf * np.ones_like(b)) # l = None u = np.array(b) # Create an OSQP object prob = osqp.OSQP() # Setup workspace and change alpha parameter prob.setup(P, q, A_, l, u) # Solve problem res = prob.solve() j = res.x[0] p_0 = p_0 + v_0 * dt + 0.5 * a_0 * dt ** 2 + 1 / 6 * j * dt ** 3 v_0 = v_0 + a_0 * dt + 0.5 * j * dt ** 2 a_0 = a_0 + j * dt j_0 = j log_p.append(p_0) log_v.append(v_0) log_a.append(a_0) log_j.append(j_0) count += 1 return log_p, log_v, log_a, log_j
def setup_OSQP(self,final_point): [nx, nu] = self._osqp_Bd.shape # Constraints umin = np.ones(nu)*0.3-self.u_hover umax = np.ones(nu)*0.9-self.u_hover xmin = np.array([0.28,-2.]) #TODO: Tune/consider when testing on hw xmax = np.array([ 5.0, 5.0]) # Sizes self.ns = nx # p_x, p_y, p_z, v_x, v_y, v_z self.nu = nu # f_x, f_y, f_z # Objective function Q = sparse.diags([5., .5]) QN = Q R = 4.0*sparse.eye(nu) # Initial and reference states x0 = np.array([1.0,0.0]) xr = np.array([final_point[2], 0.0]) # Prediction horizon N = int(self.rate*self.mpc_horizon) self._osqp_N = N # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) # - quadratic objective P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN, sparse.kron(sparse.eye(N), R)]).tocsc() # - linear objective q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr), np.zeros(N*nu)]) # - linear dynamics Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), self._osqp_Ad) Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), self._osqp_Bd) Aeq = sparse.hstack([Ax, Bu]) leq = np.hstack([-x0, np.zeros(N*nx)]) ueq = leq # - input and state constraints nf = 1 # do not add the first nf points to the constraints xminf = -np.ones(nx)*np.inf xmaxf = +np.ones(nx)*np.inf Aineq = sparse.eye((N+1)*nx + N*nu) #lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)]) #uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)]) lineq = np.hstack([np.kron(np.ones(nf), xminf), np.kron(np.ones(N+1-nf), xmin), np.kron(np.ones(N), umin)]) uineq = np.hstack([np.kron(np.ones(nf), xmaxf), np.kron(np.ones(N+1-nf), xmax), np.kron(np.ones(N), umax)]) # - OSQP constraints A = sparse.vstack([Aeq, Aineq]).tocsc() self._osqp_l = np.hstack([leq, lineq]) self._osqp_u = np.hstack([ueq, uineq]) Nx = (N+1)*nx Nu = N*nu if self.soft: Pdelta = sparse.kron(sparse.eye(N+1), self.D) P = sparse.block_diag([P,Pdelta]) qdelta = np.zeros(Nx) q = np.hstack([q,qdelta]) Adelta = sparse.csc_matrix(np.vstack([np.zeros((Nx,Nx)),np.eye(Nx),np.zeros((Nu,Nx))])) A = sparse.hstack([A, Adelta]) self.qp_size = P.shape[0] # Create an OSQP object self.prob = osqp.OSQP() # Setup workspace self.prob.setup(P, q, A, self._osqp_l, self._osqp_u, warm_start=True, verbose=False) self.first = True
def solve_loop(qp_matrices, solver='osqp', osqp_settings=None): """ Solve lasso optimization loop for all lambdas """ # Shorter name for qp_matrices qp = qp_matrices # Extract features n = qp.n print('n = %d and solver %s' % (n, solver)) # Get number of problems to solve n_prob = qp.q.shape[1] # Initialize time vector time = np.zeros(n_prob) # Initialize number of iterations vector niter = np.zeros(n_prob) if solver == 'osqp': # Setup OSQP m = osqp.OSQP() m.setup(qp.P, qp.q[:, 0], qp.A, qp.l, qp.u, **osqp_settings) for i in range(n_prob): q = qp.q[:, i] # Update linear cost m.update(q=q) # Solve results = m.solve() x = results.x y = results.y status = results.info.status_val niter[i] = results.info.iter time[i] = results.info.run_time # Check if status correct if status != m.constant('OSQP_SOLVED'): import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # DEBUG # solve with gurobi # prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp) # res = prob.solve(solver=mpbpy.GUROBI, verbose=False) # print('Norm difference OSQP-GUROBI %.3e' % # np.linalg.norm(x - res.x)) # import ipdb; ipdb.set_trace() elif solver == 'osqp_coldstart': # Setup OSQP m = osqp.OSQP() m.setup(qp.P, qp.q[:, 0], qp.A, qp.l, qp.u, **osqp_settings) for i in range(n_prob): q = qp.q[:, i] # Update linear cost m.update(q=q) # Solve results = m.solve() x = results.x y = results.y status = results.info.status_val niter[i] = results.info.iter time[i] = results.info.run_time # Check if status correct if status != m.constant('OSQP_SOLVED'): import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # DEBUG # solve with gurobi # prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u) # res = prob.solve(solver=mpbpy.GUROBI, verbose=False) # print('Norm difference OSQP-GUROBI %.3e' % # np.linalg.norm(x - res.x)) # import ipdb; ipdb.set_trace() # DEBUG print iterations per value of lambda # lambda_vals = np.logspace(-2, 2, 101)[::-1] # # import matplotlib.pylab as plt # plt.figure() # ax = plt.gca() # plt.plot(lambda_vals, niter) # ax.set_xlabel(r'$\lambda$') # ax.set_ylabel(r'iter') # plt.show(block=False) # import ipdb; ipdb.set_trace() elif solver == 'osqp_no_caching': for i in range(n_prob): # Setup OSQP m = osqp.OSQP() m.setup(qp.P, qp.q[:, i], qp.A, qp.l, qp.u, **osqp_settings) # Solve results = m.solve() x = results.x y = results.y status = results.info.status_val niter[i] = results.info.iter time[i] = results.info.run_time # Check if status correct if status != m.constant('OSQP_SOLVED'): import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # DEBUG # solve with gurobi # prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp) # res = prob.solve(solver=mpbpy.GUROBI, verbose=False) # print('Norm difference OSQP-GUROBI %.3e' % # np.linalg.norm(x - res.x)) # import ipdb; ipdb.set_trace() elif solver == 'qpoases': n_dim = qp.P.shape[0] # Number of variables m_dim = qp.A.shape[0] # Number of constraints without bounds # Initialize qpoases and set options qpoases_m = qpoases.PyQProblem(n_dim, m_dim) options = qpoases.PyOptions() options.printLevel = qpoases.PyPrintLevel.NONE qpoases_m.setOptions(options) # Setup matrix P and A P = np.ascontiguousarray(qp.P.todense()) A = np.ascontiguousarray(qp.A.todense()) for i in range(n_prob): # Get linera cost as contiguous array q = np.ascontiguousarray(qp.q[:, i]) # Reset cpu time qpoases_cpu_time = np.array([10.]) # Reset number of of working set recalculations nWSR = np.array([1000]) if i == 0: # First iteration res_qpoases = qpoases_m.init(P, q, A, np.ascontiguousarray(qp.lx), np.ascontiguousarray(qp.ux), np.ascontiguousarray(qp.l), np.ascontiguousarray(qp.u), nWSR, qpoases_cpu_time) else: # Solve new hot started problem res_qpoases = qpoases_m.hotstart(q, np.ascontiguousarray(qp.lx), np.ascontiguousarray(qp.ux), np.ascontiguousarray(qp.l), np.ascontiguousarray(qp.u), nWSR, qpoases_cpu_time) # # DEBUG Solve with gurobi # qpoases solution # sol_qpoases = np.zeros(n + k) # qpoases_m.getPrimalSolution(sol_qpoases) # import mathprogbasepy as mpbpy # Agrb = spa.vstack((qp.A, # spa.hstack((spa.eye(n), spa.csc_matrix((n, k))) # ))).tocsc() # lgrb = np.append(qp.l, qp.lx) # ugrb = np.append(qp.u, qp.ux) # prob = mpbpy.QuadprogProblem(spa.csc_matrix(qp.P), q, # Agrb, lgrb, ugrb) # res = prob.solve(solver=mpbpy.GUROBI, verbose=True) # print("Norm difference x qpoases - GUROBI = %.4f" % # np.linalg.norm(sol_qpoases - res.x)) # print("Norm difference objval qpoases - GUROBI = %.4f" % # abs(qpoases_m.getObjVal() - res.obj_val)) # import ipdb; ipdb.set_trace() # if res_qpoases != 0: # raise ValueError('qpoases did not solve the problem!') # Save time time[i] = qpoases_cpu_time[0] # Save number of iterations niter[i] = nWSR[0] elif solver == 'gurobi': for i in range(n_prob): # Get linera cost as contiguous array q = qp.q[:, i] # Solve with gurobi prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u) res = prob.solve(solver=mpbpy.GUROBI, verbose=False) # Save time time[i] = res.cputime # Save number of iterations niter[i] = res.total_iter elif solver == 'mosek': for i in range(n_prob): # Get linera cost as contiguous array q = qp.q[:, i] # Solve with mosek prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u) res = prob.solve(solver=mpbpy.MOSEK, verbose=False) # Save time time[i] = res.cputime # Save number of iterations niter[i] = res.total_iter elif solver == 'ecos': n_var = qp_matrices.A_lasso.shape[1] m_var = qp_matrices.A_lasso.shape[0] for i in range(n_prob): if n_var <= 60: # (problem becomes too big otherwise): # Model with CVXPY # minimize y' * y + lambda * 1' * t # subject to y = Ax - b # -t <= x <= t lambda_i = qp_matrices.lambdas[i] x = cvxpy.Variable(n_var) y = cvxpy.Variable(m_var) t = cvxpy.Variable(n_var) objective = cvxpy.Minimize( cvxpy.quad_form(y, spa.eye(m_var)) + lambda_i * np.ones(n_var) * t) constraints = [ y == qp_matrices.A_lasso * x - qp_matrices.b_lasso, -t <= x, x <= t ] problem = cvxpy.Problem(objective, constraints) problem.solve(solver=cvxpy.ECOS, verbose=False) # DEBUG: Solve with MOSEK q = qp.q[:, i] # Solve with mosek # prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u) # res = prob.solve(solver=mpbpy.MOSEK, verbose=False) # x_mosek = res.x[:n_var] # import ipdb; ipdb.set_trace() # Obtain time and number of iterations time[i] = problem.solver_stats.setup_time + \ problem.solver_stats.solve_time niter[i] = problem.solver_stats.num_iters else: time[i] = 0 niter[i] = 0 else: raise ValueError('Solver not understood') # Return statistics return utils.Statistics(time), utils.Statistics(niter)
def __init__(self): # output_topic = rospy.get_param('~output_topic') # subscribers and publishers trigger_topic = rospy.get_param('~trigger_topic') self.mpc_trigger_sub = rospy.Subscriber(trigger_topic, Empty, self.osqp_trigger) # model_yaml = rospy.get_param('~model_yaml') # Prediction horizon # N = 10 N = rospy.get_param('~N') self.N = N dt = rospy.get_param('~dt') # dt = 0.25; # Assuming each increment is 0.25 seconds long Ad_yaml = rospy.get_param('~Ad') Ad_data = Ad_yaml['data'] print(Ad_data) Ad = Ad_data for i in range(len(Ad_data)): # print("Ad_data[{}] = {}").format(i, Ad_data[i]) if isinstance(Ad_data[i], str): Ad[i] = eval(Ad_data[i]) # print("Ad[{}] = {}").format(i, Ad[i]) test_mat = self.yaml_matrix2csc(Ad, Ad_yaml['rows'], Ad_yaml['rows']) print(test_mat) # Discrete time model of a 1D quadcopter self.Ad = sparse.csc_matrix([[1.0, dt, dt * dt / 2], [0.0, 1.0, dt], [0.0, 0.0, 1.0]]) self.Bd = sparse.csc_matrix([[0.0], [0.0], [1.0]]) [nx, nu] = self.Bd.shape self.nx = nx self.nu = nu # Objective function Q = sparse.diags([1.0, 1.0, 0.0]) # Q = 1.0*sparse.eye(nx) QN = Q # Q at horizon N R = 0.1 * sparse.eye(self.nu) # Constraints u0 = 0.0 umin = np.array([-1.0]) - u0 umax = np.array([1.0]) - u0 xmin = np.array([-np.inf, -np.inf, -np.inf]) xmax = np.array([np.inf, np.inf, np.inf]) # Initial and reference states self.x0 = np.zeros(3) xr = np.array([1.0, 0.0, 0.0]) # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) # - quadratic objective P = sparse.block_diag( [sparse.kron(sparse.eye(N), Q), QN, sparse.kron(sparse.eye(N), R)]).tocsc() # - linear objective # Stack all q = np.hstack([ np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr), np.zeros(N * self.nu) ]) # - linear dynamics Ax = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx)) + sparse.kron( sparse.eye(N + 1, k=-1), self.Ad) Bu = sparse.kron( sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), self.Bd) Aeq = sparse.hstack([Ax, Bu]) leq = np.hstack([-self.x0, np.zeros(N * nx)]) ueq = leq # - input and state constraints Aineq = sparse.eye((N + 1) * nx + N * self.nu) lineq = np.hstack( [np.kron(np.ones(N + 1), xmin), np.kron(np.ones(N), umin)]) uineq = np.hstack( [np.kron(np.ones(N + 1), xmax), np.kron(np.ones(N), umax)]) # - OSQP constraints A = sparse.vstack([Aeq, Aineq]).tocsc() self.l = np.hstack([leq, lineq]) self.u = np.hstack([ueq, uineq]) # Create an OSQP object self.osqp_solver = osqp.OSQP() # Setup workspace self.osqp_solver.setup(P, q, A, self.l, self.u, warm_start=True) while not rospy.is_shutdown(): rospy.spin()
def test_issue14(self): m = osqp.OSQP() m.setup(self.P, self.q, self.A, self.l, self.u, linsys_solver="mkl pardiso") m.solve()
def forward_pass(self): x = np.copy(self.initial_state) feasible = False trust_region_scale = 1 while not feasible: feasible = True current_J = 0 x_new_trajectories = np.zeros( (self.system.state_size, self.horizon + 1)) u_new_trajectories = np.zeros( (self.system.control_size, self.horizon)) x = np.copy(self.initial_state) x_new_trajectories[:, 0] = np.copy(x) for i in range(self.horizon): delta_x = x - self.x_trajectories[:, i] x_new_trajectories[:, i] = np.copy(x) Q_ux = self.Q_UX[:, :, i] Q_u = self.Q_U[:, i] P = sparse.csr_matrix(self.Q_UU[:, :, i]) q = (Q_ux.dot(delta_x) + Q_u) '''lb = -self.system.control_bound - self.u_trajectories[:, i] ub = self.system.control_bound - self.u_trajectories[:, i] lb *= trust_region_scale ub *= trust_region_scale''' #constraint_A = sparse.csr_matrix(np.identity(self.system.control_size)) #initialize contraint matrix and bound constraint_A = np.zeros( (self.system.control_size + len(self.constraints), self.system.control_size)) lb = np.zeros(self.system.control_size + len(self.constraints)) ub = np.zeros(self.system.control_size + len(self.constraints)) #control limit contraint constraint_A[0:self.system.control_size, 0:self.system.control_size] = np.identity( self.system.control_size) lb[0:self.system.control_size] = 0 - self.u_trajectories[:, i] ub[0:self.system. control_size] = self.system.control_bound - self.u_trajectories[:, i] lb *= trust_region_scale ub *= trust_region_scale #formulate linearized state constraints f_x, f_u = self.system.transition_J(x, self.u_trajectories[:, i]) constraint_index = self.system.control_size for constraint in self.constraints: if i <= self.horizon - 2: #current action might cause state constraint violation x_temp = self.system.transition( x, self.u_trajectories[:, i]) D = constraint.evaluate_constraint(x_temp) #print("constraint eval", D, i, x) C = constraint.evaluate_constraint_J(x_temp) #print(C.shape, f_u.shape) C = C.dot(f_u) constraint_A[constraint_index, :] = np.copy(C) lb[constraint_index] = -np.inf #no lower bound ub[constraint_index] = -D constraint_index += 1 constraint_A = sparse.csr_matrix(constraint_A) prob = osqp.OSQP() prob.setup(P, q, constraint_A, lb, ub, alpha=1.0, verbose=False) res = prob.solve() if res.info.status != 'solved': feasible = False #print("infeasible, reduce trust region") trust_region_scale *= 0.5 break delta_u = res.x[0:self.system.control_size] u = delta_u + self.u_trajectories[:, i] u_new_trajectories[:, i] = np.copy(u) # print(u) current_J += self.system.calculate_cost(x, u) x = self.system.transition(x, u) x_new_trajectories[:, self.horizon] = np.copy(x) current_J += self.system.calculate_final_cost(x) if current_J > self.best_J + 100: feasible = False trust_region_scale *= 0.5 else: self.best_J = current_J if feasible == True: self.x_trajectories = np.copy(x_new_trajectories) self.u_trajectories = np.copy(u_new_trajectories) print("total cost", current_J)
def compute_min_derivative_multispline(order, min_derivative_order, continuity_order, traj_waypoints): num_segments = len(traj_waypoints) - 1 if num_segments < 2: return None cw = order + 1 # constraint width x_dim = cw * num_segments # num columns in the constraint matrix multi_x_dim = x_dim * traj_waypoints[0].ndim Aeq = np.zeros((0, 0)) # equality constraints A matrix Aieq = np.zeros((0, 0)) # inequality constraints A matrix beq = np.zeros((0, 1)) bieq = np.zeros((0, 1)) H = np.zeros((0, 0)) for dim in range(traj_waypoints[0].ndim): pins = [wp.spline_pins[dim] for wp in traj_waypoints] dAeq, dbeq, dAieq, dbieq, dH = compute_spline_matrices( order, min_derivative_order, continuity_order, pins) Aeq = linalg.block_diag(Aeq, dAeq) Aieq = linalg.block_diag(Aieq, dAieq) H = linalg.block_diag(H, dH) beq = np.vstack((beq, dbeq)) bieq = np.vstack((bieq, dbieq)) # build directional constraints (constraints between spline dimensions) for seg in range(num_segments): wp = traj_waypoints[seg] for sdc in wp.soft_directional_constraints: con_order = sdc[0] dvec = sdc[1] radius = sdc[2] dspace = np.zeros((wp.ndim, wp.ndim)) dspace[0, :] = np.array(dvec) nspace = linalg.null_space(dspace) nullspace_vecs = list(nspace.transpose()) # Positive dot product enforces correct direction of motion new_constraint = np.zeros((1, multi_x_dim)) for d in range(wp.ndim): scalar = dvec[d] tvec = _calc_tvec(0, order, con_order) vec = scalar * tvec new_constraint[0, x_dim * d + seg * cw:x_dim * d + (seg + 1) * cw] = vec Aieq = np.vstack((Aieq, -1 * new_constraint)) bieq = np.vstack((bieq, 0)) # Limit motion in null space: for v in nullspace_vecs: new_constraint = np.zeros((1, multi_x_dim)) for d in range(wp.ndim): scalar = v[d] tvec = _calc_tvec(0, order, con_order) vec = scalar * tvec new_constraint[0, x_dim * d + seg * cw:x_dim * d + (seg + 1) * cw] = vec Aieq = np.vstack((Aieq, new_constraint)) Aieq = np.vstack((Aieq, -1 * new_constraint)) bieq = np.vstack((bieq, radius)) bieq = np.vstack((bieq, -radius)) # Convert equality constraints to inequality constraints: Aeq_ieq = np.vstack((Aeq, -1 * Aeq)) beq_ieq = np.vstack((beq, -1 * beq)) Aieq = np.vstack((Aieq, Aeq_ieq)) bieq = np.vstack((bieq, beq_ieq)) # Solve the QP! m = osqp.OSQP() try: m.setup(P=sparse.csc_matrix(H), q=None, l=None, A=sparse.csc_matrix(Aieq), u=bieq, verbose=False) except ValueError as ve: print(ve.message) print("Could not setup QP!") return None results = m.solve() x = results.x splines = [] xwidth = len(x) / traj_waypoints[0].ndim for dim in range(traj_waypoints[0].ndim): dx = x[dim * xwidth:dim * xwidth + xwidth] coefficients = np.fliplr( np.array(dx).reshape((num_segments, order + 1))) ts = [wp.time for wp in traj_waypoints] spline = OptimalSpline(coefficients.transpose(), ts) splines.append(spline) return splines
def mpc_(Ad_mat, Bd_mat, gd_mat, x_vec, Xr, Q, QN, R, N, lb_x, ub_x, lb_y, ub_y, umin, umax): # ========== Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) ========== nx = Ad_mat.shape[0] nu = Bd_mat.shape[1] Ad = sparse.csc_matrix(Ad_mat) Bd = sparse.csc_matrix(Bd_mat) gd = np.squeeze(gd_mat, axis=1) # from (N,1) to (N,) # ----- quadratic objective ----- P = sparse.block_diag( [sparse.kron(sparse.eye(N), Q), QN, sparse.kron(sparse.eye(N), R)]).tocsc() # ----- linear objective ----- # xr_vec = np.squeeze(xr, axis=1) # q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr), # np.zeros(N*nu)]) # q = np.hstack([np.kron(np.ones(N), -Q.dot(xr_vec)), -QN.dot(xr_vec), # np.zeros(N*nu)]) q = -Q.dot(Xr[:, 0]) # index 0 for ii in range(N - 1): q = np.hstack([q, -Q.dot(Xr[:, ii + 1])]) # index 1 ~ N-1 q = np.hstack([q, -QN.dot(Xr[:, -1]), np.zeros(N * nu)]) # index N # ----- linear dynamics ----- Ax = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx)) + sparse.kron( sparse.eye(N + 1, k=-1), Ad) Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd) Aeq = sparse.hstack([Ax, Bu]) # leq = np.hstack([-x0, np.zeros(N*nx)]) # leq = np.hstack([-x0, np.kron(np.ones(N), -gd)]) leq = np.hstack([-x_vec, np.kron(np.ones(N), -gd)]) ueq = leq # ----- input and state constraints ----- Aineq = sparse.eye((N + 1) * nx + N * nu) lineq = [] uineq = [] # lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)]) # uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)]) for i in range(len(lb_x)): xmin = [lb_x[i], lb_y[i], -10, -np.pi] xmax = [ub_x[i], ub_y[i], 10, np.pi] lineq = np.hstack([lineq, xmin]) uineq = np.hstack([uineq, xmax]) lineq = np.hstack([lineq, np.kron(np.ones(N), umin)]) uineq = np.hstack([uineq, np.kron(np.ones(N), umax)]) # ----- OSQP constraints ----- A = sparse.vstack([Aeq, Aineq]).tocsc() lb = np.hstack([leq, lineq]) ub = np.hstack([ueq, uineq]) # ==========Create an OSQP object and Setup workspace ========== prob = osqp.OSQP() prob.setup(P, q, A, lb, ub, verbose=False, warm_start=True) # verbose: print output. # Solve res = prob.solve() return res
def mpc__(Ad_list, Bd_list, gd_list, x_vec, Xr, Q, QN, R, N, xmin, xmax, umin, umax): """ Initialize Nonlinear Dynamics with Shooting Method """ # ========== Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) ========== nx = Ad_list[0].shape[0] nu = Bd_list[0].shape[1] # ----- quadratic objective ----- P = sparse.block_diag( [sparse.kron(sparse.eye(N), Q), QN, sparse.kron(sparse.eye(N), R)]).tocsc() # ----- linear objective ----- # xr_vec = np.squeeze(xr, axis=1) # q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr), # np.zeros(N*nu)]) # q = np.hstack([np.kron(np.ones(N), -Q.dot(xr_vec)), -QN.dot(xr_vec), # np.zeros(N*nu)]) q = -Q.dot(Xr[:, 0]) # index 0 for ii in range(N - 1): q = np.hstack([q, -Q.dot(Xr[:, ii + 1])]) # index 1 ~ N-1 q = np.hstack([q, -QN.dot(Xr[:, -1]), np.zeros(N * nu)]) # index N # ----- linear dynamics ----- Ax_Ad = sparse.csc_matrix(Ad_list[0]) Ax_diag = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx)) Bu_Bd = sparse.csc_matrix(Bd_list[0]) for i in range(N - 1): Ad = sparse.csc_matrix(Ad_list[i + 1]) Bd = sparse.csc_matrix(Bd_list[i + 1]) Ax_Ad = sparse.block_diag([Ax_Ad, Ad]) Bu_Bd = sparse.block_diag([Bu_Bd, Bd]) Ax_Ad_top = sparse.kron(np.ones(N + 1), np.zeros((nx, nx))) Ax_Ad_side = sparse.kron(np.ones((N, 1)), np.zeros((nx, nx))) Ax = Ax_diag + sparse.vstack( [Ax_Ad_top, sparse.hstack([Ax_Ad, Ax_Ad_side])]) Bu_Bd_top = sparse.kron(np.ones(N), np.zeros((nx, nu))) Bu = sparse.vstack([Bu_Bd_top, Bu_Bd]) Aeq = sparse.hstack([Ax, Bu]) leq = -x_vec # later ueq == leq for i in range(N): gd = np.squeeze(gd_list[i], axis=1) # from (N,1) to (N,) leq = np.hstack([leq, -gd]) ueq = leq # Original Code # Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad_list[0]) # Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd_list[0]) # Aeq = sparse.hstack([Ax, Bu]) # gd = np.squeeze(gd_list[0], axis=1) # from (N,1) to (N,) # leq = np.hstack([-x_vec, np.kron(np.ones(N), -gd)]) # ueq = leq # ----- input and state constraints ----- Aineq = sparse.eye((N + 1) * nx + N * nu) lineq = np.hstack( [np.kron(np.ones(N + 1), xmin), np.kron(np.ones(N), umin)]) uineq = np.hstack( [np.kron(np.ones(N + 1), xmax), np.kron(np.ones(N), umax)]) # lineq = [] # uineq = [] # for i in range(len(lb_x)): # xmin = [lb_x[i], lb_y[i], -10, -np.pi] # xmax = [ub_x[i], ub_y[i], 10, np.pi] # lineq = np.hstack([lineq, xmin]) # uineq = np.hstack([uineq, xmax]) # lineq = np.hstack([lineq, np.kron(np.ones(N), umin)]) # uineq = np.hstack([uineq, np.kron(np.ones(N), umax)]) # ----- OSQP constraints ----- A = sparse.vstack([Aeq, Aineq]).tocsc() lb = np.hstack([leq, lineq]) ub = np.hstack([ueq, uineq]) # ==========Create an OSQP object and Setup workspace ========== prob = osqp.OSQP() prob.setup(P, q, A, lb, ub, verbose=False, warm_start=True) # verbose: print output. # Solve res = prob.solve() return res
def mpc(A, B, C, D, F0, G0, xk, ek, uk1, yrk1, xmax, xmin, umax, umin, dumax, dumin, ymax, ymin, yrmax, yrmin, dyrmax, dyrmin, qx, qe, qu, qyr, qxf, qef, zmax, zmin, Cz, Dz, Gz, N= 10, dT = 250e-3): # ========================================================================================= # Calculos # Calcular cantidad de elementos. [nx, nu] = B.shape [ny, nx] = C.shape [nz, nx] = Cz.shape # ------------------------------------------------------------------------- #Calcular las matrices del estado aumentado A_ = sparse.vstack([ sparse.hstack([A, sparse.csc_matrix((nx, ny))]), sparse.hstack([dT * C, sparse.eye(ny)]) ], format='csc') B_ = sparse.vstack([ sparse.hstack([B, sparse.csc_matrix((nx, ny))]), sparse.hstack([dT * D, -1 * dT * sparse.eye(ny)]) ], format='csc') #Condiciones iniciales del estado aumentado xk_ = np.hstack([xk, ek]) #Calcular tamaño del estado aumentado [nx_, nu_] = B_.shape #Calcular las matrices de las variables algebraicas considerando el estado aumentado. Cz_ = sparse.hstack([Cz, sparse.csc_matrix((nz, ny))], format = 'csc') Dz_ = sparse.hstack([Dz, sparse.csc_matrix((nz, ny))], format = 'csc') # ------------------------------------------------------------------------- #Generar las matrices de pesos. Q = sparse.diags(np.hstack([qx, qe]), format='csc') R = sparse.diags(np.hstack([qu, qyr]), format='csc') # Qf = computeQf(A_, B_, Q, R) # Calcular la ganancia de la condición final tal que cumpla la ecuación de Ricatti. Qf = sparse.diags(np.hstack([qxf, qef]), format='csc') #Assigned directly. # Input diferences # dU = Kdu * u # Kdu = sparse.kron(sparse.hstack([sparse.csc_matrix((N-1, 1)), sparse.eye(N-1)]), sparse.eye(nu_)) - sparse.kron(sparse.hstack([sparse.eye(N-1), sparse.csc_matrix((N-1, 1))]), sparse.eye(nu_)) # # Rdu = Kdu.T * R * Kdu # Rdu = Kdu.transpose() * sparse.kron(sparse.eye(N-1), R ) * Kdu # Plantear problema de optimización # Problema de la forma : 0.5 xT P x + qT x # - quadratic objective qH = np.arange(1, N+1) / N # P = sparse.block_diag([sparse.kron(sparse.diags(qH), Q), #Estados durante el horizonte de predicción # Qf, # Estados al final del horizonte de predicción # Rdu], format='csc') #Cambio en las entradas durante el horizonte de predicción P = sparse.block_diag([sparse.kron(sparse.diags(qH), Q), #Estados durante el horizonte de predicción Qf, # Estados al final del horizonte de predicción sparse.kron(sparse.eye(N), R)], format='csc') #Entradas durante el horizonte de predicción # - linear objective q = np.zeros( (N+1)*nx_ + N*nu_) #============================================================================== #Restrictions #------------------------------------------------------------------------------ #Equality restriction #------------------------------------------------------------------------------ #linear dynamics. # Aeq * [x, e, u, yr].T Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx_)) + sparse.kron(sparse.eye(N+1, k=-1), A_) Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), B_) Aeq = sparse.hstack([Ax, Bu]) bk = - 1 * np.hstack([xk, ek, np.zeros(N*nx_)]) b0 = - 1 * np.hstack([np.zeros(nx_), np.kron(np.ones(N), np.hstack([F0, dT * G0]))]) b = b0 + bk #Relajar un poco las restricciones de igualdad leq = b - 0.05 * abs(b) ueq = b + 0.05 * abs(b) #------------------------------------------------------------------------------ #Inequality restriction #------------------------------------------------------------------------------ # - Bounds!!!!! xmin_ = np.hstack([xmin,np.kron(np.ones(ny), -np.inf)]) xmax_ = np.hstack([xmax,np.kron(np.ones(ny), np.inf)]) umin_ = np.hstack([umin, yrmin]) umax_ = np.hstack([umax, yrmax]) ABound = sparse.eye((N+1)*nx_ + N*nu_) lBound= np.hstack([np.kron(np.ones(N+1), xmin_), np.kron(np.ones(N), umin_)]) uBound = np.hstack([np.kron(np.ones(N+1), xmax_), np.kron(np.ones(N), umax_)]) #Maximum rate of change dumin_ = np.hstack([dumin, dyrmin]) dumax_ = np.hstack([dumax, dyrmax]) uk1_ = np.hstack([uk1, yrk1]) #Maximum rate of change Au = sparse.hstack([sparse.csc_matrix((N*nu_, (N+1)*nx_)), sparse.kron(sparse.eye(N), sparse.eye(nu_)) + sparse.kron(sparse.eye(N, k=-1), -1 * sparse.eye(nu_))]) ldu = np.hstack([dumin_ + uk1_, np.kron(np.ones(N-1), dumin_)]) udu = np.hstack([dumax_ + uk1_, np.kron(np.ones(N-1), dumax_)]) # Algebraic variables Az = sparse.hstack([ sparse.kron(sparse.hstack([sparse.eye(N), sparse.csc_matrix((N, 1))]), Cz_), sparse.kron(sparse.eye(N), Dz_) ]) lz = np.kron(np.ones(N), zmin) - np.kron(np.ones(N), Gz) uz = np.kron(np.ones(N), zmax) - np.kron(np.ones(N), Gz) #------------------------------------------------------------------------------ # - Solver constraints #In general, the contraints have the form: # lb <= Ax <= ub Aconst = sparse.vstack([Aeq, ABound, Au, Az], format='csc') lb = np.hstack([leq, lBound, ldu, lz]) ub = np.hstack([ueq, uBound, udu, uz]) #Condiciones iniciales para las variables de optimización [x, e, u] a lo largo de todo el horizonte de predicción. _Xk_ = np.hstack ([np.kron(np.ones(N+1), xk_),np.kron(np.ones(N), uk1_)]) # Create an OSQP object prob = osqp.OSQP() # ------------------------------------ # Setup workspace #eps_abs: Absolute tolerance # eps_prim_inf: Primal infeasibility tolerance # eps_dual_inf: Dual infeasibility tolerance prob.setup(P, q, Aconst, lb, ub, verbose = False, polish = True, eps_abs = 1e-6, eps_prim_inf=1e-5, eps_dual_inf = 1e-03) # ------------------------------------ #Start using initial conditions. prob.warm_start(x = _Xk_) #Solve problem res = prob.solve() # Check solver status if res.info.status not in ['solved', 'solved inaccurate']: # The inaccurate status define when the optimality, primal infeasibility or dual infeasibility conditions are satisfied with tolerances 10 times largerthan the ones set. raise ValueError('OSQP did not solve the problem!') # Get firts inputs of the augmented state. _uk_ = res.x[-N*nu_:-(N-1)*nu_] uk = _uk_[:nu] yrk = _uk_[-ny:] # output = res.x[:N*nx_] # output = output.reshape((N, nx_)) # plt.plot(output[:, 28]) # plt.show() z = (Az * res.x + np.kron(np.ones(N), Gz))[-N*nz:-(N-1)*nz] return uk, yrk, z
def compute_min_derivative_spline(order, min_derivative_order, continuity_order, waypoints): num_segments = len(waypoints) - 1 if num_segments < 1: return None assert not any([wp.time is None for wp in waypoints]) waypoints.sort(key=lambda waypoint: waypoint.time) durations = [0] * num_segments for i in range(len(durations)): durations[i] = waypoints[i + 1].time - waypoints[i].time # Start with waypoint constraints: (Start of each segment, end of last segment.) # x is vertical stack of the coefficient col vectors for each segment cw = order + 1 # constraint width x_dim = cw * (num_segments) # num columns in the constraint matrix Aeq = np.zeros((0, x_dim)) # equality constraints A matrix Aieq = np.zeros((0, x_dim)) # inequality constraints A matrix beq = np.zeros((0, 1)) bieq = np.zeros((0, 1)) for seg, wp in enumerate(waypoints): for con in wp.hard_constraints: if seg == num_segments: tvec = _calc_tvec(durations[seg - 1], order, con[0]) i = seg - 1 else: tvec = _calc_tvec(0, order, con[0]) i = seg constraint = np.zeros(x_dim) # hard constraint at waypoint constraint[i * cw:(i + 1) * cw] = tvec Aeq = np.vstack((Aeq, constraint)) beq = np.vstack((beq, con[1])) for con in wp.soft_constraints: # voxel constraints around waypoint if seg == num_segments: tvec = _calc_tvec(durations[seg - 1], order, con[0]) i = seg - 1 else: tvec = _calc_tvec(0, order, con[0]) i = seg constraint_max = np.zeros(x_dim) constraint_max[i * cw:(i + 1) * cw] = tvec constraint_min = np.zeros(x_dim) constraint_min[i * cw:(i + 1) * cw] = -1 * tvec Aieq = np.vstack((Aieq, constraint_max, constraint_min)) bieq = np.vstack((bieq, con[1] + con[2], -(con[1] - con[2]))) # Continuity constraints: (tvec_a(t=end) = tvec_b(t=0)) for seg in range(0, num_segments - 1): for r in range(0, continuity_order + 1): constraint = np.zeros(x_dim) # hard constraint at waypoint tvec_end = _calc_tvec(durations[seg], order, r) tvec_start = _calc_tvec(0, order, r) constraint[seg * cw:(seg + 1) * cw] = tvec_end constraint[(seg + 1) * cw:(seg + 2) * cw] = -tvec_start Aeq = np.vstack((Aeq, constraint)) beq = np.vstack((beq, [0])) # Construct Hermitian matrix: H = np.zeros((x_dim, x_dim)) for seg in range(0, num_segments): Q = _compute_Q(order, min_derivative_order, 0, durations[seg]) H[cw * seg:cw * (seg + 1), cw * seg:cw * (seg + 1)] = Q c = np.zeros((x_dim, 1)) # Convert equality constraints to inequality constraints: Aeq_ieq = np.vstack((Aeq, -1 * Aeq)) beq_ieq = np.vstack((beq, -1 * beq)) Aieq = np.vstack((Aieq, Aeq_ieq)) bieq = np.vstack((bieq, beq_ieq)) # Solve the QP! m = osqp.OSQP() m.setup(P=sparse.csc_matrix(H), q=None, l=None, A=sparse.csc_matrix(Aieq), u=bieq, verbose=False) results = m.solve() x = results.x coefficients = np.fliplr(np.array(x).reshape((num_segments, order + 1))) ts = [wp.time for wp in waypoints] return OptimalSpline(coefficients.transpose(), ts)
def _init_problem(self): """ Initialize optimization problem for current time step. """ # Constraints umin = self.input_constraints['umin'] umax = self.input_constraints['umax'] xmin = self.state_constraints['xmin'] xmax = self.state_constraints['xmax'] # LTV System Matrices A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1))) B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N))) # Reference vector for state and input variables ur = np.zeros(self.nu*self.N) xr = np.zeros(self.nx*(self.N+1)) # Offset for equality constraint (due to B * (u - ur)) uq = np.zeros(self.N * self.nx) # Dynamic state constraints xmin_dyn = np.kron(np.ones(self.N + 1), xmin) xmax_dyn = np.kron(np.ones(self.N + 1), xmax) # Dynamic input constraints umax_dyn = np.kron(np.ones(self.N), umax) # Get curvature predictions from previous control signals kappa_pred = np.tan(np.array(self.current_control[3::] + self.current_control[-1:])) / self.model.length # Iterate over horizon for n in range(self.N): # Get information about current waypoint current_waypoint = self.model.reference_path.get_waypoint( self.model.wp_id + n) next_waypoint = self.model.reference_path.get_waypoint( self.model.wp_id + n + 1) delta_s = next_waypoint - current_waypoint kappa_ref = current_waypoint.kappa v_ref = current_waypoint.v_ref # Compute LTV matrices f, A_lin, B_lin = self.model.linearize(v_ref, kappa_ref, delta_s) A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin # Set reference for input signal ur[n*self.nu:(n+1)*self.nu] = np.array([v_ref, kappa_ref]) # Compute equality constraint offset (B*ur) uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array ([v_ref, kappa_ref])) - f # Constrain maximum speed based on predicted car curvature vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n]) + 1e-12)) if vmax_dyn < umax_dyn[self.nu*n]: umax_dyn[self.nu*n] = vmax_dyn # Compute dynamic constraints on e_y ub, lb, _ = self.model.reference_path.update_path_constraints( self.model.wp_id+1, self.N, 2*self.model.safety_margin, self.model.safety_margin) xmin_dyn[0] = self.model.spatial_state.e_y xmax_dyn[0] = self.model.spatial_state.e_y xmin_dyn[self.nx::self.nx] = lb xmax_dyn[self.nx::self.nx] = ub # Set reference for state as center-line of drivable area xr[self.nx::self.nx] = (lb + ub) / 2 # Get equality matrix Ax = sparse.kron(sparse.eye(self.N + 1), -sparse.eye(self.nx)) + sparse.csc_matrix(A) Bu = sparse.csc_matrix(B) Aeq = sparse.hstack([Ax, Bu]) # Get inequality matrix Aineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu) # Combine constraint matrices A = sparse.vstack([Aeq, Aineq], format='csc') # Get upper and lower bound vectors for equality constraints lineq = np.hstack([xmin_dyn, np.kron(np.ones(self.N), umin)]) uineq = np.hstack([xmax_dyn, umax_dyn]) # Get upper and lower bound vectors for inequality constraints x0 = np.array(self.model.spatial_state[:]) leq = np.hstack([-x0, uq]) ueq = leq # Combine upper and lower bound vectors l = np.hstack([leq, lineq]) u = np.hstack([ueq, uineq]) # Set cost matrices P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN, sparse.kron(sparse.eye(self.N), self.R)], format='csc') q = np.hstack( [-np.tile(np.diag(self.Q.A), self.N) * xr[:-self.nx], -self.QN.dot(xr[-self.nx:]), -np.tile(np.diag(self.R.A), self.N) * ur]) # Initialize optimizer self.optimizer = osqp.OSQP() self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
def solve_loop(qp_matrices, problem, nsim, solver='osqp'): """ Solve MPC loop """ # Shorter name for qp_matrices qp = qp_matrices # Get dimensions (nx, nu) = problem.B.shape N = problem.N print('N = %d and solver %s' % (N, solver)) # Initialize time and number of iterations vectors time = np.zeros(nsim) niter = np.zeros(nsim) if solver == 'osqp': # Construct qp matrices if len(problem.xmin) > 0: # If the problem has state constraints Aosqp = spa.vstack([ qp.A, spa.eye((N + 1) * nx + N * nu), ]).tocsc() else: Aosqp = spa.vstack([ qp.A, spa.hstack( [spa.csc_matrix((N * nu, (N + 1) * nx)), spa.eye(N * nu)]), ]).tocsc() losqp = np.hstack([qp.l, qp.lx]) uosqp = np.hstack([qp.u, qp.ux]) # Initial state x0 = problem.x0 # Setup OSQP m = osqp.OSQP() m.setup( qp.P, qp.q, Aosqp, losqp, uosqp, # auto_rho=False, auto_rho=True, max_iter=2500, scaling=True, scaling_iter=50, polish=False, verbose=False) for i in range(nsim): # Solve with osqp res = m.solve() # Save time and number of iterations time[i] = res.info.run_time niter[i] = res.info.iter # Check if status is correct status = res.info.status_val if status != m.constant('OSQP_SOLVED'): # # Dump file to 'bad_convergence/data'folder # import pickle # problem = {'P': qp.P, # 'q': qp.q, # 'A': Aosqp, # 'l': losqp, # 'u': uosqp} # with open('bad_convergence/data/%s.pickle' % 'helicopter_scaling_large', 'wb') as f: # pickle.dump(problem, f) import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # Apply first control input to the plant u = res.x[-N * nu:-(N - 1) * nu] x0 = problem.A.dot(x0) + problem.B.dot(u) # Update linear constraints if len(problem.tmin) > 0: losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx]) uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux]) else: losqp = np.hstack([b(x0, nx, N), qp.lx]) uosqp = np.hstack([b(x0, nx, N), qp.ux]) m.update(l=losqp, u=uosqp) elif solver == 'osqp_coldstart': # Construct qp matrices if len(problem.xmin) > 0: # If the problem has state constraints Aosqp = spa.vstack([ qp.A, spa.eye((N + 1) * nx + N * nu), ]).tocsc() else: Aosqp = spa.vstack([ qp.A, spa.hstack( [spa.csc_matrix((N * nu, (N + 1) * nx)), spa.eye(N * nu)]), ]).tocsc() losqp = np.hstack([qp.l, qp.lx]) uosqp = np.hstack([qp.u, qp.ux]) # Initial state x0 = problem.x0 # Setup OSQP m = osqp.OSQP() m.setup( qp.P, qp.q, Aosqp, losqp, uosqp, warm_start=False, auto_rho=True, # auto_rho=False, rho=0.1, max_iter=2500, scaling_iter=50, polish=False, verbose=False) for i in range(nsim): # Solve with osqp res = m.solve() # Save time and number of iterations time[i] = res.info.run_time niter[i] = res.info.iter # Check if status is correct status = res.info.status_val if status != m.constant('OSQP_SOLVED'): import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # Apply first control input to the plant u = res.x[-N * nu:-(N - 1) * nu] x0 = problem.A.dot(x0) + problem.B.dot(u) # Update linear constraints if len(problem.tmin) > 0: losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx]) uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux]) else: losqp = np.hstack([b(x0, nx, N), qp.lx]) uosqp = np.hstack([b(x0, nx, N), qp.ux]) m.update(l=losqp, u=uosqp) elif solver == 'osqp_no_caching': # Construct qp matrices if len(problem.xmin) > 0: # If the problem has state constraints Aosqp = spa.vstack([ qp.A, spa.eye((N + 1) * nx + N * nu), ]).tocsc() else: Aosqp = spa.vstack([ qp.A, spa.hstack( [spa.csc_matrix((N * nu, (N + 1) * nx)), spa.eye(N * nu)]), ]).tocsc() losqp = np.hstack([qp.l, qp.lx]) uosqp = np.hstack([qp.u, qp.ux]) # Initial state x0 = problem.x0 for i in range(nsim): # Setup OSQP m = osqp.OSQP() m.setup( qp.P, qp.q, Aosqp, losqp, uosqp, warm_start=False, auto_rho=True, # auto_rho=False, rho=0.1, max_iter=2500, scaling_iter=50, polish=False, verbose=False) # Solve res = m.solve() # Save time and number of iterations time[i] = res.info.run_time niter[i] = res.info.iter # Check if status is correct status = res.info.status_val if status != m.constant('OSQP_SOLVED'): import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # Apply first control input to the plant u = res.x[-N * nu:-(N - 1) * nu] x0 = problem.A.dot(x0) + problem.B.dot(u) # Update linear constraints if len(problem.tmin) > 0: losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx]) uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux]) else: losqp = np.hstack([b(x0, nx, N), qp.lx]) uosqp = np.hstack([b(x0, nx, N), qp.ux]) m.update(l=losqp, u=uosqp) elif solver == 'qpoases': n_dim = qp.P.shape[0] # Number of variables m_dim = qp.A.shape[0] # Number of constraints without bounds # Initialize qpoases and set options qpoases_m = qpoases.PyQProblem(n_dim, m_dim) options = qpoases.PyOptions() options.printLevel = qpoases.PyPrintLevel.NONE qpoases_m.setOptions(options) # Construct bounds for qpoases lx = np.append(-np.inf * np.ones((N + 1) * nx), qp.lx) ux = np.append(np.inf * np.ones((N + 1) * nx), qp.ux) # Setup matrix P and A P = np.ascontiguousarray(qp.P.todense()) A = np.ascontiguousarray(qp.A.todense()) # Initial state x0 = problem.x0 # RHS of the linear equality constraints lqpoases = qp.l uqpoases = qp.u for i in range(nsim): # Reset cpu time qpoases_cpu_time = np.array([60.]) # Reset number of of working set recalculations nWSR = np.array([1e6]) if i == 0: # First iteration res_qpoases = qpoases_m.init(P, np.ascontiguousarray(qp.q), A, np.ascontiguousarray(lx), np.ascontiguousarray(ux), np.ascontiguousarray(lqpoases), np.ascontiguousarray(uqpoases), nWSR, qpoases_cpu_time) else: # Solve new hot started problem res_qpoases = qpoases_m.hotstart( np.ascontiguousarray(qp.q), np.ascontiguousarray(lx), np.ascontiguousarray(ux), np.ascontiguousarray(lqpoases), np.ascontiguousarray(uqpoases), nWSR, qpoases_cpu_time) if res_qpoases != 0: raise ValueError('qpoases did not solve the problem!') # Save time and number of iterations time[i] = qpoases_cpu_time[0] niter[i] = nWSR[0] # Get qpoases solution sol_qpoases = np.zeros((N + 1) * nx + N * nu) qpoases_m.getPrimalSolution(sol_qpoases) # Apply first control input to the plant u = sol_qpoases[-N * nu:-(N - 1) * nu] x0 = problem.A.dot(x0) + problem.B.dot(u) # Update linear constraints if len(problem.tmin) > 0: lqpoases = np.hstack([b(x0, nx, N), problem.tmin]) uqpoases = np.hstack([b(x0, nx, N), problem.tmax]) else: lqpoases = np.hstack([b(x0, nx, N)]) uqpoases = np.hstack([b(x0, nx, N)]) elif solver == 'gurobi' or solver == 'mosek': # Construct qp matrices if len(problem.xmin) > 0: # If the problem has state constraints Agurobi = spa.vstack([ qp.A, spa.eye((N + 1) * nx + N * nu), ]).tocsc() else: Agurobi = spa.vstack([ qp.A, spa.hstack( [spa.csc_matrix((N * nu, (N + 1) * nx)), spa.eye(N * nu)]), ]).tocsc() lgurobi = np.hstack([qp.l, qp.lx]) ugurobi = np.hstack([qp.u, qp.ux]) # Initial state x0 = problem.x0 for i in range(nsim): # Solve with gurobi prob = mpbpy.QuadprogProblem(qp.P, qp.q, Agurobi, lgurobi, ugurobi) if solver == 'gurobi': res = prob.solve(solver=mpbpy.GUROBI, verbose=False) else: res = prob.solve(solver=mpbpy.MOSEK, verbose=False) # Save time and number of iterations time[i] = res.cputime niter[i] = res.total_iter # Check if status is correct status = res.status if status != 'optimal': import ipdb ipdb.set_trace() raise ValueError('Gurobi did not solve the problem!') # Apply first control input to the plant u = res.x[-N * nu:-(N - 1) * nu] x0 = problem.A.dot(x0) + problem.B.dot(u) # Update QP problem if len(problem.tmin) > 0: lgurobi = np.hstack([b(x0, nx, N), problem.tmin, qp.lx]) ugurobi = np.hstack([b(x0, nx, N), problem.tmax, qp.ux]) else: lgurobi = np.hstack([b(x0, nx, N), qp.lx]) ugurobi = np.hstack([b(x0, nx, N), qp.ux]) else: raise ValueError('Solver not understood') # Return statistics return utils.Statistics(time), utils.Statistics(niter)
def __init__(self, linear_dynamics, N, dt, umin, umax, xmin, xmax, Q, R, QN, xr, plotMPC=False, plotMPC_filename="", lifting=False, edmd_object=Edmd(), name="noname", soft=False, D=None): """Create an MPC Controller object. Sizes: - N: number of timesteps for predictions - Nqd: number of timesteps of the desired trajectory - nx: number of states (original or lifted) - ns: number or original states - nu: number of control inputs Inputs: - initilized linear_dynamics, LinearSystemDynamics object. It takes Ac and Bc from it. - number of timesteps, N, integer - time step, dt, float - minimum control, umin, numpy 1d array [nu,] - maximum control, umax, numpy 1d array [nu,] - minimum state, xmin, numpy 1d array [ns,] - maximum state, xmax, numpy 1d array [ns,] - state cost matrix Q, sparse numpy 2d array [ns,ns]. In practice it is always diagonal. - control cost matrix, R, sparse numpy 2d array [nu,nu]. In practice it is always diagonal. - final state cost matrix, QN, sparse numpy 2d array [ns,ns]. In practice it is always diagonal. - reference state trajectory, xr, numpy 2d array [ns,Nqd] OR numpy 1d array [ns,] (Optional) - flag to plot MPC thoughts, plotMPC=False, boolean - filename to save the previosu plot, plotMPC_filename="", string - flag to use or not lifting, lifting=False, boolean - object to store the eDMD data, edmd_object=Edmd(). It has been initialized. s """ Controller.__init__(self, linear_dynamics) # Load arguments Ac, Bc = linear_dynamics.linear_system() [nx, nu] = Bc.shape ns = xr.shape[0] self.dt = dt self.plotMPC = plotMPC self.plotMPC_filename = plotMPC_filename self.verbose = False self.q_d = xr self.Q = Q self.R = R self.lifting = lifting self.soft = soft self.nu = nu self.nx = nx self.ns = ns #Discretize dynamics: self.dt = dt if lifting: self.C = edmd_object.C self.edmd_object = edmd_object else: self.C = sparse.eye(ns) lin_model_d = sp.signal.cont2discrete((Ac, Bc, self.C, zeros((ns, 1))), dt) Ad = sparse.csc_matrix( lin_model_d[0]) #TODO: If bad behavior, delete this Bd = sparse.csc_matrix( lin_model_d[1]) #TODO: If bad behavior, delete this # Total desired path if self.q_d.ndim == 2: self.Nqd = self.q_d.shape[1] xr = self.q_d[:, :N] # Prediction horizon self.N = N x0 = np.zeros(nx) self.run_time = np.zeros([ 0, ]) self.xr = self.q_d[:, :N] Rbd = sparse.kron(sparse.eye(N), R) Qbd = sparse.kron(sparse.eye(N), Q) Bbd = block_diag(Bd, nu).tocoo() # Check Xmin and Xmax if xmin.shape[ 0] == ns and xmin.ndim == 1: # it is a single vector we tile it x_min_flat = np.kron(np.ones(N), xmin) x_max_flat = np.kron(np.ones(N), xmax) elif xmin.shape[0] == ns * N: # if it is a long vector it is ok x_min_flat = xmin x_max_flat = xmax elif xmin.shape[0] == ns and xmin.shape[ 1] == N: # if it is a block we flatten it x_min_flat = np.reshape(xmin, (N * ns, ), order='F') x_max_flat = np.reshape(xmax, (N * ns, ), order='F') else: raise ValueError('xmin has wrong dimensions. xmin shape={}'.format( xmin.shape)) self.x_min_flat = x_min_flat self.x_max_flat = x_max_flat # Check Umin and Umax if umin.shape[0] == nu and umin.ndim == 1: u_min_flat = np.kron(np.ones(N), umin) u_max_flat = np.kron(np.ones(N), umax) elif umin.shape[0] == nu * N: u_min_flat = umin u_max_flat = umax elif umin.shape[0] == nu and umin.shape[1] == N: u_min_flat = np.reshape(umin, (N * nu, ), order='F') u_max_flat = np.reshape(umax, (N * nu, ), order='F') else: raise ValueError('umin has wrong dimensions. Umin shape={}'.format( umin.shape)) self.u_min_flat = u_min_flat self.u_max_flat = u_max_flat #! GET a & b # Write B: diag_AkB = Bd data_list = Bbd.data row_list = Bbd.row col_list = Bbd.col B = sparse.coo_matrix for i in range(N): if i < N - 1: AkB_bd_temp = block_diag(diag_AkB, N - i) else: AkB_bd_temp = diag_AkB.tocoo() data_list = np.hstack([data_list, AkB_bd_temp.data]) row_list = np.hstack([ row_list, AkB_bd_temp.row + np.full( (AkB_bd_temp.row.shape[0], ), nx * i) ]) col_list = np.hstack([col_list, AkB_bd_temp.col]) diag_AkB = Ad.dot(diag_AkB) B = sparse.coo_matrix((data_list, (row_list, col_list)), shape=(N * nx, N * nu)) a = Ad.copy() Ak = Ad.copy() for i in range(N - 1): Ak = Ak.dot(Ad) a = sparse.vstack([a, Ak]) self.a = a self.B = B # check_ab = True # if check_ab: # x0 = np.linspace(-5,40,nx) # x00 = np.linspace(-5,40,nx) # # Store data Init # nsim = N # xst = np.zeros((nx,nsim)) # ust = np.zeros((nu,nsim)) # # Simulate in closed loop # for i in range(nsim): # # Fake pd controller # ctrl = np.zeros(nu,) #np.random.rand(nu,) # x0 = Ad.dot(x0) + Bd.dot(ctrl) # # Store Data # xst[:,i] = x0 # ust[:,i] = ctrl # x_dense = np.reshape(a @ x00 + B @ (ust.flatten('F')),(N,nx)).T # plt.figure() # plt.subplot(2,1,1) # for i in range(nx): # plt.plot(range(nsim),xst[i,:],'d',label="sim "+str(i)) # plt.plot(range(nsim),x_dense[i,:],'d',label="ax+bu "+str(i)) # plt.xlabel('Time(s)') # plt.grid() # plt.legend() # plt.subplot(2,1,2) # for i in range(nu): # plt.plot(range(nsim),ust[i,:],label=str(i)) # plt.xlabel('Time(s)') # plt.grid() # plt.legend() # plt.savefig("AB_check_for_"+name+".png",bbox_inches='tight') # plt.close() # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) if (self.lifting): # Compute Block Diagonal elements self.Cbd = sparse.kron(sparse.eye(N), self.C) CQCbd = self.Cbd.T @ Qbd @ self.Cbd self.CtQ = self.C.T @ Q Cbd = self.Cbd P = Rbd + B.T @ CQCbd @ B self.BTQbda = B.T @ CQCbd @ a Aineq_x = Cbd @ B xrQB = B.T @ np.reshape(self.CtQ.dot(xr), (N * nx, ), order='F') l = np.hstack([x_min_flat - Cbd @ a @ x0, u_min_flat]) u = np.hstack([x_max_flat - Cbd @ a @ x0, u_max_flat]) else: # - quadratic objective P = Rbd + B.T @ Qbd @ B self.BTQbda = B.T @ Qbd @ a xrQB = B.T @ np.reshape(Q.dot(xr), (N * nx, ), order='F') Aineq_x = B l = np.hstack([x_min_flat - a @ x0, u_min_flat]) u = np.hstack([x_max_flat - a @ x0, u_max_flat]) x0aQb = self.BTQbda @ x0 self.Cbda = Cbd @ a self.BQxr = self.B.T @ np.reshape(self.CtQ.dot(self.xr), (N * nx, ), order='F') q = x0aQb - xrQB Aineq_u = sparse.eye(N * nu) A = sparse.vstack([Aineq_x, Aineq_u]).tocsc() if soft: Pdelta = sparse.kron(sparse.eye(N), D) P = sparse.block_diag([P, Pdelta]) qdelta = np.zeros(N * ns) q = np.hstack([q, qdelta]) Adelta = sparse.csc_matrix( np.vstack([np.eye(N * ns), np.zeros((N * nu, N * ns))])) A = sparse.hstack([A, Adelta]) # plot_matrices = True # if plot_matrices: # #! Visualize Matrices # fig = plt.figure() # fig.suptitle("QP Matrices to solve MP in dense form. N={}, nx={}, nu={}".format(N,nx,nu),fontsize=20) # plt.subplot(2,4,1,xlabel="Ns*(N+1)", ylabel="Ns*(N+1)") # plt.imshow(a.toarray(), interpolation='nearest', cmap=cm.Greys_r) # plt.title("a in $x=ax_0+bu$") # plt.subplot(2,4,2,xlabel="Ns*(N+1)", ylabel="Nu*N") # plt.imshow(B.toarray(), interpolation='nearest', cmap=cm.Greys_r) # plt.title("b in $x=ax_0+bu$") # plt.subplot(2,4,3,xlabel="ns*(N+1) + ns*(N+1) + nu*N", ylabel="Ns*(N+1)+Nu*N") # plt.imshow(A.toarray(), interpolation='nearest', cmap=cm.Greys_r) # plt.title("A total in $l\\leq Ax \\geq u$") # plt.subplot(2,4,4) # plt.imshow(P.toarray(), interpolation='nearest', cmap=cm.Greys_r) # plt.title("P in $J=u^TPu+q^Tu$") # plt.subplot(2,4,5) # plt.imshow(Qbd.toarray(), interpolation='nearest', cmap=cm.Greys_r) # plt.title("Qbd") # #! Visualize Vectors # plt.subplot(2,4,6) # plt.plot(l) # plt.title('l in $l\\leq Ax \\geq u$') # plt.grid() # plt.subplot(2,4,7) # plt.plot(u) # plt.title("l in $l\\leq Ax \\geq u$") # plt.grid() # plt.subplot(2,4,8) # plt.plot(q) # plt.title("q in $J=u^TPu+q^Tu$") # plt.grid() # plt.tight_layout() # plt.savefig("MPC_matrices_for_"+name+".png",bbox_inches='tight') # plt.close() #plt.show() self.osqp_size = P.shape[0] # Create an OSQP object self.prob = osqp.OSQP() # Setup workspace self.prob.setup(P=P.tocsc(), q=q, A=A, l=l, u=u, warm_start=True, verbose=self.verbose) if self.plotMPC: # Figure to plot MPC thoughts self.fig, self.axs = plt.subplots(self.ns + self.nu) if nx == 4: ylabels = [ '$x$', '$\\theta$', '$\\dot{x}$', '$\\dot{\\theta}$' ] else: ylabels = [str(i) for i in range(nx)] for ii in range(self.ns): self.axs[ii].set(xlabel='Time(s)', ylabel=ylabels[ii]) self.axs[ii].grid() for ii in range(self.ns, self.ns + self.nu): self.axs[ii].set(xlabel='Time(s)', ylabel='u') self.axs[ii].grid()
def solve_problem(qp_matrices, n_prob, solver='osqp'): """ Solve Huber fitting problem """ # Shorter name for qp_matrices qp = qp_matrices # Get dimensions m = int(len(qp.lx) / 2) n = len(qp.q) - 2 * m print('n = %d and solver %s' % (n, solver)) # Initialize time vector time = np.zeros(n_prob) # Initialize number of iterations vector niter = np.zeros(n_prob) if solver == 'osqp': # Construct qp matrices Aosqp = spa.vstack( [qp.A, spa.hstack([spa.csc_matrix((2 * m, n)), spa.eye(2 * m)])]).tocsc() losqp = np.hstack([qp.l, qp.lx]) uosqp = np.hstack([qp.u, qp.ux]) for i in range(n_prob): # Setup OSQP m = osqp.OSQP() m.setup(qp.P, qp.q, Aosqp, losqp, uosqp, auto_rho=True, polish=False, verbose=False) # Solve results = m.solve() x = results.x status = results.info.status_val niter[i] = results.info.iter time[i] = results.info.run_time # Check if status correct if status != m.constant('OSQP_SOLVED'): import ipdb ipdb.set_trace() raise ValueError('OSQP did not solve the problem!') # DEBUG # solve with gurobi # import mathprogbasepy as mpbpy # prob = mpbpy.QuadprogProblem(qp.P, qp.q, Aosqp, losqp, uosqp) # res = prob.solve(solver=mpbpy.GUROBI, verbose=False) # print('Norm difference OSQP-GUROBI %.3e' % # np.linalg.norm(x - res.x)) # import ipdb; ipdb.set_trace() elif solver == 'qpoases': for i in range(n_prob): n_dim = qp.P.shape[0] # Number of variables m_dim = qp.A.shape[0] # Number of constraints without bounds # Initialize qpoases and set options qpoases_m = qpoases.PyQProblem(n_dim, m_dim) options = qpoases.PyOptions() options.printLevel = qpoases.PyPrintLevel.NONE qpoases_m.setOptions(options) # Construct bounds for qpoases lx = np.append(-np.inf * np.ones(n), qp.lx) ux = np.append(np.inf * np.ones(n), qp.ux) # Setup matrix P and A P = np.ascontiguousarray(qp.P.todense()) A = np.ascontiguousarray(qp.A.todense()) # Reset cpu time qpoases_cpu_time = np.array([10.]) # Reset number of working set recalculations nWSR = np.array([1000]) # Solve res_qpoases = qpoases_m.init(P, np.ascontiguousarray(qp.q), A, np.ascontiguousarray(lx), np.ascontiguousarray(ux), np.ascontiguousarray(qp.l), np.ascontiguousarray(qp.u), nWSR, qpoases_cpu_time) # if res_qpoases != 0: # raise ValueError('qpoases did not solve the problem!') # Save time and number of iterations time[i] = qpoases_cpu_time[0] niter[i] = nWSR[0] elif solver == 'gurobi': for i in range(n_prob): # Construct qp matrices Agurobi = spa.vstack([ qp.A, spa.hstack([spa.csc_matrix((2 * m, n)), spa.eye(2 * m)]) ]).tocsc() lgurobi = np.hstack([qp.l, qp.lx]) ugurobi = np.hstack([qp.u, qp.ux]) # Solve with gurobi prob = mpbpy.QuadprogProblem(qp.P, qp.q, Agurobi, lgurobi, ugurobi) res = prob.solve(solver=mpbpy.GUROBI, verbose=False) niter[i] = res.total_iter time[i] = res.cputime elif solver == 'mosek': for i in range(n_prob): # Construct qp matrices Amosek = spa.vstack([ qp.A, spa.hstack([spa.csc_matrix((2 * m, n)), spa.eye(2 * m)]) ]).tocsc() lmosek = np.hstack([qp.l, qp.lx]) umosek = np.hstack([qp.u, qp.ux]) # Solve with mosek prob = mpbpy.QuadprogProblem(qp.P, qp.q, Amosek, lmosek, umosek) res = prob.solve(solver=mpbpy.MOSEK, verbose=False) niter[i] = res.total_iter time[i] = res.cputime elif solver == 'ecos': for i in range(n_prob): # Model with CVXPY # minimize 1/2 u.T * u + np.ones(m).T * v # subject to -u - v <= Ax - b <= u + v # 0 <= u <= 1 # v >= 0 n_var = qp.A_huber.shape[1] m_var = qp.b_huber.shape[0] x = cvxpy.Variable(n_var) u = cvxpy.Variable(m_var) v = cvxpy.Variable(m_var) objective = cvxpy.Minimize(.5 * cvxpy.quad_form(u, spa.eye(m_var)) + np.ones(m_var) * v) constraints = [ -u - v <= qp.A_huber * x - qp.b_huber, qp.A_huber * x - qp.b_huber <= u + v, 0 <= u, u <= 1, v >= 0 ] problem = cvxpy.Problem(objective, constraints) problem.solve(solver=cvxpy.ECOS, verbose=False) # DEBUG: solve with MOSEK # Amosek = spa.vstack([ # qp.A, # spa.hstack([spa.csc_matrix((2*m, n)), spa.eye(2*m)]) # ]).tocsc() # lmosek = np.hstack([qp.l, qp.lx]) # umosek = np.hstack([qp.u, qp.ux]) # # # Solve with mosek # prob = mpbpy.QuadprogProblem(qp.P, qp.q, Amosek, lmosek, umosek) # res = prob.solve(solver=mpbpy.MOSEK, verbose=False) # x_mosek = res.x[:n_var] # Obtain time and number of iterations time[i] = problem.solver_stats.setup_time + \ problem.solver_stats.solve_time niter[i] = problem.solver_stats.num_iters else: raise ValueError('Solver not understood') # Return statistics return utils.Statistics(time), utils.Statistics(niter)
def init_problem(self): # constraints umin = self.input_constraints["umin"] umax = self.input_constraints["umax"] xmin = self.state_constraints["xmin"] xmax = self.state_constraints["xmax"] # LTV systems # huge matrices that consist of all matrices in a given horizon (N) A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1))) B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N))) x_ref = np.zeros(self.nx * (self.N + 1)) u_ref = np.zeros(self.nu * self.N) # offset for equality constraints u_eq = np.zeros(self.N * self.nx) # dynamic state and input constraints xmin_dyn = np.kron(np.ones(self.N + 1), xmin) print("xmin_dyn dimension ", xmin_dyn.shape) xmax_dyn = np.kron(np.ones(self.N + 1), xmax) umax_dyn = np.kron(np.ones(self.N), umax) # derive predicted curvature from last control # kappa = tan(delta) / length kappa_pred = (np.tan( np.array(self.current_control[3::] + self.current_control[-1::])) / self.model.length) # fill the information over entire horizon for n in range(self.N): # extract information from current waypoint current_waypoint = self.model.reference_path.get_waypoint( self.model.wp_id + n) next_waypoint = self.model.reference_path.get_waypoint( self.model.wp_id + n + 1) # Previous reference output # delta_s = next_waypoint - current_waypoint # kappa_ref = current_waypoint.kappa v_ref = current_waypoint.v_ref psi_ref = current_waypoint.psi # TODO: reference values below haven't created yet delta_ref = current_waypoint.delta # get linearized LTV model # TODO: different model, different states A_lin, B_lin = self.model.linearize(v_ref, psi_ref, delta_ref) A[(n + 1) * self.nx:(n + 2) * self.nx, n * self.nx:(n + 1) * self.nx] = A_lin B[(n + 1) * self.nx:(n + 2) * self.nx, n * self.nu:(n + 1) * self.nu] = B_lin # TODO: 2 inputs have changed u_ref[n * self.nu:(n + 1) * self.nu] = np.array([v_ref, delta_ref]) # compute equality constraint offset # TODO: dunno what this is u_eq[n * self.nx:(n + 1) * self.nx] = B_lin.dot( np.array([v_ref, delta_ref])) # maximum speed limited by predicted car curvature vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n] + 1e-12))) if vmax_dyn < umax_dyn[self.nu * n]: umax_dyn[self.nu * n] = vmax_dyn # compute dynamic constraints on states upp_b, low_b, _ = self.model.reference_path.update_path_constraints( self.model.wp_id + 1, self.N, 2 * self.model.safety_margin, self.model.safety_margin, ) # TODO: the constraints should place on x,y instead of e_y xmin_dyn[0], xmin_dyn[1] = ( self.model.temporal_state.x, self.model.temporal_state.y, ) xmax_dyn[0], xmax_dyn[1] = ( self.model.temporal_state.x, self.model.temporal_state.y, ) for i in range(self.N): xmin_dyn[self.nx + i * self.nx] = low_b[0 + i * self.nx] xmin_dyn[self.nx + i * self.nx + 1] = low_b[1 + i * self.nx] xmax_dyn[self.nx + i * self.nx] = upp_b[0 + i * self.nx] + 0.000001 xmax_dyn[self.nx + i * self.nx + 1] = upp_b[1 + i * self.nx] = 0.000001 # reference state = middle line of free space x_ref[self.nx + i * self.nx] = (low_b[0 + i * self.nx] + upp_b[0 + i * self.nx]) / 2 x_ref[self.nx + i * self.nx + 1] = (low_b[0 + i * self.nx + 1] + upp_b[0 + i * self.nx + 1]) / 2 """ form an QP problem using the format of OSQP we need P, q, A, l, u for more infromation, visit https://osqp.org/docs/examples/setup-and-solve.html """ ################################ # P: matrix for quadratic term # # q: matrix for linear term # ################################ P = sparse.block_diag( [ sparse.kron(sparse.eye(self.N), self.Q), self.QN, sparse.kron(sparse.eye(self.N), self.R), ], format="csc", ) # TODO: dunno how to construct q = np.hstack([ -np.tile(np.diag(self.Q.A), self.N) * x_ref[:-self.nx], -self.QN.dot(x_ref[-self.nx:]), -np.tile(np.diag(self.R.A), self.N) * u_ref, ]) ################################# # A: system matrix in osqp form # ################################# # equality part Ax = sparse.kron(sparse.eye(self.N + 1), -sparse.eye(self.nx)) + sparse.csc_matrix(A) Bu = sparse.csc_matrix(B) A_eq = sparse.hstack([Ax, Bu]) # inequality matrix A_ineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu) A = sparse.vstack([A_eq, A_ineq], format="csc") ##################################### # l, u: lower bound and upper bound # ##################################### # equality part: just for the format x0 = np.array(self.model.temporal_state[:]) low_eq = np.hstack([-x0, u_eq]) upp_eq = low_eq # format reason # inequality part low_ineq = np.hstack([xmin_dyn, np.kron(np.ones(self.N), umin)]) upp_ineq = np.hstack([xmax_dyn, umax_dyn]) # results l = np.hstack([low_eq, low_ineq]) u = np.hstack([upp_eq, upp_ineq]) ##################################### # Solve the problem # ##################################### self.optimizer = osqp.OSQP() self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
def __init__(self, linear_dynamics, N, dt, umin, umax, xmin, xmax, Q, R, QN, xr, plotMPC=False, plotMPC_filename="",lifting=False, edmd_object=Edmd()): """__init__ Create an MPC controller Arguments: linear_dynamics {dynamical sytem} -- it contains the A and B matrices in continous time N {integer} -- number of timesteps dt {float} -- time step in seconds umin {numpy array [Nu,]} -- minimum control bound umax {numpy array [Nu,]} -- maximum control bound xmin {numpy array [Ns,]} -- minimum state bound xmax {numpy array [Ns,]} -- maximum state bound Q {numpy array [Ns,Ns]} -- state cost matrix R {numpy array [Nu,Nu]} -- control cost matrix QN {numpy array [Ns,]} -- final state cost xr {numpy array [Ns,]} -- reference trajectory Keyword Arguments: plotMPC {bool} -- flag to plot results (default: {False}) plotMPC_filename {str} -- plotting filename (default: {""}) lifting {bool} -- flag to use state lifting (default: {False}) edmd_object {edmd object} -- lifting object. It contains projection matrix and lifting function (default: {Edmd()}) """ Controller.__init__(self, linear_dynamics) # Load arguments Ac, Bc = linear_dynamics.linear_system() [nx, nu] = Bc.shape self.dt = dt self._osqp_Ad = sparse.eye(nx)+Ac*self.dt self._osqp_Bd = Bc*self.dt self.plotMPC = plotMPC self.plotMPC_filename = plotMPC_filename self.q_d = xr self.ns = xr.shape[0] self.Q = Q self.lifting = lifting self.nu = nu self.nx = nx # Total desired path if self.q_d.ndim==2: self.Nqd = self.q_d.shape[1] xr = self.q_d[:,:N+1] # Prediction horizon self.N = N x0 = np.zeros(nx) # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) if (self.lifting): # Load eDMD objects self.C = edmd_object.C self.edmd_object = edmd_object # - quadratic objective CQC = sparse.csc_matrix(np.transpose(edmd_object.C).dot(Q.dot(edmd_object.C))) CQNC = sparse.csc_matrix(np.transpose(edmd_object.C).dot(QN.dot(edmd_object.C))) P = sparse.block_diag([sparse.kron(sparse.eye(N), CQC), CQNC, sparse.kron(sparse.eye(N), R)]).tocsc() # - linear objective QCT = np.transpose(Q.dot(edmd_object.C)) QNCT = np.transpose(QN.dot(edmd_object.C)) if (xr.ndim==1): q = np.hstack([np.kron(np.ones(N), -QCT.dot(xr)), -QNCT.dot(xr), np.zeros(N*nu)]) elif (xr.ndim==2): q = np.hstack([np.reshape(-QCT.dot(xr),((N+1)*nx,),order='F'), np.zeros(N*nu)]) # - input and state constraints Aineq = sparse.block_diag([edmd_object.C for i in range(N+1)]+[np.eye(N*nu)]) else: # - quadratic objective P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN, sparse.kron(sparse.eye(N), R)]).tocsc() # - linear objective if (xr.ndim==1): q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr), np.zeros(N*nu)]) elif (xr.ndim==2): q = np.hstack([np.reshape(-Q.dot(xr),((N+1)*nx,),order='F'), np.zeros(N*nu)]) #TODO: Check if reshape is reshaping in the expected order # - input and state constraints Aineq = sparse.eye((N+1)*nx + N*nu) # - linear dynamics Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), self._osqp_Ad) Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), self._osqp_Bd) Aeq = sparse.hstack([Ax, Bu]) leq = np.hstack([-x0, np.zeros(N*nx)]) lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)]) uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)]) ueq = leq self._osqp_q = q A = sparse.vstack([Aeq, Aineq]).tocsc() self._osqp_l = np.hstack([leq, lineq]) self._osqp_u = np.hstack([ueq, uineq]) # Create an OSQP object self.prob = osqp.OSQP() # Setup workspace self.prob.setup(P, q, A, self._osqp_l, self._osqp_u, warm_start=True, verbose=False) if self.plotMPC: # Figure to plot MPC thoughts self.fig, self.axs = plt.subplots(self.ns+self.nu) ylabels = ['$x$', '$\\theta$', '$\\dot{x}$', '$\\dot{\\theta}$'] for ii in range(self.ns): self.axs[ii].set(xlabel='Time(s)',ylabel=ylabels[ii]) self.axs[ii].grid() for ii in range(self.ns,self.ns+self.nu): self.axs[ii].set(xlabel='Time(s)',ylabel='u') self.axs[ii].grid()
def compute_speed_profile(self, Constraints): """ Compute a speed profile for the path. Assign a reference velocity to each waypoint based on its curvature. :param Constraints: constraints on acceleration and velocity curvature of the path """ # Set optimization horizon N = self.n_waypoints - 1 # Constraints a_min = np.ones(N - 1) * Constraints['a_min'] a_max = np.ones(N - 1) * Constraints['a_max'] v_min = np.ones(N) * Constraints['v_min'] v_max = np.ones(N) * Constraints['v_max'] # Maximum lateral acceleration ay_max = Constraints['ay_max'] # Inequality Matrix D1 = np.zeros((N - 1, N)) # Iterate over horizon for i in range(N): # Get information about current waypoint current_waypoint = self.get_waypoint(i) next_waypoint = self.get_waypoint(i + 1) # distance between waypoints li = next_waypoint - current_waypoint # curvature of waypoint ki = current_waypoint.kappa # Fill operator matrix # dynamics of acceleration if i < N - 1: D1[i, i:i + 2] = np.array([-1 / (2 * li), 1 / (2 * li)]) # Compute dynamic constraint on velocity v_max_dyn = np.sqrt(ay_max / (np.abs(ki) + self.eps)) if v_max_dyn < v_max[i]: v_max[i] = v_max_dyn # Construct inequality matrix D1 = sparse.csc_matrix(D1) D2 = sparse.eye(N) D = sparse.vstack([D1, D2], format='csc') # Get upper and lower bound vectors for inequality constraints l = np.hstack([a_min, v_min]) u = np.hstack([a_max, v_max]) # Set cost matrices P = sparse.eye(N, format='csc') q = -1 * v_max # Solve optimization problem problem = osqp.OSQP() problem.setup(P=P, q=q, A=D, l=l, u=u, verbose=False) speed_profile = problem.solve().x # Assign reference velocity to every waypoint for i, wp in enumerate(self.waypoints[:-1]): wp.v_ref = speed_profile[i] self.waypoints[-1].v_ref = self.waypoints[-2].v_ref
def solve_via_data(self, data, warm_start, verbose, solver_opts, solver_cache=None): import osqp P = data[s.P] q = data[s.Q] A = sp.vstack([data[s.A], data[s.F]]).tocsc() data['full_A'] = A uA = np.concatenate((data[s.B], data[s.G])) data['u'] = uA lA = np.concatenate([data[s.B], -np.inf * np.ones(data[s.G].shape)]) data['l'] = lA if solver_cache is not None and self.name() in solver_cache: # Use cached data. solver, old_data, results = solver_cache[self.name()] same_pattern = (P.shape == old_data[s.P].shape and all(P.indptr == old_data[s.P].indptr)) and \ (A.shape == old_data['full_A'].shape and all(A.indptr == old_data['full_A'].indptr)) else: same_pattern = False # If sparsity pattern differs need to do setup. if warm_start and same_pattern: new_args = {} for key in ['q', 'l', 'u']: if any(data[key] != old_data[key]): new_args[key] = data[key] factorizing = False if any(P.indices != old_data[s.P].indices): new_args['Px_idx'] = P.indices factorizing = True if any(P.data != old_data[s.P].data): new_args['Px'] = P.data factorizing = True if any(A.indices != old_data['full_A'].indices): new_args['Ax_idx'] = A.indices factorizing = True if any(A.data != old_data['full_A'].data): new_args['Ax'] = A.data factorizing = True if new_args: solver.update(**new_args) # Map OSQP statuses back to CVXPY statuses status = self.STATUS_MAP.get(results.info.status_val, s.SOLVER_ERROR) if status == s.OPTIMAL: solver.warm_start(results.x, results.y) # Polish if factorizing. solver_opts['polish'] = solver_opts.get('polish', factorizing) solver.update_settings(verbose=verbose, **solver_opts) else: # Initialize and solve problem solver_opts['polish'] = solver_opts.get('polish', True) solver = osqp.OSQP() solver.setup(P, q, A, lA, uA, verbose=verbose, **solver_opts) results = solver.solve() if solver_cache is not None: solver_cache[self.name()] = (solver, data, results) return results
def mpc_increment(Ad_list, Bd_list, gd_list, x_tilda_vec, Xr, pred_x_tilda, pred_del_u, Q, QN, R, N, xmin_tilda, xmax_tilda, del_umin, del_umax): """ Incremental MPC x_tilda_vec : [nx+nu, nx+nu] Xr : [nx, N+1] Q : [nx, nx] R : [nu, nu] """ tic_mat = time.time() # ========== Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) ========== nx = Ad_list[0].shape[0] nu = Bd_list[0].shape[1] # Cast MPC problem to a QP: # x = (x(0),x(1),...,x(N), u(0),...,u(N-1)) # Objective function # C_tilda = [I, 0] # Q_tilda = C_tilda.T * Q * C_tilta : (nx+nu, nx) * (nx, nx) * (nx, nx+nu) => (nx+nu, nx+nu) C_tilda = sparse.hstack([sparse.eye(nx), np.zeros([nx, nu])]) # (nx, nx+nu) Q_tilda = C_tilda.transpose() * Q * C_tilda Q_tilda_N = C_tilda.transpose() * QN * C_tilda # - quadratic objective (P) P = sparse.block_diag([ sparse.kron(sparse.eye(N), Q_tilda), # Q x (N+1) on diagonal Q_tilda_N, sparse.kron(sparse.eye(N), R), # R X (N) on diagonal ]).tocsc() # - linear objective (q) Q_C_tilda = Q * C_tilda QN_C_tilda = QN * C_tilda Q_C_tilda_trans = Q_C_tilda.transpose() QN_C_tilda_trans = QN_C_tilda.transpose() q = -Q_C_tilda_trans.dot(Xr[:, 0]) # index 0 for ii in range(N - 1): q = np.hstack([q, -Q_C_tilda_trans.dot(Xr[:, ii + 1])]) # index 1 ~ N-1 q = np.hstack([q, -QN_C_tilda_trans.dot(Xr[:, N]), np.zeros(N * nu)]) # index N # Augmentation for Incremental Control Ad_sys = Ad_list[0] Bd_sys = Bd_list[0] Aug_A_sys = np.hstack([Ad_sys, Bd_sys]) Aug_A_increment = sparse.hstack( [sparse.csr_matrix((nu, nx)), sparse.eye(nu)]) Ad_tilda = sparse.vstack([Aug_A_sys, Aug_A_increment]) Bd_tilda = sparse.vstack([Bd_sys, sparse.eye(nu)]) Ax_Ad = sparse.csc_matrix(Ad_tilda) Ax_diag = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx + nu)) Bu_Bd = sparse.csc_matrix(Bd_tilda) for i in range(N - 1): Ad_sys = Ad_list[i + 1] Bd_sys = Bd_list[i + 1] Aug_A_sys = np.hstack([Ad_sys, Bd_sys]) Aug_A_increment = sparse.hstack( [sparse.csr_matrix((nu, nx)), sparse.eye(nu)]) Ad_tilda = sparse.vstack([Aug_A_sys, Aug_A_increment]) Bd_tilda = sparse.vstack([Bd_sys, sparse.eye(nu)]) Ax_Ad = sparse.block_diag([Ax_Ad, Ad_tilda]) Bu_Bd = sparse.block_diag([Bu_Bd, Bd_tilda]) Ax_Ad_top = sparse.kron(np.ones(N + 1), np.zeros((nx + nu, nx + nu))) Ax_Ad_side = sparse.kron(np.ones((N, 1)), np.zeros((nx + nu, nx + nu))) Ax = Ax_diag + sparse.vstack( [Ax_Ad_top, sparse.hstack([Ax_Ad, Ax_Ad_side])]) Bu_Bd_top = sparse.kron(np.ones(N), np.zeros((nx + nu, nu))) Bu = sparse.vstack([Bu_Bd_top, Bu_Bd]) Aeq = sparse.hstack([Ax, Bu]) # - Equality constraint (linear dynamics) : lower bound and upper bound leq = -x_tilda_vec # later ueq == leq for i in range(N): gd_tilda = np.vstack([gd_list[i], np.zeros( (nu, 1))]) # gd_tilda for augmented system gd_tilda = np.squeeze(gd_tilda, axis=1) # from (N,1) to (N,) leq = np.hstack([leq, -gd_tilda]) # leq = np.hstack([-x_tilda_vec, np.zeros(N*nx)]) ueq = leq # Original Code # ----- input and state constraints ----- Aineq = sparse.eye((N + 1) * (nx + nu) + N * nu) lineq = np.hstack( [np.kron(np.ones(N + 1), xmin_tilda), np.kron(np.ones(N), del_umin)]) uineq = np.hstack( [np.kron(np.ones(N + 1), xmax_tilda), np.kron(np.ones(N), del_umax)]) # ----- OSQP constraints ----- A = sparse.vstack([Aeq, Aineq]).tocsc() lb = np.hstack([leq, lineq]) ub = np.hstack([ueq, uineq]) print("matrix time :", time.time() - tic_mat) # ==========Create an OSQP object and Setup workspace ========== tic_solve = time.time() prob = osqp.OSQP() prob.setup(P, q, A, lb, ub, verbose=False, polish=False, warm_start=False) # verbose: print output. # Solve res = prob.solve() # Check solver status if res.info.status != 'solved': print('OSQP did not solve the problem!') raise ValueError('OSQP did not solve the problem!') print("solver time :", time.time() - tic_solve) tic_pred = time.time() # Predictive States and Actions sol_state = res.x[:-N * nu] sol_action = res.x[-N * nu:] for ii in range((N + 1) * (nx + nu)): if ii % (nx + nu) == 0: pred_x_tilda[0, ii // (nx + nu)] = sol_state[ii] # X elif ii % (nx + nu) == 1: pred_x_tilda[1, ii // (nx + nu)] = sol_state[ii] # Y elif ii % (nx + nu) == 2: pred_x_tilda[2, ii // (nx + nu)] = sol_state[ii] # Vx elif ii % (nx + nu) == 3: pred_x_tilda[3, ii // (nx + nu)] = sol_state[ii] # Yaw elif ii % (nx + nu) == 4: pred_x_tilda[4, ii // (nx + nu)] = sol_state[ii] # Steer else: # ii % (nx+nu) == 5: pred_x_tilda[5, ii // (nx + nu)] = sol_state[ii] # accel_track for jj in range((N) * nu): if jj % nu == 0: pred_del_u[0, jj // nu] = sol_action[jj] else: # jj % nu == 1 pred_del_u[1, jj // nu] = sol_action[jj] pred_del_u[:, -1] = pred_del_u[:, -2] # append last control print("Parsing pred state action time :", time.time() - tic_pred) return pred_x_tilda, pred_del_u
'sigma': 1e-06, 'scaled_termination': False, 'check_termination': 1, 'polish': False, 'verbose': True, 'linsys_solver': 'suitesparse ldl' } qp = mpbpy.QuadprogProblem(P, q, A, l, u) res_gurobi = qp.solve(solver=mpbpy.GUROBI, verbose=True) model = osqppurepy.OSQP() model.setup(P=P, q=q, A=A, l=l, u=u, **osqp_opts) res_osqppurepy = model.solve() # Solve with SuiteSparse LDL model = osqp.OSQP() model.setup(P=P, q=q, A=A, l=l, u=u, **osqp_opts) res_osqp = model.solve() # Check difference with gurobi if res_gurobi.status == 'optimal': print("Difference OSQP vs Gurobi") print(" - primal = %.4f" % (np.linalg.norm(res_gurobi.x - res_osqp.x) / np.linalg.norm(res_gurobi.x))) print(" - dual = %.4f" % (np.linalg.norm(res_gurobi.y - res_osqp.y) / np.linalg.norm(res_gurobi.y)))
def __init__(self, m: int, sid: str, params_path: str, input_path: str, logging_path: str = None, ci: bool = False): """Class to construct QP-optimizer by a manually designed vector matrix notation. :param m: number of velocity points :param sid: optimized ID 'PerfSQP' or 'EmergSQP' :param params_path: absolute path to folder containing config file .ini :param input_path: absolute path to folder containing variable vehicle and track information :param ci: switch to construct an object of this class used within the CI/CD jobs :Authors: Thomas Herrmann <*****@*****.**> :Created on: 01.11.2019 """ # --- Retrieve SQP settings from parameter-file self.sqp_stgs = params_vp_sqp(m=m, sid=sid, params_path=params_path)[0] # Store params_path self.params_path = params_path # number of velocity optimization variables self.m = m # counter for how many velocity variables one slack is valid self.slack_every_v = self.sqp_stgs['slack_every_v'] # number of slack variables self.n = int(np.ceil(m / self.slack_every_v)) # weight on velocity slacks self.ev_vel_w = None # determines whether A, P, u, l, q-matrices have been filled once self.b_ini_done = False # ID of optimizer object self.sid = sid ################################################################################################################ # --- Pre-allocation of QP matrices ################################################################################################################ # --- Matrix setup for Emergency SQP and Performance SQP if self.sid == 'EmergSQP': # --- no F_ini-constraint in emergency profile self.Am =\ np.empty((1 * 1 + m * 0 + (m - 1) * 6 + (m - 2) * 1 + self.n * 1, m - 1 + self.n), dtype=np.float64) self.lo = np.empty( (1 * 1 + m * 0 + (m - 1) * 6 + (m - 2) * 1 + self.n * 1, 1), dtype=np.float64) self.up = np.empty( (1 * 1 + m * 0 + (m - 1) * 6 + (m - 2) * 1 + self.n * 1, 1), dtype=np.float64) else: self.Am =\ np.empty((1 * 2 + m * 0 + (m - 1) * 5 + (m - 2) * 2 + self.n * 1, m - 1 + self.n), dtype=np.float64) self.lo = np.empty( (1 * 2 + m * 0 + (m - 1) * 5 + (m - 2) * 2 + self.n * 1, 1), dtype=np.float64) self.up = np.empty( (1 * 2 + m * 0 + (m - 1) * 5 + (m - 2) * 2 + self.n * 1, 1), dtype=np.float64) print('*** --- ' + sid + ' solver initialized --- ***') ################################################################################################################ # --- Initialize Logging ################################################################################################################ if logging_path is not None: # create logger for Performance SQP self.logger_perf = logging.getLogger('sqp_logger_perf') self.logger_perf.setLevel(logging.DEBUG) os.makedirs(logging_path, exist_ok=True) fh_perf = logging.FileHandler( logging_path + '/sqp_perf_' + datetime.datetime.now().strftime("%Y_%m_%d") + '_' + datetime.datetime.now().strftime("%H_%M") + '.log') fh_perf.setLevel(logging.DEBUG) formatter = logging.Formatter('%(message)s') fh_perf.setFormatter(formatter) self.logger_perf.addHandler(fh_perf) # create logger for Emergency SQP self.logger_emerg = logging.getLogger('sqp_logger_emerg') self.logger_emerg.setLevel(logging.DEBUG) fh_emerg = logging.FileHandler( logging_path + '/sqp_emerg_' + datetime.datetime.now().strftime("%Y_%m_%d") + '_' + datetime.datetime.now().strftime("%H_%M") + '.log') fh_emerg.setLevel(logging.DEBUG) formatter = logging.Formatter('%(message)s') fh_emerg.setFormatter(formatter) self.logger_emerg.addHandler(fh_emerg) ################################################################################################################ # --- Assign numeric values to hard-coded parameters in QP ################################################################################################################ # --- Performance SQP settings # load SQP config files sqp_config = configparser.ConfigParser() # load QP config sparsity patterns sqp_sparsity = configparser.ConfigParser() if not sqp_config.read(self.params_path + 'sqp_config.ini'): raise ValueError( 'Specified SQP config file does not exist or is empty!') # check whether we are running in CI/CD job if ci: calc_sparsity(params_path=params_path, logging_path=logging_path, m_perf=m, m_emerg=m) src = logging_path + '/sparsity/.' dst = params_path os.system("cp -r -n " + src + " " + dst) if not sqp_sparsity.read(self.params_path + 'sqp_sparsity_' + sid + str(m) + '.ini') and \ self.sqp_stgs['b_sparse_matrix_fill']: raise ValueError( 'Specified SQP sparsity file does not exist or is empty!') # --- Performance SQP settings if self.sid == 'PerfSQP': sym_sc_ = { 'm_t_': sqp_config.getfloat('VEHICLE', 'mass_t'), 'Pmax_kW_': sqp_config.getfloat('VEHICLE', 'P_max_kW'), 'Fmax_kN_': sqp_config.getfloat('VEHICLE', 'F_max_kN'), 'Fmin_kN_': sqp_config.getfloat('VEHICLE', 'F_min_kN'), 'axmax_mps2_': sqp_config.getfloat('VEHICLE', 'ax_max_mps2'), 'aymax_mps2_': sqp_config.getfloat('VEHICLE', 'ay_max_mps2'), 'dF_kN_pos_': sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_pos'), 'dF_kN_neg_': sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_neg'), 'Fini_tol_': sqp_config.getfloat('SOLVER_PERFORMANCE', 'F_ini_tol'), 'c_res_': sqp_config.getfloat('VEHICLE', 'c_res'), 'vmin_mps_': sqp_config.getfloat('SOLVER_GENERAL', 'v_min_mps'), 's_v_t_lim_': sqp_config.getfloat('SOLVER_PERFORMANCE', 'slack_var_tire_lim'), 's_v_t_unit_': sqp_config.getfloat('SOLVER_GENERAL', 'slack_var_tire_unit'), 'dvdv_w_': sqp_config.getfloat('SOLVER_PERFORMANCE', 'penalty_jerk'), 'tre_cst_w_': sqp_config.getfloat('SOLVER_PERFORMANCE', 'w_tre_constraint'), 's_tre_w_lin_': sqp_config.getfloat('SOLVER_PERFORMANCE', 'penalty_slack_tire_lin'), 's_tre_w_quad_': sqp_config.getfloat('SOLVER_PERFORMANCE', 'penalty_slack_tire_quad') } if self.sqp_stgs['b_sparse_matrix_fill']: self.sparsity_pat = { 'F_ini_r': np.array( json.loads( sqp_sparsity.get('symqp.F_sym_ini_cst_jac_:_1:', 'r'))), # noqa: E501 'F_ini_c': np.array( json.loads( sqp_sparsity.get('symqp.F_sym_ini_cst_jac_:_1:', 'c'))), # noqa: E501 'F_r': np.array( json.loads( sqp_sparsity.get('symqp.F_sym_cst_jac_1:_1:', 'r'))), 'F_c': np.array( json.loads( sqp_sparsity.get('symqp.F_sym_cst_jac_1:_1:', 'c'))), 'P_r': np.array( json.loads( sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:', 'r'))), 'P_c': np.array( json.loads( sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:', 'c'))), 'Tre_r': np.array( json.loads( sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:', 'r'))), # noqa: E501 'Tre_c': np.array( json.loads( sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:', 'c'))) } # --- Emergency SQP settings else: sym_sc_ = { 'm_t_': sqp_config.getfloat('VEHICLE', 'mass_t'), 'Pmax_kW_': sqp_config.getfloat('VEHICLE', 'P_max_kW'), 'Fmax_kN_': sqp_config.getfloat('VEHICLE', 'F_max_kN'), 'Fmin_kN_': sqp_config.getfloat('VEHICLE', 'F_min_kN'), 'axmax_mps2_': sqp_config.getfloat('VEHICLE', 'ax_max_mps2'), 'aymax_mps2_': sqp_config.getfloat('VEHICLE', 'ay_max_mps2'), 'dF_kN_pos_': sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_pos'), 'dF_kN_neg_': sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_neg'), 'Fini_tol_': sqp_config.getfloat('SOLVER_EMERGENCY', 'F_ini_tol'), 'c_res_': sqp_config.getfloat('VEHICLE', 'c_res'), 'vmin_mps_': sqp_config.getfloat('SOLVER_GENERAL', 'v_min_mps'), 's_v_t_lim_': sqp_config.getfloat('SOLVER_EMERGENCY', 'slack_var_tire_lim'), 's_v_t_unit_': sqp_config.getfloat('SOLVER_GENERAL', 'slack_var_tire_unit'), 'dvdv_w_': sqp_config.getfloat('SOLVER_EMERGENCY', 'penalty_jerk'), 'tre_cst_w_': sqp_config.getfloat('SOLVER_EMERGENCY', 'w_tre_constraint'), 's_tre_w_lin_': sqp_config.getfloat('SOLVER_EMERGENCY', 'penalty_slack_tire_lin'), 's_tre_w_quad_': sqp_config.getfloat('SOLVER_EMERGENCY', 'penalty_slack_tire_quad') } if self.sqp_stgs['b_sparse_matrix_fill']: self.sparsity_pat = { 'F_r': np.array( json.loads( sqp_sparsity.get('symqp.F_sym_cst_jac_:_1:', 'r'))), 'F_c': np.array( json.loads( sqp_sparsity.get('symqp.F_sym_cst_jac_:_1:', 'c'))), 'P_r': np.array( json.loads( sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:', 'r'))), 'P_c': np.array( json.loads( sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:', 'c'))), 'Tre_r': np.array( json.loads( sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:', 'r'))), # noqa: E501 'Tre_c': np.array( json.loads( sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:', 'c'))) } # Assign settings to SQP object self.sym_sc_ = sym_sc_ ################################################################################################################ # --- Pre-define numeric matrices ################################################################################################################ # Vector with elements 1, ..., 1: m-1 x 1 self.ones_vec_red = np.ones((m - 1, 1), dtype=np.float64) # unity matrix last row removed: m-1 x m+n self.E_mat_red = np.eye(m - 1, m + self.n, dtype=np.float64) # unity matrix last 2 rows removed: m-2 x m+n self.E_mat_redd = np.eye(m - 2, m + self.n, dtype=np.float64) # unity matrix last row removed: m-1 x m+n; filled with power constraint self.EP_mat_red = np.eye(m - 1, m + self.n, dtype=np.float64) # Matrix to store inverse elements of delta t for every discretization step self.T_mat_inv = np.zeros((m - 1, m + self.n), dtype=np.float64) # --- Contribution of derivative of tire slack variables to tire jacobian for j in range(self.n): self.T_mat_inv[j * self.slack_every_v:self.slack_every_v + j * self.slack_every_v, - self.n + j] =\ - 1 * self.sym_sc_['s_v_t_unit_'] # Inverse of vehicle mass [1/t = 1/1000kg] self.m_inv = 1 / sym_sc_['m_t_'] # Initialization of attributes self.err = None self.err_inf = None self.x0 = None self.x0_s_t = None self.J_jac = None self.J_Hess = None self.F_cst = None self.F_cst_jac = None self.F_ini_cst = None self.F_ini_cst_jac = None self.v_cst_end = None self.v_cst_end_jac = None self.P_cst = None self.P_cst_jac = None self.Tre_cst1 = None self.Tre_cst1 = None self.Tre_cst1_jac = None self.Tre_cst2 = None self.Tre_cst2_jac = None self.Tre_cst3 = None self.Tre_cst3_jac = None self.Tre_cst4 = None self.Tre_cst4_jac = None self.dF_cst = None self.dF_cst_jac = None self.Am_csc = None ################################################################################################################ # --- Get variable power handler class ################################################################################################################ self.vpl = VarPowerLimits(input_path=input_path) ################################################################################################################ # --- Construct QP solver object (OSQP) ################################################################################################################ self.sol_osqp = osqp.OSQP() ################################################################################################################ # --- Initialize QP solver object (OSQP) ################################################################################################################ self.osqp_init()
Ax = sparse.kron(sparse.eye(N+1), -sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad) Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd) Aeq = sparse.hstack([Ax, Bu]) # - OSQP constraints A = sparse.vstack([Aeq, Aineq], format='csc') return A, lb, ub, q, P A, lb, ub, q, P = calc_matrices(Ad, Bd, x0, xr) # Create an OSQP object prob = osqp.OSQP() # Setup workspace prob.setup(P, q, A, lb, ub, warm_start=True) # Process observation to state def proc_obs(obs): pos_sp = np.array([obs[2], -obs[1]]) mb_vel = obs[6:9] linkpos = [0.5, 0, 0, 1.0, 0, 0] j_pos = [obs[45], -obs[46]] j_vel = obs[47:49] sc = obs[49:90] angs = np.linspace(-np.pi/2, np.pi/2, 41) sc_xs, sc_ys = np.cos(angs)*sc, np.sin(angs)*sc
def solve(self, x, x_d, mu_d, sigDelta): sigDelta = sigDelta * self.ksig sigDelta = np.clip(sigDelta, 0.0, self.max_var) # sigDelta = np.ones((self.xdim/2,1)) * self.max_var # for testing # build Q and p matrices to specify minimization expression Q = np.diag( np.append( np.append( np.ones(self.xdim / 2, dtype=np.float32) * (self.u_cost + self.u_prev_cost), self.p1_cost), self.p2_cost)) self.Q = sparse.csc_matrix(Q) self.p = 2 * np.append( np.append(-self.mu_qp_prev * self.u_prev_cost, 0), 0) #error dynamics for clf e = x[:-1, :] - x_d[:-1, :] e = np.clip(e, -self.max_error, self.max_error) eTPG = np.matmul(e.T, np.matmul(self.P, self.G)) G_dyn = np.expand_dims(np.append(np.append(eTPG, 1), 0), 0) #append 1 for clf < d Gsig = np.matmul(self.G, sigDelta) GssG = np.matmul(Gsig, Gsig.T) self.trGssGP = np.trace(np.matmul(GssG, self.P)) h_dyn = -1 * (-0.5 * np.matmul(e.T, np.matmul(Q, e)) + 0.5 * np.matmul( e.T, np.matmul(self.P, e)) / self.clf.epsilon + 0.5 * self.trGssGP) # build constraints for barriers N_cbf = len(self.cbf_list) G_cbf = np.zeros((N_cbf, self.xdim / 2 + 2), dtype=np.float32) h_cbf = np.zeros((N_cbf, 1), dtype=np.float32) A0x_Gmud = np.matmul(self.A0, x[:-1, :]) + np.matmul(self.G, mu_d) GssG_22 = GssG[2:, 2:] for i, cbf in enumerate(self.cbf_list): h_x, dB, d2B = cbf.get_B_derivatives(x) G_cbf[i, :] = np.append( np.append(np.einsum('ij,jk', dB, self.G), 0), 1) trGssGd2B = np.einsum('ii', np.einsum('ij,jk', GssG_22, d2B)) h_cbf[i, :] = -1 * (np.einsum('ij,jk', dB, A0x_Gmud) - cbf.gamma * h_x + 0.5 * trGssGd2B) # build constraints for control limits ginv = np.linalg.inv(self.dyn.g(x)) l_ctrl = np.matmul(ginv, mu_d - self.dyn.f(x)) A_ctrl = ginv G_ctrl = np.zeros((self.udim * 2, self.xdim / 2 + 2), dtype=np.float32) h_ctrl = np.zeros((self.udim * 2, 1), dtype=np.float32) for i in range(self.udim): G_ctrl[i * 2, :self.xdim / 2] = -A_ctrl[i, :] h_ctrl[i * 2] = -self.u_lim[i, 0] + l_ctrl[i] G_ctrl[i * 2 + 1, :self.xdim / 2] = A_ctrl[i, :] h_ctrl[i * 2 + 1] = self.u_lim[i, 1] - l_ctrl[i] # stack into one matrix and vector G = np.concatenate((G_dyn, G_cbf, G_ctrl), axis=0) h = np.concatenate((h_dyn, h_cbf, h_ctrl), axis=0) self.G_csc = sparse.csc_matrix(G) self.h = h # dummy lower bound l = np.ones(h.shape, dtype=np.float32) * np.inf * -1 #Solve QP self.prob = osqp.OSQP() exception_called = False mu_bar = np.zeros((self.xdim + 1), dtype=np.float32) # try: self.prob.setup(P=self.Q, q=self.p, A=self.G_csc, l=l, u=self.h, verbose=self.verbose) self.res = self.prob.solve() # except: # exception_called = True # else: mu_bar = self.res.x # if exception_called or u_bar[0] is None or np.isnan(u_bar).any(): if mu_bar[0] is None or np.isnan(mu_bar).any(): mu_bar = np.zeros((self.xdim + 1)) self.res = None rospy.logwarn("QP failed!") self.mu_qp_prev = np.expand_dims(mu_bar[0:self.xdim / 2], axis=0).T self.V = self.clf.V(x, x_d) if self.verbose: print('z_ref: ', x.T) print('z_des: ', x_d.T) print('u_lim', self.u_lim) print('V: ', self.V) print('Q:', Q) print('p:', np.array(self.p)) print('G_dyn:', G_dyn) print('h_dyn:', h_dyn) print('trGssGP', self.trGssGP) if h_cbf.shape[0] < 10: print('G_cbf:', G_cbf) print('h_cbf:', h_cbf) print('G_ctrl:', G_ctrl) print('h_ctrl:', h_ctrl) print('result:', mu_bar) return self.mu_qp_prev
def step(self): u_set = [None] * self.NV xx_set = [None] * self.NV QQ_set = [None] * self.NV u0_set = [None] * self.NV Qt_set = [None] * self.NV x_set = [None] * self.NV umax = np.array([self.cons.am, self.cons.rm]) # generate backup trajectories self.xbackup = np.empty([0, (self.mpc.N + 1) * 4]) for i in range(0, self.NV): xx_set[i] = [None] * self.m QQ_set[i] = [None] * self.m Qt_set[i] = [None] * self.m if abs(self.veh_set[i].state[1] - (1.8 + self.veh_set[i].laneidx * 3.6)) < 0.4: if i == 0: mindis = 1000 idx = 0 for ii in range(1, self.NV): if self.veh_set[ii].laneidx != self.veh_set[ 0].laneidx and abs( self.veh_set[ii].state[0] - self.veh_set[0].state[0]) < mindis: mindis = abs(self.veh_set[ii].state[0] - self.veh_set[0].state[0]) idx = ii if mindis < 4: self.veh_set[0].laneidx = self.veh_set[idx].laneidx else: if with_probability(0.05): if self.veh_set[i].laneidx == 0: self.veh_set[i].laneidx = 1 elif self.veh_set[i].laneidx == self.N_lane - 1: self.veh_set[i].laneidx = self.N_lane - 2 else: if with_probability(0.5): self.veh_set[i].laneidx += 1 else: self.veh_set[i].laneidx -= 1 x0 = self.veh_set[i].state.copy() x0[1] = 1.8 + self.veh_set[i].laneidx * 3.6 x0[2] = self.veh_set[0].state[2] + 0.5 * ( self.veh_set[0].state[0] - self.veh_set[i].state[0]) x0[3] = 0 con = lambda x: veh_con(x, x0, umax) u0_set[i] = con(self.veh_set[i].state) stop_crit = lambda x, t: t > self.dt * self.mpc.N + 2 for j in range(0, self.m): tt, xx, uu, QQ, Qt = generate_backup_traj( self.veh_set[i].state, self.backupcons[j], stop_crit, f0, self.dt, True) xx_set[i][j] = xx Qt_set[i][j] = Qt QQ_set[i][j] = QQ if i > 0: self.xbackup = np.vstack( (self.xbackup, np.reshape(np.array(xx[0:self.mpc.N + 1]), [1, -1]))) Ydes = 1.8 + self.veh_set[0].laneidx * 3.6 vdes = v0 xRef = np.array([0, Ydes, vdes, 0]) # print(self.veh_set[1].backupidx) self.mpc.solve(self.veh_set[0].state, self.b, self.xbackup, xRef) u_set[0] = self.mpc.uPred[0] self.veh_set[0].step(u_set[0]) x_set[0] = self.veh_set[0].state ## no control # u_set[0]=np.zeros(2) # self.veh_set[0].step(u_set[0]) # x_set[0] = self.veh_set[0].state ## backup CBF QP for i in range(1, self.NV): A = [] b = [] x = self.veh_set[i].state fi, g = dubin_fg(x) for t in range(0, len(xx_set[i][self.veh_set[i].backupidx])): if t % 3 == 0: xi = xx_set[i][self.veh_set[i].backupidx][t] h, dh = X_bdry(xi, [0, lm[self.N_lane]], self.veh_set[i].v_width) if h < 0.5: dhdx = np.matmul( dh, QQ_set[i][self.veh_set[i].backupidx][t]) if norm(dhdx.dot(g)) > 1e-6: A.append(-dhdx.dot(g)) b.append( dhdx.dot(fi - f0) - np.matmul( dh, Qt_set[i][self.veh_set[i].backupidx] [t]) + self.cons.alpha * h) for j in range(0, self.NV): if j != i: if t >= len(xx_set[j][self.veh_set[j].backupidx]): xj = xx_set[j][self.veh_set[j].backupidx][ -1] + f0 * self.dt * (t - len(xx_set[j][ self.veh_set[j].backupidx]) + 1) else: xj = xx_set[j][self.veh_set[j].backupidx][t] eps = 1e-6 h = veh_col( xi, xj, [(self.veh_set[i].v_length + self.veh_set[j].v_length) / 2 + 1, (self.veh_set[i].v_width + self.veh_set[j].v_width) / 2 + 0.2]) if h < 2: dh = np.zeros(4) dh[0] = (veh_col(xi + [eps, 0, 0, 0], xj, [ (self.veh_set[i].v_length + self.veh_set[j].v_length) / 2 + 1, (self.veh_set[i].v_width + self.veh_set[j].v_width) / 2 + 0.2 ]) - h) / eps dh[1] = (veh_col(xi + [0, eps, 0, 0], xj, [ (self.veh_set[i].v_length + self.veh_set[j].v_length) / 2 + 1, (self.veh_set[i].v_width + self.veh_set[j].v_width) / 2 + 0.2 ]) - h) / eps dhdx = np.matmul( dh, QQ_set[i][ self.veh_set[i].backupidx][t]) if norm(dhdx.dot(g)) > 1e-6: A.append(-dhdx.dot(g)) b.append( dhdx.dot(fi - f0) + self.cons.alpha * h - np.matmul( dh, Qt_set[i][self.veh_set[i]. backupidx][t])) if A: A = np.array(A) A = np.append(A, -np.ones([A.shape[0], 1]), 1) AA = np.concatenate((A, np.identity(3))) AA = sparse.csc_matrix(AA) ub = np.append(np.append(np.array(b), np.array(umax)), np.inf) lb = np.append( np.append(-np.inf * np.ones(len(b)), np.array(-umax)), 0.0) P = np.eye(3) P[2][2] = 0 P = sparse.csc_matrix(P) q = np.append(-u0_set[i], 1e6) prob = osqp.OSQP() prob.setup(P, q, AA, lb, ub, alpha=1.0, verbose=False) res = prob.solve() if res.info.status_val == 1 or res.info.status_val == 2: u_set[i] = res.x[0:2] else: if res.x.shape[0] > 0: u_set[i] = res.x[0:2] else: u_set[i] = u0_set[i] else: uu = np.maximum(u0_set[i], -umax) u_set[i] = np.minimum(uu, umax) self.veh_set[i].step(u_set[i]) x_set[i] = self.veh_set[i].state ## update belief and replace vehicles far away from host vehicle if self.veh_set[i].state[0] - self.veh_set[0].state[ 0] > 15 or self.veh_set[i].state[0] - self.veh_set[ 0].state[0] < -15: succ = self.replace_veh(i, 0) if not succ: self.replace_veh(i, 2) else: cbfcond = np.zeros(self.m) xdot = dubin(x_set[i], u_set[i]) hi = np.zeros(self.m) for j in range(0, self.m): hij = np.zeros(self.mpc.N) dhij = np.zeros(self.mpc.N) for tt in range(0, self.mpc.N): hij[tt] = veh_col(xx_set[i][j][tt],self.mpc.xPred[tt][0:4],[(self.veh_set[i].v_length+self.veh_set[0].v_length)/2,\ (self.veh_set[i].v_width+self.veh_set[0].v_width)/2],self.cons.col_alpha) dh = np.zeros(4) dh[0] = (veh_col(xx_set[i][j][tt]+[eps,0,0,0],self.mpc.xPred[tt][0:4],[(self.veh_set[i].v_length+self.veh_set[0].v_length)/2,\ (self.veh_set[i].v_width+self.veh_set[0].v_width)/2],self.cons.col_alpha)-hij[tt])/eps dh[0] = (veh_col(xx_set[i][j][tt]+[0,eps,0,0],self.mpc.xPred[tt][0:4],[(self.veh_set[i].v_length+self.veh_set[0].v_length)/2,\ (self.veh_set[i].v_width+self.veh_set[0].v_width)/2],self.cons.col_alpha)-hij[tt])/eps dhij[tt] = dh @ (QQ_set[i][j][tt] @ (xdot - f0) - Qt_set[i][j][tt]) hi[j] = np.min(hij) cbfcond[j] = np.mean(hij + dhij) bi = self.b[i - 1].copy() H = backup_trans(hi, self.cons) bi = bi @ H for j in range(0, self.m): bi[j] = bi[j] * backup_input_prob(cbfcond[j], self.cons) self.b[i - 1] = bi / np.sum(bi) ## change backup self.veh_set[i].backupidx = np.random.choice( range(0, self.m), 1, p=H[self.veh_set[i].backupidx])[0] if np.isnan(self.b).any(): pdb.set_trace() return u_set, x_set, xx_set, self.mpc.xPred[1:, 0:4]
def qp_shortest_path(wx, wy): tx, ty, tyaw, tc, csp = generate_target_course( wx, wy) # 2m间隔的参考路径,可以计算每点的frenet坐标 法向量+切向量 # tx, ty, tyaw, tc, csp = generate_target_course(wx, wy) # tx_ori, ty_ori, tyaw_ori, tc_ori, csp_ori = generate_target_course(wx, wy) # osqp for nearest route total_dis = 0.0 sampling_dis = 2.0 sampling_num = int(csp.s[-1] // sampling_dis) E = np.zeros([2, sampling_num]) H = np.zeros([sampling_num, sampling_num]) B = np.zeros([1, sampling_num]) next_pos = csp.calc_position(total_dis) pos_s = [next_pos] phi_1 = np.array(csp.calc_norm(total_dis)) phi_s = [phi_1] for i in range(sampling_num - 1): cur_pos = next_pos next_pos = csp.calc_position(total_dis + sampling_dis) pos_s.append(next_pos) delta_x = next_pos[0] - cur_pos[0] delta_y = next_pos[1] - cur_pos[1] E_i = np.zeros([2, sampling_num]) phi = phi_1 phi_1 = np.array(csp.calc_norm(total_dis + sampling_dis)) phi_s.append(phi_1) phi_xi = np.array([[phi_1[0], -phi[0]]]) phi_yi = np.array([[phi_1[1], -phi[1]]]) E_i[0][i] = 1 E_i[1][i + 1] = 1 B_i = 2 * delta_x * np.dot(phi_xi, E_i) + 2 * delta_y * np.dot( phi_yi, E_i) H_si_t = np.dot(phi_xi.T, phi_xi) + np.dot(phi_yi.T, phi_yi) H += np.dot(np.dot(E_i.T, H_si_t), E_i) B += B_i total_dis += sampling_dis H = H * 2 P = sparse.csc_matrix(H) q = B[0] A = sparse.csc_matrix(np.eye(sampling_num)) l = np.array([-2.5 for i in range(sampling_num)]) l[0] = -0.1 l[-1] = -0.1 u = np.array([2.5 for i in range(sampling_num)]) u[0] = 0.1 u[-1] = 0.1 prob = osqp.OSQP() # Setup workspace and change alpha parameter prob.setup(P, q, A, l, u, alpha=1.0) # Solve problem res = prob.solve() print(res.x) shortest_x = [] shortest_y = [] for i in range(sampling_num): shortest_x.append(pos_s[i][0] + res.x[i] * phi_s[i][0]) shortest_y.append(pos_s[i][1] + res.x[i] * phi_s[i][1]) tx_shortest, ty_shortest, tyaw_shortest, tc_shortest, csp_shortest = generate_target_course( shortest_x, shortest_y) # x,y,yaw,k,config,P,Q return tx_shortest, ty_shortest, tyaw_shortest, tc_shortest, csp_shortest, H, q
rho_plot = np.zeros(n_rho * n_sigma) sigma_plot = np.zeros(n_rho * n_sigma) r_KKT_plot = np.zeros(n_rho * n_sigma) r_res_plot = np.zeros(n_rho * n_sigma) cond_KKT_plot = np.zeros(n_rho * n_sigma) n_iter_plot = np.zeros(n_rho * n_sigma) z_idx = 0 for i in tqdm(range(len(rho_vec))): for j in range(len(sigma_vec)): # Get ratios of norms for columns and rows r_KKT[i, j], cond_KKT[i, j] = get_KKT_info(P_sc, A_sc, sigma_vec[j], rho_vec[i]) # Solve problem m = osqp.OSQP() m.setup(P, q, A, l, u, rho=rho_vec[i], sigma=sigma_vec[j], scaling=scaling, polish=False, verbose=False) res = m.solve() # Store number of iterations n_iter[i, j] = res.info.iter
import osqp import numpy as np from scipy import sparse # Define problem data P = sparse.csc_matrix([[4, 1], [1, 2]]) q = np.array([1, 1]) A = sparse.csc_matrix([[1, 1], [1, 0], [0, 1]]) l = np.array([1, 0, 0]) u = np.array([1, 0.7, 0.7]) # Create an OSQP object prob = osqp.OSQP() # Setup workspace and change alpha parameter prob.setup(P, q, A, l, u, alpha=1.0) # Solve problem res = prob.solve() # ================================================== # # ====== Create the C code ========================= # ## Create an OSQP object m = osqp.OSQP() ## Solver initialization ###settings = [eps_abs='1.e-3'] m.setup(P, q, A, l, u, eps_abs=1.e-3) ## Generate code m.codegen('code1', project_type='Makefile', parameters='vectors')