In [1]:
import numpy as np
import cvxpy as cp
import plotly.express as px
import copy
Define constants for problem $$\mathrm{min}_{x\in[x^{min}, x^{max}]}~c_0x^5 + c_1x^4 + c_2x^3 + c_3x^2 + c_4x + c_5~\mathrm{s.t.}~\lVert x - x_c \rVert \geq r$$
In [2]:
obstacle_radius = 0.25
obstacle_center = 0.25
c = [2.8, 3.7, -3.5, -3.1, 0.6, 0.6]
x_min = -1.5
x_max = 1
n_agents = 3
initial_guess_list = np.linspace(x_min, x_max, n_agents)
Objective and constraint functions
In [3]:
f = lambda x: c[0]*x**5 + c[1]*x**4 + c[2]*x**3 + c[3]*x**2 + c[4]*x + c[5]
g = lambda x: obstacle_radius - np.linalg.norm(x - obstacle_center)
grad_f_lin = lambda x: 5*c[0]*x**4 + 4*c[1]*x**3 + 3*c[2]*x**2 + 2*c[3]*x + c[4]
grad_g_lin = lambda x: -(x - obstacle_center) / np.linalg.norm(x - obstacle_center) if np.linalg.norm(x - obstacle_center) > 0 else np.zeros_like(x)
Solve with SCP
In [4]:
# Parameters
initial_guess = 0.2
w_tr = 10
w_vb = 1.2
In [5]:
# Define optimization problem
x_var = cp.Variable(name='x')
x_bar = cp.Parameter(name='x_bar')
f_x = cp.Parameter(name='f_x')
g_x = cp.Parameter(name='g_x')
grad_fx = cp.Parameter(name='grad_fx')
grad_gx = cp.Parameter(name='grad_gx')
constraints = [x_var <= x_max, x_var >= x_min]
objective = cp.Minimize(f_x + grad_fx*(x_var - x_bar) + w_tr*cp.sum_squares(x_var - x_bar)
+ w_vb*cp.maximum(0, g_x + grad_gx*(x_var - x_bar)))
problem = cp.Problem(objective, constraints)
In [6]:
# Solve problem
x_list = []
for i in range(n_agents):
x_p = initial_guess_list[i]
x_list.append([])
k = 0
while k < 1000:
x_m = x_p
x_list[i].append(x_p)
problem.param_dict['x_bar'].value = x_m
problem.param_dict['f_x'].value = f(x_m)
problem.param_dict['g_x'].value = g(x_m)
problem.param_dict['grad_fx'].value = grad_f_lin(x_m)
problem.param_dict['grad_gx'].value = grad_g_lin(x_m)
problem.solve()
x_p = x_var.value
if abs(x_p - x_m) < 1e-6:
x_list[i].append(x_p)
break
k += 1
print(f"Optimal x: {x_var.value}, f(x): {f(x_var.value)}, g(x): {g(x_var.value)}, iterations: {k}")
Optimal x: -1.5, f(x): 2.0062499999999996, g(x): -1.5, iterations: 0 Optimal x: -0.5117153843942663, f(x): 0.10566112071375727, g(x): -0.5117153843942663, iterations: 23 Optimal x: 0.7209174997030583, f(x): -0.3453079369713802, g(x): -0.2209174997030583, iterations: 9
In [7]:
# Plot results
x = np.linspace(-1.5, 1, 100)
y = f(x)
fig = px.line(x=x, y=y, title='Plot of f(x)')
fig.add_shape(type="rect",
x0=obstacle_center-obstacle_radius, x1=obstacle_center+obstacle_radius,
y0=np.min(y), y1=np.max(y),
fillcolor="LightSalmon", opacity=0.3, line_width=0, layer="below")
opacities = np.linspace(0.2, 1.0, len(x_list))
for i in range(n_agents):
fig.add_scatter(x=x_list[i], y=f(np.array(x_list[i])), mode='markers+lines', name=f'Agent {i+1} iterate',
line=dict(width=2),
marker=dict(opacity=opacities, size=8))
x_sol = x_list[i][-1]
fig.add_scatter(x=[x_sol], y=[f(x_sol)], mode='markers', name=f'Solution Agent {i+1}',
marker=dict(size=10, color='black', symbol='x', line=dict(color='white', width=2)))
fig.show()
Solve with OS-SCP
In [8]:
# Parameters
# Re-tuned from conda sweep to avoid the x=-1.5 attractor and favor x ~= 0.75.
w_tr = 5
w_vb = 0.7
w_c = 0.2
alpha = 2.331
dual_weight = 0.01795
In [9]:
# Define optimization problem
x_var = cp.Variable(name='x')
x_bar = cp.Parameter(name='x_bar')
f_x = cp.Parameter(name='f_x')
g_x = cp.Parameter(name='g_x')
grad_fx = cp.Parameter(name='grad_fx')
grad_gx = cp.Parameter(name='grad_gx')
x_av = cp.Parameter(name='x_av')
xi = cp.Parameter(name='xi')
constraints = [x_var <= x_max, x_var >= x_min]
objective = cp.Minimize(w_c * (f_x + grad_fx*(x_var - x_bar)) + alpha/2 * cp.sum_squares(x_var - x_av + xi)
+ w_tr*cp.sum_squares(x_var - x_bar)
+ w_vb*cp.maximum(0, g_x + grad_gx*(x_var - x_bar)))
problem = cp.Problem(objective, constraints)
In [10]:
# Solve problem
x_m_list = np.zeros(n_agents)
x_p_list = initial_guess_list
x_list = []
xi_val = np.zeros(n_agents)
k = 0
iter_max = 10000
while k < iter_max:
x_m_list = copy.deepcopy(x_p_list)
x_list.append(copy.deepcopy(x_p_list))
f_x = np.array([f(x_m) for x_m in x_m_list])
g_x = np.array([g(x_m) for x_m in x_m_list])
g_viol = np.maximum(g_x, 0)
# Quality metric: higher quality for lower cost and lower violations
quality = 1.0 / (1.0 + 500*np.abs(f_x) + 1.0 * g_viol)
w = quality / np.sum(quality) # L1 normalization: sum(w) = 1
x_av = np.sum(w * x_m_list)
xi_val = xi_val + dual_weight * (x_m_list - x_av)
problem.param_dict['x_av'].value = x_av
for i in range(n_agents):
problem.param_dict['xi'].value = xi_val[i]
problem.param_dict['x_bar'].value = x_m_list[i]
problem.param_dict['f_x'].value = f(x_m_list[i])
problem.param_dict['g_x'].value = g(x_m_list[i])
problem.param_dict['grad_fx'].value = grad_f_lin(x_m_list[i])
problem.param_dict['grad_gx'].value = grad_g_lin(x_m_list[i])
problem.solve()
x_p_list[i] = x_var.value
cnt = 0
for i in range(n_agents):
if abs(x_p_list[i] - x_m_list[i]) < 1e-6:
cnt += 1
if cnt == n_agents:
x_list.append(copy.deepcopy(x_p_list))
break
k += 1
x_list = np.array(x_list)
print(f"Optimal x: {x_list[-1]}, f(x): {f(x_list[-1])}, g(x): {g(x_list[-1])}, iterations: {k}")
Optimal x: [0.75721115 0.75711214 0.75688255], f(x): [-0.3292828 -0.32937306 -0.32958133], g(x): -0.6282686343997237, iterations: 1024
In [11]:
# Plot results
fig = px.line(x=x, y=y, title='Plot of f(x)')
fig.add_shape(type="rect",
x0=obstacle_center-obstacle_radius, x1=obstacle_center+obstacle_radius,
y0=np.min(y), y1=np.max(y),
fillcolor="LightSalmon", opacity=0.3, line_width=0, layer="below")
opacities = np.linspace(0.2, 1.0, len(x_list))
for i in range(n_agents):
fig.add_scatter(x=x_list[:, i], y=f(np.array(x_list[:, i])), mode='markers+lines', name=f'ADMM Iterates Agent {i+1}',
line=dict(width=2),
marker=dict(opacity=opacities, size=8))
# Highlight final solution for each agent.
x_sol = x_list[-1, i]
fig.add_scatter(x=[x_sol], y=[f(x_sol)], mode='markers', name=f'Solution Agent {i+1}',
marker=dict(size=10, color='black', symbol='x', line=dict(color='white', width=2)))
fig.show()