├── .gitignore ├── LICENSE ├── README.md ├── SOVA_ADMM_mpc_trj_data.npz ├── distributed_ADMM ├── .gitignore ├── MPC_casadi_demo.ipynb ├── logs │ ├── ADMM_mpc_3_to_7_agents.xlsm │ ├── ADMM_mpc_3_to_8_agents.xlsm │ ├── ADMM_mpc_max_Iters_1_to_10.xlsm │ ├── ICRA2024_Monte_Carlo.xlsm │ ├── ICRA2024_Monte_Carlo.xlsm:Zone.Identifier │ ├── dpilqr_3_to_8.xlsm │ ├── ilqr_vs_centralized.xlsm │ ├── optimality_gap_60Iters.csv:Zone.Identifier │ ├── optimality_gap_60Trials.xlsm │ ├── optimality_gap_admm.xlsm │ └── ~$ilqr_vs_centralized.xlsm ├── main.py ├── monte_carlo_analysis.ipynb ├── paper_results │ ├── ADMM_IPOPT_trial.png │ ├── ADMM_mpc.png │ ├── ADMM_std_computation_time.png │ ├── ADMM_vs_DPILQR.png │ ├── ADMM_vs_ilqr.png │ ├── SOVA_ADMM_mpc.png │ ├── SOVA_admm_optimality_gap.png │ ├── avg_computation_times_comparison.png │ ├── convergence_rate.png │ ├── convergence_rate_QP.png │ ├── optimality_gaps.png │ ├── pairwise_distances(ADMM).png │ ├── pairwise_distances(ADMM_QP).png │ ├── pairwise_distances(SOVA_mpc).png │ ├── pairwise_distances(vanilla_mpc).png │ ├── pairwise_distances(vanilla_mpc_QP).png │ ├── potential_admm_optimality_gap.png │ └── std_computation_times_comparison.png ├── plot_trj.ipynb ├── trials(scratch book) │ ├── Pairwise_distance_QP.png │ ├── QP_pairwise_distance.png │ ├── SCP_pairwise_distance.png │ ├── SCP_random_init.png │ ├── dynamics.py │ ├── main_Convex.py │ ├── mpc_QP(cvxpy).ipynb │ ├── scp_ADMM.ipynb │ └── scratch.ipynb └── util.py ├── regular_ADMM_mpc_trj_data.npz └── requirements.txt /.gitignore: -------------------------------------------------------------------------------- 1 | **/.ipynb_checkpoints/ 2 | *.pyc 3 | *.lock 4 | notebooks/ 5 | #logs/ 6 | env/ 7 | distributed_ADMM/trials(scratch book) 8 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Jushan Chen 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Code implementation for distributed Potential-ADMM 2 | [![License: 3 | MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 4 | 5 | Code for the paper: "Efficient and Distributed Multi-Agent Interactive Trajectory 6 | Optimization via ADMM and Dynamic Potential Games" 7 | 8 | ## Installation 9 | You can recreate the python environment used to run these files via: 10 | ``` 11 | conda create --name --file requirements.txt 12 | ``` 13 | 14 | Similiarily, using a standard Python 3 installation, you can also use: 15 | ``` 16 | python3 -m venv env 17 | source env/bin/activate 18 | pip install -r pipRequirements.txt 19 | ``` 20 | 21 | ## Simulations 22 | To re-run the comparison between potential-ADMM and [distributed potential iLQR](https://ieeexplore.ieee.org/document/10161176), please download the dp-ilqr repo [here](https://github.com/labicon/dp-ilqr) 23 | 24 | 25 | -------------------------------------------------------------------------------- /SOVA_ADMM_mpc_trj_data.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/SOVA_ADMM_mpc_trj_data.npz -------------------------------------------------------------------------------- /distributed_ADMM/.gitignore: -------------------------------------------------------------------------------- 1 | .ipynb_checkpoints/ 2 | ._pycache_/ 3 | 4 | -------------------------------------------------------------------------------- /distributed_ADMM/logs/ADMM_mpc_3_to_7_agents.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/ADMM_mpc_3_to_7_agents.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/ADMM_mpc_3_to_8_agents.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/ADMM_mpc_3_to_8_agents.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/ADMM_mpc_max_Iters_1_to_10.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/ADMM_mpc_max_Iters_1_to_10.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/ICRA2024_Monte_Carlo.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/ICRA2024_Monte_Carlo.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/ICRA2024_Monte_Carlo.xlsm:Zone.Identifier: -------------------------------------------------------------------------------- 1 | [ZoneTransfer] 2 | ZoneId=3 3 | HostUrl=https://files.slack.com/files-pri/T019BGU9HM3-F05S53JQP7E/download/icra2024_monte_carlo.xlsm?origin_team=T019BGU9HM3 4 | -------------------------------------------------------------------------------- /distributed_ADMM/logs/dpilqr_3_to_8.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/dpilqr_3_to_8.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/ilqr_vs_centralized.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/ilqr_vs_centralized.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/optimality_gap_60Iters.csv:Zone.Identifier: -------------------------------------------------------------------------------- 1 | [ZoneTransfer] 2 | ZoneId=3 3 | ReferrerUrl=https://hal-ondemand.ncsa.illinois.edu/pun/sys/dashboard/files/fs//home/jushanchen/projects/distributed_ADMM/logs 4 | HostUrl=https://hal-ondemand.ncsa.illinois.edu/pun/sys/dashboard/files/fs//home/jushanchen/projects/distributed_ADMM/logs/ADMM-mpc-_08-06-23_14.19.56_%7Bgetpid()%7D.csv?download=1691418476969 5 | -------------------------------------------------------------------------------- /distributed_ADMM/logs/optimality_gap_60Trials.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/optimality_gap_60Trials.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/optimality_gap_admm.xlsm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/logs/optimality_gap_admm.xlsm -------------------------------------------------------------------------------- /distributed_ADMM/logs/~$ilqr_vs_centralized.xlsm: -------------------------------------------------------------------------------- 1 | Randy666 Randy666 -------------------------------------------------------------------------------- /distributed_ADMM/main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from casadi import * 4 | import casadi as cs 5 | import dpilqr 6 | from time import perf_counter 7 | import sys 8 | 9 | import logging 10 | from pathlib import Path 11 | import multiprocessing as mp 12 | from os import getpid 13 | import os 14 | from time import strftime 15 | 16 | from util import ( 17 | compute_pairwise_distance_nd_Sym, 18 | define_inter_graph_threshold, 19 | distance_to_goal, 20 | split_graph, 21 | objective, 22 | ) 23 | 24 | from dpilqr.cost import GameCost, ProximityCost, ReferenceCost 25 | from dpilqr.dynamics import ( 26 | QuadcopterDynamics6D, 27 | MultiDynamicalModel, 28 | ) 29 | from dpilqr.distributed import solve_rhc 30 | from dpilqr.problem import ilqrProblem 31 | from dpilqr.util import split_agents_gen, random_setup 32 | 33 | import util 34 | from multiprocessing import Process, Pipe 35 | 36 | opts = {'error_on_fail':False} 37 | 38 | def solve_iteration(n_states, n_inputs, n_agents, x0, xr, T, radius, Q, R, Qf, MAX_ITER = 5): 39 | """Define constants""" 40 | #T is the horizon 41 | nx = n_states*n_agents 42 | nu = n_inputs*n_agents 43 | N = n_agents 44 | 45 | x_dims = [n_states] * N 46 | n_dims = [3]*N 47 | 48 | # u_ref = np.array([0, 0, 9.8] * N).reshape(-1,1) 49 | u_ref = np.array([0, 0, 0]*N).reshape(-1,1) 50 | 51 | """Creating empty dicts to hold Casadi variables for each worker machine""" 52 | f_list = {} 53 | d = {} 54 | states = {} 55 | dt = 0.1 56 | for id in range(N): 57 | d["opti_{0}".format(id)] = Opti() 58 | 59 | #Augmented state : Y = (x(0),x(1),...,x(N),u(0),...,u(N-1)) 60 | 61 | states["Y_{0}".format(id)] = d[f"opti_{id}"].variable((T+1)*nx + T* nu) 62 | cost = 0 63 | 64 | #Quadratic tracking cost 65 | 66 | for t in range(T): 67 | for idx in range(nx): 68 | cost += (states[f"Y_{id}"][:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) * \ 69 | Q[idx,idx]* (states[f"Y_{id}"][:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) 70 | for idu in range(nu): 71 | cost += (states[f"Y_{id}"][(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) * \ 72 | R[idu,idu] * (states[f"Y_{id}"][(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) 73 | 74 | for idf in range(nx): 75 | cost += (states[f"Y_{id}"][:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) * \ 76 | Qf[idf,idf] * (states[f"Y_{id}"][:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) 77 | 78 | # f_list.append(cost) 79 | f_list["cost_{0}".format(id)] = cost 80 | 81 | 82 | def run_worker(agent_id, cost, pipe): 83 | xbar = d[f"opti_{agent_id}"].parameter((T+1)*nx + T*nu) 84 | d[f"opti_{agent_id}"].set_value(xbar, cs.GenDM_zeros((T+1)*nx + T*nu,1)) 85 | 86 | u = d[f"opti_{agent_id}"].parameter((T+1)*nx + T*nu) 87 | d[f"opti_{agent_id}"].set_value(u, cs.GenDM_zeros((T+1)*nx + T*nu,1)) 88 | 89 | #This is the scaled Lagrange multiplier 90 | # rho = 1.0 91 | rho = 0.1 92 | cost += (rho/2)*sumsqr(states[f"Y_{agent_id}"] - xbar + u) 93 | 94 | # ADMM loop 95 | iter = 0 96 | 97 | while True: 98 | try: 99 | coll_cost = 0 100 | smooth_trj_cost = 0 101 | f = util.generate_f(x_dims) 102 | for k in range(T): 103 | k1 = f(states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx],states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 104 | k2 = f(states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx]+dt/2*k1, states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 105 | k3 = f(states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx]+dt/2*k2, states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 106 | k4 = f(states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx]+dt*k3, states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 107 | x_next = states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx] + dt/6*(k1+2*k2+2*k3+k4) 108 | d[f"opti_{agent_id}"].subject_to(states[f"Y_{agent_id}"][:(T+1)*nx][(k+1)*nx:(k+2)*nx]==x_next) # close the gaps 109 | 110 | d[f"opti_{agent_id}"].subject_to(states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu] <= np.tile(np.array([np.pi/6, np.pi/6, 20]),(N,)).reshape(-1,1)) 111 | d[f"opti_{agent_id}"].subject_to(np.tile(np.array([-np.pi/6, -np.pi/6, 0]),(N,)).reshape(-1,1) <= states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 112 | 113 | #Soft collision-avoidance constraints 114 | if n_agents > 1: 115 | distances = util.compute_pairwise_distance_nd_Sym(states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx], x_dims, n_dims) 116 | #Collision avoidance cost 117 | for dist in distances: 118 | # coll_cost += fmin(0,(dist - 2*radius))**2 * 1500 #Works for centralized ADMM MPC 119 | coll_cost += fmin(0,(dist - 2*radius))**2 * 1200 120 | 121 | #Trajectory smoothing term 122 | for ind in range(nx): 123 | smooth_trj_cost += (states[f"Y_{agent_id}"][:(T+1)*nx][(k+1)*nx:(k+2)*nx][ind]-\ 124 | states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx][ind])**2 125 | 126 | X0 = d[f"opti_{agent_id}"].parameter(x0.shape[0],1) 127 | 128 | d[f"opti_{agent_id}"].subject_to(states[f"Y_{agent_id}"][0:nx] == X0) 129 | 130 | cost_tot = cost + coll_cost/N + smooth_trj_cost 131 | 132 | d[f"opti_{agent_id}"].minimize(cost_tot) 133 | d[f"opti_{agent_id}"].solver("ipopt") 134 | 135 | if iter > 0: 136 | d[f"opti_{agent_id}"].set_initial(sol_prev.value_variables()) 137 | 138 | d[f"opti_{agent_id}"].set_value(X0,x0) 139 | sol = d[f"opti_{agent_id}"].solve() 140 | 141 | sol_prev = sol 142 | pipe.send(sol.value(states[f"Y_{agent_id}"])) 143 | 144 | d[f"opti_{agent_id}"].set_value(xbar, pipe.recv()) #receive the averaged result from the main process. 145 | d[f"opti_{agent_id}"].set_value(u, sol.value( u + states[f"Y_{agent_id}"] - xbar)) 146 | 147 | # print(f'Current iteration is {iter}') 148 | 149 | d[f"opti_{agent_id}"].subject_to() 150 | 151 | iter += 1 152 | 153 | except EOFError: 154 | print("Connection closed.") 155 | break 156 | 157 | pipes = [] 158 | procs = [] 159 | for i in range(N): 160 | local, remote = Pipe() 161 | pipes += [local] 162 | procs += [Process(target=run_worker, args=(i, f_list[f"cost_{i}"], remote))] 163 | procs[-1].start() 164 | 165 | 166 | solution_list = [] 167 | admm_iter_t0 = perf_counter() 168 | 169 | x_bar_history = [np.ones((nx, 1))*np.inf] 170 | iter = 0 171 | t0 = perf_counter() 172 | for i in range(MAX_ITER): 173 | 174 | # Gather and average Y_i 175 | xbar = sum(pipe.recv() for pipe in pipes)/N 176 | 177 | x_bar_history.append(xbar) 178 | solution_list.append(xbar) 179 | 180 | # Scatter xbar 181 | for pipe in pipes: 182 | pipe.send(xbar) 183 | 184 | iter += 1 185 | 186 | # if np.linalg.norm(x_bar_history[iter]-x_bar_history[iter-1]) <= 1e-5: 187 | # print(f'Consensus reached after {iter} iterations!') 188 | 189 | # break 190 | 191 | admm_iter_time = perf_counter() - admm_iter_t0 192 | [p.terminate() for p in procs] 193 | 194 | 195 | x_trj_converged = solution_list[-1][:(T+1)*nx].reshape((T+1,nx)) 196 | u_trj_converged = solution_list[-1][(T+1)*nx:].reshape((T,nu)) 197 | 198 | coupling_cost = 0 199 | for k in range(x_trj_converged.shape[0]): 200 | distances = util.compute_pairwise_distance_nd_Sym(x_trj_converged[k,:].reshape(-1,1), x_dims, n_dims) 201 | for pair in distances: 202 | coupling_cost += fmin(0,(pair - 2*radius))**2 * 1200 203 | 204 | return x_trj_converged, u_trj_converged, admm_iter_time, coupling_cost 205 | 206 | 207 | def solve_admm_mpc(n_states, n_inputs, n_agents, x0, xr, T, radius, Q, R, Qf, MAX_ITER, n_trial=None): 208 | SOVA_admm = False 209 | nx = n_states*n_agents 210 | nu = n_inputs*n_agents 211 | n_dims = [3]*n_agents 212 | x_dims = [n_states]*n_agents 213 | X_full = np.zeros((0, nx)) 214 | U_full = np.zeros((0, nu)) 215 | X_full = np.r_[X_full, x0.reshape(1,-1)] 216 | u_ref = np.array([0, 0, 0]*n_agents) 217 | # u_ref = np.array([0, 0, 9.8]*n_agents) 218 | 219 | x_curr = x0 220 | mpc_iter = 0 221 | obj_history = [np.inf] 222 | solve_times = [] 223 | t = 0 224 | dt = 0.1 225 | t_kill = n_agents * T * dt 226 | 227 | while not np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 228 | 229 | try: 230 | x_trj_converged, u_trj_converged, admm_time, coupling_cost = solve_iteration(n_states, n_inputs, n_agents, x_curr, \ 231 | xr, T, radius, Q, R, Qf, MAX_ITER) 232 | 233 | solve_times.append(admm_time) 234 | except EOFError or RuntimeError: 235 | admm_time = np.inf 236 | solve_times.append(admm_time) 237 | print('Error encountered in solve_iteration!! Exiting...') 238 | converged = False 239 | 240 | logging.info( 241 | f'{n_trial},' 242 | f'{n_agents},{t},{converged},' 243 | f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times)},{np.std(solve_times)}, {MAX_ITER},' 244 | f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 245 | ) 246 | return X_full, U_full, obj_trj, np.mean(solve_times), obj_history 247 | 248 | 249 | # if admm_time >= 5*t_kill: 250 | # converged = False 251 | # obj_trj = np.inf 252 | # print('Solve time exceeded t_kill!！ Exiting...') 253 | # logging.info( 254 | # f'{n_trial},' 255 | # f'{n_agents},{t},{converged},' 256 | # f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times)},{np.std(solve_times)}, {MAX_ITER},' 257 | # f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 258 | # ) 259 | # return X_full, U_full, obj_trj, np.mean(solve_times), obj_history 260 | 261 | obj_history.append(float(objective(x_trj_converged.T, u_trj_converged.T, u_ref, xr, Q, R, Qf))\ 262 | +float(coupling_cost)) 263 | 264 | x_curr = x_trj_converged[1] 265 | u_curr = u_trj_converged[0] 266 | 267 | X_full = np.r_[X_full, x_curr.reshape(1,-1)] 268 | U_full = np.r_[U_full, u_curr.reshape(1,-1)] 269 | 270 | mpc_iter += 1 271 | t += dt 272 | if mpc_iter > 35: 273 | print('Max MPC iters reached!Exiting MPC loops...') 274 | converged = False 275 | break 276 | 277 | print(f'Final distance to goal is {dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)}') 278 | 279 | if np.all(dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 280 | converged = True 281 | 282 | coll_cost = 0 283 | for k in range(X_full.shape[0]): 284 | distances = util.compute_pairwise_distance_nd_Sym(X_full[k,:].reshape(-1,1), x_dims, n_dims) 285 | for pair in distances: 286 | coll_cost += fmin(0,(pair - 2*radius))**2 * 1200 287 | 288 | obj_trj = float(objective(X_full.T, U_full.T, u_ref, xr, Q, R, Qf) + coll_cost) 289 | 290 | logging.info( 291 | f'{n_trial},' 292 | f'{n_agents},{t},{converged},' 293 | f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times)},{np.std(solve_times)}, {MAX_ITER},' 294 | f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 295 | ) 296 | 297 | 298 | return X_full, U_full, obj_trj, np.mean(solve_times), obj_history 299 | 300 | def solve_distributed_rhc(ids, n_states, n_inputs, n_agents, x0, xr, T, radius, Q, R, Qf, MAX_ITER, n_trial=None): 301 | 302 | n_dims = [3]*n_agents 303 | u_ref = np.array([0, 0, 0]*n_agents) 304 | x_dims = [n_states]*n_agents 305 | u_dims = [n_inputs]*n_agents 306 | 307 | nx = n_states*n_agents 308 | nu = n_inputs*n_agents 309 | X_full = np.zeros((0, nx)) 310 | U_full = np.zeros((0, nu)) 311 | X_full = np.r_[X_full, x0.reshape(1,-1)] 312 | 313 | distributed_mpc_iters =0 314 | solve_times_mean = [] 315 | solve_times_std = [] 316 | 317 | x_curr = x0 318 | obj_history = [np.inf] 319 | t = 0 320 | dt = 0.1 321 | 322 | SOVA_admm = True 323 | 324 | t_kill = 5*n_agents * T * dt 325 | while not np.all(np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), \ 326 | n_agents, n_states, 3) <= 0.1)): 327 | # rel_dists = util.compute_pairwise_distance_nd_Sym(x0,x_dims,n_dims) 328 | graph = util.define_inter_graph_threshold(x_curr, radius, x_dims, ids, n_dims) 329 | 330 | split_states_initial = split_graph(x_curr.T, x_dims, graph) 331 | split_states_final = split_graph(xr.T, x_dims, graph) 332 | split_inputs_ref = split_graph(u_ref.reshape(-1, 1).T, u_dims, graph) 333 | 334 | X_dec = np.zeros((nx, 1)) 335 | U_dec = np.zeros((nu, 1)) 336 | 337 | # X_trj = np.zeros((nx, T+1)) 338 | # U_trj = np.zeros((nu, T)) 339 | 340 | solve_times_per_problem = [] 341 | 342 | for (x0_i, xf_i, u_ref_i , (prob, ids_) , place_holder) in zip(split_states_initial, 343 | split_states_final, 344 | split_inputs_ref, 345 | graph.items(), 346 | range(len(graph))): 347 | print(f'Current sub-problem has {x0_i.size//n_states} agents \n') 348 | 349 | try: 350 | x_trj_converged_i, u_trj_converged_i, iter_time_i, _ = solve_iteration(n_states, n_inputs, \ 351 | x0_i.size//n_states,\ 352 | x0_i.reshape(-1,1), \ 353 | xf_i.reshape(-1,1), \ 354 | T, radius, 355 | Q[:x0_i.size,:x0_i.size], 356 | R[:u_ref_i.size,:u_ref_i.size], 357 | Qf[:x0_i.size,:x0_i.size], MAX_ITER) 358 | solve_times_per_problem.append(iter_time_i) 359 | 360 | except EOFError or RuntimeError: 361 | converged = False 362 | obj_trj = np.inf 363 | logging.info( 364 | f'{n_trial},' 365 | f'{n_agents},{t},{converged},' 366 | f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times_mean)},{np.mean(solve_times_std)},{MAX_ITER},' 367 | f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 368 | ) 369 | return X_full, U_full, obj_trj, np.mean(solve_times_mean), obj_history 370 | 371 | # if iter_time_i >= t_kill: 372 | # converged = False 373 | # obj_trj = np.inf 374 | 375 | # logging.info( 376 | # f'{n_trial},' 377 | # f'{n_agents},{t},{converged},' 378 | # f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times_mean)},{np.mean(solve_times_std)},{MAX_ITER},' 379 | # f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 380 | # ) 381 | # return X_full, U_full, obj_trj, np.mean(solve_times_mean), obj_history 382 | 383 | i_prob = ids_.index(prob) 384 | 385 | #Collecting solutions from different potential game sub-problems at current time step K: 386 | X_dec[place_holder * n_states : (place_holder + 1) * n_states, :] = x_trj_converged_i[ 387 | 1, i_prob * n_states : (i_prob + 1) * n_states 388 | ].reshape(-1,1) 389 | 390 | U_dec[place_holder * n_inputs : (place_holder + 1) * n_inputs, :] = u_trj_converged_i[ 391 | 0, i_prob * n_inputs : (i_prob + 1) * n_inputs 392 | ].reshape(-1,1) 393 | 394 | coll_cost = 0 395 | for k in range(X_dec.reshape(1,-1).shape[0]-1): 396 | distances = util.compute_pairwise_distance_nd_Sym(X_dec[k,:], x_dims, n_dims) 397 | for pair in distances: 398 | coll_cost += fmin(0,(pair - 2*radius))**2 * 1200 399 | 400 | obj_curr = float(objective(X_dec,U_dec, u_ref, xr, Q, R, Qf) + coll_cost) 401 | obj_history.append(obj_curr) 402 | 403 | solve_times_mean.append(np.mean(solve_times_per_problem)) 404 | solve_times_std.append(np.std(solve_times_per_problem)) 405 | 406 | t += dt 407 | x_curr = X_dec 408 | 409 | X_full = np.r_[X_full, X_dec.reshape(1,-1)] 410 | U_full = np.r_[U_full, U_dec.reshape(1,-1)] 411 | 412 | distributed_mpc_iters += 1 413 | 414 | if distributed_mpc_iters > 35: 415 | print(f'Max iters reached; exiting MPC loops') 416 | converged = False 417 | break 418 | 419 | if np.all(dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 420 | converged = True 421 | 422 | print(f'Final distance to goal is {dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)}') 423 | 424 | 425 | coll_cost = 0 426 | for k in range(X_full.shape[0]): 427 | distances = util.compute_pairwise_distance_nd_Sym(X_full[k,:].reshape(-1,1), x_dims, n_dims) 428 | for pair in distances: 429 | coll_cost += fmin(0,(pair - 2*radius))**2 * 1200 430 | 431 | obj_trj = float(objective(X_full.T, U_full.T, u_ref, xr, Q, R, Qf) + coll_cost) 432 | 433 | 434 | logging.info( 435 | f'{n_trial},' 436 | f'{n_agents},{t},{converged},' 437 | f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times_mean)},{np.mean(solve_times_std)},{MAX_ITER},' 438 | f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 439 | ) 440 | 441 | return X_full, U_full, obj_trj, np.mean(solve_times_mean), obj_history 442 | 443 | def solve_mpc_centralized(n_agents, x0, xr, T, radius, Q, R, Qf, MAX_ITER, n_trial = None): 444 | SOVA_admm = 'centralized_mpc' 445 | nx = n_agents * 6 446 | nu = n_agents * 3 447 | N = n_agents 448 | opti = Opti() 449 | Y_state = opti.variable((T+1)*nx + T*nu) 450 | cost = 0 451 | 452 | u_ref = np.array([0, 0, 0] * N).reshape(-1,1) 453 | 454 | for t in range(T): 455 | for idx in range(nx): 456 | cost += (Y_state[:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) * \ 457 | Q[idx,idx]* (Y_state[:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) 458 | for idu in range(nu): 459 | cost += (Y_state[(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) * \ 460 | R[idu,idu] * (Y_state[(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) 461 | 462 | for idf in range(nx): 463 | cost += (Y_state[:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) * \ 464 | Qf[idf,idf] * (Y_state[:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) 465 | 466 | obj_hist = [np.inf] 467 | x_curr = x0 468 | 469 | X_trj = np.zeros((0, nx)) 470 | U_trj = np.zeros((0, nu)) 471 | X_trj = np.r_[X_trj, x0.T] 472 | iters = 0 473 | 474 | solve_times = [] 475 | t = 0 476 | dt = 0.1 477 | 478 | x_dims = [6]*N 479 | n_dims = [3]*N 480 | f = util.generate_f(x_dims) 481 | 482 | while not np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 483 | coll_cost = 0 484 | smooth_trj_cost = 0 485 | for k in range(T): 486 | 487 | k1 = f(Y_state[:(T+1)*nx][k*nx:(k+1)*nx], Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 488 | k2 = f(Y_state[:(T+1)*nx][k*nx:(k+1)*nx]+dt/2*k1, Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 489 | k3 = f(Y_state[:(T+1)*nx][k*nx:(k+1)*nx]+dt/2*k2, Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 490 | k4 = f(Y_state[:(T+1)*nx][k*nx:(k+1)*nx]+dt*k3, Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 491 | x_next = Y_state[:(T+1)*nx][k*nx:(k+1)*nx] + dt/6*(k1+2*k2+2*k3+k4) 492 | 493 | opti.subject_to(Y_state[:(T+1)*nx][(k+1)*nx:(k+2)*nx]==x_next) # close the gaps 494 | 495 | opti.subject_to(Y_state[(T+1)*nx:][k*nu:(k+1)*nu] <= np.tile(np.array([np.pi/6, np.pi/6, 20]),(N,)).reshape(-1,1)) 496 | opti.subject_to(np.tile(np.array([-np.pi/6, -np.pi/6, 0]),(N,)).reshape(-1,1) <= Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 497 | 498 | #Pair-wise Euclidean distance between each pair of agents 499 | distances = util.compute_pairwise_distance_nd_Sym(Y_state[:(T+1)*nx][k*nx:(k+1)*nx],x_dims, n_dims) 500 | #Collision avoidance cost 501 | for dist in distances: 502 | coll_cost += fmin(0,(dist - 2*radius))**2 * 1200 503 | # coll_cost += fmin(0,(dist - radius))**2 * 400 504 | 505 | #Smoothing term 506 | for ind in range(nx): 507 | smooth_trj_cost += (Y_state[:(T+1)*nx][(k+1)*nx:(k+2)*nx][ind]-\ 508 | Y_state[:(T+1)*nx][k*nx:(k+1)*nx][ind])**2 509 | 510 | X0 = opti.parameter(x0.shape[0],1) 511 | opti.subject_to(Y_state[0:nx] == X0) 512 | 513 | cost_tot = cost + coll_cost + smooth_trj_cost 514 | 515 | opti.minimize(cost_tot) 516 | 517 | opti.solver("ipopt") 518 | opti.set_value(X0,x_curr) 519 | 520 | if iters > 0: 521 | opti.set_initial(sol_prev.value_variables()) 522 | 523 | t0 = perf_counter() 524 | try: 525 | sol = opti.solve() 526 | 527 | except RuntimeError: 528 | converged=False 529 | break 530 | 531 | sol_prev = sol 532 | solve_times.append(perf_counter() - t0) 533 | obj_hist.append(sol.value(cost_tot)) 534 | 535 | ctrl = sol.value(Y_state)[(T+1)*nx:].reshape((T, nu))[0] 536 | x_curr = sol.value(Y_state)[:(T+1)*nx].reshape((T+1,nx))[1] #Same as above 537 | X_trj = np.r_[X_trj, x_curr.reshape(1,-1)] 538 | U_trj = np.r_[U_trj, ctrl.reshape(1,-1)] 539 | 540 | opti.subject_to() 541 | 542 | iters += 1 543 | t += dt 544 | if iters > 35: 545 | converged = False 546 | print(f'Max MPC iters reached; exiting MPC loops.....') 547 | break 548 | 549 | if np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 550 | converged = True 551 | 552 | 553 | obj_trj = float(util.objective(X_trj.T, U_trj.T, u_ref, xr, Q, R, Qf)) 554 | logging.info( 555 | f'{n_trial},' 556 | f'{n_agents},{t},{converged},' 557 | f'{obj_trj},{T},{dt},{radius},{SOVA_admm},{np.mean(solve_times)}, {np.std(solve_times)}, {MAX_ITER},' 558 | f'{dpilqr.distance_to_goal(X_trj[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 559 | ) 560 | 561 | return X_trj, U_trj, obj_trj, np.mean(solve_times), obj_hist 562 | 563 | 564 | def setup_logger(): 565 | 566 | # if centralized == True: 567 | 568 | LOG_PATH = Path(__file__).parent/ "logs" 569 | LOG_FILE = LOG_PATH / strftime( 570 | "ADMM-mpc-_%m-%d-%y_%H.%M.%S_{getpid()}.csv" 571 | ) 572 | if not LOG_PATH.is_dir(): 573 | LOG_PATH.mkdir() 574 | print(f"Logging results to {LOG_FILE}") 575 | logging.basicConfig(filename=LOG_FILE, format="%(message)s", level=logging.INFO) 576 | logging.info( 577 | "i_trial, n_agents, t, converged, obj_trj,T,dt,radius,\ 578 | SOVA_admm ,t_solve_avg, t_solve_std, MAX_ITER, dist_to_goal" 579 | ) 580 | 581 | 582 | def multi_agent_run(trial, 583 | n_states, 584 | n_inputs, 585 | n_agents, 586 | T, 587 | radius, 588 | Q, 589 | R, 590 | Qf, 591 | admm_iter): 592 | """simulation comparing the centralized and decentralized solvers""" 593 | 594 | if n_agents == 3: 595 | x0,xr = util.setup_3_quads() 596 | 597 | elif n_agents==4: 598 | x0, xr=util.setup_4_quads() 599 | 600 | elif n_agents == 5: 601 | x0,xr = util.setup_5_quads() 602 | 603 | elif n_agents==6: 604 | x0, xr=util.setup_6_quads() 605 | 606 | elif n_agents==7: 607 | x0, xr= util.setup_7_quads() 608 | 609 | elif n_agents==8: 610 | x0, xr= util.setup_8_quads() 611 | 612 | elif n_agents==9: 613 | x0, xr= util.setup_9_quads() 614 | 615 | elif n_agents == 10: 616 | x0,xr = util.setup_10_quads() 617 | 618 | ids = [100 + i for i in range(n_agents)] 619 | x_dims = [n_states] * n_agents 620 | n_dims = [3] * n_agents 621 | Q = np.diag([5., 5., 5., 1., 1., 1.]*n_agents) 622 | Qf = Q*500 623 | R = 0.1*np.eye(n_agents*n_inputs) 624 | 625 | """DP-ILQR""" 626 | ids = [100 + i for i in range(n_agents)] 627 | model = QuadcopterDynamics6D 628 | dt = 0.1 629 | dynamics = MultiDynamicalModel([model(dt, id_) for id_ in ids]) 630 | Q_ilqr = np.diag([5., 5., 5., 1., 1., 1.]) 631 | Qf_ilqr = Q_ilqr * 500 632 | R_ilqr = 0.1*np.eye(n_inputs) 633 | goal_costs = [ 634 | ReferenceCost(xf_i, Q_ilqr.copy(), R_ilqr.copy(), Qf_ilqr.copy(), id_) 635 | for xf_i, id_ in zip(split_agents_gen(xr, x_dims), ids) 636 | ] 637 | prox_cost = ProximityCost(x_dims, radius, n_dims) 638 | game_cost = GameCost(goal_costs, prox_cost) 639 | problem = ilqrProblem(dynamics, game_cost) 640 | pool = None 641 | STEP_SIZE=1 642 | t_diverge = T * 30 * dt 643 | t_kill = None 644 | 645 | n_d = 3 646 | STEP_SIZE = 1 647 | Xd, Ud, Jd = solve_rhc( 648 | problem, 649 | x0, 650 | T, 651 | centralized=False, 652 | n_d = n_d, 653 | step_size = STEP_SIZE, 654 | radius = radius, 655 | pool = None, 656 | t_kill=t_kill, 657 | dist_converge=0.1, 658 | t_diverge=t_diverge, 659 | i_trial=trial, 660 | verbose=False, 661 | ) 662 | 663 | """Centralized MPC baseline:""" 664 | X_full, U_full, obj, avg_SolveTime, _ = solve_mpc_centralized(n_agents, 665 | x0, 666 | xr, 667 | T, 668 | radius, 669 | Q, 670 | R, 671 | Qf, 672 | admm_iter, 673 | trial, 674 | ) 675 | 676 | 677 | """Regular Consensus ADMM:""" 678 | # X_full, U_full, obj, avg_SolveTime, _ = solve_admm_mpc(n_states, 679 | # n_inputs, 680 | # n_agents, 681 | # x0, 682 | # xr, 683 | # T, 684 | # radius, 685 | # Q, 686 | # R, 687 | # Qf, 688 | # admm_iter, 689 | # trial) 690 | 691 | """ Potential ADMM:""" 692 | X_full, U_full, obj, avg_SolveTime, _ = solve_distributed_rhc(ids, 693 | n_states, 694 | n_inputs, 695 | n_agents, 696 | x0, 697 | xr, 698 | T, 699 | radius, 700 | Q, 701 | R, 702 | Qf, 703 | admm_iter, 704 | trial) 705 | 706 | 707 | def monte_carlo_analysis(): 708 | """Benchmark to evaluate algorithm over many random initial conditions""" 709 | 710 | setup_logger() 711 | 712 | n_trials_iter = range(60) 713 | 714 | n_agents_iter = [3,4,5,6,7,8] 715 | 716 | admm_iters = [3] 717 | # admm_iters = [10] 718 | radius = 0.2 719 | 720 | # Change the for loops into multi-processing? 721 | 722 | for n_agents in n_agents_iter: 723 | print(f"\tn_agents: {n_agents}") 724 | 725 | # if n_agents >=5 and n_agents <=7: 726 | # radius = 0.3 727 | 728 | # if n_agents >= 8 : 729 | # radius = 0.15 730 | 731 | for i_trial in n_trials_iter: 732 | print(f"\t\ttrial: {i_trial}") 733 | 734 | for max_admm_iters in admm_iters: 735 | 736 | multi_agent_run( 737 | i_trial, 738 | n_states, 739 | n_inputs, 740 | n_agents, 741 | T, 742 | radius, 743 | Q, 744 | R, 745 | Qf, 746 | max_admm_iters) 747 | 748 | 749 | if __name__ == "__main__": 750 | 751 | n_states = 6 752 | n_inputs = 3 753 | n_agents = 3 754 | x_dims = [n_states]*n_agents 755 | T = 8 756 | # T = 10 757 | radius = 0.5 758 | Q = np.diag([5., 5., 5., 1., 1., 1.]*n_agents) 759 | Qf = Q*500 760 | R = 0.1*np.eye(n_agents*n_inputs) 761 | 762 | ids = [100 + n for n in range(n_agents)] #Assigning random IDs for agents 763 | 764 | Log_Data = True 765 | if not Log_Data: 766 | # admm_iter = 15 767 | # admm_iter = 5 768 | admm_iter = 5 769 | x0, xr = util.paper_setup_3_quads() 770 | 771 | x_dims = [6]*3 772 | n_dims = [3]*3 773 | 774 | X_full, U_full, obj_centralized, avg_SolveTime, obj_history_admm = solve_admm_mpc(n_states, 775 | n_inputs, 776 | n_agents, 777 | x0, 778 | xr, 779 | T, 780 | radius, 781 | Q, 782 | R, 783 | Qf, 784 | admm_iter) 785 | 786 | 787 | print(f'The average solve time is {avg_SolveTime} seconds!') 788 | 789 | np.savez("regular_ADMM_mpc_trj_data.npz", X_full = X_full, obj_sova = obj_centralized, xr = xr, x_dims = x_dims) 790 | 791 | #Plot trajectory 792 | plt.figure(dpi=150) 793 | dpilqr.plot_solve(X_full, float(obj_centralized), xr, x_dims, True, 3) 794 | plt.savefig('regular_ADMM_MPC_trj', bbox_inches='tight', dpi=300, transparent=True) 795 | 796 | plt.figure(dpi=150) 797 | dpilqr.plot_pairwise_distances(X_full, x_dims, n_dims, radius) 798 | plt.title('Pairwise-distances from C-ADMM') 799 | plt.savefig('pairwise_distances(ADMM).png') 800 | 801 | X_full, U_full, obj_sova, avg_SolveTime, obj_history_sova = solve_distributed_rhc(ids, 802 | n_states, 803 | n_inputs, 804 | n_agents, 805 | x0, 806 | xr, 807 | T, radius, 808 | Q, R, 809 | Qf, 810 | admm_iter 811 | ) 812 | 813 | print(f'The average solve time is {avg_SolveTime} seconds!') 814 | 815 | np.savez("SOVA_ADMM_mpc_trj_data.npz", X_full = X_full, obj_sova = obj_sova, xr = xr, x_dims = x_dims) 816 | 817 | plt.figure(dpi=150) 818 | dpilqr.plot_solve(X_full, float(obj_sova), xr, x_dims, True, 3) 819 | plt.savefig('SOVA_ADMM_mpc_trj', bbox_inches='tight', dpi=300, transparent=True) 820 | 821 | plt.figure(dpi=150) 822 | dpilqr.plot_pairwise_distances(X_full, x_dims, n_dims, radius) 823 | plt.title('Pairwise-distances from SOVA-ADMM MPC') 824 | plt.savefig('pairwise_distances(SOVA_mpc).png') 825 | 826 | 827 | # plt.figure(dpi=150) 828 | # plt.plot(np.linalg.norm(obj_centralized - obj_history_admm), 'r', label='Consensus-ADMM') 829 | # plt.plot(np.linalg.norm(obj_sova - obj_history_sova), 'b', label='Potential-ADMM') 830 | # plt.ylabel('Total Cost-to-go') 831 | # plt.xlabel('Horizon') 832 | # plt.legend(loc='best') 833 | # plt.savefig('convergence_rate.png') 834 | 835 | 836 | if Log_Data: 837 | monte_carlo_analysis() 838 | 839 | 840 | 841 | 842 | 843 | 844 | 845 | 846 | 847 | -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/ADMM_IPOPT_trial.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/ADMM_IPOPT_trial.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/ADMM_mpc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/ADMM_mpc.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/ADMM_std_computation_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/ADMM_std_computation_time.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/ADMM_vs_DPILQR.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/ADMM_vs_DPILQR.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/ADMM_vs_ilqr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/ADMM_vs_ilqr.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/SOVA_ADMM_mpc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/SOVA_ADMM_mpc.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/SOVA_admm_optimality_gap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/SOVA_admm_optimality_gap.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/avg_computation_times_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/avg_computation_times_comparison.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/convergence_rate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/convergence_rate.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/convergence_rate_QP.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/convergence_rate_QP.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/optimality_gaps.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/optimality_gaps.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/pairwise_distances(ADMM).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/pairwise_distances(ADMM).png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/pairwise_distances(ADMM_QP).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/pairwise_distances(ADMM_QP).png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/pairwise_distances(SOVA_mpc).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/pairwise_distances(SOVA_mpc).png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/pairwise_distances(vanilla_mpc).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/pairwise_distances(vanilla_mpc).png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/pairwise_distances(vanilla_mpc_QP).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/pairwise_distances(vanilla_mpc_QP).png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/potential_admm_optimality_gap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/potential_admm_optimality_gap.png -------------------------------------------------------------------------------- /distributed_ADMM/paper_results/std_computation_times_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/paper_results/std_computation_times_comparison.png -------------------------------------------------------------------------------- /distributed_ADMM/trials(scratch book)/Pairwise_distance_QP.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/trials(scratch book)/Pairwise_distance_QP.png -------------------------------------------------------------------------------- /distributed_ADMM/trials(scratch book)/QP_pairwise_distance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/trials(scratch book)/QP_pairwise_distance.png -------------------------------------------------------------------------------- /distributed_ADMM/trials(scratch book)/SCP_pairwise_distance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/trials(scratch book)/SCP_pairwise_distance.png -------------------------------------------------------------------------------- /distributed_ADMM/trials(scratch book)/SCP_random_init.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/distributed_ADMM/trials(scratch book)/SCP_random_init.png -------------------------------------------------------------------------------- /distributed_ADMM/trials(scratch book)/dynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import osqp 3 | import matplotlib.pyplot as plt 4 | from scipy import sparse 5 | from casadi import * 6 | import casadi as cs 7 | import dpilqr 8 | import itertools 9 | import cvxpy as cp 10 | 11 | 12 | 13 | def linear_kinodynamics(dt,n_agent): 14 | #Decision vector is a = [a_x, a_y, a_z] 15 | #State vector is X = [p_x, p_y, p_z, v_x, v_y, v_z] 16 | #Discretization time step is dt 17 | A_tot = np.zeros((6*n_agent, 6*n_agent)) 18 | B_tot = np.zeros((6*n_agent, 3*n_agent)) 19 | A = np.array([[1, 0, 0, dt, 0, 0], 20 | [0, 1, 0, 0 , dt ,0],\ 21 | [0, 0, 1, 0, 0 , dt],\ 22 | [0, 0, 0, 1, 0 ,0],\ 23 | [0, 0, 0, 0, 1 ,0],\ 24 | [0, 0, 0, 0, 0, 1]]) 25 | B = np.array([[dt**2/2, 0, 0],\ 26 | [0, dt**2/2, 0],\ 27 | [0, 0, dt**2/2],\ 28 | [dt, 0, 0 ],\ 29 | [0, dt , 0],\ 30 | [0, 0, dt]]) 31 | 32 | for i in range(n_agent): 33 | A_tot[i*6:(i+1)*6,i*6:(i+1)*6] = A 34 | B_tot[i*6:(i+1)*6,i*3:(i+1)*3] = B 35 | 36 | 37 | return A_tot, B_tot -------------------------------------------------------------------------------- /distributed_ADMM/trials(scratch book)/main_Convex.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from casadi import * 4 | import casadi as cs 5 | import dpilqr 6 | from time import perf_counter 7 | import sys 8 | 9 | import logging 10 | from pathlib import Path 11 | import multiprocessing as mp 12 | from os import getpid 13 | import os 14 | from time import strftime 15 | 16 | from solvers.util import ( 17 | compute_pairwise_distance_nd_Sym, 18 | define_inter_graph_threshold, 19 | distance_to_goal, 20 | split_graph, 21 | objective, 22 | ) 23 | 24 | from dpilqr.cost import GameCost, ProximityCost, ReferenceCost 25 | from dpilqr.dynamics import ( 26 | QuadcopterDynamics6D, 27 | MultiDynamicalModel, 28 | ) 29 | from dpilqr.distributed import solve_rhc 30 | from dpilqr.problem import ilqrProblem 31 | from dpilqr.util import split_agents_gen, random_setup 32 | 33 | 34 | from solvers import util 35 | from multiprocessing import Process, Pipe 36 | from dynamics import linear_kinodynamics 37 | 38 | opts = {'error_on_fail':False} 39 | 40 | def solve_iteration(n_states, n_inputs, n_agents, x0, xr, T, radius, Q, R, Qf, x_trj_init, state_prev, local_iter, MAX_ITER = 5): 41 | """Define constants""" 42 | #T is the horizon 43 | nx = n_states*n_agents 44 | nu = n_inputs*n_agents 45 | N = n_agents 46 | 47 | x_dims = [n_states] * N 48 | n_dims = [3]*N 49 | 50 | # u_ref = np.array([0, 0, 9.8] * N).reshape(-1,1) 51 | u_ref = np.array([0, 0, 0]*N).reshape(-1,1) 52 | 53 | """Creating empty dicts to hold Casadi variables for each worker machine""" 54 | f_list = {} 55 | d = {} 56 | states = {} 57 | dt = 0.1 58 | for id in range(N): 59 | d["opti_{0}".format(id)] = Opti('conic') 60 | 61 | #Augmented state : Y = (x(0),x(1),...,x(N),u(0),...,u(N-1)) 62 | 63 | states["Y_{0}".format(id)] = d[f"opti_{id}"].variable((T+1)*nx + T* nu) 64 | cost = 0 65 | 66 | #Quadratic tracking cost 67 | 68 | for t in range(T): 69 | for idx in range(nx): 70 | cost += (states[f"Y_{id}"][:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) * \ 71 | Q[idx,idx]* (states[f"Y_{id}"][:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) 72 | for idu in range(nu): 73 | cost += (states[f"Y_{id}"][(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) * \ 74 | R[idu,idu] * (states[f"Y_{id}"][(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) 75 | 76 | for idf in range(nx): 77 | cost += (states[f"Y_{id}"][:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) * \ 78 | Qf[idf,idf] * (states[f"Y_{id}"][:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) 79 | 80 | # f_list.append(cost) 81 | f_list["cost_{0}".format(id)] = cost 82 | 83 | 84 | def run_worker(agent_id, cost, pipe): 85 | xbar = d[f"opti_{agent_id}"].parameter((T+1)*nx + T*nu) 86 | d[f"opti_{agent_id}"].set_value(xbar, cs.GenDM_zeros((T+1)*nx + T*nu,1)) 87 | 88 | u = d[f"opti_{agent_id}"].parameter((T+1)*nx + T*nu) 89 | d[f"opti_{agent_id}"].set_value(u, cs.GenDM_zeros((T+1)*nx + T*nu,1)) 90 | 91 | #This is the scaled Lagrange multiplier 92 | 93 | rho = 1 94 | cost += (rho/2)*sumsqr(states[f"Y_{agent_id}"] - xbar + u) 95 | 96 | # ADMM loop 97 | iter = 0 98 | 99 | scaling_matrix = np.diag([1, 1, 2]) 100 | Ad,Bd = linear_kinodynamics(0.1, N) 101 | 102 | while True: 103 | try: 104 | smooth_trj_cost = 0 105 | for k in range(T): 106 | 107 | 108 | d[f"opti_{agent_id}"].subject_to(states[f"Y_{agent_id}"][:(T+1)*nx][(k+1)*nx:(k+2)*nx] \ 109 | == Ad @ states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx] \ 110 | + Bd @ states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 111 | 112 | d[f"opti_{agent_id}"].subject_to(states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu] <= np.tile(np.array([3, 3, 3]),(N,)).reshape(-1,1)) 113 | d[f"opti_{agent_id}"].subject_to(np.tile(np.array([-3, -3, -3]),(N,)).reshape(-1,1) <= states[f"Y_{agent_id}"][(T+1)*nx:][k*nu:(k+1)*nu]) 114 | 115 | #Linearized collision constraints: 116 | if N > 1: 117 | 118 | if local_iter <= 0: 119 | pos_prev = x_trj_init[k] 120 | print(f'pos_prev has shape {pos_prev.shape}') 121 | else: 122 | 123 | pos_prev = state_prev[:(T+1)*nx].reshape(T+1, nx)[k] 124 | # pos_prev = X_full[iter-1] 125 | # pos_curr = cp.reshape(y_state[:(T+1)*nx],[T+1,nx])[k] 126 | 127 | for i in range(N): 128 | for j in range(N): 129 | if j != i: 130 | #See "Generation of collision-free trajectories for a quadrocopter fleet: 131 | # A sequential convex programming approach" for the linearization step; 132 | linearized_dist = cs.norm_2(scaling_matrix@(pos_prev[j*n_states:j*n_states+3]- \ 133 | pos_prev[i*n_states:i*n_states+3])) + \ 134 | (pos_prev[j*n_states:j*n_states+3].reshape(1,-1)- \ 135 | pos_prev[i*n_states:i*n_states+3].reshape(1,-1))/cs.norm_2(scaling_matrix@(pos_prev[j*n_states:j*n_states+3]\ 136 | -pos_prev[i*n_states:i*n_states+3]))@ \ 137 | (states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx][j*n_states:j*n_states+3] \ 138 | -states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx][i*n_states:i*n_states+3]) 139 | 140 | d[f"opti_{agent_id}"].subject_to(linearized_dist >= radius) 141 | 142 | #Trajectory smoothing term 143 | for ind in range(nx): 144 | smooth_trj_cost += (states[f"Y_{agent_id}"][:(T+1)*nx][(k+1)*nx:(k+2)*nx][ind]-\ 145 | states[f"Y_{agent_id}"][:(T+1)*nx][k*nx:(k+1)*nx][ind])**2 146 | 147 | X0 = d[f"opti_{agent_id}"].parameter(x0.shape[0],1) 148 | 149 | d[f"opti_{agent_id}"].subject_to(states[f"Y_{agent_id}"][0:nx] == X0) 150 | 151 | cost_tot = cost + smooth_trj_cost 152 | 153 | d[f"opti_{agent_id}"].minimize(cost_tot) 154 | d[f"opti_{agent_id}"].solver("osqp",opts) 155 | # d[f"opti_{agent_id}"].solver("qpoases",opts) #this one is too slow 156 | # d[f"opti_{agent_id}"].solver("ipopt") 157 | 158 | if iter > 0: 159 | d[f"opti_{agent_id}"].set_initial(sol_prev.value_variables()) 160 | 161 | d[f"opti_{agent_id}"].set_value(X0,x0) 162 | sol = d[f"opti_{agent_id}"].solve() 163 | 164 | sol_prev = sol 165 | pipe.send(sol.value(states[f"Y_{agent_id}"])) 166 | 167 | d[f"opti_{agent_id}"].set_value(xbar, pipe.recv()) #receive the averaged result from the main process. 168 | d[f"opti_{agent_id}"].set_value(u, sol.value( u + states[f"Y_{agent_id}"] - xbar)) 169 | 170 | # print(f'Current iteration is {iter}') 171 | 172 | d[f"opti_{agent_id}"].subject_to() 173 | 174 | iter += 1 175 | 176 | except EOFError: 177 | print("Connection closed.") 178 | break 179 | 180 | pipes = [] 181 | procs = [] 182 | for i in range(N): 183 | local, remote = Pipe() 184 | pipes += [local] 185 | procs += [Process(target=run_worker, args=(i, f_list[f"cost_{i}"], remote))] 186 | procs[-1].start() 187 | 188 | 189 | solution_list = [] 190 | admm_iter_time = [] 191 | 192 | x_bar_history = [np.ones((nx, 1))*np.inf] 193 | iter = 0 194 | t0 = perf_counter() 195 | for i in range(MAX_ITER): 196 | 197 | # Gather and average Y_i 198 | xbar = sum(pipe.recv() for pipe in pipes)/N 199 | 200 | x_bar_history.append(xbar) 201 | solution_list.append(xbar) 202 | 203 | # Scatter xbar 204 | for pipe in pipes: 205 | pipe.send(xbar) 206 | 207 | iter += 1 208 | 209 | if np.linalg.norm(x_bar_history[iter]-x_bar_history[iter-1]) <= 1e-3: 210 | print(f'Consensus reached after {iter} iterations!') 211 | 212 | break 213 | 214 | admm_iter_time.append(perf_counter() - t0) 215 | [p.terminate() for p in procs] 216 | 217 | 218 | x_trj_converged = solution_list[-1][:(T+1)*nx].reshape((T+1,nx)) 219 | u_trj_converged = solution_list[-1][(T+1)*nx:].reshape((T,nu)) 220 | 221 | coupling_cost = 0 222 | # for k in range(x_trj_converged.shape[0]-1): 223 | # distances = util.compute_pairwise_distance_nd_Sym(x_trj_converged[k,:].reshape(-1,1), x_dims, n_dims) 224 | # for pair in distances: 225 | # coupling_cost += fmin(0,(pair - 2*radius))**2 * 1200 226 | 227 | return x_trj_converged, u_trj_converged, admm_iter_time, coupling_cost 228 | 229 | 230 | def solve_admm_mpc(n_states, n_inputs, n_agents, x0, xr, T, radius, Q, R, Qf, MAX_ITER, n_trial=None): 231 | centralized = True 232 | nx = n_states*n_agents 233 | nu = n_inputs*n_agents 234 | 235 | X_full = np.zeros((0, nx)) 236 | U_full = np.zeros((0, nu)) 237 | X_full = np.r_[X_full, x0.reshape(1,-1)] 238 | u_ref = np.array([0, 0, 0]*n_agents) 239 | # u_ref = np.array([0, 0, 9.8]*n_agents) 240 | 241 | x_curr = x0 242 | mpc_iter = 0 243 | obj_history = [np.inf] 244 | solve_times = [] 245 | t = 0 246 | dt = 0.1 247 | # t_kill = T*dt 248 | 249 | state_prev = None 250 | 251 | u_init = np.random.rand(3*n_agents)*0.1 252 | x_trj_init = np.zeros((0, nx)) 253 | x_trj_init = np.r_[x_trj_init, x0.reshape(1,-1)] 254 | Ad,Bd = linear_kinodynamics(0.1,n_agents) 255 | x_nominal = x0 256 | 257 | for _ in range(T): 258 | x_nominal = Ad@x_nominal + Bd@u_init.reshape(-1,1) 259 | x_trj_init = np.r_[x_trj_init, x_nominal.reshape(1,-1)] 260 | 261 | while not np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 262 | 263 | try: 264 | x_trj_converged, u_trj_converged, admm_time,coupling_cost = solve_iteration(n_states, n_inputs, n_agents, x_curr, \ 265 | xr, T, radius, Q, R, Qf, x_trj_init, state_prev, mpc_iter, MAX_ITER) 266 | except RuntimeError: 267 | converged = False 268 | 269 | 270 | solve_times.append(admm_time) 271 | state_prev = np.hstack((x_trj_converged.flatten(),u_trj_converged.flatten())) 272 | 273 | # if solve_times[-1] > t_kill: 274 | # converged = False 275 | # print('Solve time exceeded t_kill!') 276 | # break 277 | 278 | obj_history.append(float(objective(x_trj_converged.T, u_trj_converged.T, u_ref, xr, Q, R, Qf))\ 279 | +float(coupling_cost)) 280 | 281 | x_curr = x_trj_converged[1] 282 | u_curr = u_trj_converged[0] 283 | 284 | X_full = np.r_[X_full, x_curr.reshape(1,-1)] 285 | U_full = np.r_[U_full, u_curr.reshape(1,-1)] 286 | 287 | mpc_iter += 1 288 | t += dt 289 | if mpc_iter > 35: 290 | print('Max MPC iters reached!Exiting MPC loops...') 291 | converged = False 292 | break 293 | 294 | print(f'Final distance to goal is {dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)}') 295 | 296 | if np.all(dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 297 | converged = True 298 | 299 | 300 | obj_trj = float(objective(X_full.T, U_full.T, u_ref, xr, Q, R, Qf)) 301 | 302 | logging.info( 303 | f'{n_trial},' 304 | f'{n_agents},{t},{converged},' 305 | f'{obj_trj},{T},{dt},{radius},{centralized},{np.mean(solve_times)},{MAX_ITER},' 306 | f'{dpilqr.distance_to_goal(X_full[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 307 | ) 308 | 309 | 310 | return X_full, U_full, obj_trj, np.mean(solve_times), obj_history 311 | 312 | def solve_mpc_centralized(n_agents, x0, xr, T, radius, Q, R, Qf, n_trial = None): 313 | ADMM = False 314 | nx = n_agents * 6 315 | nu = n_agents * 3 316 | N = n_agents 317 | opti = Opti('conic') 318 | Y_state = opti.variable((T+1)*nx + T*nu) 319 | cost = 0 320 | 321 | u_ref = np.array([0, 0, 0] * N).reshape(-1,1) 322 | 323 | for t in range(T): 324 | for idx in range(nx): 325 | cost += (Y_state[:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) * \ 326 | Q[idx,idx]* (Y_state[:(T+1)*nx][t*nx:(t+1)*nx][idx]-xr[idx]) 327 | for idu in range(nu): 328 | cost += (Y_state[(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) * \ 329 | R[idu,idu] * (Y_state[(T+1)*nx:][t*nu:(t+1)*nu][idu] - u_ref[idu]) 330 | 331 | for idf in range(nx): 332 | cost += (Y_state[:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) * \ 333 | Qf[idf,idf] * (Y_state[:(T+1)*nx][T*nx:(T+1)*nx][idf] - xr[idf]) 334 | 335 | obj_hist = [np.inf] 336 | x_curr = x0 337 | 338 | X_trj = np.zeros((0, nx)) 339 | U_trj = np.zeros((0, nu)) 340 | X_trj = np.r_[X_trj, x0.T] 341 | iters = 0 342 | 343 | solve_times = [] 344 | t = 0 345 | dt = 0.1 346 | 347 | x_dims = [6]*N 348 | n_dims = [3]*N 349 | f = util.generate_f(x_dims) 350 | Ad,Bd = linear_kinodynamics(0.1,N) 351 | 352 | u_init = np.random.rand(3*n_agents)*0.1 353 | x_trj_init = np.zeros((0, nx)) 354 | x_trj_init = np.r_[x_trj_init, x0.reshape(1,-1)] 355 | Ad,Bd = linear_kinodynamics(0.1,n_agents) 356 | x_nominal = x0 357 | 358 | scaling_matrix = np.diag([1, 1, 2]) 359 | 360 | for _ in range(T): 361 | x_nominal = Ad@x_nominal + Bd@u_init.reshape(-1,1) 362 | x_trj_init = np.r_[x_trj_init, x_nominal.reshape(1,-1)] 363 | 364 | while not np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 365 | coll_cost = 0 366 | smooth_trj_cost = 0 367 | for k in range(T): 368 | 369 | opti.subject_to(Y_state[:(T+1)*nx][(k+1)*nx:(k+2)*nx] \ 370 | == Ad @ Y_state[:(T+1)*nx][k*nx:(k+1)*nx] \ 371 | + Bd @ Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 372 | 373 | opti.subject_to(Y_state[(T+1)*nx:][k*nu:(k+1)*nu] <= np.tile(np.array([3, 3, 3]),(N,)).reshape(-1,1)) 374 | opti.subject_to(np.tile(np.array([-3, -3, -3]),(N,)).reshape(-1,1) <= Y_state[(T+1)*nx:][k*nu:(k+1)*nu]) 375 | 376 | #Linearized collision constraints: 377 | if N > 1: 378 | if iters <= 0: 379 | pos_prev = x_trj_init[k] 380 | print(f'pos_prev has shape {pos_prev.shape}') 381 | 382 | else: 383 | pos_prev = state_prev[:(T+1)*nx].reshape(T+1, nx)[k] 384 | 385 | # pos_prev = X_full[iter-1] 386 | # pos_curr = cp.reshape(y_state[:(T+1)*nx],[T+1,nx])[k] 387 | 388 | for i in range(N): 389 | for j in range(N): 390 | if j != i: 391 | #See "Generation of collision-free trajectories for a quadrocopter fleet: 392 | # A sequential convex programming approach" for the linearization step; 393 | linearized_dist = cs.norm_2(scaling_matrix@(pos_prev[j*n_states:j*n_states+3]- \ 394 | pos_prev[i*n_states:i*n_states+3])) + \ 395 | (pos_prev[j*n_states:j*n_states+3].reshape(1,-1)- \ 396 | pos_prev[i*n_states:i*n_states+3].reshape(1,-1))/cs.norm_2(scaling_matrix@(pos_prev[j*n_states:j*n_states+3]\ 397 | -pos_prev[i*n_states:i*n_states+3]))@ \ 398 | (Y_state[:(T+1)*nx][k*nx:(k+1)*nx][j*n_states:j*n_states+3] \ 399 | -Y_state[:(T+1)*nx][k*nx:(k+1)*nx][i*n_states:i*n_states+3]) 400 | 401 | opti.subject_to(linearized_dist >= radius) 402 | 403 | #Smoothing term 404 | for ind in range(nx): 405 | smooth_trj_cost += (Y_state[:(T+1)*nx][(k+1)*nx:(k+2)*nx][ind]-\ 406 | Y_state[:(T+1)*nx][k*nx:(k+1)*nx][ind])**2 407 | 408 | X0 = opti.parameter(x0.shape[0],1) 409 | opti.subject_to(Y_state[0:nx] == X0) 410 | 411 | cost_tot = cost + coll_cost/N + smooth_trj_cost 412 | 413 | opti.minimize(cost_tot) 414 | 415 | opti.solver('osqp',opts) 416 | # opti.solver('qpoases',opts) 417 | # opti.solver("ipopt") 418 | opti.set_value(X0,x_curr) 419 | 420 | if iters > 0: 421 | opti.set_initial(sol_prev.value_variables()) 422 | 423 | t0 = perf_counter() 424 | try: 425 | sol = opti.solve() 426 | 427 | except RuntimeError: 428 | converged=False 429 | break 430 | 431 | sol_prev = sol 432 | solve_times.append(perf_counter() - t0) 433 | obj_hist.append(sol.value(cost_tot)) 434 | 435 | state_prev = sol.value(Y_state) 436 | 437 | ctrl = sol.value(Y_state)[(T+1)*nx:].reshape((T, nu))[0] 438 | x_curr = sol.value(Y_state)[:(T+1)*nx].reshape((T+1,nx))[1] #Same as above 439 | X_trj = np.r_[X_trj, x_curr.reshape(1,-1)] 440 | U_trj = np.r_[U_trj, ctrl.reshape(1,-1)] 441 | 442 | opti.subject_to() 443 | 444 | iters += 1 445 | t += dt 446 | if iters > 35: 447 | converged = False 448 | print(f'Max MPC iters reached; exiting MPC loops.....') 449 | break 450 | 451 | print(f'Final distance to goal is {dpilqr.distance_to_goal(X_trj[-1].flatten(), xr.flatten(), n_agents, n_states, 3)}') 452 | 453 | if np.all(dpilqr.distance_to_goal(x_curr.flatten(), xr.flatten(), n_agents, n_states, 3) <= 0.1): 454 | converged = True 455 | 456 | MAX_ITER = None 457 | obj_trj = float(util.objective(X_trj.T, U_trj.T, u_ref, xr, Q, R, Qf)) 458 | logging.info( 459 | f'{n_trial},' 460 | f'{n_agents},{t},{converged},' 461 | f'{obj_trj},{T},{dt},{radius},{ADMM},{np.mean(solve_times)}, {MAX_ITER},' 462 | f'{dpilqr.distance_to_goal(X_trj[-1].flatten(), xr.flatten(), n_agents, n_states, 3)},' 463 | ) 464 | 465 | return X_trj, U_trj, obj_trj, np.mean(solve_times), obj_hist 466 | 467 | 468 | def setup_logger(): 469 | 470 | # if centralized == True: 471 | 472 | LOG_PATH = Path(__file__).parent/ "logs" 473 | LOG_FILE = LOG_PATH / strftime( 474 | "ADMM-mpc-_%m-%d-%y_%H.%M.%S_{getpid()}.csv" 475 | ) 476 | if not LOG_PATH.is_dir(): 477 | LOG_PATH.mkdir() 478 | print(f"Logging results to {LOG_FILE}") 479 | logging.basicConfig(filename=LOG_FILE, format="%(message)s", level=logging.INFO) 480 | logging.info( 481 | "i_trial, n_agents, t, converged, obj_trj,T,dt,radius,\ 482 | ADMM ,t_solve_step, MAX_ITER, dist_to_goal" 483 | ) 484 | 485 | 486 | def multi_agent_run(trial, 487 | n_states, 488 | n_inputs, 489 | n_agents, 490 | T, 491 | radius, 492 | Q, 493 | R, 494 | Qf): 495 | """simulation comparing the centralized and decentralized solvers""" 496 | 497 | if n_agents == 3: 498 | x0,xr = util.setup_3_quads() 499 | 500 | elif n_agents==4: 501 | x0, xr=util.setup_4_quads() 502 | 503 | elif n_agents == 5: 504 | x0,xr = util.setup_5_quads() 505 | 506 | elif n_agents==6: 507 | x0, xr=util.setup_6_quads() 508 | 509 | elif n_agents==7: 510 | x0, xr= util.setup_7_quads() 511 | 512 | elif n_agents==8: 513 | x0, xr= util.setup_8_quads() 514 | 515 | elif n_agents==9: 516 | x0, xr= util.setup_9_quads() 517 | 518 | elif n_agents == 10: 519 | x0,xr = util.setup_10_quads() 520 | 521 | ids = [100 + i for i in range(n_agents)] 522 | x_dims = [n_states] * n_agents 523 | n_dims = [3] * n_agents 524 | Q = np.diag([5., 5., 5., 1., 1., 1.]*n_agents) 525 | Qf = Q*500 526 | R = 0.1*np.eye(n_agents*n_inputs) 527 | 528 | admm_iter = 3 529 | 530 | X_full, U_full, obj, avg_SolveTime, _ = solve_mpc_centralized(n_agents, 531 | x0, 532 | xr, 533 | T, 534 | radius, 535 | Q, 536 | R, 537 | Qf, 538 | trial) 539 | 540 | X_full, U_full, obj, avg_SolveTime, _ = solve_admm_mpc(n_states, 541 | n_inputs, 542 | n_agents, 543 | x0, 544 | xr, 545 | T, 546 | radius, 547 | Q, 548 | R, 549 | Qf, 550 | admm_iter, 551 | trial) 552 | 553 | def monte_carlo_analysis(): 554 | """Benchmark to evaluate algorithm over many random initial conditions""" 555 | 556 | setup_logger() 557 | 558 | n_trials_iter = range(30) 559 | 560 | # n_agents_iter = [3, 4, 5, 6, 7, 8] 561 | n_agents_iter = [3, 5, 10] 562 | # n_agents_iter = [10] 563 | 564 | radius = 0.5 565 | 566 | # Change the for loops into multi-processing? 567 | 568 | for n_agents in n_agents_iter: 569 | print(f"\tn_agents: {n_agents}") 570 | 571 | if n_agents >=5 and n_agents <=7: 572 | radius = 0.3 573 | 574 | if n_agents >= 8 : 575 | radius = 0.15 576 | 577 | for i_trial in n_trials_iter: 578 | print(f"\t\ttrial: {i_trial}") 579 | 580 | multi_agent_run( 581 | i_trial, 582 | n_states, 583 | n_inputs, 584 | n_agents, 585 | T, 586 | radius, 587 | Q, 588 | R, 589 | Qf) 590 | 591 | 592 | if __name__ == "__main__": 593 | 594 | n_states = 6 595 | n_inputs = 3 596 | n_agents = 3 597 | x_dims = [n_states]*n_agents 598 | T = 8 599 | # T = 10 600 | radius = 0.5 601 | Q = np.diag([5., 5., 5., 1., 1., 1.]*n_agents) 602 | Qf = Q*500 603 | R = 0.1*np.eye(n_agents*n_inputs) 604 | 605 | ids = [100 + n for n in range(n_agents)] #Assigning random IDs for agents 606 | 607 | Log_Data = True 608 | if not Log_Data: 609 | # admm_iter = 15 610 | # admm_iter = 5 611 | admm_iter = 5 612 | x0, xr = util.paper_setup_3_quads(True) 613 | 614 | X_full, U_full, obj, avg_SolveTime, obj_history_admm = solve_admm_mpc(n_states, 615 | n_inputs, 616 | n_agents, 617 | x0, 618 | xr, 619 | T, 620 | radius, 621 | Q, 622 | R, 623 | Qf, 624 | admm_iter) 625 | print(f'The average solve time is {avg_SolveTime} seconds!') 626 | #Plot trajectory 627 | plt.figure(dpi=150) 628 | dpilqr.plot_solve(X_full, float(obj), xr, x_dims, True, 3) 629 | # plt.gca().set_zticks([0.8,1.2], minor=False) 630 | plt.legend(plt.gca().get_children()[1:3], ["Start Position", "Goal Position"]) 631 | plt.savefig('ADMM_mpc(QP).png') 632 | 633 | 634 | plt.figure(dpi=150) 635 | dpilqr.plot_pairwise_distances(X_full, [6,6,6], [3,3,3], radius) 636 | plt.title('Pairwise-distances from C-ADMM (QP)') 637 | plt.savefig('pairwise_distances(ADMM_QP).png') 638 | # else: 639 | 640 | X_full, U_full, obj, avg_SolveTime, obj_history_centralized = solve_mpc_centralized(n_agents, 641 | x0, 642 | xr, 643 | T, 644 | radius, 645 | Q, 646 | R, 647 | Qf 648 | ) 649 | print(f'The average solve time is {avg_SolveTime} seconds!') 650 | 651 | plt.figure(dpi=150) 652 | dpilqr.plot_solve(X_full, float(obj), xr, x_dims, True, 3) 653 | # plt.gca().set_zticks([0.8,1.2], minor=False) 654 | plt.legend(plt.gca().get_children()[1:3], ["Start Position", "Goal Position"]) 655 | plt.savefig('centralized_mpc_baseline(QP).png') 656 | 657 | plt.figure(dpi=150) 658 | dpilqr.plot_pairwise_distances(X_full, [6,6,6], [3,3,3], radius) 659 | plt.title('Pairwise-distances from vanilla MPC(QP)') 660 | plt.savefig('pairwise_distances(vanilla_mpc_QP).png') 661 | 662 | 663 | plt.figure(dpi=150) 664 | plt.plot(obj_history_admm, 'r', label='Potential Consensus-ADMM') 665 | plt.plot(obj_history_centralized, 'b', label='Baseline Centralized MPC') 666 | plt.ylabel('Total Cost-to-go') 667 | plt.xlabel('Horizon') 668 | plt.legend(loc='best') 669 | plt.savefig('convergence_rate_QP.png') 670 | 671 | 672 | if Log_Data: 673 | monte_carlo_analysis() 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | -------------------------------------------------------------------------------- /distributed_ADMM/util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import itertools 3 | from casadi import * 4 | import casadi as cs 5 | import random 6 | from scipy.spatial.transform import Rotation 7 | π = np.pi 8 | 9 | 10 | 11 | def randomize_locs(n_pts, random=False, rel_dist=3.0, var=3.0, n_d=2): 12 | """Uniformly randomize locations of points in N-D while enforcing 13 | a minimum separation between them. 14 | """ 15 | 16 | # Distance to move away from center if we're too close. 17 | Δ = 0.1 * n_pts 18 | x = var * np.random.uniform(-1, 1, (n_pts, n_d)) 19 | 20 | if random: 21 | return x 22 | 23 | # Determine the pair-wise indicies for an arbitrary number of agents. 24 | pair_inds = np.array(list(itertools.combinations(range(n_pts), 2))) 25 | move_inds = np.arange(n_pts) 26 | 27 | # Keep moving points away from center until we satisfy radius 28 | while move_inds.size: 29 | center = np.mean(x, axis=0) 30 | distances = compute_pairwise_distance(x.flatten(), [n_d] * n_pts).T 31 | 32 | move_inds = pair_inds[distances.flatten() <= rel_dist] 33 | x[move_inds] += Δ * (x[move_inds] - center) 34 | 35 | return x 36 | 37 | 38 | def face_goal(x0, xf): 39 | """Make the agents face the direction of their goal with a little noise""" 40 | 41 | VAR = 0.01 42 | dX = xf[:, :2] - x0[:, :2] 43 | headings = np.arctan2(*np.rot90(dX, 1)) 44 | 45 | x0[:, -1] = headings + VAR * np.random.randn(x0.shape[0]) 46 | xf[:, -1] = headings + VAR * np.random.randn(x0.shape[0]) 47 | 48 | return x0, xf 49 | 50 | 51 | def random_setup( 52 | n_agents, n_states, is_rotation=False, n_d=2, energy=None, do_face=False, **kwargs 53 | ): 54 | """Create a randomized set up of initial and final positions""" 55 | 56 | # We don't have to normlize for energy here 57 | x_i = randomize_locs(n_agents, n_d=n_d, **kwargs) 58 | 59 | # Rotate the initial points by some amount about the center. 60 | if is_rotation: 61 | θ = π + random.uniform(-π / 4, π / 4) 62 | R = Rotation.from_euler("z", θ).as_matrix()[:2, :2] 63 | x_f = x_i @ R - x_i.mean(axis=0) 64 | else: 65 | x_f = randomize_locs(n_agents, n_d=n_d, **kwargs) 66 | 67 | x0 = np.c_[x_i, np.zeros((n_agents, n_states - n_d))] 68 | xf = np.c_[x_f, np.zeros((n_agents, n_states - n_d))] 69 | 70 | if do_face: 71 | x0, xf = face_goal(x0, xf) 72 | 73 | x0 = x0.reshape(-1, 1) 74 | xf = xf.reshape(-1, 1) 75 | 76 | # Normalize to satisfy the desired energy of the problem. 77 | if energy: 78 | x0 = normalize_energy(x0, [n_states] * n_agents, energy, n_d) 79 | xf = normalize_energy(xf, [n_states] * n_agents, energy, n_d) 80 | 81 | return x0, xf 82 | 83 | 84 | def compute_energy(x, x_dims, n_d=2): 85 | """Determine the sum of distances from the origin""" 86 | return np.linalg.norm(x[pos_mask(x_dims, n_d)].reshape(-1, n_d), axis=1).sum() 87 | 88 | 89 | def normalize_energy(x, x_dims, energy=10.0, n_d=2): 90 | """Zero-center the coordinates and then ensure the sum of 91 | squared distances == energy 92 | """ 93 | 94 | # Don't mutate x's data for this function, keep it pure. 95 | x = x.copy() 96 | n_agents = len(x_dims) 97 | center = x[pos_mask(x_dims, n_d)].reshape(-1, n_d).mean(0) 98 | 99 | x[pos_mask(x_dims, n_d)] -= np.tile(center, n_agents).reshape(-1, 1) 100 | x[pos_mask(x_dims, n_d)] *= energy / compute_energy(x, x_dims, n_d) 101 | assert x.size == sum(x_dims) 102 | 103 | return x 104 | 105 | 106 | def perturb_state(x, x_dims, n_d=2, var=0.5): 107 | """Add a little noise to the start to knock off perfect symmetries""" 108 | 109 | x = x.copy() 110 | x[pos_mask(x_dims, n_d)] += var * np.random.randn(*x[pos_mask(x_dims, n_d)].shape) 111 | 112 | return x 113 | 114 | 115 | def pos_mask(x_dims, n_d=2): 116 | """Return a mask that's true wherever there's a spatial position""" 117 | return np.array([i % x_dims[0] < n_d for i in range(sum(x_dims))]) 118 | 119 | 120 | def paper_setup_2_quads(random = False): 121 | 122 | x0 = np.array([[0.5, 1.5, 1, 0, 0, 0, 123 | 2.5, 1.5, 1, 0, 0, 0]], 124 | dtype=float).T 125 | xf = np.array([[2.5, 1.5, 1, 0, 0, 0, 126 | 0.5, 1.5, 1, 0, 0, 0]]).T 127 | if random == True: 128 | x0[pos_mask([6]*2, 3)] += 0.05*np.random.randn(6, 1) 129 | xf[pos_mask([6]*2, 3)] += 0.05*np.random.randn(6, 1) 130 | return x0, xf 131 | 132 | 133 | def paper_setup_3_quads(random = False): 134 | 135 | x0 = np.array([[0.5, 1.5, 1, 0, 0, 0, 136 | 2.5, 1.5, 1, 0, 0, 0, 137 | 1.5, 1.3, 1, 0, 0, 0]], 138 | dtype=float).T 139 | xf = np.array([[2.5, 1.5, 1, 0, 0, 0, 140 | 0.5, 1.5, 1, 0, 0, 0, 141 | 1.5, 2.2, 1, 0, 0, 0]]).T 142 | if random == True: 143 | x0[pos_mask([6]*3, 3)] += 0.05*np.random.randn(9, 1) 144 | xf[pos_mask([6]*3, 3)] += 0.05*np.random.randn(9, 1) 145 | return x0, xf 146 | 147 | def setup_3_quads(): 148 | 149 | x0,xf = random_setup(3,6,n_d=3,energy=3,var=3.0) 150 | 151 | for i in range(2,len(x0),6): 152 | if x0[i] <= 0.5: 153 | x0[i] = 1.2 + np.random.rand(1,) 154 | 155 | if xf[i] <= 0.5: 156 | xf[i] = 1.1 + np.random.rand(1,)*0.5 157 | 158 | return x0, xf 159 | 160 | 161 | def setup_4_quads(): 162 | 163 | x0,xf = random_setup(4,6,n_d=3,energy=4,var=3.0) 164 | 165 | for i in range(2,len(x0),6): 166 | if x0[i] <= 0.5: 167 | x0[i] = 1.2 + np.random.rand(1,) 168 | 169 | if xf[i] <= 0.5: 170 | xf[i] = 1.1 + np.random.rand(1,)*0.5 171 | 172 | return x0, xf 173 | 174 | def paper_setup_5_quads(random = False): 175 | 176 | x0 = np.array([[0.5, 1.5, 1, 0, 0, 0, 177 | 2.5, 1.5, 1, 0, 0, 0, 178 | 1.5, 1.3, 1, 0, 0, 0, 179 | -0.5 ,0.5, 1, 0, 0, 0, 180 | 1.1, 1.1, 1, 0, 0, 0]], 181 | dtype=float).T 182 | xf = np.array([[2.5, 1.5, 1, 0, 0, 0, 183 | 0.5, 1.5, 1, 0, 0, 0, 184 | 1.5, 2.2, 1, 0, 0, 0, 185 | -0.5, 1.5, 1, 0, 0, 0, 186 | -1.1, 1.1, 1, 0, 0, 0]]).T 187 | 188 | if random == True: 189 | x0[pos_mask([6]*5, 3)] += 0.05*np.random.randn(15, 1) 190 | xf[pos_mask([6]*5, 3)] += 0.05*np.random.randn(15, 1) 191 | 192 | return x0,xf 193 | 194 | 195 | def setup_5_quads(): 196 | 197 | x0,xf = random_setup(5,6,n_d=3,energy=5,var=3.0) 198 | 199 | for i in range(2,len(x0),6): 200 | if x0[i] <= 0.5: 201 | x0[i] = 1.2 + np.random.rand(1,) 202 | 203 | if xf[i] <= 0.5: 204 | xf[i] = 1.1 + np.random.rand(1,)*0.5 205 | 206 | return x0,xf 207 | 208 | def setup_6_quads(): 209 | 210 | x0,xf = random_setup(6,6,n_d=3,energy=6,var=3.0) 211 | 212 | for i in range(2,len(x0),6): 213 | if x0[i] <= 0.5: 214 | x0[i] = 1.2 + np.random.rand(1,) 215 | 216 | if xf[i] <= 0.5: 217 | xf[i] = 1.1 + np.random.rand(1,)*0.5 218 | 219 | 220 | return x0, xf 221 | 222 | def setup_7_quads(): 223 | 224 | x0,xf = random_setup(7,6,n_d=3,energy=7,var=3.0) 225 | 226 | for i in range(2,len(x0),6): 227 | if x0[i] <= 0.5: 228 | x0[i] = 1.2 + np.random.rand(1,) 229 | 230 | if xf[i] <= 0.5: 231 | xf[i] = 1.1 + np.random.rand(1,)*0.5 232 | 233 | return x0, xf 234 | 235 | def setup_8_quads(): 236 | 237 | x0,xf = random_setup(8,6,n_d=3,energy=8,var=3.0) 238 | 239 | for i in range(2,len(x0),6): 240 | if x0[i] <= 0.5: 241 | x0[i] = 1.2 + np.random.rand(1,) 242 | 243 | if xf[i] <= 0.5: 244 | xf[i] = 1.1 + np.random.rand(1,)*0.5 245 | 246 | return x0, xf 247 | 248 | def setup_9_quads(): 249 | x0,xf = random_setup(9,6,n_d=3,energy=8,var=3.0) 250 | 251 | for i in range(2,len(x0),6): 252 | if x0[i] <= 0.5: 253 | x0[i] = 1.2 + np.random.rand(1,) 254 | 255 | if xf[i] <= 0.5: 256 | xf[i] = 1.1 + np.random.rand(1,)*0.5 257 | 258 | return x0, xf 259 | 260 | 261 | def generate_f(x_dims_local): 262 | g = 9.8 263 | # NOTE: Assume homogeneity of agents. 264 | n_agents = len(x_dims_local) 265 | n_states = x_dims_local[0] 266 | n_controls = 3 267 | 268 | def f(x, u): 269 | x_dot = cs.MX.zeros(x.numel()) 270 | for i_agent in range(n_agents): 271 | i_xstart = i_agent * n_states 272 | i_ustart = i_agent * n_controls 273 | x_dot[i_xstart:i_xstart + n_states] = cs.vertcat( 274 | x[i_xstart + 3: i_xstart + 6], 275 | g*cs.tan(u[i_ustart]), -g*cs.tan(u[i_ustart+1]), u[i_ustart+2] - g 276 | ) 277 | 278 | return x_dot 279 | 280 | return f 281 | 282 | def objective(X, U, u_ref, xf, Q, R, Qf): 283 | total_stage_cost = 0 284 | for j in range(X.shape[1] - 1): 285 | for i in range(X.shape[0]): 286 | total_stage_cost += (X[i, j] - xf[i]) * Q[i, i] * (X[i, j] - xf[i]) 287 | 288 | for j in range(U.shape[1]): 289 | for i in range(U.shape[0]): 290 | total_stage_cost += (U[i, j] - u_ref[i]) * R[i, i] * (U[i, j] - u_ref[i]) 291 | 292 | # Quadratic terminal cost: 293 | total_terminal_cost = 0 294 | 295 | for i in range(X.shape[0]): 296 | total_terminal_cost += (X[i, -1] - xf[i]) * Qf[i, i] * (X[i, -1] - xf[i]) 297 | 298 | return total_stage_cost + total_terminal_cost 299 | 300 | def generate_min_max_input(inputs_dict, n_inputs,theta_max, 301 | theta_min,tau_max,tau_min,phi_max,phi_min,human_count = None): 302 | 303 | n_agents = [u.shape[0] // n_inputs for u in inputs_dict.values()] 304 | 305 | u_min = np.array([[theta_min, phi_min, tau_min]]) 306 | u_max = np.array([[theta_max, phi_max, tau_max]]) 307 | 308 | if human_count: 309 | return [ 310 | (np.tile(u_min, n_agents_i), np.tile(u_max, n_agents_i)) 311 | for n_agents_i in (n_agents-human_count) 312 | ] 313 | 314 | else: 315 | return [ 316 | (np.tile(u_min, n_agents_i), np.tile(u_max, n_agents_i)) 317 | for n_agents_i in n_agents 318 | ] 319 | 320 | 321 | def generate_min_max_state(states_dict, n_states, x_min, 322 | x_max,y_min,y_max,z_min,z_max,v_min,v_max,human_count = None): 323 | 324 | n_agents = [x.shape[0] // n_states for x in states_dict.values()] 325 | x_min = np.array([[x_min, y_min, z_min, v_min, v_min, v_min]]) 326 | x_max = np.array([[x_max, y_max, z_max, v_max, v_max, v_max]]) 327 | 328 | if human_count: 329 | return [ 330 | (np.tile(x_min, n_agents_i), np.tile(x_max, n_agents_i)) 331 | for n_agents_i in (n_agents-human_count) 332 | ] 333 | 334 | else: 335 | 336 | return [ 337 | (np.tile(x_min, n_agents_i), np.tile(x_max, n_agents_i)) 338 | for n_agents_i in n_agents 339 | ] 340 | 341 | 342 | def distance_to_goal(x,xf,n_agents,n_states): 343 | n_d = 3 344 | return np.linalg.norm((x - xf).reshape(n_agents, n_states)[:, :n_d], axis=1) 345 | 346 | 347 | def split_agents(Z, z_dims): 348 | """Partition a cartesian product state or control for individual agents""" 349 | return np.split(np.atleast_2d(Z), np.cumsum(z_dims[:-1]), axis=1) 350 | 351 | 352 | def split_agents_gen(z, z_dims): 353 | """Generator version of ``split_agents``""" 354 | dim = z_dims[0] 355 | for i in range(len(z_dims)): 356 | yield z[i * dim : (i + 1) * dim] 357 | 358 | 359 | def split_graph(Z, z_dims, graph): 360 | """Split up the state or control by grouping their ID's according to the graph""" 361 | assert len(set(z_dims)) == 1 362 | 363 | # Create a mapping from the graph to indicies. 364 | mapping = {id_: i for i, id_ in enumerate(list(graph))} 365 | 366 | n_z = z_dims[0] 367 | z_split = [] 368 | for ids in graph.values(): 369 | inds = [mapping[id_] for id_ in ids] 370 | z_split.append( 371 | np.concatenate([Z[:, i * n_z : (i + 1) * n_z] for i in inds], axis=1) 372 | ) 373 | 374 | return z_split 375 | 376 | 377 | def define_inter_graph_threshold(X, radius, x_dims, ids, n_dims=None): 378 | """Compute the interaction graph based on a simple thresholded distance 379 | for each pair of agents sampled over the trajectory 380 | """ 381 | 382 | planning_radii = 2.2 * radius 383 | 384 | if n_dims: 385 | rel_dists = np.array([compute_pairwise_distance_nd_Sym(X, x_dims, n_dims)]) 386 | else: 387 | rel_dists = compute_pairwise_distance(X, x_dims) 388 | 389 | print(f'determining interaction graph with the following pair-wise distance : {rel_dists}') 390 | # N = X.shape[0] 391 | # n_samples = 10 392 | # sample_step = max(N // n_samples, 1) 393 | # sample_slice = slice(0, N + 1, sample_step) 394 | 395 | # Put each pair of agents within each others' graphs if they are within 396 | # some threshold distance from each other. 397 | graph = {id_: [id_] for id_ in ids} 398 | # print(graph) 399 | pair_inds = np.array(list(itertools.combinations(ids, 2))) 400 | for i, pair in enumerate(pair_inds): 401 | if np.any(rel_dists[:,i] < planning_radii): 402 | graph[pair[0]].append(pair[1]) 403 | graph[pair[1]].append(pair[0]) 404 | 405 | graph = {agent_id: sorted(prob_ids) for agent_id, prob_ids in graph.items()} 406 | return graph 407 | 408 | def compute_pairwise_distance_nd_Sym(X, x_dims, n_dims): 409 | """Analog to the above whenever some agents only use distance in the x-y plane""" 410 | CYLINDER_RADIUS = 0.2 411 | 412 | n_states = x_dims[0] 413 | n_agents = len(x_dims) 414 | distances = [] 415 | eps = 1e-3 416 | 417 | for i, n_dim_i in zip(range(n_agents), n_dims): 418 | for j, n_dim_j in zip(range(i + 1, n_agents), n_dims[i + 1 :]): 419 | n_dim = min(n_dim_i, n_dim_j) 420 | 421 | Xi = X[i * n_states : i * n_states + n_dim, :] 422 | Xj = X[j * n_states : j * n_states + n_dim, :] 423 | dX = Xi-Xj 424 | 425 | if n_dim == 3: 426 | distances.append(sqrt(dX[0,:]**2+dX[1,:]**2+dX[2,:]**2+eps)) 427 | else: 428 | distances.append(sqrt(dX[0,:]**2+dX[1,:]**2 + eps)+CYLINDER_RADIUS) 429 | 430 | return distances 431 | 432 | def compute_pairwise_distance(X, x_dims, n_d=3): 433 | """Compute the distance between each pair of agents""" 434 | assert len(set(x_dims)) == 1 435 | 436 | n_agents = len(x_dims) 437 | n_states = x_dims[0] 438 | 439 | if n_agents == 1: 440 | raise ValueError("Can't compute pairwise distance for one agent.") 441 | 442 | pair_inds = np.array(list(itertools.combinations(range(n_agents), 2))) 443 | X_agent = X.reshape(-1, n_agents, n_states).swapaxes(0, 2) 444 | dX = X_agent[:n_d, pair_inds[:, 0]] - X_agent[:n_d, pair_inds[:, 1]] 445 | return np.linalg.norm(dX, axis=0).T -------------------------------------------------------------------------------- /regular_ADMM_mpc_trj_data.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/labicon/Potential-ADMM_code/1e1b943fbe6906a59c8f76da8eee8eca86412f7b/regular_ADMM_mpc_trj_data.npz -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | casadi==3.5.5 2 | matplotlib==3.5.1 3 | numpy==1.22.3 4 | scipy==1.9.1 5 | --------------------------------------------------------------------------------