def norm(x, p=2, axis=None): """Wrapper on the different norm atoms. Parameters ---------- x : Expression or numeric constant The value to take the norm of. If `x` is 2D and `axis` is None, this function constructs a matrix norm. p : int or str, optional The type of norm. Valid options include any positive integer, 'fro' (for frobenius), 'nuc' (sum of singular values), np.inf or 'inf' (infinity norm). axis : The axis along which to apply the norm, if any. Returns ------- Expression An Expression representing the norm. """ x = Expression.cast_to_const(x) # matrix norms take precedence num_nontrivial_idxs = sum([d > 1 for d in x.shape]) if axis is None and x.ndim == 2: if p == 1: # matrix 1-norm return cvxpy.atoms.max(norm1(x, axis=0)) # Frobenius norm elif p == 'fro' or (p == 2 and num_nontrivial_idxs == 1): return pnorm(vec(x), 2) elif p == 2: # matrix 2-norm is largest singular value return sigma_max(x) elif p == 'nuc': # the nuclear norm (sum of singular values) return normNuc(x) elif p in [np.inf, "inf", "Inf"]: # the matrix infinity-norm return cvxpy.atoms.max(norm1(x, axis=1)) else: raise RuntimeError('Unsupported matrix norm.') else: if p == 1 or x.is_scalar(): return norm1(x, axis=axis) elif p in [np.inf, "inf", "Inf"]: return norm_inf(x, axis) else: return pnorm(x, p, axis)
def pnorm(x, p=2, axis=None, keepdims=False, max_denom=1024): """Factory function for a mathematical p-norm. Parameters ---------- p : numeric type or string The type of norm to construct; set this to np.inf or 'inf' to construct an infinity norm. Returns ------- Atom A norm1, norm_inf, or Pnorm object. """ if p == 1: return norm1(x, axis=axis, keepdims=keepdims) elif p in [np.inf, 'inf', 'Inf']: return norm_inf(x, axis=axis, keepdims=keepdims) else: return Pnorm(x, p=p, axis=axis, keepdims=keepdims, max_denom=max_denom)
def TP_MPC(mu, J_x, J_u, x_max, x_min, init_traj, T_mpc, x_0, xf, Q, R): feas_flag = True Q = 10 # state cost R = 1 # control cost dt = 1.0 # time step #flag = 3 u_thresh = 10.0 x = Variable((1, T_mpc + 1)) # state variable u = Variable((1, T_mpc)) # control variable #d = Variable((1, T_mpc+1)) # control invariant set cost = 0 constr = [] constr += [x[:, 0] == x_0] for t in range(T_mpc): cost += Q * norm(x[:, t] - xf)**2 + R * norm( u[:, t] )**2 #Q*norm(x[:,t]-d[:,t])**2 +R*norm(u[:,t]-(-1/dt*np.cbrt(root_cnst*xf)-1/(dt*3)*np.cbrt(root_cnst)*(np.cbrt(xf))**(-2)*(d[:,t]-xf)+xf))**2 # nonlinear cases constr += [ x[:, t + 1] == mu[t] + J_x[t] * (x[:, t] - init_traj[t]) + J_u[t] * u[:, t], # x[:,t] <= x_max[t], x[:,t] >= x_min[t], norm_inf(u[:, t]) <= u_thresh ] constr += [x[:, T_mpc][0] <= xf + 0.1, x[:, T_mpc][0] >= xf - 0.1] cost += Q * norm(x[:, T_mpc] - xf)**2 problem = Problem(Minimize(cost), constr) problem.solve( solver=OSQP, verbose=False ) # eps_abs = 1.0e-02, eps_rel = 1.0e-02, eps_prim_inf = 1.0e-02 # print('Total cost in iteration {} is {}.'.format(j, problem.value)) # print('******************************') # print('MPC iteration {}.'.format(j)) # print('******************************') # print('Target state is {}.'.format(xf)) # print('******************************') # print('Actual end State is {}.'.format(x.value[0][0])) # print('******************************') return x.value[0], u.value[0]
def norm(x, p=2, axis=None): """Wrapper on the different norm atoms. Parameters ---------- x : Expression or numeric constant The value to take the norm of. p : int or str, optional The type of norm. Returns ------- Expression An Expression representing the norm. """ x = Expression.cast_to_const(x) # matrix norms take precedence if axis is None and x.ndim == 2: if p == 1: # matrix 1-norm return cvxpy.atoms.max(norm1(x, axis=0)) elif p == 2: # matrix 2-norm is largest singular value return sigma_max(x) elif p == 'nuc': # the nuclear norm (sum of singular values) return normNuc(x) elif p == 'fro': # Frobenius norm return pnorm(vec(x), 2) elif p in [np.inf, "inf", "Inf"]: # the matrix infinity-norm return cvxpy.atoms.max(norm1(x, axis=1)) else: raise RuntimeError('Unsupported matrix norm.') else: if p == 1 or x.is_scalar(): return norm1(x, axis=axis) elif p in [np.inf, "inf", "Inf"]: return norm_inf(x, axis) else: return pnorm(x, p, axis)
def policy(self, observations): A = np.zeros((len(observations), self.action_dim_per_agent)) print('t: ', self.env.times[self.env.time_step]) for agent_i in self.env.agents: observation = observations[agent_i.i] relative_goal = observation[0:self.state_dim_per_agent] relative_neighbors = observation[self.state_dim_per_agent:] n_neighbor = int( len(relative_neighbors) / self.state_dim_per_agent) # calculate nominal controller a_nom = self.param.cbf_kp * relative_goal[ 0:2] + self.param.cbf_kv * relative_goal[ 2:] # [pgx - pix, pgy - piy] scale = self.alpha / np.max(np.abs(a_nom)) if scale < 1: a_nom = scale * a_nom # print() # print('n_neighbor:',n_neighbor) # print('i: ', agent_i.i) # print('observation: ', observation) # print('relative_goal: ', relative_goal) # print('vi: ', relative_goal[2:]) # print('relative_neighbors: ', relative_neighbors) # print('sg: ', agent_i.s_g) # print('agent_i.p: ', agent_i.p) # print('a_nom: ', a_nom) # print() # exit() # CVX a_i = cp.Variable(self.action_dim_per_agent) v_i = -1 * relative_goal[2:] dt = self.param.sim_dt constraints = [] if not n_neighbor == 0: for j in range(n_neighbor): rn_idx = self.state_dim_per_agent * j + np.arange( 0, self.state_dim_per_agent, dtype=int) p_ij = -1 * relative_neighbors[rn_idx[0:2]] v_ij = -1 * relative_neighbors[rn_idx[2:]] A_ij = -p_ij.T b_ij = 1 / 2 * self.get_b_ij(p_ij, v_ij) constraints.append(A_ij @ a_i <= b_ij) # acceleration and velocity limits constraints.append(norm_inf(a_i) <= self.alpha) constraints.append(norm_inf(v_i + a_i * dt) <= self.param.v_max) obj = cp.Minimize(cp.sum_squares(a_i - a_nom)) prob = cp.Problem(obj, constraints) # print('Solving...') try: prob.solve(verbose=False, solver=cp.GUROBI) # prob.solve(verbose=True) if prob.status in ["optimal"]: a_i = np.array(a_i.value) else: # do nothing # a_i = 0*a_nom # brake # a_i = -self.alpha*relative_goal[0:2] # backup a_i = -self.alpha * v_i / np.linalg.norm(v_i) except Exception as e: print(e) # do nothing # a_i = 0*a_nom # brake # a_i = -self.alpha*relative_goal[0:2] # backup a_i = -self.alpha * v_i / np.linalg.norm(v_i) A[agent_i.i, :] = a_i + self.param.cbf_noise * np.random.normal( size=(1, 2)) # A[agent_i.i,:] = a_i # exit() # print('A: ',A) return A