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
