├── problems ├── ground │ ├── config.yaml │ ├── ground.py │ └── problem.yaml ├── mit │ ├── config.yaml │ ├── mit.py │ └── problem.yaml ├── zigzag │ ├── config.yaml │ ├── problem.yaml │ └── zigzag.py ├── parking │ ├── config.yaml │ ├── problem.yaml │ └── parking.py ├── platform │ ├── config.yaml │ ├── platform.py │ └── problem.yaml └── util.py ├── models ├── agent.py ├── car.py ├── hovercraft.py └── auv.py ├── algs ├── centralized.py ├── ref2traj.py ├── collision.py ├── decentralized.py └── xref.py ├── README.md ├── test.py ├── LICENSE ├── MAMP.py ├── viz ├── plot.py ├── animate.py └── util.py └── util.py /problems/ground/config.yaml: -------------------------------------------------------------------------------- 1 | {min_segs: 2, max_segs: 7, obstacle_segs: 10} 2 | -------------------------------------------------------------------------------- /problems/mit/config.yaml: -------------------------------------------------------------------------------- 1 | {min_segs: 7, max_segs: 8, obstacle_segs: 10} 2 | -------------------------------------------------------------------------------- /problems/zigzag/config.yaml: -------------------------------------------------------------------------------- 1 | {min_segs: 9, max_segs: 10, obstacle_segs: 10} 2 | -------------------------------------------------------------------------------- /problems/parking/config.yaml: -------------------------------------------------------------------------------- 1 | {min_segs: 2, max_segs: 10, obstacle_segs: 20} 2 | -------------------------------------------------------------------------------- /problems/platform/config.yaml: -------------------------------------------------------------------------------- 1 | {min_segs: 2, max_segs: 8, obstacle_segs: 10} 2 | -------------------------------------------------------------------------------- /models/agent.py: -------------------------------------------------------------------------------- 1 | 2 | class Agent: 3 | def __init__(self, size, velocity, k): 4 | self.size = size 5 | self.velocity = velocity 6 | self.k = k 7 | 8 | def model(self, q, t, u): 9 | pass 10 | 11 | def controller(self, q, qref, uref): 12 | pass 13 | 14 | def bloating(self, n): 15 | pass 16 | 17 | def run_model(self, q0, t, qref, uref): 18 | pass -------------------------------------------------------------------------------- /algs/centralized.py: -------------------------------------------------------------------------------- 1 | from algs.xref import * 2 | 3 | # Main controller algorithm 4 | def centralized_algo(velocities, sizes, bloating_functions, Thetas, goals, 5 | limits, obstacles, min_segs, max_segs): 6 | agent_num = len(sizes) 7 | collisions = [[]] * agent_num 8 | # Get the reference nodes 9 | print("Search Begins") 10 | ma_nodes = get_gb_xref(Thetas, goals, obstacles, limits, 11 | velocities, sizes, bloating_functions, collisions, 12 | min_segs, max_segs) 13 | if ma_nodes != None: 14 | print("Solution Found!") 15 | return ma_nodes 16 | print("Fail!") 17 | return None 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Scalable and Safe Multi-agent Motion Planner (S2M2) 2 | 3 | This is a code repository of Scalable and Safe Multi-agent Motion Planner (S2M2). 4 | 5 | This repository is associated with the paper [_Scalable and Safe Multi-Agent Motion Planning with Nonlinear Dynamics and Bounded Disturbances_](https://jkchengh.github.io/files/chen2021scalable.pdf). If you have any question, please email me at jkchen 'at' csail.mit.edu, or create an issue [here](https://github.com/jkchengh/s2m2/issues). 6 | 7 | ## Citation 8 | Please cite our work if you would like to use the code. 9 | ``` 10 | @inproceedings{chen2021scalable, 11 | author = {Jingkai Chen and Jiaoyang Li and Chuchu Fan and Brian C. Williams}, 12 | title = {Scalable and Safe Multi-Agent Motion Planning with Nonlinear Dynamics and Bounded Disturbances}, 13 | booktitle = {Proceedings of the Thirty-Fifth AAAI Conference on Artificial Intelligence (AAAI 2021)}, 14 | year = {2021} 15 | } 16 | ``` 17 | -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | from algs.decentralized import decentralized_algo 2 | from algs.ref2traj import ref2traj 3 | from problems.util import * 4 | from timeit import * 5 | from util import * 6 | 7 | from viz.plot import * 8 | from viz.animate import * 9 | from viz.util import * 10 | 11 | def test(env, problem_path, config_path): 12 | name, limits, Obstacles, agents, Thetas, Goals = read_problem(problem_path) 13 | min_segs, max_segs, obs_steps = read_configuration(config_path) 14 | 15 | 16 | 17 | start = default_timer() 18 | refs = decentralized_algo(agents, Thetas, Goals, limits, Obstacles, min_segs, max_segs, obs_steps, 0) 19 | end = default_timer() 20 | print("Total Time = ", end - start) 21 | name = '[%s]'%(env) 22 | 23 | trajs = ref2traj(refs) 24 | # plot_results(agents, limits, Obstacles, Thetas, Goals, trajs, name, refs=refs) 25 | 26 | return refs 27 | 28 | test("zigzag", "problems/zigzag/problem.yaml", "problems/zigzag/config.yaml") 29 | -------------------------------------------------------------------------------- /problems/ground/ground.py: -------------------------------------------------------------------------------- 1 | from problems.util import * 2 | 3 | limits = [[0, 60], [0, 60]] 4 | Obstacles = [] 5 | 6 | Thetas = [[20, 48], [30, 48], [40, 48], 7 | [20, 12], [30, 12], [40, 12], 8 | [12, 20], [12, 30], [12, 40], 9 | [48, 20], [48, 30], [48, 40]] 10 | 11 | A_rect = np.array([[-1, 0], 12 | [1, 0], 13 | [0, -1], 14 | [0, 1]]) 15 | Goals = [] 16 | lG = [2, 2] 17 | for pG in [[20, 12], [30, 12], [40, 12], 18 | [20, 48], [30, 48], [40, 48], 19 | [48, 20], [48, 30], [48, 40], 20 | [12, 20], [12, 30], [12, 40]]: 21 | b = np.array([[-(pG[0] - lG[0])], [pG[0] + lG[0]], [-(pG[1] - lG[1])], [pG[1] + lG[1]]]) 22 | Goals.append([A_rect, b]) 23 | 24 | file_name = "ground/problem.yaml" 25 | name = "ground" 26 | size = 1 27 | velocity = 1 28 | k = [0.5, 0.5, 0.5] 29 | agents = [Car(size, velocity, k) for _ in range(12)] 30 | 31 | write_problem(file_name, name, limits, Obstacles, agents, Thetas, Goals) 32 | -------------------------------------------------------------------------------- /problems/platform/platform.py: -------------------------------------------------------------------------------- 1 | from util import * 2 | from problems.util import * 3 | 4 | A_rect = np.array([[-1, 0], 5 | [1, 0], 6 | [0, -1], 7 | [0, 1]]) 8 | 9 | b01 = np.array([[-20], [40], [-20], [40]]) 10 | Obstacles = [(A_rect, b01)] 11 | 12 | Thetas = [[20, 48], [30, 48], [40, 48], 13 | [20, 12], [30, 12], [40, 12], 14 | [12, 20], [12, 30], [12, 40], 15 | [48, 20], [48, 30], [48, 40]] 16 | 17 | 18 | Goals = [] 19 | lG = [2, 2] 20 | for pG in [[20, 12], [30, 12], [40, 12], 21 | [20, 48], [30, 48], [40, 48], 22 | [48, 20], [48, 30], [48, 40], 23 | [12, 20], [12, 30], [12, 40]]: 24 | b = np.array([[-(pG[0] - lG[0])], [pG[0] + lG[0]], [-(pG[1] - lG[1])], [pG[1] + lG[1]]]) 25 | Goals.append((A_rect, b)) 26 | 27 | limits = [[0, 60], [0, 60]] 28 | 29 | 30 | file_name = "problem.yaml" 31 | name = "platform" 32 | 33 | size = 1 34 | velocity = 1 35 | k = [0.5, 0.5, 0.5] 36 | agents = [Car(size, velocity, k) for _ in range(12)] 37 | write_problem(file_name, name, limits, Obstacles, agents, Thetas, Goals) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Jingkai 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 | -------------------------------------------------------------------------------- /MAMP.py: -------------------------------------------------------------------------------- 1 | # Run benchmarks 2 | 3 | import argparse 4 | import sys 5 | from algs.centralized import centralized_algo 6 | from algs.decentralized import decentralized_algo 7 | 8 | parser = argparse.ArgumentParser() 9 | parser.add_argument("models", nargs='+') 10 | parser.add_argument("--env") 11 | parser.add_argument("--min_segs") 12 | parser.add_argument("--max_segs") 13 | parser.add_argument("--method") 14 | parser.add_argument("--viz") 15 | args = parser.parse_args() 16 | 17 | # check method (Default = Centralized) 18 | if args.method == None: method = "decentralized" 19 | else: method = args.method 20 | 21 | # check the number of line segments 22 | if args.max_segs == None: max_segs = 100 23 | else: max_segs = int(args.max_segs) 24 | if args.min_segs == None: min_segs = 3 25 | else: min_segs = int(args.min_segs) 26 | 27 | # Plot the problem and the test runs 28 | if __name__ == '__main__': 29 | if method == "centralized": 30 | trajs = centralized_algo(args.models, args.env, min_segs, max_segs) 31 | elif method == "decentralized": 32 | trajs = decentralized_algo(args.models, args.env, min_segs, max_segs) 33 | if bool(args.plot) == True: 34 | True 35 | else: sys.exit() -------------------------------------------------------------------------------- /viz/plot.py: -------------------------------------------------------------------------------- 1 | from viz.util import * 2 | from algs.collision import * 3 | import os 4 | 5 | def plot_results(models, limits, Obstacles, Thetas, Goals, ma_segs, name, MO = [], refs = None): 6 | plt.close('all') 7 | agent_num = len(models) 8 | fig, axes = plot_env(limits, Obstacles) 9 | # plot_goals(Goals) 10 | # plot_thetas(Thetas) 11 | 12 | # for mo in MO: 13 | # for obs in mo: 14 | # A = obs[2] 15 | # b = np.array([[e] for e in obs[3]]) 16 | # plot_polytope_3d(A, b, axes) 17 | 18 | paths = extract_paths(models, Thetas, ma_segs) 19 | for idx in range(agent_num): 20 | if len(Thetas[idx]) == 2: 21 | ref_x, ref_y, _, tru_x, tru_y, _, _ = paths[idx] 22 | ref_x, ref_y, _, tru_x, tru_y, _, _ = paths[idx] 23 | # axes.plot(ref_x, ref_y, color='k', linewidth = 2, linestyle = 'dashed') 24 | # axes.plot(tru_x, tru_y, color='purple', linewidth = 3, linestyle='dashed',) 25 | if refs is not None: 26 | x = [p[1] for p in refs[idx]] 27 | y = [p[2] for p in refs[idx]] 28 | axes.plot(x, y,color='k', linestyle = 'dashed', linewidth = 2, marker = 'o') 29 | elif len(Thetas[idx]) == 3: 30 | ref_x, ref_y, ref_z, tru_x, tru_y, tru_z, _, _, _, _ = paths[idx] 31 | axes.plot(ref_x, ref_y, ref_z, color='k') 32 | axes.plot(tru_x, tru_y, tru_z, 'r--') 33 | 34 | fig.show() 35 | path = os.path.abspath("results/%s.svg" % (name)) 36 | fig.savefig(path, bbox_inches='tight', pad_inches = 0) 37 | return None 38 | 39 | -------------------------------------------------------------------------------- /models/car.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | from scipy.integrate import odeint 3 | from models.agent import * 4 | import random 5 | 6 | class Car(Agent): 7 | 8 | def __init__(self, size, velocity, k): 9 | self.size = size 10 | self.velocity = velocity 11 | self.k = k 12 | 13 | def model(self, q, t, u): 14 | x, y, theta = q 15 | v, w = u 16 | 17 | dxdt = v * cos(theta) 18 | dydt = v * sin(theta) 19 | dthetadt = w 20 | return [dxdt, dydt, dthetadt] 21 | 22 | def controller(self, q, qref, uref): 23 | x, y, theta = q 24 | xref, yref, thetaref = qref 25 | vref, wref = uref 26 | k1, k2, k3 = self.k 27 | 28 | xe = cos(theta) * (xref - x) + sin(theta) * (yref - y) 29 | ye = -sin(theta) * (xref - x) + cos(theta) * (yref - y) 30 | thetae = thetaref - theta 31 | v = vref * cos(thetae) + k1 * xe 32 | w = wref + vref * (k2 * ye + k3 * sin(thetae)) 33 | 34 | return [v, w] 35 | 36 | def bloating(self, n): 37 | return 0 38 | k1, k2, k3 = self.k 39 | if n != 0: 40 | return sqrt(4 * n / k2) 41 | else: 42 | return sqrt(4 / k2) 43 | 44 | def run_model(self, q0, t, qref, uref): 45 | q = [q0] 46 | u0 = [0, 0] 47 | for i in range(0,len(t)): 48 | t_step = [t[i-1], t[i]] 49 | 50 | q1 = odeint(self.model, q0, t_step, args = (u0,)) # + [random.uniform(-0.005, 0.005), 51 | # random.uniform(-0.005, 0.005), 52 | # random.uniform(-0.005, 0.005),] 53 | q0 = q1[1] 54 | q.append(q0) 55 | u0 = self.controller(q0, qref[i], uref[i]) 56 | return q -------------------------------------------------------------------------------- /problems/zigzag/problem.yaml: -------------------------------------------------------------------------------- 1 | agents: 2 | - k: [0.5, 0.5, 0.5] 3 | size: 1.5 4 | type: car 5 | velocity: 1 6 | - k: [0.5, 0.5, 0.5] 7 | size: 1.5 8 | type: car 9 | velocity: 1 10 | - k: [0.5, 0.5, 0.5] 11 | size: 1.5 12 | type: car 13 | velocity: 1 14 | - k: [0.5, 0.5, 0.5] 15 | size: 1.5 16 | type: car 17 | velocity: 1 18 | goals: 19 | - - [-1, 0, 12] 20 | - [1, 0, -8] 21 | - [0, -1, -18] 22 | - [0, 1, 22] 23 | - - [-1, 0, 12] 24 | - [1, 0, -8] 25 | - [0, -1, -3] 26 | - [0, 1, 7] 27 | - - [-1, 0, -38] 28 | - [1, 0, 42] 29 | - [0, -1, -18] 30 | - [0, 1, 22] 31 | - - [-1, 0, -38] 32 | - [1, 0, 42] 33 | - [0, -1, -3] 34 | - [0, 1, 7] 35 | limits: 36 | - [-16, 51] 37 | - [-1, 31] 38 | name: zigzag 39 | obstacles: 40 | - - [-1, 1, 5] 41 | - [1, 1, 20] 42 | - [0, -1, 0] 43 | - - [-1, 1, -10] 44 | - [1, 1, 35] 45 | - [0, -1, 0] 46 | - - [-1, -1, -30] 47 | - [1, -1, 0] 48 | - [0, 1, 30] 49 | - - [-1, -1, -15] 50 | - [1, -1, -15] 51 | - [0, 1, 30] 52 | - - [-1, -1, -45] 53 | - [1, -1, 15] 54 | - [0, 1, 30] 55 | - - [-1, 0, 16] 56 | - [1, 0, 51] 57 | - [0, -1, 1] 58 | - [0, 1, 0] 59 | - - [-1, 0, 16] 60 | - [1, 0, 51] 61 | - [0, -1, -30] 62 | - [0, 1, 31] 63 | - - [-1, 0, 16] 64 | - [1, 0, -15] 65 | - [0, -1, 0] 66 | - [0, 1, 30] 67 | - - [-1, 0, -50] 68 | - [1, 0, 51] 69 | - [0, -1, 0] 70 | - [0, 1, 30] 71 | starts: 72 | - [40, 5] 73 | - [40, 20] 74 | - [-10, 5] 75 | - [-10, 20] 76 | -------------------------------------------------------------------------------- /problems/mit/mit.py: -------------------------------------------------------------------------------- 1 | from util import * 2 | from problems.util import * 3 | 4 | Obstacles = [make_box_line(-12, -8, 2, 2, 0, 5.8), 5 | make_box_line(8, 12, 2, 2, 0, 5.8)] 6 | 7 | Thetas = [[-6, -2, 3], 8 | [-4, -2, 3], 9 | [-2, -2, 3], 10 | [0, -2, 3], 11 | [2, -2, 3], 12 | [4, -2, 3], 13 | [6, -2, 3], 14 | 15 | [-2, -4, 3], 16 | [-2, -6, 3], 17 | [-2, -8, 3], 18 | [-4, -8, 3], 19 | [-6, -8, 3], 20 | 21 | [2, -4, 3], 22 | [2, -6, 3], 23 | [2, -8, 3], 24 | [4, -8, 3], 25 | [6, -8, 3], 26 | ] 27 | 28 | Goals = [make_box_center(-14, 8, 1, 2, 2, 2), 29 | make_box_center(-12, 8, 3, 2, 2, 2), 30 | make_box_center(-10, 8, 5, 2, 2, 2), 31 | make_box_center(-8, 8, 3, 2, 2, 2), 32 | make_box_center(-6, 8, 1, 2, 2, 2), 33 | make_box_center(-4, 8, 3, 2, 2, 2), 34 | make_box_center(-2, 8, 5, 2, 2, 2), 35 | make_box_center(0, 8, 3, 2, 2, 2), 36 | make_box_center(2, 8, 1, 2, 2, 2), 37 | 38 | make_box_center(6, 8, 1, 2, 2, 2), 39 | make_box_center(6, 8, 3, 2, 2, 2), 40 | make_box_center(6, 8, 5, 2, 2, 2), 41 | 42 | make_box_center(10, 8, 5, 2, 2, 2), 43 | make_box_center(12, 8, 5, 2, 2, 2), 44 | make_box_center(12, 8, 3, 2, 2, 2), 45 | make_box_center(12, 8, 1, 2, 2, 2), 46 | make_box_center(14, 8, 5, 2, 2, 2),] 47 | 48 | 49 | limits = [[-16, 16], [-10, 10], [0.5, 6]] 50 | 51 | size = 0.5 52 | velocity = 1 53 | k = [2, 2, 2] 54 | file_name = "problem.yaml" 55 | name = "mit" 56 | 57 | agents = [AUV(size, velocity, k) for _ in range(17)] 58 | write_problem(file_name, name, limits, Obstacles, agents, Thetas, Goals) 59 | 60 | -------------------------------------------------------------------------------- /models/hovercraft.py: -------------------------------------------------------------------------------- 1 | # 3d Kinematic Model 2 | # Written by: Kristina Miller 3 | 4 | from math import * 5 | from scipy.integrate import odeint 6 | from models.agent import * 7 | 8 | class Hovercraft(Agent): 9 | 10 | def __init__(self, size, velocity, k): 11 | self.size = size 12 | self.velocity = velocity 13 | self.k = k 14 | 15 | def model(self, q, t, u): 16 | x, y, z, theta = q 17 | v, vz, w = u 18 | dxdt = v*cos(theta) 19 | dydt = v*sin(theta) 20 | dzdt = vz 21 | dthetadt = w 22 | return [dxdt, dydt, dzdt, dthetadt] 23 | 24 | def controller(self, q, qref, uref): 25 | 26 | k1, k2, k3, kp = self.k 27 | xref, yref, zref, rollref, pitchref, yawref = qref 28 | x, y, z, theta = q 29 | 30 | vxyref = uref[0]*cos(pitchref) 31 | vzref = uref[0]*sin(pitchref) 32 | _, _, uyaw_ref = uref[1] 33 | 34 | xe = cos(theta) * (xref - x) + sin(theta) * (yref - y) 35 | ye = -sin(theta) * (xref - x) + cos(theta) * (yref - y) 36 | ze = zref - z 37 | thetae = pitchref - theta 38 | 39 | vxy = vxyref * cos(thetae) + k1 * xe 40 | w = uyaw_ref + vxyref * (k2 * ye + k3 * sin(thetae)) 41 | vz = vzref + kp * ze 42 | 43 | return [vxy, vz, w] 44 | 45 | def bloating(self, n): 46 | return 0.15 47 | k1, k2, k3, kp = self.k 48 | if n != 0: 49 | return sqrt(4*n/k2) 50 | else: 51 | return 0 52 | 53 | def run_model(self, q0, t, qref, uref): 54 | q = [q0] 55 | u0 = [0, 0, 0] 56 | for i in range(0,len(t)): 57 | t_step = [t[i-1], t[i]] 58 | q1 = odeint(self.model, q0, t_step, args = (u0,)) 59 | q0 = q1[1] 60 | # print(q0) 61 | q.append(q0) 62 | u0 = self.controller(q0, qref[i], uref[i]) 63 | return q 64 | -------------------------------------------------------------------------------- /problems/parking/problem.yaml: -------------------------------------------------------------------------------- 1 | agents: 2 | - k: [0.5, 0.5, 0.5] 3 | size: 2.5 4 | type: car 5 | velocity: 1 6 | - k: [0.5, 0.5, 0.5] 7 | size: 2.5 8 | type: car 9 | velocity: 1 10 | - k: [0.5, 0.5, 0.5] 11 | size: 2.5 12 | type: car 13 | velocity: 1 14 | goals: 15 | - - [-1, 0, -35] 16 | - [1, 0, 55] 17 | - [0, -1, 5] 18 | - [0, 1, 15] 19 | - - [-1, 0, -20] 20 | - [1, 0, 40] 21 | - [0, -1, 5] 22 | - [0, 1, 15] 23 | - - [-1, 0, -5] 24 | - [1, 0, 25] 25 | - [0, -1, 5] 26 | - [0, 1, 15] 27 | limits: 28 | - [0, 90] 29 | - [0, 60] 30 | name: parking 31 | obstacles: 32 | - - [-1, 0, -25] 33 | - [1, 0, 80] 34 | - [0, -1, -45] 35 | - [0, 1, 50] 36 | - - [-1, 0, -8] 37 | - [1, 0, 58] 38 | - [0, -1, -8] 39 | - [0, 1, 19] 40 | - - [-1, 0, -40] 41 | - [1, 0, 41] 42 | - [0, -1, -30] 43 | - [0, 1, 40] 44 | - - [-1, 0, -1] 45 | - [1, 0, 41] 46 | - [0, -1, -29] 47 | - [0, 1, 30] 48 | - - [-1, 0, -1] 49 | - [1, 0, 2] 50 | - [0, -1, -1] 51 | - [0, 1, 29] 52 | - - [-1, 0, -2] 53 | - [1, 0, 59] 54 | - [0, -1, -1] 55 | - [0, 1, 2] 56 | - - [-1, 0, -58] 57 | - [1, 0, 59] 58 | - [0, -1, -2] 59 | - [0, 1, 40] 60 | - - [-1, 0, -58] 61 | - [1, 0, 89] 62 | - [0, -1, -40] 63 | - [0, 1, 41] 64 | - - [-1, 0, -88] 65 | - [1, 0, 89] 66 | - [0, -1, -41] 67 | - [0, 1, 58] 68 | - - [-1, 0, -15] 69 | - [1, 0, 89] 70 | - [0, -1, -58] 71 | - [0, 1, 59] 72 | - - [-1, 0, -15] 73 | - [1, 0, 16] 74 | - [0, -1, -40] 75 | - [0, 1, 58] 76 | - - [-1, 0, -15] 77 | - [1, 0, 40] 78 | - [0, -1, -39] 79 | - [0, 1, 40] 80 | starts: 81 | - [35, 55] 82 | - [50, 55] 83 | - [65, 55] 84 | -------------------------------------------------------------------------------- /problems/parking/parking.py: -------------------------------------------------------------------------------- 1 | from problems.util import * 2 | 3 | A_rect = np.array([[-1,0], 4 | [1,0], 5 | [0,-1], 6 | [0,1]]) 7 | 8 | b01 = np.array([[-25], [80], [-45], [50]]) 9 | b02 = np.array([[-8], [58], [-8], [19]]) 10 | 11 | b1 = np.array([[-40], [41], [-30], [40]]) 12 | b2 = np.array([[-1], [41], [-29], [30]]) 13 | b3 = np.array([[-1], [2], [-1], [29]]) 14 | b4 = np.array([[-2], [59], [-1], [2]]) 15 | b5 = np.array([[-58], [59], [-2], [40]]) 16 | b6 = np.array([[-58], [89], [-40], [41]]) 17 | b7 = np.array([[-88], [89], [-41], [58]]) 18 | b8 = np.array([[-15], [89], [-58], [59]]) 19 | b9 = np.array([[-15], [16], [-40], [58]]) 20 | b10 = np.array([[-15], [40], [-39], [40]]) 21 | 22 | Obstacles = [(A_rect, b01), 23 | (A_rect, b02), 24 | (A_rect, b1), 25 | (A_rect, b2), 26 | (A_rect, b3), 27 | (A_rect, b4), 28 | (A_rect, b5), 29 | (A_rect, b6), 30 | (A_rect, b7), 31 | (A_rect, b8), 32 | (A_rect, b9), 33 | (A_rect, b10)] 34 | 35 | Thetas = [[35, 55], 36 | [50, 55], 37 | [65, 55]] 38 | 39 | 40 | Goals = [] 41 | A_rect = np.array([[-1,0], 42 | [1,0], 43 | [0,-1], 44 | [0,1]]) 45 | 46 | pG1 = [45, 5] 47 | lG1 = [10, 10] 48 | b3 = np.array([[-(pG1[0] - lG1[0])], [pG1[0] + lG1[0]], [-(pG1[1] - lG1[1])], [pG1[1] + lG1[1]]]) 49 | Goals.append((A_rect, b3)) 50 | 51 | pG2 = [30, 5] 52 | lG2 = [10, 10] 53 | b4 = np.array([[-(pG2[0] - lG2[0])], [pG2[0] + lG2[0]], [-(pG2[1] - lG2[1])], [pG2[1] + lG2[1]]]) 54 | Goals.append((A_rect, b4)) 55 | 56 | pG3 = [15, 5] 57 | lG3 = [10, 10] 58 | b5 = np.array([[-(pG3[0] - lG3[0])], [pG3[0] + lG3[0]], [-(pG3[1] - lG3[1])], [pG3[1] + lG3[1]]]) 59 | Goals.append((A_rect, b5)) 60 | 61 | limits = [[0, 90], [0, 60]] 62 | 63 | file_name = "problem.yaml" 64 | name = "parking" 65 | 66 | size = 2.5 67 | velocity = 1 68 | k = [0.5, 0.5, 0.5] 69 | agents = [Car(size, velocity, k) for _ in range(3)] 70 | write_problem(file_name, name, limits, Obstacles, agents, Thetas, Goals) -------------------------------------------------------------------------------- /problems/zigzag/zigzag.py: -------------------------------------------------------------------------------- 1 | from util import * 2 | from problems.util import * 3 | 4 | A_rect = np.array([[-1,0], 5 | [1,0], 6 | [0,-1], 7 | [0,1]]) 8 | A_tri1 = np.array([[-1,-1], 9 | [1,-1], 10 | [0,1]]) 11 | A_tri2 = np.array([[-1,1], 12 | [1,1], 13 | [0,-1]]) 14 | b1 = np.array([[5], [20], [0]]) 15 | b2 = np.array([[-10], [35], [0]]) 16 | b3 = np.array([[-30], [0], [30]]) 17 | b4 = np.array([[-15], [-15], [30]]) 18 | b5 = np.array([[-45], [15], [30]]) 19 | 20 | b6 = np.array([[16], [51], [1], [0]]) 21 | b7 = np.array([[16], [51], [-30], [31]]) 22 | b8 = np.array([[16], [-15], [0], [30]]) 23 | b9 = np.array([[-50], [51], [0], [30]]) 24 | 25 | Obstacles = [(A_tri2, b1), 26 | (A_tri2, b2), 27 | (A_tri1, b3), 28 | (A_tri1, b4), 29 | (A_tri1, b5), 30 | (A_rect, b6), 31 | (A_rect, b7), 32 | (A_rect, b8), 33 | (A_rect, b9)] 34 | 35 | 36 | Thetas = [[40, 5], 37 | [40, 20], 38 | [-10, 5], 39 | [-10, 20]] 40 | 41 | 42 | 43 | Goals = [] 44 | 45 | pG1 = [-10, 20] 46 | lG1 = [2, 2] 47 | b1 = np.array([[-(pG1[0] - lG1[0])], [pG1[0] + lG1[0]], [-(pG1[1] - lG1[1])], [pG1[1] + lG1[1]]]) 48 | Goal1 = (A_rect, b1) 49 | Goals.append(Goal1) 50 | 51 | pG2 = [-10, 5] 52 | lG2 = [2, 2] 53 | b2 = np.array([[-(pG2[0] - lG2[0])], [pG2[0] + lG2[0]], [-(pG2[1] - lG2[1])], [pG2[1] + lG2[1]]]) 54 | Goal2 = (A_rect, b2) 55 | Goals.append(Goal2) 56 | 57 | pG3 = [40, 20] 58 | lG3 = [2, 2] 59 | b3 = np.array([[-(pG3[0] - lG3[0])], [pG3[0] + lG3[0]], [-(pG3[1] - lG3[1])], [pG3[1] + lG3[1]]]) 60 | Goal3 = (A_rect, b3) 61 | Goals.append(Goal3) 62 | 63 | pG4 = [40, 5] 64 | lG4 = [2, 2] 65 | b4 = np.array([[-(pG4[0] - lG4[0])], [pG4[0] + lG4[0]], [-(pG4[1] - lG4[1])], [pG4[1] + lG4[1]]]) 66 | Goal4 = (A_rect, b4) 67 | Goals.append(Goal4) 68 | 69 | 70 | limits = [[-16, 51], [-1, 31]] 71 | 72 | 73 | file_name = "problem.yaml" 74 | name = "zigzag" 75 | 76 | size = 1.5 77 | velocity = 1 78 | k = [0.5, 0.5, 0.5] 79 | 80 | agents = [Car(size, velocity, k) for _ in range(4)] 81 | write_problem(file_name, name, limits, Obstacles, agents, Thetas, Goals) 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /problems/ground/problem.yaml: -------------------------------------------------------------------------------- 1 | agents: 2 | - k: [0.5, 0.5, 0.5] 3 | size: 1 4 | type: car 5 | velocity: 1 6 | - k: [0.5, 0.5, 0.5] 7 | size: 1 8 | type: car 9 | velocity: 1 10 | - k: [0.5, 0.5, 0.5] 11 | size: 1 12 | type: car 13 | velocity: 1 14 | - k: [0.5, 0.5, 0.5] 15 | size: 1 16 | type: car 17 | velocity: 1 18 | - k: [0.5, 0.5, 0.5] 19 | size: 1 20 | type: car 21 | velocity: 1 22 | - k: [0.5, 0.5, 0.5] 23 | size: 1 24 | type: car 25 | velocity: 1 26 | - k: [0.5, 0.5, 0.5] 27 | size: 1 28 | type: car 29 | velocity: 1 30 | - k: [0.5, 0.5, 0.5] 31 | size: 1 32 | type: car 33 | velocity: 1 34 | - k: [0.5, 0.5, 0.5] 35 | size: 1 36 | type: car 37 | velocity: 1 38 | - k: [0.5, 0.5, 0.5] 39 | size: 1 40 | type: car 41 | velocity: 1 42 | - k: [0.5, 0.5, 0.5] 43 | size: 1 44 | type: car 45 | velocity: 1 46 | - k: [0.5, 0.5, 0.5] 47 | size: 1 48 | type: car 49 | velocity: 1 50 | goals: 51 | - - [-1, 0, -18] 52 | - [1, 0, 22] 53 | - [0, -1, -10] 54 | - [0, 1, 14] 55 | - - [-1, 0, -28] 56 | - [1, 0, 32] 57 | - [0, -1, -10] 58 | - [0, 1, 14] 59 | - - [-1, 0, -38] 60 | - [1, 0, 42] 61 | - [0, -1, -10] 62 | - [0, 1, 14] 63 | - - [-1, 0, -18] 64 | - [1, 0, 22] 65 | - [0, -1, -46] 66 | - [0, 1, 50] 67 | - - [-1, 0, -28] 68 | - [1, 0, 32] 69 | - [0, -1, -46] 70 | - [0, 1, 50] 71 | - - [-1, 0, -38] 72 | - [1, 0, 42] 73 | - [0, -1, -46] 74 | - [0, 1, 50] 75 | - - [-1, 0, -46] 76 | - [1, 0, 50] 77 | - [0, -1, -18] 78 | - [0, 1, 22] 79 | - - [-1, 0, -46] 80 | - [1, 0, 50] 81 | - [0, -1, -28] 82 | - [0, 1, 32] 83 | - - [-1, 0, -46] 84 | - [1, 0, 50] 85 | - [0, -1, -38] 86 | - [0, 1, 42] 87 | - - [-1, 0, -10] 88 | - [1, 0, 14] 89 | - [0, -1, -18] 90 | - [0, 1, 22] 91 | - - [-1, 0, -10] 92 | - [1, 0, 14] 93 | - [0, -1, -28] 94 | - [0, 1, 32] 95 | - - [-1, 0, -10] 96 | - [1, 0, 14] 97 | - [0, -1, -38] 98 | - [0, 1, 42] 99 | limits: 100 | - [0, 60] 101 | - [0, 60] 102 | name: ground 103 | obstacles: [] 104 | starts: 105 | - [20, 48] 106 | - [30, 48] 107 | - [40, 48] 108 | - [20, 12] 109 | - [30, 12] 110 | - [40, 12] 111 | - [12, 20] 112 | - [12, 30] 113 | - [12, 40] 114 | - [48, 20] 115 | - [48, 30] 116 | - [48, 40] 117 | -------------------------------------------------------------------------------- /problems/platform/problem.yaml: -------------------------------------------------------------------------------- 1 | agents: 2 | - k: [0.5, 0.5, 0.5] 3 | size: 1 4 | type: car 5 | velocity: 1 6 | - k: [0.5, 0.5, 0.5] 7 | size: 1 8 | type: car 9 | velocity: 1 10 | - k: [0.5, 0.5, 0.5] 11 | size: 1 12 | type: car 13 | velocity: 1 14 | - k: [0.5, 0.5, 0.5] 15 | size: 1 16 | type: car 17 | velocity: 1 18 | - k: [0.5, 0.5, 0.5] 19 | size: 1 20 | type: car 21 | velocity: 1 22 | - k: [0.5, 0.5, 0.5] 23 | size: 1 24 | type: car 25 | velocity: 1 26 | - k: [0.5, 0.5, 0.5] 27 | size: 1 28 | type: car 29 | velocity: 1 30 | - k: [0.5, 0.5, 0.5] 31 | size: 1 32 | type: car 33 | velocity: 1 34 | - k: [0.5, 0.5, 0.5] 35 | size: 1 36 | type: car 37 | velocity: 1 38 | - k: [0.5, 0.5, 0.5] 39 | size: 1 40 | type: car 41 | velocity: 1 42 | - k: [0.5, 0.5, 0.5] 43 | size: 1 44 | type: car 45 | velocity: 1 46 | - k: [0.5, 0.5, 0.5] 47 | size: 1 48 | type: car 49 | velocity: 1 50 | goals: 51 | - - [-1, 0, -18] 52 | - [1, 0, 22] 53 | - [0, -1, -10] 54 | - [0, 1, 14] 55 | - - [-1, 0, -28] 56 | - [1, 0, 32] 57 | - [0, -1, -10] 58 | - [0, 1, 14] 59 | - - [-1, 0, -38] 60 | - [1, 0, 42] 61 | - [0, -1, -10] 62 | - [0, 1, 14] 63 | - - [-1, 0, -18] 64 | - [1, 0, 22] 65 | - [0, -1, -46] 66 | - [0, 1, 50] 67 | - - [-1, 0, -28] 68 | - [1, 0, 32] 69 | - [0, -1, -46] 70 | - [0, 1, 50] 71 | - - [-1, 0, -38] 72 | - [1, 0, 42] 73 | - [0, -1, -46] 74 | - [0, 1, 50] 75 | - - [-1, 0, -46] 76 | - [1, 0, 50] 77 | - [0, -1, -18] 78 | - [0, 1, 22] 79 | - - [-1, 0, -46] 80 | - [1, 0, 50] 81 | - [0, -1, -28] 82 | - [0, 1, 32] 83 | - - [-1, 0, -46] 84 | - [1, 0, 50] 85 | - [0, -1, -38] 86 | - [0, 1, 42] 87 | - - [-1, 0, -10] 88 | - [1, 0, 14] 89 | - [0, -1, -18] 90 | - [0, 1, 22] 91 | - - [-1, 0, -10] 92 | - [1, 0, 14] 93 | - [0, -1, -28] 94 | - [0, 1, 32] 95 | - - [-1, 0, -10] 96 | - [1, 0, 14] 97 | - [0, -1, -38] 98 | - [0, 1, 42] 99 | limits: 100 | - [0, 60] 101 | - [0, 60] 102 | name: platform 103 | obstacles: 104 | - - [-1, 0, -20] 105 | - [1, 0, 40] 106 | - [0, -1, -20] 107 | - [0, 1, 40] 108 | starts: 109 | - [20, 48] 110 | - [30, 48] 111 | - [40, 48] 112 | - [20, 12] 113 | - [30, 12] 114 | - [40, 12] 115 | - [12, 20] 116 | - [12, 30] 117 | - [12, 40] 118 | - [48, 20] 119 | - [48, 30] 120 | - [48, 40] 121 | -------------------------------------------------------------------------------- /util.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import numpy as np 3 | import csv 4 | 5 | def save_reftrajs(trajs, path): 6 | data = dict() 7 | for idx in range(len(trajs)): 8 | data["Agent %s"%(idx)] = trajs[idx] 9 | yaml.Dumper.ignore_aliases = lambda *args: True 10 | with open(path, "w") as file: 11 | yaml.dump(data, file, indent=4) 12 | 13 | def write_line(path, line): 14 | f = open(path, "a+") 15 | f.write(line) 16 | f.close() 17 | 18 | def write_csv(path, list): 19 | with open(path, 'a+') as file: 20 | writer = csv.writer(file) 21 | writer.writerows([list]) 22 | 23 | def make_rectangle_line(xmin, xmax, ymin, ymax): 24 | A_rect = np.array([[-1, 0], 25 | [1, 0], 26 | [0, -1], 27 | [0, 1]]) 28 | b = np.array([[-xmin], [xmax], [-ymin], [ymax]]) 29 | return (A_rect, b) 30 | 31 | def make_rectangle_center(x, y, xl, yl): 32 | A_rect = np.array([[-1, 0], 33 | [1, 0], 34 | [0, -1], 35 | [0, 1]]) 36 | b = np.array([[-(x - xl/2)], [x + xl/2], [-(y - yl/2)], [y + yl/2]]) 37 | 38 | return (A_rect, b) 39 | 40 | def make_box_line(xmin, xmax, ymin, ymax, zmin, zmax): 41 | 42 | if not zmin and not zmax: 43 | A_cube = np.array([[-1, 0, 0], 44 | [1, 0, 0], 45 | [0, -1, 0], 46 | [0, 1, 0]]) 47 | b = np.array([[-xmin], [xmax], [-ymin], [ymax]]) 48 | else: 49 | A_cube = np.array([[-1, 0, 0], 50 | [1, 0, 0], 51 | [0, -1, 0], 52 | [0, 1, 0], 53 | [0, 0, -1], 54 | [0, 0, 1]]) 55 | b = np.array([[-xmin], [xmax], [-ymin], [ymax], [-zmin], [zmax]]) 56 | 57 | return (A_cube, b) 58 | 59 | def make_box_center(x, y, z, xl, yl, zl): 60 | if not z and not zl: 61 | A_cube = np.array([[-1, 0, 0], 62 | [1, 0, 0], 63 | [0, -1, 0], 64 | [0, 1, 0]]) 65 | b = np.array([[-(x - xl / 2)], [x + xl / 2], 66 | [-(y - yl / 2)], [y + yl / 2]]) 67 | else: 68 | A_cube = np.array([[-1, 0, 0], 69 | [1, 0, 0], 70 | [0, -1, 0], 71 | [0, 1, 0], 72 | [0, 0, -1], 73 | [0, 0, 1]]) 74 | b = np.array([[-(x - xl / 2)], [x + xl / 2], 75 | [-(y - yl / 2)], [y + yl / 2], 76 | [-(z - zl / 2)], [z + zl / 2]]) 77 | 78 | return (A_cube, b) 79 | -------------------------------------------------------------------------------- /algs/ref2traj.py: -------------------------------------------------------------------------------- 1 | # Ref2Traj 2 | 3 | import numpy as np 4 | from numpy.linalg import norm 5 | 6 | def get_xref(p1, p2, times): 7 | 8 | dim = len(p1) 9 | t1, t2 = times[0], times[-1] 10 | 11 | if dim == 2: 12 | mx = p2[0] - p1[0] 13 | bx = p1[0] 14 | 15 | my = p2[1] - p1[1] 16 | by = p1[1] 17 | 18 | theta = np.arctan2((np.array(p2) - np.array(p1))[1], (np.array(p2) - np.array(p1))[0]) 19 | 20 | qref = [] # append the states to this list 21 | for time in times: 22 | xref = mx*((time - t1) / (t2 - t1)) + bx 23 | yref = my*((time - t1) / (t2 - t1)) + by 24 | qref.append([xref, yref, theta]) 25 | 26 | elif dim == 3: 27 | mx = p2[0] - p1[0] 28 | bx = p1[0] 29 | 30 | my = p2[1] - p1[1] 31 | by = p1[1] 32 | 33 | mz = p2[2] - p1[2] 34 | bz = p1[2] 35 | 36 | roll = 0 37 | pitch = np.arctan2((np.array(p2) - np.array(p1))[2], norm(np.array(p2)[0:2] - np.array(p1)[0:2])) 38 | yaw = np.arctan2((np.array(p2) - np.array(p1))[1], (np.array(p2) - np.array(p1))[0]) 39 | 40 | qref = [] 41 | for time in times: 42 | xref = mx * ((time - t1) / (t2 - t1)) + bx 43 | yref = my * ((time - t1) / (t2 - t1)) + by 44 | zref = mz * ((time - t1) / (t2 - t1)) + bz 45 | qref.append([xref, yref, zref, roll, pitch, yaw]) 46 | 47 | else: 48 | print('Nodes not in workspace!') 49 | 50 | return qref 51 | 52 | def get_uref(p1, p2, times): 53 | dist = norm(np.array(p2) - np.array(p1)) 54 | v_ref = dist / (times[-1] - times[0]) 55 | dim = len(p1) 56 | if dim == 2: 57 | uref = [] 58 | for _ in times: 59 | vref = v_ref 60 | wref = 0 61 | uref.append([vref, wref]) 62 | elif dim == 3: 63 | uref = [] 64 | for _ in times: 65 | vref = v_ref 66 | roll_ref = 0 67 | pitch_ref = 0 68 | yaw_ref = 0 69 | uref.append([vref, [roll_ref, pitch_ref, yaw_ref]]) 70 | else: 71 | print('Nodes not in workspace!') 72 | 73 | return uref 74 | 75 | def ref2traj(ma_nodes): 76 | dim = len(ma_nodes[0][0])-1 77 | agent_num = len(ma_nodes) 78 | 79 | # compute step size 80 | step_num = 10000 81 | max_t = max([ma_nodes[idx][-1][0] for idx in range(agent_num)]) 82 | 83 | step_size = max_t / step_num 84 | 85 | ref_trajs = [] 86 | # calculate controller 87 | for idx in range(agent_num): 88 | ref_traj = [] 89 | nodes = ma_nodes[idx] 90 | for j in range(len(nodes) - 1): 91 | s1, s2 = nodes[j:j+2] 92 | t1, t2 = s1[0], s2[0] 93 | p1, p2 = s1[1:], s2[1:] 94 | times = np.arange(t1, t2, step_size) 95 | if times != []: 96 | qref = get_xref(p1, p2, times) 97 | uref = get_uref(p1, p2, times) 98 | ref_traj.append([times, qref, uref]) 99 | # keep still 100 | times = np.arange(ma_nodes[idx][-1][0], max_t, step_size) 101 | p = ma_nodes[idx][-1][1:] 102 | if times != []: 103 | qref = get_xref(p, p, times) 104 | uref = get_uref(p, p, times) 105 | ref_traj.append([times, qref, uref]) 106 | ref_trajs.append(ref_traj) 107 | 108 | return ref_trajs 109 | -------------------------------------------------------------------------------- /viz/animate.py: -------------------------------------------------------------------------------- 1 | import matplotlib.animation as animation 2 | from viz.util import * 3 | import os 4 | from models.agent import * 5 | 6 | def animate_results(models, limits, obstacles, Thetas, goals, ma_segs, name): 7 | agent_num = len(models) 8 | dim = len(limits) 9 | fig, axes = plot_env(limits, obstacles) 10 | plot_goals(goals) 11 | 12 | interval = 20 13 | total_frames = 500 14 | total_time = max([ma_segs[idx][-1][0][-1] for idx in range(agent_num)]) 15 | 16 | if dim == 2: 17 | paths = extract_paths(models, Thetas, ma_segs) 18 | ref_patches = [] 19 | tru_patches = [] 20 | err_patches = [] 21 | for idx in range(agent_num): 22 | ref_x, ref_y, _, tru_x, tru_y, _, times = paths[idx] 23 | ref_patch = plt.Circle((ref_x[0], ref_y[0]), 0, fc='k') 24 | tru_patch = plt.Circle((tru_x[0], tru_y[0]), models[idx].size, fc='navy', alpha = 0.7) 25 | err_patch = plt.Circle((ref_x[0], ref_y[0]), models[idx].size, fc='lightsteelblue', alpha = 0.4) 26 | ref_patches.append(ref_patch) 27 | tru_patches.append(tru_patch) 28 | err_patches.append(err_patch) 29 | # init 30 | def init(): 31 | for idx in range(agent_num): 32 | ref_x, ref_y, _, tru_x, tru_y, _, times = paths[idx] 33 | ref_patches[idx].center = (ref_x[0], ref_y[0]) 34 | tru_patches[idx].center = (tru_x[0], tru_y[0]) 35 | err_patches[idx].center = (ref_x[0], ref_y[0]) 36 | 37 | for patch in ref_patches + tru_patches + err_patches: axes.add_patch(patch) 38 | return ref_patches + tru_patches + err_patches 39 | # animate 40 | tpf = total_time / total_frames 41 | 42 | def animate(f): 43 | ref, tru = [], [] 44 | for idx in range(agent_num): 45 | ref_x, ref_y, _, tru_x, tru_y, _, times = paths[idx] 46 | step = 0 47 | while (step < len(times) - 1) and (times[step] < tpf * f): 48 | step = step + 1 49 | ref_patches[idx].center = (ref_x[step], ref_y[step]) 50 | tru_patches[idx].center = (tru_x[step], tru_y[step]) 51 | err_patches[idx].center = (ref_x[step], ref_y[step]) 52 | if step == len(ref_x) - 1: error = models[idx].size 53 | else: error = (models[idx].size + models[idx].bloating(step)) 54 | err_patches[idx].width = 2 * error 55 | err_patches[idx].height = 2 * error 56 | return ref_patches + tru_patches + err_patches 57 | 58 | 59 | ani = animation.FuncAnimation(fig, animate, frames = total_frames, init_func=init, 60 | blit=True, interval = interval) 61 | 62 | # fig.subplots_adjust(left=0.01, bottom=0.01, right=0.99, top=0.99, wspace=None, hspace=None) 63 | path = os.path.abspath("results/plots/%s.mp4" % (name)) 64 | ani.save(path, writer='ffmpeg') 65 | -------------------------------------------------------------------------------- /problems/util.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | from models.agent import * 3 | from models.auv import * 4 | from models.car import * 5 | from models.hovercraft import * 6 | 7 | 8 | def write_results(file_name, paths, problem_path = None): 9 | 10 | data = dict() 11 | 12 | if problem_path is not None: 13 | name, limits, Obstacles, agents, Thetas, Goals = read_problem(problem_path) 14 | data['name'] = name 15 | data["limits"] = limits 16 | data["obstacles"] = [write_polytope(obs) for obs in Obstacles] 17 | data["agents"] = [write_agent(agent) for agent in agents] 18 | data["starts"] = Thetas 19 | data["goals"] = [write_polytope(goal) for goal in Goals] 20 | 21 | data["paths"] = paths 22 | 23 | yaml.Dumper.ignore_aliases = lambda *args: True 24 | with open(file_name, "w") as file: 25 | yaml.dump(data, file, indent=4) 26 | 27 | def read_configuration(file_name): 28 | 29 | with open(file_name, "r") as file: 30 | data = yaml.load(file) 31 | 32 | min_segs = data["min_segs"] 33 | max_segs = data["max_segs"] 34 | obstacle_segs = data["obstacle_segs"] 35 | 36 | return [min_segs, max_segs, obstacle_segs] 37 | 38 | def read_problem(file_name): 39 | 40 | with open(file_name, "r") as file: 41 | data = yaml.load(file) 42 | name = data["name"] 43 | limits = data["limits"] 44 | Obstacles = [read_polytope(obs) for obs in data["obstacles"]] 45 | agents = [read_agent(agent) for agent in data["agents"]] 46 | Thetas = data["starts"] 47 | Goals = [read_polytope(goal) for goal in data["goals"]] 48 | 49 | return [name, limits, Obstacles, agents, Thetas, Goals] 50 | 51 | def write_problem(file_name, name, limits, Obstacles, agents, Thetas, Goals): 52 | data = dict() 53 | data['name'] = name 54 | data["limits"] = limits 55 | data["obstacles"] = [write_polytope(obs) for obs in Obstacles] 56 | data["agents"] = [write_agent(agent) for agent in agents] 57 | data["starts"] = Thetas 58 | data["goals"] = [write_polytope(goal) for goal in Goals] 59 | yaml.Dumper.ignore_aliases = lambda *args: True 60 | with open(file_name, "w") as file: 61 | yaml.dump(data, file, indent=4) 62 | 63 | def write_agent(agent): 64 | agent_dict = dict() 65 | agent_dict["k"] = agent.k 66 | agent_dict["velocity"] = agent.velocity 67 | agent_dict["size"] = agent.size 68 | if isinstance(agent, Car): 69 | agent_dict["type"] = "car" 70 | elif isinstance(agent, AUV): 71 | agent_dict["type"] = "auv" 72 | elif isinstance(agent, Hovercraft): 73 | agent_dict["type"] = "hovercraf" 74 | 75 | return agent_dict 76 | 77 | def read_agent(agent): 78 | type = agent["type"] 79 | k = agent["k"] 80 | velocity = agent["velocity"] 81 | size = agent["size"] 82 | if agent["type"] == "car": 83 | return Car(size, velocity, k) 84 | elif type == "auv": 85 | return AUV(size, velocity, k) 86 | elif type == "hovercraft": 87 | return Hovercraft(size, velocity, k) 88 | 89 | def write_polytope(poly): 90 | A, b = poly 91 | lines = [] 92 | for idx in range(len(A)): 93 | lines.append(A[idx].tolist()+b[idx].tolist()) 94 | return lines 95 | 96 | def read_polytope(poly): 97 | A = [] 98 | b = [] 99 | for idx in range(len(poly)): 100 | line = list(poly[idx]) 101 | A.append(line[:-1]) 102 | b.append([line[-1]]) 103 | return [np.array(A), np.array(b)] 104 | -------------------------------------------------------------------------------- /models/auv.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | import numpy as np 3 | import random 4 | from scipy.integrate import odeint 5 | from models.agent import * 6 | from math import * 7 | import numpy as np 8 | from numpy.linalg import inv, norm 9 | 10 | class AUV(Agent): 11 | 12 | def __init__(self, size, velocity, k): 13 | self.size = size 14 | self.velocity = velocity 15 | self.k = k 16 | 17 | def model(self, q, t, u): 18 | # x, y, z, roll, pitch, yaw 19 | x, y, z, phi, theta, psi = q 20 | v = u[0] 21 | wx, wy, wz = u[1] 22 | 23 | xdot = v * cos(psi) * cos(theta) 24 | ydot = v * sin(psi) * cos(theta) 25 | zdot = v * sin(theta) 26 | phidot = wx + wy * sin(phi) * tan(theta) + wz * cos(phi) * tan(theta) 27 | thetadot = wy * cos(phi) - wz * sin(phi) 28 | psidot = wy * sin(phi) * (1 / cos(theta)) + wz * cos(phi) * (1 / cos(theta)) 29 | 30 | return [xdot, ydot, zdot, phidot, thetadot, psidot] 31 | 32 | def controller(self, q, qref, uref): 33 | 34 | x, y, z, phi, theta, psi = q 35 | c_phi = cos(phi) 36 | c_theta = cos(theta) 37 | c_psi = cos(psi) 38 | s_phi = sin(phi) 39 | s_theta = sin(theta) 40 | s_psi = sin(psi) 41 | 42 | k_1, k_2, k_3 = self.k 43 | R = np.array( 44 | [[c_psi * c_theta, c_psi * s_theta * s_phi - s_psi * c_phi, c_psi * s_theta * c_phi + s_psi * s_phi], 45 | [s_psi * c_theta, s_psi * s_theta * s_phi + c_psi * c_phi, s_psi * s_theta * c_phi - c_psi * s_phi], 46 | [-s_theta, c_theta * s_phi, c_theta * c_phi]]) 47 | 48 | B_2 = np.array([[1, sin(phi) * tan(theta), cos(phi) * tan(theta)], 49 | [0, cos(phi), -sin(phi)], 50 | [0, sin(phi) * (1 / cos(theta)), cos(phi) * (1 / cos(theta))]]) 51 | 52 | phi_d, theta_d, psi_d = qref[3:6] 53 | u_2d = np.array([[phi_d]]) 54 | 55 | v_d = uref[0] 56 | # The error is calculated here. This is the error between the waypoint and 57 | # the current state 58 | e_x, e_y, e_z, e_phi, e_theta, e_psi = calculate_error(q, qref, uref, R) 59 | u_2d = np.transpose(np.array(uref[1])) 60 | 61 | B_2d = np.array([[1, sin(phi_d) * tan(theta_d), cos(phi_d) * tan(theta_d)], 62 | [0, cos(phi_d), -sin(phi_d)], 63 | [0, sin(phi_d) * (1 / cos(theta_d)), cos(phi_d) * (1 / cos(theta_d))]]) 64 | 65 | Q = np.transpose(np.array([0, e_z * v_d / k_2, e_y * v_d * cos(e_theta) / k_3])) # te 66 | P_e = np.transpose(np.array([k_1 * sin(e_phi), k_2 * sin(e_theta), k_3 * sin(e_psi)])) 67 | 68 | # This is the control law 69 | gamma = 1 70 | v_b = v_d * (cos(e_psi) * cos(e_theta) - 1) + e_x * gamma ** 2 71 | u_2b = inv(B_2) * (Q + (B_2d - B_2) * u_2d + P_e) 72 | 73 | u_1 = v_d + v_b 74 | u_2 = u_2d + u_2b 75 | u_2 = [u_2[0][0], u_2[1][1], u_2[2][2]] 76 | 77 | return [u_1, u_2] 78 | 79 | def bloating(self, n): 80 | return 0 81 | k1, k2, k3, kp = self.k 82 | if n != 0: 83 | return sqrt(4*n/k2) 84 | else: 85 | return 0 86 | 87 | def run_model(self, q0, t, qref, uref): 88 | q = [q0] 89 | u0 = [0, [0, 0, 0]] 90 | for i in range(0, len(t)): 91 | t_step = [t[i - 1], t[i]] 92 | dx, dy, dz, dphi, dtheta, dpsi = [random.uniform(-0.001, 0.001), 93 | random.uniform(-0.001, 0.001), 94 | random.uniform(-0.001, 0.001), 95 | random.uniform(-0.001, 0.001), 96 | random.uniform(-0.001, 0.001), 97 | random.uniform(-0.001, 0.001)] 98 | q1 = odeint(self.model, q0, t_step, args=(u0,)) + [dx, dy, dz, dphi, dtheta, dpsi] 99 | q0 = q1[1] 100 | q.append(q0) 101 | u0 = self.controller(q0, qref[i], uref[i]) 102 | return q 103 | 104 | def calculate_error(q, qref, uref, R): 105 | X = np.array(q[0:3]) 106 | Theta = np.array(q[3:6]) 107 | 108 | # X_d = trajectory(t[idx], T, params) 109 | X_d = np.array(qref[0:3]) 110 | Theta_d = np.array(qref[3:6]) 111 | 112 | v_d = uref[0] 113 | wx,wy,wz = uref[1] 114 | 115 | X_e = np.transpose(R)*(X_d - X) 116 | Theta_e = Theta_d - Theta 117 | 118 | return [X_e[0][0], X_e[1][1], X_e[2][2], Theta_e[0], Theta_e[1], Theta_e[2]] 119 | -------------------------------------------------------------------------------- /problems/mit/problem.yaml: -------------------------------------------------------------------------------- 1 | agents: 2 | - k: [2, 2, 2] 3 | size: 0.5 4 | type: auv 5 | velocity: 1 6 | - k: [2, 2, 2] 7 | size: 0.5 8 | type: auv 9 | velocity: 1 10 | - k: [2, 2, 2] 11 | size: 0.5 12 | type: auv 13 | velocity: 1 14 | - k: [2, 2, 2] 15 | size: 0.5 16 | type: auv 17 | velocity: 1 18 | - k: [2, 2, 2] 19 | size: 0.5 20 | type: auv 21 | velocity: 1 22 | - k: [2, 2, 2] 23 | size: 0.5 24 | type: auv 25 | velocity: 1 26 | - k: [2, 2, 2] 27 | size: 0.5 28 | type: auv 29 | velocity: 1 30 | - k: [2, 2, 2] 31 | size: 0.5 32 | type: auv 33 | velocity: 1 34 | - k: [2, 2, 2] 35 | size: 0.5 36 | type: auv 37 | velocity: 1 38 | - k: [2, 2, 2] 39 | size: 0.5 40 | type: auv 41 | velocity: 1 42 | - k: [2, 2, 2] 43 | size: 0.5 44 | type: auv 45 | velocity: 1 46 | - k: [2, 2, 2] 47 | size: 0.5 48 | type: auv 49 | velocity: 1 50 | - k: [2, 2, 2] 51 | size: 0.5 52 | type: auv 53 | velocity: 1 54 | - k: [2, 2, 2] 55 | size: 0.5 56 | type: auv 57 | velocity: 1 58 | - k: [2, 2, 2] 59 | size: 0.5 60 | type: auv 61 | velocity: 1 62 | - k: [2, 2, 2] 63 | size: 0.5 64 | type: auv 65 | velocity: 1 66 | - k: [2, 2, 2] 67 | size: 0.5 68 | type: auv 69 | velocity: 1 70 | goals: 71 | - - [-1, 0, 0, 15.0] 72 | - [1, 0, 0, -13.0] 73 | - [0, -1, 0, -7.0] 74 | - [0, 1, 0, 9.0] 75 | - [0, 0, -1, -0.0] 76 | - [0, 0, 1, 2.0] 77 | - - [-1, 0, 0, 13.0] 78 | - [1, 0, 0, -11.0] 79 | - [0, -1, 0, -7.0] 80 | - [0, 1, 0, 9.0] 81 | - [0, 0, -1, -2.0] 82 | - [0, 0, 1, 4.0] 83 | - - [-1, 0, 0, 11.0] 84 | - [1, 0, 0, -9.0] 85 | - [0, -1, 0, -7.0] 86 | - [0, 1, 0, 9.0] 87 | - [0, 0, -1, -4.0] 88 | - [0, 0, 1, 6.0] 89 | - - [-1, 0, 0, 9.0] 90 | - [1, 0, 0, -7.0] 91 | - [0, -1, 0, -7.0] 92 | - [0, 1, 0, 9.0] 93 | - [0, 0, -1, -2.0] 94 | - [0, 0, 1, 4.0] 95 | - - [-1, 0, 0, 7.0] 96 | - [1, 0, 0, -5.0] 97 | - [0, -1, 0, -7.0] 98 | - [0, 1, 0, 9.0] 99 | - [0, 0, -1, -0.0] 100 | - [0, 0, 1, 2.0] 101 | - - [-1, 0, 0, 5.0] 102 | - [1, 0, 0, -3.0] 103 | - [0, -1, 0, -7.0] 104 | - [0, 1, 0, 9.0] 105 | - [0, 0, -1, -2.0] 106 | - [0, 0, 1, 4.0] 107 | - - [-1, 0, 0, 3.0] 108 | - [1, 0, 0, -1.0] 109 | - [0, -1, 0, -7.0] 110 | - [0, 1, 0, 9.0] 111 | - [0, 0, -1, -4.0] 112 | - [0, 0, 1, 6.0] 113 | - - [-1, 0, 0, 1.0] 114 | - [1, 0, 0, 1.0] 115 | - [0, -1, 0, -7.0] 116 | - [0, 1, 0, 9.0] 117 | - [0, 0, -1, -2.0] 118 | - [0, 0, 1, 4.0] 119 | - - [-1, 0, 0, -1.0] 120 | - [1, 0, 0, 3.0] 121 | - [0, -1, 0, -7.0] 122 | - [0, 1, 0, 9.0] 123 | - [0, 0, -1, -0.0] 124 | - [0, 0, 1, 2.0] 125 | - - [-1, 0, 0, -5.0] 126 | - [1, 0, 0, 7.0] 127 | - [0, -1, 0, -7.0] 128 | - [0, 1, 0, 9.0] 129 | - [0, 0, -1, -0.0] 130 | - [0, 0, 1, 2.0] 131 | - - [-1, 0, 0, -5.0] 132 | - [1, 0, 0, 7.0] 133 | - [0, -1, 0, -7.0] 134 | - [0, 1, 0, 9.0] 135 | - [0, 0, -1, -2.0] 136 | - [0, 0, 1, 4.0] 137 | - - [-1, 0, 0, -5.0] 138 | - [1, 0, 0, 7.0] 139 | - [0, -1, 0, -7.0] 140 | - [0, 1, 0, 9.0] 141 | - [0, 0, -1, -4.0] 142 | - [0, 0, 1, 6.0] 143 | - - [-1, 0, 0, -9.0] 144 | - [1, 0, 0, 11.0] 145 | - [0, -1, 0, -7.0] 146 | - [0, 1, 0, 9.0] 147 | - [0, 0, -1, -4.0] 148 | - [0, 0, 1, 6.0] 149 | - - [-1, 0, 0, -11.0] 150 | - [1, 0, 0, 13.0] 151 | - [0, -1, 0, -7.0] 152 | - [0, 1, 0, 9.0] 153 | - [0, 0, -1, -4.0] 154 | - [0, 0, 1, 6.0] 155 | - - [-1, 0, 0, -11.0] 156 | - [1, 0, 0, 13.0] 157 | - [0, -1, 0, -7.0] 158 | - [0, 1, 0, 9.0] 159 | - [0, 0, -1, -2.0] 160 | - [0, 0, 1, 4.0] 161 | - - [-1, 0, 0, -11.0] 162 | - [1, 0, 0, 13.0] 163 | - [0, -1, 0, -7.0] 164 | - [0, 1, 0, 9.0] 165 | - [0, 0, -1, -0.0] 166 | - [0, 0, 1, 2.0] 167 | - - [-1, 0, 0, -13.0] 168 | - [1, 0, 0, 15.0] 169 | - [0, -1, 0, -7.0] 170 | - [0, 1, 0, 9.0] 171 | - [0, 0, -1, -4.0] 172 | - [0, 0, 1, 6.0] 173 | limits: 174 | - [-16, 16] 175 | - [-10, 10] 176 | - [0.5, 6] 177 | name: mit 178 | obstacles: 179 | - - [-1, 0, 0, 12.0] 180 | - [1, 0, 0, -8.0] 181 | - [0, -1, 0, -2.0] 182 | - [0, 1, 0, 2.0] 183 | - [0, 0, -1, 0.0] 184 | - [0, 0, 1, 5.8] 185 | - - [-1, 0, 0, -8.0] 186 | - [1, 0, 0, 12.0] 187 | - [0, -1, 0, -2.0] 188 | - [0, 1, 0, 2.0] 189 | - [0, 0, -1, 0.0] 190 | - [0, 0, 1, 5.8] 191 | starts: 192 | - [-6, -2, 3] 193 | - [-4, -2, 3] 194 | - [-2, -2, 3] 195 | - [0, -2, 3] 196 | - [2, -2, 3] 197 | - [4, -2, 3] 198 | - [6, -2, 3] 199 | - [-2, -4, 3] 200 | - [-2, -6, 3] 201 | - [-2, -8, 3] 202 | - [-4, -8, 3] 203 | - [-6, -8, 3] 204 | - [2, -4, 3] 205 | - [2, -6, 3] 206 | - [2, -8, 3] 207 | - [4, -8, 3] 208 | - [6, -8, 3] 209 | -------------------------------------------------------------------------------- /algs/collision.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import linalg, float16, float64 3 | from math import * 4 | import pypoman as ppm 5 | 6 | 7 | from models.agent import * 8 | 9 | def traj2obs(ref, model, e = 0.001, steps = 10): 10 | O = [] 11 | time_step = (ref[-1][0] - ref[0][0]) / steps 12 | rref = ref.copy() 13 | # print("Start", ref[0][0], "End", ref[-1][0]) 14 | 15 | 16 | for j in range(1,steps): 17 | t = j * time_step + rref[0][0] 18 | # print(j, t) 19 | index = next((i for i in range(len(rref) - 1) 20 | if rref[i][0] <= t and t <= rref[i + 1][0]), None) 21 | assert index != None 22 | t1, t2 = rref[index][0], rref[index+1][0] 23 | p1, p2 = np.array(rref[index][1:]), np.array(rref[index+1][1:]) 24 | k = (p2 - p1) / (t2 - t1) 25 | 26 | p = p1 + k * (t - t1) 27 | rref.insert(index+1, [t] + [x for x in p]) 28 | 29 | # Each Segment To Obstacle 30 | for i in range(len(rref)-1): 31 | # print("Traj Seg[%s/%s] to Moving Obstacles"%(i, len(rref)-1)) 32 | t1, t2 = rref[i][0], rref[i+1][0] 33 | p1, p2 = np.array(rref[i][1:]), np.array(rref[i+1][1:]) 34 | R = model.size + model.bloating(i) + e 35 | # print(R) 36 | # Static during This Segment 37 | if t2 == t1: k = np.array([0] * len(p1)) 38 | # Moving during This Segment 39 | else: k = (p2 - p1) / (t2 - t1) 40 | obs = seg2obs(t1, t2, k, R, p1) 41 | O.append(obs) 42 | # Final Position as A Staic Obstacle 43 | # print("Traj Seg[%s/%s] to Moving Obstacles" % (len(rref)-1, len(rref)-1)) 44 | obs = seg2obs(rref[-1][0], None, np.array([0] * len(p1)), model.size + e, np.array(rref[-1][1:])) 45 | O.append(obs) 46 | 47 | return O 48 | 49 | def seg2obs(lb, ub, k, R, p): 50 | for idx in range(len(k)): 51 | if -0.01 < k[idx] < 0.01: k[idx] = 0 52 | 53 | norm = linalg.norm(k) 54 | 55 | if len(k) == 2: 56 | if linalg.norm(k) != 0: 57 | k = k / linalg.norm(k) 58 | vk = np.array([-k[1], k[0]]) 59 | # print(linalg.norm(k)) 60 | # print("Direction", k, "Vertical Direction", vk) 61 | vertices = [p + R * (- k + vk), p + R * (- k - vk), 62 | p + k * norm * (ub - lb) + R * (k + vk), 63 | p + k * norm * (ub - lb) + R * (k - vk)] 64 | else: 65 | vertices = [p + np.array([R, R]), 66 | p + np.array([R, -R]), 67 | p + np.array([-R, R]), 68 | p + np.array([-R, -R])] 69 | # TODO CDD LIB cannot Handle Float64 70 | 71 | elif len(k) == 3: 72 | if linalg.norm(k) != 0: 73 | k = k / linalg.norm(k) 74 | k1, k2 = [0] * 3, [0] * 3 75 | for ia, ib, ic in [[0, 1, 2], [0, 2, 1], [2, 0, 1]]: 76 | k1[ia], k1[ib], k1[ic] = [-k[ic], -k[ic], k[ia] + k[ib]] 77 | if linalg.norm(np.array(k1)) > 1e-3: break 78 | assert(linalg.norm(np.array(k1)) > 1e-3) 79 | k2[ia], k2[ib], k2[ic] = [-k[ia]*k[ib] - k[ib]*k[ib] - k[ic]*k[ic], 80 | k[ia]*k[ia] + k[ia]*k[ib] + k[ic]*k[ic], 81 | k[ia]*k[ic] - k[ib]*k[ic]] 82 | k1 = np.array(k1) 83 | k2 = np.array(k2) 84 | k1 = float64(k1 / linalg.norm(k1)) 85 | k2 = float64(k2 / linalg.norm(k2)) 86 | vertices = [p + R * (- k + k1 + k2), 87 | p + R * (- k + k1 - k2), 88 | p + R * (- k - k1 + k2), 89 | p + R * (- k - k1 - k2), 90 | p + k * norm * (ub - lb) + R * (- k + k1 + k2), 91 | p + k * norm * (ub - lb) + R * (- k + k1 - k2), 92 | p + k * norm * (ub - lb) + R * (- k - k1 + k2), 93 | p + k * norm * (ub - lb) + R * (- k - k1 - k2)] 94 | else: 95 | vertices = [p + np.array([R, R, R]), 96 | p + np.array([R, R, -R]), 97 | p + np.array([R, -R, R]), 98 | p + np.array([R, -R, -R]), 99 | p + np.array([-R, R, R]), 100 | p + np.array([-R, R, -R]), 101 | p + np.array([-R, -R, R]), 102 | p + np.array([-R, -R, -R])] 103 | # TODO CDD LIB cannot Handle Float64 104 | vertices = [float64(v) for v in vertices] 105 | #print(lb, ub, k, R, p) 106 | #print(vertices) 107 | # print("\n") 108 | A, b = ppm.compute_polytope_halfspaces(vertices) 109 | return lb, ub, A, b 110 | 111 | def find_collision(refA, modelA, refB, modelB, request = 'first'): 112 | rA = modelA.size 113 | rB = modelB.size 114 | bloatingA = modelA.bloating 115 | bloatingB = modelB.bloating 116 | 117 | refA = refA + [np.array([1e4] + list(refA[-1][1:]))] 118 | refB = refB + [np.array([1e4] + list(refB[-1][1:]))] 119 | areaA, areaB = [], [] 120 | 121 | timesA = [refA[i][0] for i in range(len(refA))] 122 | timesB = [refB[i][0] for i in range(len(refB))] 123 | times = sorted(list(set().union(timesA, timesB))) 124 | 125 | # print("A, Times", timesA) 126 | # print("B, Times", timesB) 127 | # print("Times", times) 128 | 129 | for i in range(len(times)-1): 130 | start_time, end_time = times[i], times[i+1] 131 | # print("\n-- [%s, %s]"%(start_time, end_time)) 132 | 133 | indexA = next((i for i in range(len(timesA)-1) 134 | if timesA[i] <= start_time and end_time <= timesA[i+1]), None) 135 | assert indexA != None 136 | tA1, tA2 = timesA[indexA], timesA[indexA+1] 137 | pA1, pA2 = np.array(refA[indexA][1:]), np.array(refA[indexA+1][1:]) 138 | RA = rA 139 | if indexA + 1 != len(refA) - 1: RA = RA + + bloatingA(indexA) 140 | 141 | kA = (pA2 - pA1) / (tA2 - tA1) 142 | bA = pA1 + kA * (start_time - tA1) # positions at start_time of this segment 143 | 144 | indexB = next((i for i in range(len(timesB) - 1) 145 | if timesB[i] <= start_time and timesB[i+1] >= end_time), None) 146 | assert indexB != None 147 | tB1, tB2 = timesB[indexB], timesB[indexB+1] 148 | pB1, pB2 = np.array(refB[indexB][1:]), np.array(refB[indexB+1][1:]) 149 | RB = rB 150 | if indexB + 1 != len(refB) - 1: RB = RB + + bloatingB(indexB) 151 | kB = (pB2 - pB1) / (tB2 - tB1) 152 | bB = pB1 + kB * (start_time - tB1) 153 | 154 | kAB = kA - kB 155 | bAB = bA - bB 156 | R = RA + RB 157 | if len(kAB) == 2: 158 | a = kAB[0] ** 2 + kAB[1] ** 2 159 | b = 2 * kAB[0] * bAB[0] + 2 * kAB[1] * bAB[1] 160 | c = bAB[0] ** 2 + bAB[1] ** 2 - R ** 2 161 | elif len(kAB) == 3: 162 | a = kAB[0] ** 2 + kAB[1] ** 2 + kAB[2] ** 2 163 | b = 2 * kAB[0] * bAB[0] + 2 * kAB[1] * bAB[1] + 2 * kAB[2] * bAB[2] 164 | c = bAB[0] ** 2 + bAB[1] ** 2 + bAB[2] ** 2 - R ** 2 165 | 166 | # print("IndexA: %s, IndexB: %s"%(indexA, indexB)) 167 | # print("A: [%s -> %s]"% (refA[indexA], refA[indexA+1])) 168 | # print("B: [%s -> %s]"% (refB[indexB], refB[indexB+1])) 169 | # print("R:", R, "RA:", RA, "RB", RB) 170 | # print("kAB:", kAB, "kA:", kA, "kB:", kB) 171 | # print("bAB:", bAB, "bA:", bA, "bB:", bB) 172 | # print("a = %s, b = %s, c = %s"%(a, b, c)) 173 | if a == 0: 174 | # print("Constant Relative Distance") 175 | assert b == 0 176 | if c <= 0: 177 | lb, ub = start_time, end_time 178 | areaA.append([lb, ub, kA, RA, bA]) 179 | areaB.append([lb, ub, kB, RB, bB]) 180 | # print("Collision Found") 181 | # print("A", areaA) 182 | # print("B", areaB) 183 | if request == 'first': 184 | return [areaA[0], areaB[0]] 185 | elif b ** 2 - 4 * a * c >= 0: 186 | d = sqrt(b ** 2 - 4 * a * c) 187 | roots = sorted([(-b - d) / (2 * a), (-b + d) / (2 * a)]) 188 | lb, ub = np.array(roots) + np.array([start_time, start_time]) # add time offset 189 | # print("Meet during [%s, %s]"%(lb, ub)) 190 | if start_time <= ub and lb <= end_time: 191 | lb = max(start_time, lb) 192 | ubA = min(end_time, ub) 193 | ubB = min(end_time, ub) 194 | areaA.append([lb, ubA, kA, RA, bA + kA * (lb - start_time)]) 195 | areaB.append([lb, ubB, kB, RB, bB + kB * (lb - start_time)]) 196 | # print("Collision Found") 197 | # print("A", areaA) 198 | # print("B", areaB) 199 | if request == 'first': return [areaA[0], areaB[0]] 200 | 201 | if request == 'first': return [None, None] 202 | elif request == 'all': return [areaA, areaB] -------------------------------------------------------------------------------- /viz/util.py: -------------------------------------------------------------------------------- 1 | from numpy import linalg 2 | import numpy as np 3 | from scipy.spatial import ConvexHull 4 | import mpl_toolkits.mplot3d as a3 5 | import matplotlib.pyplot as plt 6 | import pypoman as ppm 7 | from shapely.geometry.polygon import Polygon 8 | from models.auv import * 9 | from models.hovercraft import * 10 | from util import * 11 | 12 | def plot_env(map_limits, Obstacles): 13 | fig = plt.figure() 14 | if len(map_limits) == 2: 15 | # Configure 16 | xmin, xmax = map_limits[0] 17 | ymin, ymax = map_limits[1] 18 | plt.xlim(xmin-2, xmax+2) 19 | plt.ylim(ymin-2, ymax+2) 20 | # # viz obstacles 21 | for A, b in Obstacles: 22 | poly = Polygon(ppm.duality.compute_polytope_vertices(A, b)) 23 | x, y = poly.exterior.xy 24 | plt.fill(x, y, facecolor='r', alpha=0.3) 25 | ax = fig.gca() 26 | plt.gca().set_aspect('equal', adjustable='box') 27 | plt.axis('off') 28 | ax.get_xaxis().set_visible(False) 29 | ax.get_yaxis().set_visible(False) 30 | plt.margins(0, 0) 31 | 32 | elif len(map_limits) == 3: 33 | ax = fig.add_subplot(111, projection='3d') 34 | xmin, xmax = map_limits[0] 35 | ymin, ymax = map_limits[1] 36 | zmin, zmax = map_limits[2] 37 | ax.set_xlim3d(xmin, xmax) 38 | ax.set_ylim3d(ymin, ymax) 39 | ax.set_zlim3d(zmin, zmax) 40 | # set_equal TODO 41 | plt.axis('off') 42 | ax.get_xaxis().set_visible(False) 43 | ax.get_yaxis().set_visible(False) 44 | ax.get_zaxis().set_visible(False) 45 | plt.margins(0, 0, 0) 46 | for obs in Obstacles: 47 | A, b = obs[0], obs[1] 48 | if len(A) == 4: 49 | A = np.array(A.tolist() + [[0, 0, -1], [0, 0, 1]]) 50 | b = np.array(b.tolist() + [[-map_limits[2][0]], [map_limits[2][1]]]) 51 | plot_polytope_3d(A, b, ax=ax, color='yellow', trans=0.2) 52 | 53 | return fig, ax 54 | 55 | def plot_goals(Goals): 56 | True 57 | for A, b in Goals: 58 | poly = Polygon(ppm.duality.compute_polytope_vertices(A, b)) 59 | x, y = poly.exterior.xy 60 | plt.fill(x, y, facecolor='g', alpha=0.3) 61 | 62 | def plot_thetas(thetas): 63 | for x, y in thetas: 64 | A, b = make_rectangle_center(x, y, 5, 5) 65 | poly = Polygon(ppm.duality.compute_polytope_vertices(A, b)) 66 | x, y = poly.exterior.xy 67 | plt.fill(x, y, facecolor='blue', alpha=0.3) 68 | 69 | def extract_paths(models, ma_thetas, ma_segs): 70 | # viz initial states and trajectories 71 | agent_num = len(models) 72 | paths = [] 73 | for idx in range(agent_num): 74 | theta = ma_thetas[idx] 75 | segs = ma_segs[idx] 76 | 77 | if len(theta) == 2: 78 | ref_x, ref_y, ref_theta, tru_x, tru_y, tru_theta, times = [], [], [], [], [], [], [] 79 | q0 = theta + [0] 80 | for seg in segs: 81 | t, qref, uref = seg 82 | ref_x = ref_x + [qref[i][0] for i in range(len(qref))] 83 | ref_y = ref_y + [qref[i][1] for i in range(len(qref))] 84 | ref_theta = ref_theta + [qref[i][2] for i in range(len(qref))] 85 | times = times + [t[i] for i in range(len(t))] 86 | 87 | run = models[idx].run_model 88 | q = run(q0, t, qref, uref) 89 | q0 = q[-1] 90 | tru_x = tru_x + [q[i][0] for i in range(len(q)-1)] 91 | tru_y = tru_y + [q[i][1] for i in range(len(q)-1)] 92 | tru_theta = tru_theta + [q[i][2] for i in range(len(q)-1)] 93 | paths.append([ref_x, ref_y, ref_theta, tru_x, tru_y, tru_theta, times]) 94 | 95 | elif len(theta) == 3: 96 | 97 | if isinstance(models[0], AUV): 98 | q0 = theta + [0, 0, 0] 99 | ref_x, ref_y, ref_z, times = [], [], [], [] 100 | tru_x, tru_y, tru_z, tru_thi, tru_theta, tru_psi = [], [], [], [], [], [] 101 | for seg in segs: 102 | t, qref, uref = seg 103 | ref_x = ref_x + [qref[i][0] for i in range(len(qref))] 104 | ref_y = ref_y + [qref[i][1] for i in range(len(qref))] 105 | ref_z = ref_z + [qref[i][2] for i in range(len(qref))] 106 | times = times + [t[i] for i in range(len(t))] 107 | 108 | run = models[idx].run_model 109 | q = run(q0, t, qref, uref) 110 | q0 = q[-1] 111 | tru_x = tru_x + [q[i][0] for i in range(len(q)-1)] 112 | tru_y = tru_y + [q[i][1] for i in range(len(q)-1)] 113 | tru_z = tru_z + [q[i][2] for i in range(len(q)-1)] 114 | tru_thi = tru_thi + [q[i][3] for i in range(len(q)-1)] 115 | tru_theta = tru_theta + [q[i][4] for i in range(len(q)-1)] 116 | tru_psi = tru_psi + [q[i][5] for i in range(len(q)-1)] 117 | 118 | paths.append([ref_x, ref_y, ref_z, 119 | tru_x, tru_y, tru_z, tru_thi, tru_theta, tru_psi, times]) 120 | 121 | elif isinstance(models[0], Hovercraft): 122 | q0 = theta + [0, 0, 0] 123 | ref_x, ref_y, ref_z, tru_x, tru_y, tru_z, times = [], [], [], [], [], [], [] 124 | 125 | for seg in segs: 126 | t, qref, uref = seg 127 | ref_x = ref_x + [qref[i][0] for i in range(len(qref))] 128 | ref_y = ref_y + [qref[i][1] for i in range(len(qref))] 129 | ref_z = ref_z + [qref[i][2] for i in range(len(qref))] 130 | times = times + [t[i] for i in range(len(t))] 131 | 132 | run = models[idx].run_model 133 | q = run(q0, t, qref, uref) 134 | q0 = q[-1] 135 | tru_x = tru_x + [q[i][0] for i in range(len(q)-1)] 136 | tru_y = tru_y + [q[i][1] for i in range(len(q)-1)] 137 | tru_z = tru_z + [q[i][2] for i in range(len(q)-1)] 138 | paths.append([ref_x, ref_y, ref_z, tru_x, tru_y, tru_z, times]) 139 | 140 | return paths 141 | 142 | class Faces(): 143 | def __init__(self,tri, sig_dig=12, method="convexhull"): 144 | self.method=method 145 | self.tri = np.around(np.array(tri), sig_dig) 146 | self.grpinx = list(range(len(tri))) 147 | norms = np.around([self.norm(s) for s in self.tri], sig_dig) 148 | _, self.inv = np.unique(norms,return_inverse=True, axis=0) 149 | 150 | def norm(self,sq): 151 | cr = np.cross(sq[2]-sq[0],sq[1]-sq[0]) 152 | return np.abs(cr/np.linalg.norm(cr)) 153 | 154 | def isneighbor(self, tr1,tr2): 155 | a = np.concatenate((tr1,tr2), axis=0) 156 | return len(a) == len(np.unique(a, axis=0))+2 157 | 158 | def order(self, v): 159 | if len(v) <= 3: 160 | return v 161 | v = np.unique(v, axis=0) 162 | n = self.norm(v[:3]) 163 | y = np.cross(n,v[1]-v[0]) 164 | y = y/np.linalg.norm(y) 165 | c = np.dot(v, np.c_[v[1]-v[0],y]) 166 | if self.method == "convexhull": 167 | h = ConvexHull(c) 168 | return v[h.vertices] 169 | else: 170 | mean = np.mean(c,axis=0) 171 | d = c-mean 172 | s = np.arctan2(d[:,0], d[:,1]) 173 | return v[np.argsort(s)] 174 | 175 | def simplify(self): 176 | for i, tri1 in enumerate(self.tri): 177 | for j,tri2 in enumerate(self.tri): 178 | if j > i: 179 | if self.isneighbor(tri1,tri2) and \ 180 | self.inv[i]==self.inv[j]: 181 | self.grpinx[j] = self.grpinx[i] 182 | groups = [] 183 | for i in np.unique(self.grpinx): 184 | u = self.tri[self.grpinx == i] 185 | u = np.concatenate([d for d in u]) 186 | u = self.order(u) 187 | groups.append(u) 188 | return groups 189 | 190 | def plot_polytope_3d(A, b, ax = None, color = 'red', trans = 0.2): 191 | verts = np.array(ppm.compute_polytope_vertices(A, b)) 192 | # compute the triangles that make up the convex hull of the data points 193 | hull = ConvexHull(verts) 194 | triangles = [verts[s] for s in hull.simplices] 195 | # combine co-planar triangles into a single face 196 | faces = Faces(triangles, sig_dig=1).simplify() 197 | # viz 198 | if ax == None: 199 | ax = a3.Axes3D(plt.figure()) 200 | 201 | pc = a3.art3d.Poly3DCollection(faces, 202 | facecolor=color, 203 | edgecolor="k", alpha=trans) 204 | ax.add_collection3d(pc) 205 | # define view 206 | yllim, ytlim = ax.get_ylim() 207 | xllim, xtlim = ax.get_xlim() 208 | zllim, ztlim = ax.get_zlim() 209 | x = verts[:,0] 210 | x = np.append(x, [xllim, xtlim]) 211 | y = verts[:,1] 212 | y = np.append(y, [yllim, ytlim]) 213 | z = verts[:,2] 214 | z = np.append(z, [zllim, ztlim]) 215 | ax.set_xlim(np.min(x)-1, np.max(x)+1) 216 | ax.set_ylim(np.min(y)-1, np.max(y)+1) 217 | ax.set_zlim(np.min(z)-1, np.max(z)+1) 218 | -------------------------------------------------------------------------------- /algs/decentralized.py: -------------------------------------------------------------------------------- 1 | from algs.xref import * 2 | from algs.collision import * 3 | from queue import * 4 | from networkx import * 5 | from copy import deepcopy 6 | from timeit import * 7 | 8 | # Main controller algorithm 9 | def decentralized_algo(models, thetas, goals, limits, obstacles, 10 | min_segs, max_segs, obs_steps, min_dur, timeout = 300): 11 | agent_num = len(models) 12 | params = [models, thetas, goals, limits, obstacles, min_segs, max_segs, obs_steps, min_dur] 13 | 14 | print("\n----- 0 -----") 15 | root = Node(agent_num) 16 | for idx in range(agent_num): 17 | plan = root.update_plan(idx, params) 18 | if plan == None: 19 | return None 20 | q = PriorityQueue() 21 | q.put(root) 22 | times = 0 23 | start_time = default_timer() 24 | while not q.empty(): 25 | if default_timer() - start_time > timeout: 26 | print("Timeout!") 27 | return None 28 | times = times + 1 29 | node = q.get() 30 | print("\n-----", times, "-----") 31 | print("Orders:", node.G.edges) 32 | print("Makespan =", node.makespan) 33 | # Check Collision 34 | colliding_agents = node.earliest_colliding_agents(params) 35 | print("Colliding Agents:", colliding_agents) 36 | # Solution Found 37 | if colliding_agents == None: 38 | print("[*] Solution Found!") 39 | return node.plans 40 | # Resolve Collision 41 | for (j, i) in [colliding_agents, (colliding_agents[1], colliding_agents[0])]: 42 | # Update Node 43 | print("\n[*] Split on (%s < %s)"%(j, i)) 44 | new_node = node.copy() 45 | new_node = new_node.update_node((j, i), params) 46 | if new_node == None: print("Infeasiblae") 47 | else: 48 | print("Feasible and Push") 49 | q.put(new_node) 50 | return None 51 | 52 | class Node: 53 | def __init__(self, num, plans = [], makespan = 0, 54 | G = DiGraph(), orders = set(), 55 | collisions = {}): 56 | # params [num] 57 | self.num = num 58 | # plans [plans, makespan] 59 | if plans == []: plans = [None for i in range(num)] 60 | self.plans = plans 61 | self.makespan = makespan 62 | # orders [G] 63 | G.add_nodes_from(list(range(num))) 64 | if orders != set(): G.add_edges_from(orders) 65 | self.G = G 66 | # collisions 67 | self.collisions = collisions # only specified for agents with certain orderigns 68 | 69 | def __lt__(self, other): 70 | # if (self.makespan < other.makespan): return True 71 | # elif (self.makespan > other.makespan): return False 72 | # else: return len(self.G) < len(other.G) 73 | 74 | if len(self.G.edges) > len(other.G.edges): return True 75 | elif len(self.G.edges) < len(other.G.edges): return False 76 | else: 77 | return self.makespan < other.makespan 78 | 79 | def copy(self, memodict={}): 80 | return Node(self.num, 81 | plans = deepcopy(self.plans), makespan = self.makespan, 82 | G = self.G.copy(), collisions = deepcopy(self.collisions)) 83 | 84 | def agents(self): 85 | return set(list(range(self.num))) 86 | 87 | def higher_agents(self, idx): 88 | return ancestors(self.G, idx) 89 | 90 | def lower_agents(self, idx): 91 | return descendants(self.G, idx) 92 | 93 | def sorted_agents(self, nodes): 94 | H = self.G.subgraph(nodes) 95 | return list(topological_sort(H)) 96 | 97 | def add_order(self, order): 98 | higher_agents = self.higher_agents(order[1]) 99 | lower_agents = self.lower_agents(order[1]) 100 | if order[0] in higher_agents: status = "existing" 101 | elif order[0] in lower_agents: status = "inconsistent" 102 | else: 103 | self.G.add_edge(*order) 104 | status = "added" 105 | return status 106 | 107 | def earliest_colliding_agents(self, params): 108 | 109 | models, _, _, _, _, _, _, _, _ = params 110 | 111 | earliest_time = 1e6 112 | colliding_agents = None 113 | for i in range(self.num): 114 | for j in self.agents() - set([i]): 115 | time = 1e8 116 | # print(" -- Check Collison for (%s, %s)" % (i, j)) 117 | if ("%s-%s" % (i, j) in self.collisions 118 | and self.collisions["%s-%s" % (i, j)] == "free"): 119 | 1 120 | # print(" -- [Collision Free] Previously Checked.") 121 | elif ("%s-%s" % (i, j) in self.collisions 122 | and self.collisions["%s-%s" % (i, j)] != "free"): 123 | # print(" -- [Collision Found] Previously Checked.") 124 | time = self.collisions["%s-%s" % (i, j)] 125 | else: 126 | area, _ = find_collision(self.plans[i], models[i], 127 | self.plans[j], models[j]) 128 | if area != None: 129 | # print(" -- [Collision Found].") 130 | self.collisions["%s-%s" % (i, j)] = area[0] 131 | self.collisions["%s-%s" % (j, i)] = area[0] 132 | time = area[0] 133 | else: 134 | # print(" -- [Collision Free].") 135 | self.collisions["%s-%s" % (i, j)] = "free" 136 | self.collisions["%s-%s" % (j, i)] = "free" 137 | if time < earliest_time: 138 | earliest_time = time 139 | colliding_agents = (i, j) 140 | print(" -- [\] Update Colliding Agents.", i, "-", j) 141 | return colliding_agents 142 | 143 | def update_node(self, order, params): 144 | # Check Ordering Status 145 | status = self.add_order(order) 146 | print("Adding Order Status: ", status) 147 | # assert status == "added" 148 | if status == "inconsistent": return None 149 | # Plan for All the Affected Agents 150 | idx = order[1] 151 | models, _, _, _, _, _, _, _, _ = params 152 | agents = [idx] # Only Update Affected Agents 153 | # agents = [idx] + self.sorted_agents(self.lower_agents(idx)) 154 | print("Agents to Update:", agents) 155 | for k in agents: 156 | print("- Update Plan for Agent", k) 157 | higher_agents = self.higher_agents(k) 158 | # check colliding 159 | replan = False 160 | for j in higher_agents: 161 | # print(" -- Check Collison for (%s, %s)" % (k, j)) 162 | if ("%s-%s" % (k, j) in self.collisions 163 | and self.collisions["%s-%s" % (k, j)] == "free"): 164 | 1 165 | # print(" -- [Collision Free] Previously Checked.") 166 | elif ("%s-%s" % (k, j) in self.collisions 167 | and self.collisions["%s-%s" % (k, j)] != "free"): 168 | # print(" -- [Collision Found] Previously Checked.") 169 | replan = True 170 | break 171 | else: 172 | if self.plans[j] != None: 173 | self.collisions["%s-%s" % (k, j)] != area[0] 174 | self.collisions["%s-%s" % (j, k)] != area[0] 175 | area, _ = find_collision(self.plans[k], models[k], 176 | self.plans[j], models[j]) 177 | if area != None: 178 | self.collisions["%s-%s" % (k, j)] != "free" 179 | self.collisions["%s-%s" % (j, k)] != "free" 180 | # print(" -- [Collision Found].") 181 | replan = True 182 | break 183 | # else: print(" -- [Collision Free].") 184 | 185 | # Update If colliding 186 | if k == idx or replan: 187 | plan = self.update_plan(k, params) 188 | if plan == None: 189 | print("- Update Fails for Agent", k) 190 | return None 191 | print("- Update Succeed for Agent", k) 192 | else: print("- No need to update for Agent", k) 193 | return self 194 | 195 | def update_plan(self, k, params): 196 | 197 | models, thetas, goals, limits, obstacles, min_segs, max_segs, obs_steps, min_dur = params 198 | 199 | print("- Find Collision and Reroute") 200 | MO = [] 201 | for j in self.higher_agents(k): 202 | MO = MO + traj2obs(self.plans[j], models[j], steps = obs_steps) 203 | # if self.plans[k] == None: 204 | # Nmin, Nmax = min_segs, max_segs 205 | # else: 206 | # Nmin = len(self.plans[k]) + 1 207 | # Nmax = Nmin + 2 208 | Nmin, Nmax = min_segs, max_segs 209 | new_plan = get_gb_xref([models[k]], [thetas[k]], [goals[k]], 210 | limits, obstacles, MO, Nmin, Nmax, min_dur = min_dur) 211 | if new_plan == None: 212 | print("- No Plan.") 213 | return None 214 | else: 215 | # Update Plan and Makespan 216 | plan = new_plan[0] 217 | self.plans[k] = plan 218 | self.makespan = max(plan[-1][0], self.makespan) 219 | self.makespan = sum([plan[-1][0] for plan in self.plans if plan != None]) 220 | 221 | print("- Feasible Plan Found with Makespan", plan[-1][0]) 222 | for j in self.higher_agents(k): 223 | self.collisions["%s-%s" % (k, j)] = "free" 224 | self.collisions["%s-%s" % (j, k)] = "free" 225 | # Unfreeze Collision Indicator 226 | for j in self.agents() - self.higher_agents(k): 227 | if "%s-%s" % (k, j) in self.collisions: self.collisions.pop("%s-%s" % (k, j)) 228 | if "%s-%s" % (j, k) in self.collisions: self.collisions.pop("%s-%s" % (j, k)) 229 | return plan 230 | -------------------------------------------------------------------------------- /algs/xref.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | 3 | from math import * 4 | import polytope as pc 5 | from gurobi import * 6 | from numpy import linalg 7 | from models.agent import * 8 | from timeit import * 9 | 10 | def add_initial_constraints(problem, vars, Theta): 11 | # print("Add Intiail Constraint for Agent") 12 | dim = len(Theta) 13 | problem.addConstrs(vars[i] == Theta[i] for i in range(dim)) 14 | 15 | def add_final_constraints(problem, vars, Goal, r): 16 | # print("Add Final Constraint for Agent") 17 | Af = Goal[0] 18 | bf = Goal[1] 19 | 20 | # Make sure that ending point is at the center of the goal set 21 | poly = pc.Polytope(Af, bf) 22 | pt = poly.chebXc # Find the center of the goal set 23 | problem.addConstrs(vars[i] == pt[i] for i in range(len(pt))) 24 | 25 | # # Additionally, make sure all the error is contained within the goal set 26 | # for row in range(len(Af)): 27 | # b = bf[row][0] - linalg.norm(Af[row]) * r 28 | # problem.addConstr(LinExpr(Af[row], vars) <= b) 29 | 30 | def add_obstacle_constraints(problem, p1, p2, O, r): 31 | # print("Add Reference Constraints for Agent") 32 | if O == []: return None 33 | # Add the constraints to avoid the obstacles 34 | for (Ao, bo) in O: 35 | flags = problem.addVars(len(Ao), vtype=GRB.BINARY) 36 | problem.addConstr(flags.sum() <= len(Ao) - 1) 37 | for row in range(len(Ao)): 38 | b = bo[row] + linalg.norm(Ao[row]) * r 39 | problem.addConstr(LinExpr(Ao[row], p1) + 1e5 * flags[row] >= b) 40 | problem.addConstr(LinExpr(Ao[row], p2) + 1e5 * flags[row] >= b) 41 | 42 | def add_moving_obstacle_constraints(problem, p1, p2, t1, t2, MO, r): 43 | # print("Add Reference Constraints for Agent") 44 | if MO == []: return None 45 | # Add the constraints to avoid the obstacles 46 | 47 | for lb, ub, Ao, bo in MO: 48 | # Distance Constraints or Our of Time Window 49 | if t2 == None and ub == None: 50 | flags = problem.addVars(len(Ao), vtype=GRB.BINARY) 51 | problem.addConstr(flags.sum() <= len(Ao)-1) 52 | for row in range(len(Ao)): 53 | b = bo[row] + linalg.norm(Ao[row]) * r 54 | problem.addConstr(LinExpr(Ao[row], p1) + 1e5 * flags[row] >= b) 55 | problem.addConstr(LinExpr(Ao[row], p2) + 1e5 * flags[row] >= b) 56 | elif t2 == None: 57 | flags = problem.addVars(len(Ao) + 1, vtype=GRB.BINARY) 58 | problem.addConstr(flags.sum() <= len(Ao)) 59 | problem.addConstr(t1 + 1e5 * flags[len(Ao)] >= ub) 60 | for row in range(len(Ao)): 61 | b = bo[row] + linalg.norm(Ao[row]) * r 62 | problem.addConstr(LinExpr(Ao[row], p1) + 1e5 * flags[row] >= b) 63 | problem.addConstr(LinExpr(Ao[row], p2) + 1e5 * flags[row] >= b) 64 | elif ub == None: 65 | flags = problem.addVars(len(Ao) + 1, vtype=GRB.BINARY) 66 | problem.addConstr(flags.sum() <= len(Ao)) 67 | problem.addConstr(t2 - 1e5 * flags[len(Ao)] <= lb) 68 | for row in range(len(Ao)): 69 | b = bo[row] + linalg.norm(Ao[row]) * r 70 | problem.addConstr(LinExpr(Ao[row], p1) + 1e5 * flags[row] >= b) 71 | problem.addConstr(LinExpr(Ao[row], p2) + 1e5 * flags[row] >= b) 72 | else: 73 | flags = problem.addVars(len(Ao) + 2, vtype=GRB.BINARY) 74 | problem.addConstr(flags.sum() <= len(Ao) + 1) 75 | problem.addConstr(t1 + 1e5 * flags[len(Ao)] >= ub) 76 | problem.addConstr(t2 - 1e5 * flags[len(Ao)+1] <= lb) 77 | for row in range(len(Ao)): 78 | b = bo[row] + linalg.norm(Ao[row]) * r 79 | problem.addConstr(LinExpr(Ao[row], p1) + 1e5 * flags[row] >= b) 80 | problem.addConstr(LinExpr(Ao[row], p2) + 1e5 * flags[row] >= b) 81 | 82 | def add_avoiding_constraints_linear(problem, pA1, pA2, pB1, pB2, r): 83 | basis_num = 2 84 | dim = len(pA1) 85 | bases = ball_bases(basis_num, dim = dim) 86 | # distance variables LAs, LBs 87 | lAs = problem.addVars(basis_num, vtype=GRB.CONTINUOUS, lb=0) 88 | lBs = problem.addVars(basis_num, vtype=GRB.CONTINUOUS, lb=0) 89 | for i in range(basis_num): 90 | basis = bases[i] 91 | # distance lower bound 92 | problem.addConstr(lAs[i] >= LinExpr(basis, pA1) - LinExpr(basis, pA2)) 93 | problem.addConstr(lAs[i] >= LinExpr(basis, pA2) - LinExpr(basis, pA1)) 94 | problem.addConstr(lBs[i] >= LinExpr(basis, pB1) - LinExpr(basis, pB2)) 95 | problem.addConstr(lBs[i] >= LinExpr(basis, pB2) - LinExpr(basis, pB1)) 96 | # distance variables LAB 97 | lAB2 = problem.addVar(vtype=GRB.CONTINUOUS, lb=0) 98 | lAB_flags = problem.addVars(2 * basis_num, vtype=GRB.BINARY) 99 | 100 | for i in range(basis_num): 101 | basis = bases[i] 102 | # distance upper bound 103 | problem.addConstr(lAB2 - 1e5 * lAB_flags[2 * i] 104 | <= LinExpr(basis, pA1) + LinExpr(basis, pA2) 105 | - LinExpr(basis, pB1) - LinExpr(basis, pB2)) 106 | problem.addConstr(lAB2 - 1e5 * lAB_flags[2 * i + 1] 107 | <= - LinExpr(basis, pA1) - LinExpr(basis, pA2) 108 | + LinExpr(basis, pB1) + LinExpr(basis, pB2)) 109 | # Distance Constraints 110 | problem.addConstr(lAB2 >= lAs.sum() + lBs.sum() + 2 * r) 111 | 112 | def add_constraints(problem, vars, N, dim, 113 | Thetas, Goals, O, MO, 114 | velocities, sizes, bloat_funcs, min_dur): 115 | # Get all the constraints in one function 116 | agent_num = len(Thetas) 117 | add_time_constraints(problem, vars, agent_num, velocities, N, dim, min_dur) 118 | # Add constraints for each agent 119 | for idx in range(agent_num): 120 | errors = [bloat_funcs[idx](n) + sizes[idx] for n in range(N+1)] 121 | p0 = [vars['(A%sN%sD%s)' % (idx, 0, k)] for k in range(dim)] 122 | pN = [vars['(A%sN%sD%s)' % (idx, N, k)] for k in range(dim)] 123 | add_initial_constraints(problem, p0, Thetas[idx]) 124 | add_final_constraints(problem, pN, Goals[idx], errors[N]) 125 | for n in range(N): 126 | p1 = [vars['(A%sN%sD%s)' % (idx, n, k)] for k in range(dim)] 127 | p2 = [vars['(A%sN%sD%s)' % (idx, n+1, k)] for k in range(dim)] 128 | t1, t2 = vars['T%s'%(n)], vars['T%s'%(n+1)] 129 | add_obstacle_constraints(problem, p1, p2, O, errors[n]) 130 | add_moving_obstacle_constraints(problem, p1, p2, t1, t2, MO, errors[n]) 131 | pN = [vars['(A%sN%sD%s)' % (idx, N, k)] for k in range(dim)] 132 | tN = vars['T%s'%(N)] 133 | add_moving_obstacle_constraints(problem, pN, pN, tN, None, MO, sizes[idx]) 134 | for i in range(agent_num): 135 | for j in range(i): 136 | for n in range(N): 137 | pA1 = [vars['(A%sN%sD%s)' % (i, n, k)] for k in range(dim)] 138 | pA2 = [vars['(A%sN%sD%s)' % (i, n+1, k)] for k in range(dim)] 139 | pB1 = [vars['(A%sN%sD%s)' % (j, n, k)] for k in range(dim)] 140 | pB2 = [vars['(A%sN%sD%s)' % (j, n+1, k)] for k in range(dim)] 141 | rA = bloat_funcs[i](n) 142 | rB = bloat_funcs[j](n) 143 | r = rA + rB + sizes[i] + sizes[j] 144 | add_avoiding_constraints_linear(problem, pA1, pA2, pB1, pB2, r) 145 | 146 | def get_variables(problem, agent_num, N, dim, limits): 147 | vars = {} 148 | for i in range(agent_num): 149 | for n in range(N + 1): 150 | for k in range(dim): 151 | lb, ub = limits[k] 152 | name = '(A%sN%sD%s)' % (i, n, k) 153 | vars[name] = problem.addVar(vtype=GRB.CONTINUOUS, name= name, lb=lb, ub=ub) 154 | for n in range(N+1): vars['T%s'%(n)] = problem.addVar(vtype=GRB.CONTINUOUS, 155 | name = 'T%s'%(n), lb=0, ub=1e3) 156 | return vars 157 | 158 | def add_time_constraints(problem, vars, agent_num, velocities, N, dim, min_dur): 159 | problem.addConstr(vars['T0'] == 0) 160 | for idx in range(agent_num): 161 | v = velocities[idx] 162 | for n in range(N): 163 | t1 = v * vars['T%s'%(n)] 164 | t2 = v * vars['T%s'%(n + 1)] 165 | p1 = [vars['(A%sN%sD%s)' % (idx, n, k)] for k in range(dim)] 166 | p2 = [vars['(A%sN%sD%s)' % (idx, n+1, k)] for k in range(dim)] 167 | problem.addConstr(t2 - t1 >= min_dur) 168 | if dim == 2: 169 | for basis in ball_bases(N = 3, dim = dim): 170 | problem.addConstr(v * t2 - v * t1 >= LinExpr(basis, p1) - LinExpr(basis, p2)) 171 | problem.addConstr(v * t2 - v * t1 >= LinExpr(basis, p2) - LinExpr(basis, p1)) 172 | 173 | elif dim == 3: 174 | problem.addConstr(v * t2 - v * t1 >= p1[0]-p2[0]+p1[1]-p2[1]+p1[2]-p2[2]) 175 | problem.addConstr(v * t2 - v * t1 >= p1[0]-p2[0]+p1[1]-p2[1]+p2[2]-p1[2]) 176 | problem.addConstr(v * t2 - v * t1 >= p1[0]-p2[0]+p2[1]-p1[1]+p1[2]-p2[2]) 177 | problem.addConstr(v * t2 - v * t1 >= p1[0]-p2[0]+p2[1]-p1[1]+p2[2]-p1[2]) 178 | problem.addConstr(v * t2 - v * t1 >= p2[0]-p1[0]+p1[1]-p2[1]+p1[2]-p2[2]) 179 | problem.addConstr(v * t2 - v * t1 >= p2[0]-p1[0]+p1[1]-p2[1]+p2[2]-p1[2]) 180 | problem.addConstr(v * t2 - v * t1 >= p2[0]-p1[0]+p2[1]-p1[1]+p1[2]-p2[2]) 181 | problem.addConstr(v * t2 - v * t1 >= p2[0]-p1[0]+p2[1]-p1[1]+p2[2]-p1[2]) 182 | 183 | def set_objective(problem, vars, N): 184 | problem.setObjective(vars['T%s'%(N)], GRB.MINIMIZE) 185 | 186 | def get_gb_xref(models, Thetas, Goals, limits, O, MO, N_min, N, min_dur = 0): 187 | # print("Start Searching for Reference Trajectories") 188 | agent_num = len(models) 189 | velocities = [model.velocity for model in models] 190 | sizes = [model.size for model in models] 191 | bloat_funcs = [model.bloating for model in models] 192 | dim = len(Goals[0][0][0]) 193 | for n in range(N_min, N): 194 | # print("--------") 195 | # print("Horizon", n) 196 | problem = Model("xref") 197 | problem.setParam(GRB.Param.OutputFlag, 0) 198 | problem.setParam(GRB.Param.IntFeasTol, 1e-9) 199 | problem.setParam(GRB.Param.MIPGap, 0.5) 200 | # problem.setParam(GRB.Param.NonConvex, 2) # Temporary 201 | vars = get_variables(problem, agent_num, n, dim, limits) 202 | add_constraints(problem, vars, n, dim, 203 | Thetas, Goals, O, MO, 204 | velocities, sizes, bloat_funcs, min_dur) 205 | set_objective(problem, vars, n) 206 | # problem.write("test.lp") 207 | start = default_timer() 208 | problem.optimize() 209 | end = default_timer() 210 | if problem.status == GRB.OPTIMAL: 211 | # for v in problem.getVars(): print(v.varName, v.x) 212 | ma_nodes = [] 213 | front_time = -1 214 | for idx in range(agent_num): 215 | nodes = [] 216 | for i in range(n + 1): 217 | time = vars['T%s'%(i)].x 218 | x_new = [time] 219 | for k in range(dim): 220 | x_new = x_new + [vars['(A%sN%sD%s)' % (idx, i, k)].x] 221 | if x_new[0] > front_time: 222 | front_time = x_new[0] 223 | nodes = nodes + [x_new] 224 | ma_nodes = ma_nodes + [nodes] 225 | problem.dispose() 226 | print_solution(ma_nodes, end-start) 227 | return ma_nodes 228 | else: 229 | # print('No solution') 230 | problem.dispose() 231 | 232 | def ball_bases(N=3, dim=2): 233 | if dim == 2: 234 | p = pi / N 235 | bases = [[cos(n * p), sin(n * p)] for n in range(N)] 236 | # print(bases) 237 | return bases 238 | elif dim == 3: 239 | return [[1, 0, 0], [0, 1, 0], [0, 0, 1]] 240 | 241 | def print_solution(ma_nodes, time): 242 | agent_num = len(ma_nodes) 243 | for idx in range(agent_num): 244 | print("Printing Solution with Solving Time ", time, "s") 245 | # print("Agent %s Solution"%(idx)) 246 | nodes = ma_nodes[idx] 247 | seg_num = len(nodes) 248 | for i in range(seg_num): print("[%s] %s"%(i, nodes[i])) --------------------------------------------------------------------------------