├── .gitignore ├── README.md ├── context.py ├── libmcpf ├── cbss.py ├── cbss_lowlevel.py ├── cbss_mcpf.py ├── cbss_msmp.py ├── common.py ├── kbtsp.py ├── seq_mcpf.py └── seq_msmp.py ├── pytspbridge ├── .gitignore ├── tf_mtsp.py ├── tsp_solver │ └── readme.txt └── tsp_wrapper.py ├── run_example_cbss.py └── runtime_files ├── mcpf.par ├── mcpf.tour ├── mcpf.tsp ├── msmp.par ├── msmp.tour ├── msmp.tsp └── placeholder.txt /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Agent Combinatorial Path Finding 2 | 3 | This proejct is about Multi-Agent Combinatorial Path Finding (MCPF). The goal is to compute collision-free paths for multiple agents from their starts to destinations while visiting a large number of intermediate target locations along the paths. Intuitively, MCPF is a combination of mTSP (multiple traveling salesman problem) and MAPF (multi-agent path finding). MCPF also involves assignment constraints, which specify the subsets of agents that are eligible to visit each target/destination. This repo provides a python implementation of the Conflict-Based Steiner Search (CBSS) algorithm which solves MCPF. More technical details can be found in the [paper](http://www.roboticsproceedings.org/rss18/p058.pdf), [video](https://youtu.be/xwLoCiJ2vJY) or [contact](https://wonderren.github.io/). 4 | 5 |

6 | 7 |

8 | 9 | (Fig 1: A conflict-free joint path to a MCPF problem instance. Triangles are intermediate targets while stars are destinations. For the assignment constraints: each agent has a pre-assigned destination and one pre-assigned target (indicated by the color), while all remaining targets are fully anonymous (blue triangles).) 10 | 11 | The code is distributed for academic and non-commercial use. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | 20 | ## Requirements 21 | 22 | * We use Python (3.8.10) and Ubuntu 20.04. Lower or higher version may also work. 23 | * [LKH-2.0.9](http://webhotel4.ruc.dk/~keld/research/LKH/) is required as the underlying TSP solver. The executable of LKH should be placed at location: pytspbridge/tsp_solver/LKH-2.0.9/LKH. In other words, run `pytspbridge/tsp_solver/LKH-2.0.9/LKH` command in the terminal should be able to invoke LKH. 24 | 25 | ## Instructions: 26 | 27 | * Run `python3 run_example_cbss.py` to run the example, which shows how to use the code. 28 | 29 | ## Others 30 | 31 | ### About the Low-Level Search 32 | 33 | The original implementation has a flaw in its low-level search, which runs sequential A\* and ignores the influence across targets. As a result, it may not return an optimal individual path. The latest version (tag: v1.1 and thereafter) has fixes this issue on the low-level search. 34 | 35 | ### About Solution Optimality 36 | 37 | CBSS is theoretically guaranteed to find an optimal or bounded sub-optimal solution joint path, when the underlying TSP solver is guaranteed to solve TSPs to optimality. 38 | The current implementation of CBSS depends on LKH, which is a popular heuristic algorithm that is not guaranteed to find an optimal solution to TSP. Therefore, the resulting CBSS implementation is not guaranteed to return an optimal solution. 39 | However, LKH has been shown to return an optimal solution for numerous TSP instances in practice, this implementation of CBSS should also be able to provide optimal solutions for many MCPF instances. 40 | If the optimality of the solution must be guaranteed, one can consider leveraging the [Concorde](https://www.math.uwaterloo.ca/tsp/concorde.html) TSP solver (or other TSP solvers that can guarantee solution optimality) to replace LKH. 41 | 42 | ### Related Papers 43 | 44 | [1] Ren, Zhongqiang, Sivakumar Rathinam, and Howie Choset. "CBSS: A New Approach for Multiagent Combinatorial Path Finding." IEEE Transaction on Robotics (T-RO), 2023.\ 45 | [[Bibtex](https://wonderren.github.io/files/bibtex_ren23cbssTRO.txt)] 46 | [[Paper](https://wonderren.github.io/files/ren23_CBSS_TRO.pdf)] 47 | [[Talk](https://youtu.be/V17vQSZP5Zs?t=2853)] 48 | 49 | [2] Ren, Zhongqiang, Sivakumar Rathinam, and Howie Choset. "MS*: A new exact algorithm for multi-agent simultaneous multi-goal sequencing and path finding." In 2021 IEEE International Conference on Robotics and Automation (ICRA), pp. 11560-11565. IEEE, 2021.\ 50 | [[Bibtex](https://wonderren.github.io/files/bibtex_ren21ms.txt)] 51 | [[Paper](https://wonderren.github.io/files/ren21-MSstar.pdf)] 52 | [[Talk](https://youtu.be/cjwO4yycfpo)] 53 | -------------------------------------------------------------------------------- /context.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | Oeffentlich fuer: RSS22 5 | """ 6 | 7 | import os, sys 8 | sys.path.append(os.path.abspath('./libmcpf')) 9 | sys.path.append(os.path.abspath('./pytspbridge')) 10 | 11 | -------------------------------------------------------------------------------- /libmcpf/cbss.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | ABOUT: this file contains CBSS framework (abstract). 5 | Oeffentlich fuer: RSS22 6 | """ 7 | 8 | import kbtsp as kb 9 | import cbss_lowlevel as sipp_ml 10 | import common as cm 11 | import copy 12 | import time 13 | import numpy as np 14 | import sys 15 | 16 | DEBUG_CBSS = 0 17 | 18 | class CbsConstraint: 19 | """ 20 | borrowed from my previous code. 21 | """ 22 | def __init__(self, i, va,vb, ta,tb, j=-1, flag=-1): 23 | """ 24 | create a CCbsConstraint, if a single point, then va=vb 25 | """ 26 | self.i = i # i<0, iff not valid 27 | self.va = va 28 | self.vb = vb 29 | self.ta = ta 30 | self.tb = tb 31 | self.j = j 32 | self.flag = flag # flag = 1, vertex conflict, flag = 2 swap conflict 33 | 34 | def __str__(self): 35 | return "{i:"+str(self.i)+",va:"+str(self.va)+",vb:"+str(self.vb)+\ 36 | ",ta:"+str(self.ta)+",tb:"+str(self.tb)+",j:"+str(self.j)+",flag:"+str(self.flag)+"}" 37 | 38 | def FindPathCost(p): 39 | """ 40 | find the cost of a path 41 | """ 42 | last_idx = -2 43 | for idx in range(len(p[0])): # find last loc_id that reach goal 44 | i1 = len(p[0]) - 1 - idx # kth loc id 45 | i2 = i1-1 # (k-1)th loc id 46 | if i2 < 0: 47 | break 48 | if p[0][i2] == p[0][i1]: 49 | last_idx = i2 50 | else: 51 | break 52 | return p[1][last_idx] - p[1][0] 53 | 54 | class CbsSol: 55 | """ 56 | The solution in CBS high level node. A dict of paths for all robots. 57 | """ 58 | def __init__(self): 59 | self.paths = dict() 60 | return 61 | 62 | def __str__(self): 63 | return str(self.paths) 64 | 65 | def AddPath(self, i, lv, lt,lo=[]): 66 | """ 67 | lv is a list of loc id 68 | lt is a list of time 69 | """ 70 | nlv = lv 71 | nlt = lt 72 | nlo = lo 73 | # add a final infinity interval 74 | nlv.append(lv[-1]) 75 | nlt.append(np.inf) 76 | nlo.append(lo[-1]) 77 | self.paths[i] = [nlv, nlt,nlo] 78 | return 79 | 80 | def DelPath(self, i): 81 | self.paths.pop(i) 82 | return 83 | 84 | def GetPath(self, i): 85 | return self.paths[i] 86 | 87 | def CheckConflict(self, i,j): 88 | """ 89 | return the first constraint found along path i and j. 90 | If no conflict, return empty list. 91 | """ 92 | ix = 0 93 | while ix < len(self.paths[i][1])-1: 94 | for jx in range(len(self.paths[j][1])-1): 95 | jtb = self.paths[j][1][jx+1] 96 | jta = self.paths[j][1][jx] 97 | itb = self.paths[i][1][ix+1] 98 | ita = self.paths[i][1][ix] 99 | iva = self.paths[i][0][ix] 100 | ivb = self.paths[i][0][ix+1] 101 | jva = self.paths[j][0][jx] 102 | jvb = self.paths[j][0][jx+1] 103 | overlaps, t_lb, t_ub = cm.ItvOverlap(ita,itb,jta,jtb) 104 | if not overlaps: 105 | continue 106 | if ivb == jvb: # vertex conflict at ivb (=jvb) 107 | return [CbsConstraint(i, ivb, ivb, t_lb+1, t_lb+1, j, 1), CbsConstraint(j, jvb, jvb, t_lb+1, t_lb+1, i, 1)] # t_ub might be inf? 108 | # use min(itb,jtb) to avoid infinity 109 | if (ivb == jva) and (iva == jvb): # swap location 110 | return [CbsConstraint(i, iva, ivb, t_lb, t_lb+1, j, 2), CbsConstraint(j, jva, jvb, t_lb, t_lb+1, i, 2)] 111 | ix = ix + 1 112 | return [] 113 | 114 | def CheckConflict_ml(self, i,j): 115 | """ 116 | return the first constraint found along path i and j. 117 | If no conflict, return empty list. 118 | """ 119 | ix = 0 120 | while ix < len(self.paths[i][1])-1: 121 | for jx in range(len(self.paths[j][1])-1): 122 | jtb = self.paths[j][1][jx+1] 123 | jta = self.paths[j][1][jx] 124 | itb = self.paths[i][1][ix+1] 125 | ita = self.paths[i][1][ix] 126 | iva = self.paths[i][0][ix] 127 | ivb = self.paths[i][0][ix+1] 128 | jva = self.paths[j][0][jx] 129 | jvb = self.paths[j][0][jx+1] 130 | ioa = self.paths[i][2][ix] 131 | iob = self.paths[i][2][ix + 1] 132 | joa = self.paths[j][2][jx] 133 | job = self.paths[j][2][jx + 1] 134 | overlaps, t_lb, t_ub = cm.ItvOverlap(ita,itb,jta,jtb) 135 | if not overlaps: 136 | continue 137 | #occupylistoverlapstag = 0 138 | common_elements = [value for value in iob if value in job] 139 | if len(common_elements) != 0: # vertex conflict at ivb (=jvb) 140 | return [CbsConstraint(i, common_elements[0], common_elements[0], t_lb+1, t_lb+1, j, 1), CbsConstraint(j, common_elements[0], common_elements[0], t_lb+1, t_lb+1, i, 1)] # t_ub might be inf? 141 | # use min(itb,jtb) to avoid infinity 142 | if (ivb == jva) and (iva == jvb): # swap location 143 | return [CbsConstraint(i, iva, ivb, t_lb, t_lb+1, j, 2), CbsConstraint(j, jva, jvb, t_lb, t_lb+1, i, 2)] 144 | ix = ix + 1 145 | return [] 146 | 147 | def ComputeCost(self): 148 | """ 149 | """ 150 | sic = 0 151 | for k in self.paths: 152 | sic = sic + FindPathCost(self.paths[k]) 153 | return sic 154 | 155 | class CbssNode: 156 | """ 157 | CBSS ! (Steiner) 158 | High level search tree node 159 | """ 160 | def __init__(self, id0, sol=CbsSol(), cstr=CbsConstraint(-1,-1,-1,-1,-1,-1), c=0, parent=-1): 161 | """ 162 | id = id of this high level CT node 163 | sol = an object of type CCbsSol. 164 | cstr = a list of CCbsConstraint, either empty or of length 2. 165 | newly added constraint in this node, to get all constraints, 166 | need to backtrack from this node down to the root node. 167 | parent = id of the parent node of this node. 168 | """ 169 | self.id = id0 170 | self.sol = sol 171 | self.cstr = cstr 172 | self.cost = c 173 | self.parent = -1 # root node 174 | self.root_id = -1 175 | return 176 | 177 | def __str__(self): 178 | str1 = "{id:"+str(self.id)+",cost:"+str(self.cost)+",parent:"+str(self.parent) 179 | return str1+",cstr:"+str(self.cstr)+",sol:"+str(self.sol)+"}" 180 | 181 | def CheckConflict(self): 182 | """ 183 | check for conflicts along paths of all pairs of robots. 184 | record the first one conflict. 185 | Note that one conflict should be splited to 2 constraints. 186 | """ 187 | done_set = set() 188 | for k1 in self.sol.paths: 189 | for k2 in self.sol.paths: 190 | if k2 in done_set or k2 == k1: 191 | continue 192 | # check for collision 193 | res = self.sol.CheckConflict(k1,k2) 194 | if len(res) > 0: 195 | # self.cstr = res # update member 196 | return res 197 | # end for k2 198 | done_set.add(k1) 199 | # end for k1 200 | return [] # no conflict 201 | 202 | def CheckConflict_ml(self): 203 | """ 204 | check for conflicts along paths of all pairs of robots. 205 | record the first one conflict. 206 | Note that one conflict should be splited to 2 constraints. 207 | """ 208 | done_set = set() 209 | for k1 in self.sol.paths: 210 | for k2 in self.sol.paths: 211 | if k2 in done_set or k2 == k1: 212 | continue 213 | # check for collision 214 | res = self.sol.CheckConflict_ml(k1,k2) 215 | if len(res) > 0: 216 | # self.cstr = res # update member 217 | return res 218 | # end for k2 219 | done_set.add(k1) 220 | # end for k1 221 | return [] # no conflict 222 | 223 | def ComputeCost(self): 224 | """ 225 | compute sic cost, update member, also return cost value 226 | """ 227 | self.cost = self.sol.ComputeCost() 228 | return self.cost 229 | 230 | class CbssFramework: 231 | """ 232 | """ 233 | def __init__(self, mtsp_solver, grids, starts, goals, dests, ac_dict, configs): 234 | """ 235 | grids is 2d static grids. 236 | """ 237 | self.tstart = time.perf_counter() # must be re-initialized in Search() 238 | self.grids = grids 239 | (self.yd, self.xd) = self.grids.shape 240 | self.starts = starts 241 | self.goals = goals 242 | self.dests = dests 243 | self.total_num_nodes = len(starts) + len(dests) + len(goals) 244 | self.num_robots = len(starts) 245 | self.eps = configs["eps"] 246 | self.configs = configs 247 | self.nodes = dict() # high level nodes 248 | self.open_list = cm.PrioritySet() 249 | self.closed_set = set() 250 | self.num_closed_low_level_states = 0 251 | self.total_low_level_time = 0 252 | self.num_low_level_calls = 0 253 | self.node_id_gen = 1 254 | self.root_set = set() # a set of all root IDs. 255 | self.root_seq_dict = dict() # map a root ID to its joint sequence 256 | self.mtsp = mtsp_solver 257 | self.kbtsp = kb.KBestMTSP(self.mtsp) 258 | self.next_seq = None 259 | self.eps_cost = np.inf 260 | return 261 | 262 | def BacktrackCstrs(self, nid, ri = -1): 263 | """ 264 | given a node, trace back to the root, find all constraints relavant. 265 | """ 266 | node_cs = list() 267 | swap_cs = list() 268 | cid = nid 269 | if ri < 0: 270 | ri = self.nodes[nid].cstr.i 271 | # if ri < 0, then find constraints related to robot ri. 272 | while cid != -1: 273 | # print("cid = ",cid) 274 | if self.nodes[cid].cstr.i == ri: # not a valid constraint 275 | # init call of mocbs will not enter this. 276 | cstr = self.nodes[cid].cstr 277 | if self.nodes[cid].cstr.flag == 1: # vertex constraint 278 | node_cs.append( (cstr.vb, cstr.tb) ) 279 | elif self.nodes[cid].cstr.flag == 2: # swap constraint 280 | swap_cs.append( (cstr.va, cstr.vb, cstr.ta) ) 281 | node_cs.append( (cstr.va, cstr.tb) ) # since another robot is coming to v=va at t=tb 282 | cid = self.nodes[cid].parent 283 | return node_cs, swap_cs 284 | 285 | def _IfNewRoot(self, curr_node): 286 | """ 287 | """ 288 | cval = curr_node.cost 289 | if cval > self.eps_cost: 290 | if not self.next_seq: # next_seq not computed yet, compute next seq 291 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 292 | flag = self.kbtsp.ComputeNextBest(tlimit, self.total_num_nodes) 293 | if not flag: # no joint sequence any more. 294 | self.next_seq = None 295 | else: 296 | self.next_seq = self.kbtsp.GetKthBestSol() # will be used to check if new root needs to be generated. 297 | else: 298 | return False 299 | 300 | ### if reach here, must be the case: cval > (1+eps)*curr_root_cost. 301 | if DEBUG_CBSS: 302 | print("### CBSS _IfNewRoot 2nd phase, input cost = ", cval, " eps_cost = ", self.eps_cost, " next_cost = ", (1+self.eps)*self.next_seq.cost) 303 | if (self.next_seq is None): 304 | self.eps_cost = np.inf # no next root! 305 | return False 306 | else: 307 | if (cval > self.next_seq.cost): 308 | return True 309 | else: 310 | return False 311 | 312 | def _GenCbssNode(self, nid): 313 | return CbssNode(nid) 314 | 315 | def _UpdateEpsCost(self, c): 316 | self.eps_cost = (1+self.eps)*c # update eps cost. 317 | # print(" _UpdateEpsCost input ", c, " eps_cost = ", self.eps_cost) 318 | return 319 | 320 | def _GenNewRoot(self): 321 | """ 322 | called at the beginning of the search. 323 | generate first High level node. 324 | compute individual optimal path for each robot. 325 | """ 326 | 327 | ### Generate the first HL node, a root node ### 328 | nid = self.node_id_gen 329 | self.nodes[nid] = self._GenCbssNode(nid) 330 | self.node_id_gen = self.node_id_gen + 1 331 | self.root_set.add(nid) 332 | self.nodes[nid].root_id = nid 333 | 334 | ### Init sequencing related ### 335 | if not self.next_seq: 336 | if (nid == 1): # init 337 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 338 | flag = self.kbtsp.ComputeNextBest(tlimit, self.total_num_nodes) 339 | if not flag: 340 | print("[ERROR] CBSS: No feasible joint sequence or time out at init!") 341 | sys.exit("[ERROR]") 342 | self.root_seq_dict[nid] = self.kbtsp.GetKthBestSol() # assign seq data to root node. 343 | else: 344 | return False # no new root to be generated. 345 | else: # during search 346 | self.root_seq_dict[nid] = self.next_seq 347 | self.next_seq = None # next_seq has been used, make it empty. 348 | 349 | ### plan path based on goal sequence for all agents ### 350 | for ri in range(self.num_robots): # loop over agents, plan their paths 351 | lv, lt, stats = self.Lsearch(nid, ri) 352 | if len(lv) == 0: # fail to init, time out or sth. 353 | return False 354 | self.nodes[nid].sol.AddPath(ri,lv,lt) 355 | 356 | ### update cost and insert into OPEN ### 357 | c = self.nodes[nid].ComputeCost() # update node cost and return cost value 358 | self.open_list.add(c,nid) 359 | self._UpdateEpsCost(c) 360 | return True 361 | 362 | 363 | def _GenNewRoot_multi_ml(self): 364 | """ 365 | called at the beginning of the search. 366 | generate first High level node. 367 | compute individual optimal path for each robot. 368 | """ 369 | 370 | ### Generate the first HL node, a root node ### 371 | nid = self.node_id_gen 372 | self.nodes[nid] = self._GenCbssNode(nid) 373 | self.node_id_gen = self.node_id_gen + 1 374 | self.root_set.add(nid) 375 | self.nodes[nid].root_id = nid 376 | 377 | ### Init sequencing related ### 378 | if not self.next_seq: 379 | if (nid == 1): # init 380 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 381 | flag = self.kbtsp.ComputeNextBest(tlimit, self.total_num_nodes) 382 | if not flag: 383 | print("[ERROR] CBSS: No feasible joint sequence or time out at init!") 384 | sys.exit("[ERROR]") 385 | self.root_seq_dict[nid] = self.kbtsp.GetKthBestSol() # assign seq data to root node. 386 | else: 387 | return False # no new root to be generated. 388 | else: # during search 389 | self.root_seq_dict[nid] = self.next_seq 390 | self.next_seq = None # next_seq has been used, make it empty. 391 | 392 | ### plan path based on goal sequence for all agents ### 393 | for ri in range(self.num_robots): # loop over agents, plan their paths 394 | lv, lt,lo, stats = self.Lsearch_ml(nid, ri) 395 | if len(lv) == 0: # fail to init, time out or sth. 396 | return False 397 | self.nodes[nid].sol.AddPath(ri,lv,lt,lo) 398 | 399 | ### update cost and insert into OPEN ### 400 | c = self.nodes[nid].ComputeCost() # update node cost and return cost value 401 | self.open_list.add(c,nid) 402 | self._UpdateEpsCost(c) 403 | return True 404 | 405 | def Lsearch_ml(self, nid, ri): 406 | """ 407 | input a high level node, ri is optional. 408 | """ 409 | if DEBUG_CBSS: 410 | print("Lsearch, nid:",nid) 411 | nd = self.nodes[nid] 412 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 413 | 414 | # plan from start to assigned goals and to dest as specified in goal sequence 415 | gseq = self.root_seq_dict[self.nodes[nid].root_id].sol[ri] 416 | ss = gseq[0] 417 | kth = 1 418 | t0 = 0 419 | all_lv = [] # all vertex agent ri occupyed 420 | all_lv.append(self.starts[ri]) 421 | all_lt = [] 422 | all_lt.append(0) 423 | all_lo = [] 424 | all_lo.append([self.starts[ri]]) 425 | success = True 426 | ignore_goal_cstr = False # ask robot can stay destination forever 427 | nd = self.nodes[nid] 428 | if ri < 0: # to support init search. 429 | ri = nd.cstr.i 430 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 431 | ncs, ecs = self.BacktrackCstrs(nid, ri) 432 | 433 | res_path, sipp_stats = sipp_ml.RunSipp_ml(self.grids, gseq,t0, ignore_goal_cstr, 3.0, 0.0, tlimit, ncs, ecs) # note the t0 here! 434 | 435 | if DEBUG_CBSS: 436 | if len(res_path) != 0: 437 | invalidtag = 0 438 | for iv in range(len(res_path[2])): 439 | for incs in ncs: 440 | if incs[0] in res_path[2][iv] and incs[1] == res_path[1][iv]: 441 | print(" no satisfy constraints",incs) 442 | invalidtag = 1 443 | if invalidtag==1: 444 | break 445 | if invalidtag==0: 446 | print("satisfy constraints") 447 | else: 448 | print(" no satisfy constraints") 449 | return [], [], False 450 | 451 | # print("res_path = ", res_path, " sipp_stats = ", sipp_stats) 452 | 453 | if len(res_path) == 0: # failed 454 | success = False 455 | else: # good 456 | self.UpdateStats(sipp_stats) 457 | #all_lv, all_lt = self.ConcatePath(all_lv, all_lt, res_path[0], res_path[1]) 458 | all_lv, all_lt, all_lo = self.ConcatePath_ml(all_lv, all_lt, all_lo, res_path[0], res_path[1],res_path[2]) 459 | 460 | if not success: 461 | return [], [], [], success 462 | else: 463 | return all_lv, all_lt, all_lo, success 464 | 465 | 466 | def Lsearch(self, nid, ri): 467 | """ 468 | input a high level node, ri is optional. 469 | """ 470 | if DEBUG_CBSS: 471 | print("Lsearch, nid:",nid) 472 | nd = self.nodes[nid] 473 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 474 | 475 | # plan from start to assigned goals and to dest as specified in goal sequence 476 | gseq = self.root_seq_dict[self.nodes[nid].root_id].sol[ri] 477 | ss = gseq[0] 478 | kth = 1 479 | t0 = 0 480 | all_lv = [] # all vertex agent ri occupyed 481 | all_lv.append(self.starts[ri]) 482 | all_lt = [] 483 | all_lt.append(0) 484 | success = True 485 | for kth in range(1, len(gseq)): 486 | # TODO, this can be optimized, 487 | # no need to plan path between every pair of waypoints each time! Impl detail. 488 | gg = gseq[kth] 489 | ignore_goal_cstr = True 490 | if kth == len(gseq)-1: # last goal 491 | ignore_goal_cstr = False 492 | lv, lt, sipp_stats = self.LsearchP2P(nid, ri, ss, gg, t0, ignore_goal_cstr) 493 | if DEBUG_CBSS: 494 | print("---LsearchP2P--- for agent ", ri, ", ignore_goal_cstr = ", ignore_goal_cstr, ", lv = ", lv, ", lt = ", lt) 495 | if len(lv) == 0: # failed 496 | success = False 497 | break 498 | else: # good 499 | self.UpdateStats(sipp_stats) 500 | all_lv, all_lt = self.ConcatePath(all_lv, all_lt, lv, lt) 501 | ss = gg # update start for the next call 502 | t0 = lt[-1] 503 | # end for kth 504 | if not success: 505 | return [], [], success 506 | else: 507 | return all_lv, all_lt, success 508 | 509 | def LsearchP2P(self, nid, ri, ss, gg, t0, ignore_goal_cstr): 510 | """ 511 | Do low level search for agent-i from vertex ss with starting time step t0 512 | to vertex gg subject to constraints in HL node nid. 513 | """ 514 | nd = self.nodes[nid] 515 | if ri < 0: # to support init search. 516 | ri = nd.cstr.i 517 | tlimit = self.time_limit - (time.perf_counter() - self.tstart) 518 | ncs, ecs = self.BacktrackCstrs(nid, ri) 519 | # plan from start to assigned goals and to dest as specified in goal sequence 520 | ssy = int(np.floor(ss/self.xd)) # start y 521 | ssx = int(ss%self.xd) # start x 522 | ggy = int(np.floor(gg/self.xd)) # goal y 523 | ggx = int(gg%self.xd) # goal x 524 | res_path, sipp_stats = sipp.RunSipp(self.grids, ssx, ssy, \ 525 | ggx, ggy, t0, ignore_goal_cstr, 1.0, 0.0, tlimit, ncs, ecs) # note the t0 here! 526 | if len(res_path)==0: 527 | return [],[],sipp_stats 528 | else: 529 | return res_path[0], res_path[1], sipp_stats 530 | 531 | def ConcatePath(self, all_lv, all_lt, lv, lt): 532 | """ 533 | remove the first node in lv,lt and then concate with all_xx. 534 | """ 535 | if (len(all_lt) > 0) and (lt[0] != all_lt[-1]): 536 | print("[ERROR] ConcatePath lv = ", lv, " lt = ", lt, " all_lv = ", all_lv, " all_lt = ", all_lt) 537 | sys.exit("[ERROR] ConcatePath, time step mismatch !") 538 | return all_lv + lv[1:], all_lt + lt[1:] 539 | 540 | def ConcatePath_ml(self, all_lv, all_lt,all_lo, lv, lt,lo): 541 | """ 542 | remove the first node in lv,lt and then concate with all_xx. 543 | """ 544 | if (len(all_lt) > 0) and (lt[0] != all_lt[-1]): 545 | print("[ERROR] ConcatePath lv = ", lv, " lt = ", lt, " all_lv = ", all_lv, " all_lt = ", all_lt) 546 | sys.exit("[ERROR] ConcatePath, time step mismatch !") 547 | return all_lv + lv[1:], all_lt + lt[1:], all_lo + lo[1:] 548 | 549 | def FirstConflict(self, nd): 550 | return nd.CheckConflict() 551 | 552 | def FirstConflict_ml(self, nd): 553 | return nd.CheckConflict_ml() 554 | 555 | def UpdateStats(self, stats): 556 | """ 557 | """ 558 | if DEBUG_CBSS: 559 | print("UpdateStats, ", stats) 560 | self.num_closed_low_level_states = self.num_closed_low_level_states + stats[0] 561 | self.total_low_level_time = self.total_low_level_time + stats[2] 562 | return 563 | 564 | def ReconstructPath(self, nid): 565 | """ 566 | """ 567 | path_set = dict() 568 | #print("num_robots",self.num_robots) 569 | for i in range(self.num_robots): 570 | lx = list() 571 | ly = list() 572 | lv = self.nodes[nid].sol.GetPath(i)[0] 573 | for v in lv: 574 | y = int(np.floor(v / self.xd)) 575 | x = int(v % self.xd) 576 | ly.append(y) 577 | lx.append(x) 578 | lt = self.nodes[nid].sol.GetPath(i)[1] 579 | lo = self.nodes[nid].sol.GetPath(i)[2] 580 | 581 | 582 | path_set[i] = [lx,ly,lt] 583 | #print(path_set) 584 | return path_set 585 | 586 | def _HandleRootGen(self, curr_node): 587 | """ 588 | generate new root if needed 589 | """ 590 | # print(" popped node ID = ", curr_node.id) 591 | if self._IfNewRoot(curr_node): 592 | if DEBUG_CBSS: 593 | print(" ### CBSS _GenNewRoot...") 594 | self._GenNewRoot() 595 | self.open_list.add(curr_node.cost, curr_node.id) # re-insert into OPEN for future expansion. 596 | popped = self.open_list.pop() # pop_node = (f-value, high-level-node-id) 597 | curr_node = self.nodes[popped[1]] 598 | else: 599 | # print(" self._IfNewRoot returns false...") 600 | place_holder = 1 601 | # end of if/while _IfNewRoot 602 | # print("### CBSS, expand high-level node ID = ", curr_node.id) 603 | return curr_node 604 | 605 | def _HandleRootGen_ml(self, curr_node): 606 | """ 607 | generate new root if needed 608 | """ 609 | # print(" popped node ID = ", curr_node.id) 610 | if self._IfNewRoot(curr_node): 611 | if DEBUG_CBSS: 612 | print(" ### CBSS _GenNewRoot...") 613 | self._GenNewRoot_multi_ml() 614 | self.open_list.add(curr_node.cost, curr_node.id) # re-insert into OPEN for future expansion. 615 | popped = self.open_list.pop() # pop_node = (f-value, high-level-node-id) 616 | curr_node = self.nodes[popped[1]] 617 | else: 618 | # print(" self._IfNewRoot returns false...") 619 | place_holder = 1 620 | # end of if/while _IfNewRoot 621 | # print("### CBSS, expand high-level node ID = ", curr_node.id) 622 | return curr_node 623 | 624 | 625 | def Search(self): 626 | """ 627 | = high level search 628 | """ 629 | print("CBSS search begin!") 630 | self.time_limit = self.configs["time_limit"] 631 | self.tstart = time.perf_counter() 632 | 633 | good = self._GenNewRoot() 634 | if not good: 635 | output_res = [ int(0), float(-1), int(0), int(0), \ 636 | int(self.num_closed_low_level_states), 0, float(time.perf_counter()-self.tstart), \ 637 | int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set)) ] 638 | return dict(), output_res 639 | 640 | tnow = time.perf_counter() 641 | # print("After init, tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit) 642 | if (tnow - self.tstart > self.time_limit): 643 | print(" FAIL! timeout! ") 644 | search_success = False 645 | output_res = [ int(0), float(-1), int(0), int(0), \ 646 | int(self.num_closed_low_level_states), 0, float(time.perf_counter()-self.tstart), \ 647 | int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set)) ] 648 | return dict(), output_res 649 | 650 | search_success = False 651 | best_g_value = -1 652 | reached_goal_id = -1 653 | 654 | while True: 655 | tnow = time.perf_counter() 656 | rd = len(self.closed_set) 657 | # print("tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit) 658 | if (tnow - self.tstart > self.time_limit): 659 | print(" FAIL! timeout! ") 660 | search_success = False 661 | break 662 | if (self.open_list.size()) == 0: 663 | print(" FAIL! openlist is empty! ") 664 | search_success = False 665 | break 666 | 667 | popped = self.open_list.pop() # pop_node = (f-value, high-level-node-id) 668 | curr_node = self.nodes[popped[1]] 669 | curr_node = self._HandleRootGen(curr_node) # generate new root if needed 670 | tnow = time.perf_counter() 671 | # print("tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit) 672 | 673 | if (tnow - self.tstart > self.time_limit): 674 | print(" FAIL! timeout! ") 675 | search_success = False 676 | break 677 | 678 | self.closed_set.add(popped[1]) # only used to count numbers 679 | 680 | if DEBUG_CBSS: 681 | print("### CBSS popped node: ", curr_node) 682 | 683 | cstrs = self.FirstConflict(curr_node) 684 | 685 | if len(cstrs) == 0: # no conflict, terminates 686 | print("! CBSS succeed !") 687 | search_success = True 688 | best_g_value = curr_node.cost 689 | reached_goal_id = curr_node.id 690 | break 691 | 692 | max_child_cost = 0 693 | for cstr in cstrs: 694 | if DEBUG_CBSS: 695 | print("CBSS loop over cstr:",cstr) 696 | 697 | ### generate constraint and new HL node ### 698 | new_id = self.node_id_gen 699 | self.node_id_gen = self.node_id_gen + 1 700 | self.nodes[new_id] = copy.deepcopy(curr_node) 701 | self.nodes[new_id].id = new_id 702 | self.nodes[new_id].parent = curr_node.id 703 | self.nodes[new_id].cstr = cstr 704 | self.nodes[new_id].root_id = self.nodes[curr_node.id].root_id # copy root id. 705 | ri = cstr.i 706 | 707 | ### replan paths for the agent, subject to new constraint ### 708 | lv,lt,flag = self.Lsearch(new_id, ri) 709 | self.num_low_level_calls = self.num_low_level_calls + 1 710 | if len(lv)==0: 711 | # this branch fails, robot ri cannot find a consistent path. 712 | continue 713 | self.nodes[new_id].sol.DelPath(ri) 714 | self.nodes[new_id].sol.AddPath(ri,lv,lt) 715 | nn_cost = self.nodes[new_id].ComputeCost() 716 | if DEBUG_CBSS: 717 | print("### CBSS add node ", self.nodes[new_id], " into OPEN,,, nn_cost = ", nn_cost) 718 | self.open_list.add(nn_cost, new_id) 719 | max_child_cost = np.max([nn_cost, max_child_cost]) 720 | # end of for cstr 721 | # print(">>>>>>>>>>>>>>>>>>>> end of an iteration") 722 | # end of while 723 | 724 | output_res = [ int(len(self.closed_set)), float(best_g_value), int(0), int(self.open_list.size()), \ 725 | int(self.num_closed_low_level_states), int(search_success), float(time.perf_counter()-self.tstart),\ 726 | int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set)) ] 727 | 728 | if search_success: 729 | return self.ReconstructPath(reached_goal_id), output_res 730 | else: 731 | return dict(), output_res 732 | 733 | 734 | def Search_ml(self): 735 | """ 736 | = high level search 737 | """ 738 | print("CBSS search begin!") 739 | self.time_limit = self.configs["time_limit"] 740 | self.tstart = time.perf_counter() 741 | 742 | good = self._GenNewRoot_multi_ml() 743 | if not good: 744 | output_res = [int(0), float(-1), int(0), int(0), \ 745 | int(self.num_closed_low_level_states), 0, float(time.perf_counter() - self.tstart), \ 746 | int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set))] 747 | return dict(), output_res 748 | 749 | tnow = time.perf_counter() 750 | # print("After init, tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit) 751 | if (tnow - self.tstart > self.time_limit): 752 | print(" FAIL! timeout! ") 753 | search_success = False 754 | output_res = [int(0), float(-1), int(0), int(0), \ 755 | int(self.num_closed_low_level_states), 0, float(time.perf_counter() - self.tstart), \ 756 | int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set))] 757 | return dict(), output_res 758 | 759 | search_success = False 760 | best_g_value = -1 761 | reached_goal_id = -1 762 | 763 | while True: 764 | tnow = time.perf_counter() 765 | rd = len(self.closed_set) 766 | # print("tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit) 767 | if (tnow - self.tstart > self.time_limit): 768 | print(" FAIL! timeout! ") 769 | search_success = False 770 | break 771 | if (self.open_list.size()) == 0: 772 | print(" FAIL! openlist is empty! ") 773 | search_success = False 774 | break 775 | 776 | popped = self.open_list.pop() # pop_node = (f-value, high-level-node-id) 777 | curr_node = self.nodes[popped[1]] 778 | curr_node = self._HandleRootGen_ml(curr_node) # generate new root if needed 779 | tnow = time.perf_counter() 780 | # print("tnow - self.tstart = ", tnow - self.tstart, " tlimit = ", self.time_limit) 781 | 782 | if (tnow - self.tstart > self.time_limit): 783 | print(" FAIL! timeout! ") 784 | search_success = False 785 | break 786 | 787 | self.closed_set.add(popped[1]) # only used to count numbers 788 | 789 | if DEBUG_CBSS: 790 | print("### CBSS popped node: ", curr_node) 791 | 792 | cstrs = self.FirstConflict_ml(curr_node) 793 | #print(len(cstrs)) 794 | if len(cstrs) == 0: # no conflict, terminates 795 | print("! CBSS succeed !") 796 | search_success = True 797 | best_g_value = curr_node.cost 798 | reached_goal_id = curr_node.id 799 | break 800 | 801 | max_child_cost = 0 802 | for cstr in cstrs: 803 | if DEBUG_CBSS: 804 | print("CBSS loop over cstr:", cstr) 805 | 806 | ### generate constraint and new HL node ### 807 | new_id = self.node_id_gen 808 | self.node_id_gen = self.node_id_gen + 1 809 | self.nodes[new_id] = copy.deepcopy(curr_node) 810 | self.nodes[new_id].id = new_id 811 | self.nodes[new_id].parent = curr_node.id 812 | self.nodes[new_id].cstr = cstr 813 | self.nodes[new_id].root_id = self.nodes[curr_node.id].root_id # copy root id. 814 | ri = cstr.i 815 | 816 | ### replan paths for the agent, subject to new constraint ### 817 | lv, lt, lo,flag = self.Lsearch_ml(new_id, ri) 818 | self.num_low_level_calls = self.num_low_level_calls + 1 819 | if len(lv) == 0: 820 | # this branch fails, robot ri cannot find a consistent path. 821 | continue 822 | self.nodes[new_id].sol.DelPath(ri) 823 | self.nodes[new_id].sol.AddPath(ri, lv, lt,lo) 824 | nn_cost = self.nodes[new_id].ComputeCost() 825 | if DEBUG_CBSS: 826 | print("### CBSS add node ", self.nodes[new_id], " into OPEN,,, nn_cost = ", nn_cost) 827 | self.open_list.add(nn_cost, new_id) 828 | max_child_cost = np.max([nn_cost, max_child_cost]) 829 | # end of for cstr 830 | # print(">>>>>>>>>>>>>>>>>>>> end of an iteration") 831 | # end of while 832 | 833 | output_res = [int(len(self.closed_set)), float(best_g_value), int(0), int(self.open_list.size()), \ 834 | int(self.num_closed_low_level_states), int(search_success), float(time.perf_counter() - self.tstart), \ 835 | int(self.kbtsp.GetTotalCalls()), float(self.kbtsp.GetTotalTime()), int(len(self.root_set))] 836 | 837 | if search_success: 838 | return self.ReconstructPath(reached_goal_id), output_res 839 | else: 840 | return dict(), output_res 841 | -------------------------------------------------------------------------------- /libmcpf/cbss_lowlevel.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | ABOUT: this file leverages my python MO-SIPP. 5 | Oeffentlich fuer: RSS22 6 | """ 7 | 8 | import numpy as np 9 | import heapq as hpq 10 | import common as cm 11 | import itertools as itt 12 | import time 13 | import copy 14 | import sys 15 | import common as cm 16 | import os 17 | import matplotlib.pyplot as plt 18 | 19 | # this debug flag is due to some legacy, just leave it there. 20 | DEBUG_MOSTASTAR = False 21 | 22 | tasktime = 0 23 | 24 | class SippState: 25 | """ 26 | Search state for SIPP, changed MO-SIPP to single-objective. 27 | """ 28 | def __init__(self, sid, loc, gval, t, tb, occupylist=[], carttag=0): 29 | """ 30 | """ 31 | self.id = sid 32 | self.loc = loc # location id 33 | self.g = gval # g-value 34 | self.t = t # arrival time step 35 | self.tb = tb # ending time step, mark the ending time of an interval 36 | self.occupylist = occupylist #occupy location id 37 | self.carttag = carttag # equal to the number of the executed tasks 38 | 39 | def __str__(self): 40 | return "{id:"+str(self.id)+",loc:"+str(self.loc)+",g("+str(self.g)+"),itv(t,tb:"+str(self.t)+","+str(self.tb)+"),carttag("+str(self.carttag)+"),lenofoccupylist("+str(len(self.occupylist))+")}" 41 | 42 | def Equal(self, other): 43 | return cm.Equal(self.cost_vec, other.cost_vec) 44 | 45 | class SippGridSpace: 46 | """ 47 | Assume four connected grid graph. 48 | """ 49 | def __init__(self, grid): 50 | """ 51 | """ 52 | self.grid = grid 53 | 54 | self.node_cstrs = dict() 55 | # map a node to a set of forbidden timesteps. 56 | 57 | self.edge_cstrs = dict() 58 | # map a node to a dict of (node, set of forbidden timesteps) key-value pairs, where timesteps are saved in a set 59 | # NOTE: this differs a bit from previous implementation, where key-value pairs are (forbidden time, set of nodes) 60 | 61 | def LookUpItv(self, nid, t): 62 | """ 63 | given a node ID nid and a time step t, find the safe interval enclosing t. 64 | """ 65 | lb = 0 66 | ub = np.inf 67 | if nid not in self.node_cstrs: 68 | return (lb, ub), True 69 | 70 | for tt in self.node_cstrs[nid]: 71 | if tt == t: 72 | print("[ERROR] LookUpItv, try to loop up a node-timestep (", nid, ",", t, ") that is unsafe !") 73 | return [], False 74 | # print(" tt = ", tt) 75 | if tt > t: 76 | ub = int( np.min([tt-1, ub]) ) 77 | if tt < t: 78 | lb = int( np.max([tt+1, lb]) ) 79 | return (lb, ub), True 80 | 81 | def GetSuccSafeItv(self, nid, nnid, tlb, tub): 82 | """ 83 | return a list of safe intervals, tlist, locate at nnid that are reachable/successors from nid with safe interval [tlb, tub]. 84 | """ 85 | out = list() 86 | unsafe_tpoint = list() 87 | # print(" nid ", nid, " nnid ", nnid, " cstrs : ", self.node_cstrs) 88 | if nnid in self.node_cstrs: 89 | for tt in self.node_cstrs[nnid]: 90 | unsafe_tpoint.append(tt) 91 | if nid in self.edge_cstrs: 92 | if nnid in self.edge_cstrs[nid]: 93 | for tt in self.edge_cstrs[nid][nnid]: 94 | unsafe_tpoint.append(tt+1) ## note that this is tt+1, not tt. 95 | 96 | unsafe_tpoint.sort() ## sort from small to large 97 | # print("unsafe_tpoint at ", nnid, " is ", unsafe_tpoint, " tlb,tub = ", tlb, tub) 98 | 99 | t0 = tlb + 1 ## the earliest possible arrival time at node nnid. 100 | if t0 > tub: 101 | return out # empty list 102 | 103 | if len(unsafe_tpoint) > 0: 104 | for idx in range(len(unsafe_tpoint)): 105 | tk = unsafe_tpoint[idx] 106 | if tk <= t0: 107 | if tk == t0: 108 | t0 = tk + 1 ## earliest possible arrival time should be shift forwards. 109 | if t0 > tub: # everytime when t0 changes, need to check 110 | break 111 | continue 112 | ## now tk >= t0 + 1 113 | if tk <= tub + 1: 114 | out.append( (t0,tk-1) ) 115 | else: 116 | ## tk >= tub + 2 117 | # if t0 <= tub + 1: 118 | out.append( (t0, tub+1) ) 119 | t0 = tk + 1 120 | if t0 > tub: # everytime when t0 changes, need to check 121 | break 122 | ## check if inf interval is needed 123 | # if tub == np.inf: 124 | # out.append( (t0, np.inf) ) 125 | if t0 <= tub: 126 | out.append( (t0, np.inf) ) 127 | else: 128 | out.append( (t0, np.inf) ) 129 | return out 130 | 131 | def GetSuccSafeItv_ml(self, nid, nnid, tlb, tub): 132 | """ 133 | return a list of safe intervals, tlist, locate at nnid that are reachable/successors from nid with safe interval [tlb, tub]. 134 | 135 | update0624:t0 is arrival time at node nnid, so closed interval [tlb,t0-1] is waiting time at node nid, so t0-1 must be less than or equal to tub 136 | """ 137 | out = list() 138 | unsafe_tpoint = list() 139 | # print(" nid ", nid, " nnid ", nnid, " cstrs : ", self.node_cstrs) 140 | if nnid in self.node_cstrs: 141 | for tt in self.node_cstrs[nnid]: 142 | unsafe_tpoint.append(tt) 143 | if nid in self.edge_cstrs: 144 | if nnid in self.edge_cstrs[nid]: 145 | for tt in self.edge_cstrs[nid][nnid]: 146 | unsafe_tpoint.append(tt + 1) ## note that this is tt+1, not tt. 147 | 148 | unsafe_tpoint.sort() ## sort from small to large 149 | # print("unsafe_tpoint at ", nnid, " is ", unsafe_tpoint, " tlb,tub = ", tlb, tub) 150 | 151 | t0 = tlb + 1 ## the earliest possible arrival time at node nnid. 152 | if t0-1 > tub: 153 | return out # empty list 154 | 155 | if len(unsafe_tpoint) > 0: 156 | for idx in range(len(unsafe_tpoint)): 157 | tk = unsafe_tpoint[idx] 158 | if tk <= t0: 159 | if tk == t0: 160 | t0 = tk + 1 ## earliest possible arrival time should be shift forwards. 161 | if t0-1 > tub: # everytime when t0 changes, need to check 162 | break 163 | continue 164 | ## now tk >= t0 + 1 165 | if tk <= tub + 1: 166 | out.append((t0, tk - 1)) 167 | else: 168 | ## tk >= tub + 2 169 | # if t0 <= tub + 1: 170 | out.append((t0, tub + 1)) 171 | t0 = tk + 1 172 | if t0-1 > tub: # everytime when t0 changes, need to check 173 | break 174 | ## check if inf interval is needed 175 | # if tub == np.inf: 176 | # out.append( (t0, np.inf) ) 177 | if t0-1 <= tub: 178 | out.append((t0, np.inf)) 179 | else: 180 | out.append((t0, np.inf)) 181 | return out 182 | 183 | def GetSuccs(self, u, v, tlb, tub): 184 | """ 185 | move from u to v, with time in closed interval [tlb, tub]. 186 | return all reachable safe intervals with earliest arrival time at node v. 187 | The returned arg is a list of SippState. 188 | NOTE that the ID and cost_vec in those returned states are invalid! 189 | """ 190 | out = list() 191 | tlist = self.GetSuccSafeItv(u,v,tlb,tub) 192 | # print("tlist = ", tlist) 193 | for itv in tlist: 194 | itv0, success = self.LookUpItv(v, itv[0]) 195 | if not success: 196 | sys.exit("[ERROR] GetSuccs, look up itv fails !") 197 | continue 198 | # sid, loc, gval, t, tb, occupylist=[], carttag=0 199 | out.append( SippState(-1, v, -1.0, int(itv[0]), itv[1] ) ) # Note the order, t, tb, ta 200 | return out 201 | 202 | def GetSuccs_ml(self, u, v, tlb, tub,newoccupylist,oldcarttag): 203 | """ 204 | move from u to v, with time in closed interval [tlb, tub]. 205 | return all reachable safe intervals with earliest arrival time at node v. 206 | The returned arg is a list of SippState. 207 | NOTE that the ID and cost_vec in those returned states are invalid! 208 | """ 209 | out = list() 210 | tlist = self.GetSuccSafeItv_ml(u,v,tlb,tub) 211 | # print("tlist = ", tlist) 212 | for itv in tlist: 213 | itv0, success = self.LookUpItv(v, itv[0]) 214 | if not success: 215 | sys.exit("[ERROR] GetSuccs, look up itv fails !") 216 | continue 217 | # sid, loc, gval, t, tb, occupylist=[], carttag=0 218 | out.append( SippState(-1, v, -1.0, int(itv[0]), itv[1] ,newoccupylist,oldcarttag) ) #keep the same carttag 219 | return out 220 | 221 | 222 | 223 | def AddNodeCstr(self, nid, t): 224 | """ 225 | """ 226 | if nid not in self.node_cstrs: 227 | self.node_cstrs[nid] = set() 228 | self.node_cstrs[nid].add(t) 229 | # print(" SIPP Space add node cstr , ", nid, " t ", t) 230 | return 231 | 232 | def AddEdgeCstr(self, u, v, t): 233 | """ 234 | """ 235 | if u not in self.edge_cstrs: 236 | self.edge_cstrs[u] = dict() 237 | if v not in self.edge_cstrs[u]: 238 | self.edge_cstrs[u][v] = set() 239 | self.edge_cstrs[u][v].add(t) 240 | return 241 | 242 | class SIPP: 243 | """ 244 | SIPP, modified from MO-SIPP (and remove the inheritance from MOSTA*). years ago... 245 | no wait action in the action_set_x,_y. 246 | """ 247 | def __init__(self, grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w=1.0, eps=0.0, action_set_x = [-1,0,1,0], action_set_y = [0,-1,0,1], gseq =[]): 248 | """ 249 | """ 250 | self.grids = grids 251 | (self.nyt, self.nxt) = self.grids.shape 252 | self.state_gen_id = 3 # 1 is start, 2 is goal 253 | self.t0 = t0 # starting time step. 254 | self.ignore_goal_cstr = ignore_goal_cstr 255 | # start state 256 | self.sx = sx 257 | self.sy = sy 258 | # goal state 259 | self.gx = gx 260 | self.gy = gy 261 | # search params and data structures 262 | self.weight = w 263 | self.eps = eps 264 | self.action_set_x = action_set_x 265 | self.action_set_y = action_set_y 266 | self.all_visited_s = dict() # map state id to state (sid, nid, cost vec) 267 | self.frontier_map = dict() # map nid to a set of sid 268 | self.open_list = cm.PrioritySet() 269 | self.f_value = dict() # map a state id to its f-value vector (np.array) 270 | self.close_set = set() 271 | self.backtrack_dict = dict() # track parents 272 | self.reached_goal_state_id = 0 # record the ID 273 | self.time_limit = 30 # seconds, default 274 | self.node_constr = dict() 275 | # a dict that maps a vertex id to a list of forbidden timestamps 276 | self.swap_constr = dict() 277 | # a dict that maps a vertex id to a dict with (forbidden time, set(forbidden next-vertex)) as key-value pair. 278 | self.sipp_space = SippGridSpace(grids) 279 | self.gseq = gseq#for multi astar 280 | return 281 | 282 | def GetHeuristic(self, s): 283 | """ 284 | Manhattan distance 285 | """ 286 | cy = int(np.floor(s.loc/self.nxt)) # curr y 287 | cx = int(s.loc%self.nxt) # curr x 288 | return abs(cy-self.gy) + abs(cx - self.gx) 289 | 290 | def GetHeuristic_ml(self, s): 291 | """ 292 | sum of targets' Manhattan distance 293 | """ 294 | cy = int(np.floor(s.loc/self.nxt)) # curr y 295 | cx = int(s.loc%self.nxt) # curr x 296 | hdistance = 0 297 | #tasktime = 1 # should be updated 298 | for i in self.gseq[1+s.carttag:]: 299 | iy = int(np.floor(i/self.nxt)) 300 | ix = int(i%self.nxt) 301 | hdistance = hdistance + abs(cy-iy) + abs(cx - ix) + tasktime 302 | cy = iy 303 | cx = ix 304 | hdistance = hdistance - tasktime 305 | return hdistance 306 | 307 | def GetCost(self, loc, nloc, dt=1): 308 | """ 309 | minimize arrival time. cost is time. 310 | """ 311 | return dt 312 | 313 | def GetStateIdentifier(self, s): 314 | """ 315 | override the method in parent class. 316 | """ 317 | return (s.loc, s.tb) # this is a new attempt @2021-03-31 318 | 319 | def GetStateIdentifier_ml(self, s): 320 | """ 321 | override the method in parent class. 322 | """ 323 | return (s.loc, s.tb, s.carttag) #add carttag 324 | def GetNeighbors(self, s, tstart): 325 | """ 326 | tstart is useless... can be deleted. 327 | input a state s, compute its neighboring states. 328 | output a list of states. 329 | """ 330 | s_ngh = list() 331 | cy = int(np.floor(s.loc/self.nxt)) # current x 332 | cx = int(s.loc%self.nxt) # current y 333 | 334 | # loop over all four possible actions 335 | for action_idx in range(len(self.action_set_x)): 336 | nx = cx+self.action_set_x[action_idx] # next x 337 | ny = cy+self.action_set_y[action_idx] # next y 338 | nnid = ny*self.nxt+nx 339 | if (nx >= self.nxt) or (nx < 0) or (ny >= self.nyt) or (ny < 0): # out of border of grid 340 | continue 341 | if (self.grids[ny,nx] > 0): # static obstacle 342 | continue 343 | ### this is where SIPP takes effects ### 344 | snghs = self.sipp_space.GetSuccs(s.loc, nnid, s.t, s.tb) 345 | for sngh in snghs: 346 | if sngh.t > sngh.tb: 347 | sys.exit("[ERROR] state " + str(sngh) + " t > tb !!!") 348 | ## updat state id 349 | sngh.id = self.state_gen_id 350 | self.state_gen_id = self.state_gen_id + 1 351 | ## update state cost vector 352 | dt = 1 # @2021-04-09 353 | if sngh.t > s.t: 354 | dt = sngh.t - s.t 355 | sngh.g = s.g+self.GetCost(s.loc, nnid, dt) 356 | s_ngh.append(sngh) 357 | if DEBUG_MOSTASTAR: 358 | print(" get ngh from ", s, " to node ", nnid) 359 | return s_ngh, True 360 | 361 | 362 | def GetNeighbors_ml(self, s, tstart): 363 | """ 364 | tstart is useless... can be deleted. 365 | input a state s, compute its neighboring states. 366 | output a list of states. 367 | do not engage carts 368 | use sipp 369 | """ 370 | s_ngh = list() 371 | cy = int(np.floor(s.loc/self.nxt)) # current x 372 | cx = int(s.loc%self.nxt) # current y 373 | 374 | # loop over all four possible actions 375 | for action_idx in range(len(self.action_set_x)): 376 | nx = cx+self.action_set_x[action_idx] # next x 377 | ny = cy+self.action_set_y[action_idx] # next y 378 | nnid = ny*self.nxt+nx 379 | if (nx >= self.nxt) or (nx < 0) or (ny >= self.nyt) or (ny < 0): # out of border of grid 380 | continue 381 | if (self.grids[ny,nx] > 0): # static obstacle 382 | continue 383 | ### this is where SIPP takes effects ### 384 | snghs = self.sipp_space.GetSuccs_ml(s.loc, nnid, s.t, s.tb,[nnid],s.carttag) 385 | for sngh in snghs: 386 | if sngh.t > sngh.tb: 387 | sys.exit("[ERROR] state " + str(sngh) + " t > tb !!!") 388 | ## updat state id 389 | sngh.id = self.state_gen_id 390 | self.state_gen_id = self.state_gen_id + 1 391 | ## update state cost vector 392 | dt = 1 # @2021-04-09 393 | if sngh.t > s.t: 394 | dt = sngh.t - s.t 395 | sngh.g = s.g+self.GetCost(s.loc, nnid, dt) 396 | s_ngh.append(sngh) 397 | if DEBUG_MOSTASTAR: 398 | print(" get ngh from ", s, " to node ", nnid) 399 | 400 | if s.carttag < len(self.gseq) - 2: # not include destination 401 | goalloc = self.gseq[s.carttag + 1] 402 | if s.loc == goalloc and s.tb-s.t >= tasktime: 403 | sngh_t = SippState(-1, goalloc, -1.0, s.t + tasktime, s.tb, [goalloc], 404 | s.carttag + 1) 405 | sngh_t.id = self.state_gen_id 406 | self.state_gen_id = self.state_gen_id + 1 407 | ## update state cost vector 408 | dt = 0 # @2021-04-09 409 | if sngh_t.t > s.t: 410 | dt = sngh_t.t - s.t 411 | sngh_t.g = s.g + self.GetCost(s.loc, goalloc, dt) 412 | s_ngh.append(sngh_t) 413 | 414 | 415 | return s_ngh, True 416 | 417 | 418 | def ReconstructPath(self, sid): 419 | """ 420 | input state is the one that reached, 421 | return a list of joint vertices in right order. 422 | """ 423 | jpath = [] # in reverse order 424 | tt = [] # in reverse order 425 | while sid in self.backtrack_dict: 426 | jpath.append(self.all_visited_s[sid].loc) 427 | tt.append(self.all_visited_s[sid].t) 428 | sid = self.backtrack_dict[sid] 429 | jpath.append(self.all_visited_s[sid].loc) 430 | tt.append(self.all_visited_s[sid].t) 431 | 432 | # reverse output path here. 433 | nodes = [] 434 | times = [] 435 | for idx in range(len(jpath)): 436 | nodes.append(jpath[len(jpath)-1-idx]) 437 | times.append(tt[len(jpath)-1-idx]) 438 | return nodes, times 439 | 440 | def ReconstructPath_ml(self, sid): 441 | """ 442 | input state is the one that reached, 443 | return a list of joint vertices in right order. 444 | """ 445 | jpath = [] # in reverse order 446 | joccupylist = [] # in reverse order 447 | tt = [] # in reverse order 448 | while sid in self.backtrack_dict: 449 | jpath.append(self.all_visited_s[sid].loc) 450 | joccupylist.append(self.all_visited_s[sid].occupylist) 451 | tt.append(self.all_visited_s[sid].t) 452 | sid = self.backtrack_dict[sid] 453 | jpath.append(self.all_visited_s[sid].loc) 454 | joccupylist.append(self.all_visited_s[sid].occupylist) 455 | tt.append(self.all_visited_s[sid].t) 456 | 457 | # reverse output path here. 458 | nodes = [] 459 | occupylists = [] 460 | times = [] 461 | for idx in range(len(jpath)): 462 | nodes.append(jpath[len(jpath)-1-idx]) 463 | occupylists.append(joccupylist[len(jpath)-1-idx]) 464 | times.append(tt[len(jpath)-1-idx]) 465 | 466 | # unload the cart 467 | mylen = len(occupylists[-1])-1 468 | myfinal = occupylists[-1] 469 | myfinalnode = nodes[-1] 470 | myfinaltime = times[-1] 471 | for i in range(mylen): 472 | occupylists.append(myfinal[i+1:]) 473 | #print(lo[-1][i+1:]) 474 | nodes.append(myfinalnode) 475 | times.append(myfinaltime+1) 476 | myfinaltime = myfinaltime + 1 477 | return nodes, times, occupylists 478 | 479 | def CheckReachGoal(self, s): 480 | """ 481 | verify if s is a state that reaches goal and robot can stay there forever !! 482 | """ 483 | if (s.loc != self.s_f.loc): 484 | return False 485 | ## s.loc == s_f.loc is True now 486 | if self.ignore_goal_cstr: 487 | return True # no need to consider constraints at goal. 488 | if s.loc not in self.node_constr: 489 | return True 490 | # print("s.t = ", s.t, " last cstr = ", self.node_constr[s.loc][-1]) 491 | if s.t > self.node_constr[s.loc][-1]: 492 | # print("true") 493 | return True 494 | return False 495 | 496 | def CheckReachGoal_ml(self, s): 497 | """ 498 | verify if s is a state that reaches goal and the head of robot can stay there forever !! 499 | """ 500 | if (s.loc != self.s_f.loc): 501 | return False 502 | ## s.loc == s_f.loc is True now 503 | if self.ignore_goal_cstr: 504 | return True # no need to consider constraints at goal. 505 | 506 | 507 | if s.loc not in self.node_constr: 508 | return True 509 | # print("s.t = ", s.t, " last cstr = ", self.node_constr[s.loc][-1]) 510 | if s.t > self.node_constr[s.loc][-1]: 511 | # print("true") 512 | return True 513 | return False 514 | 515 | 516 | def InitSearch(self): 517 | """ 518 | override parent method 519 | """ 520 | # start state 521 | self.s_o = SippState(1, self.sy*self.nxt+self.sx, 0.0, self.t0, np.inf) 522 | if (self.s_o.loc in self.node_constr) and len(self.node_constr[self.s_o.loc]) > 0: 523 | self.s_o.tb = min(self.node_constr[self.s_o.loc]) - 1 # leave the start node earlier than the min time in node constraints. 524 | # goal state 525 | self.s_f = SippState(2, self.gy*self.nxt+self.gx, -1.0, 0, np.inf) 526 | if (self.s_f.loc in self.node_constr) and len(self.node_constr[self.s_f.loc]) > 0: 527 | self.s_f.t = max(self.node_constr[self.s_f.loc]) + 1 # arrive at destination later than the max time in node constraints. 528 | if DEBUG_MOSTASTAR: 529 | print(" s_o = ", self.s_o, " self.node_constr = ", self.node_constr) 530 | print(" s_f = ", self.s_f, " self.node_constr = ", self.node_constr) 531 | self.all_visited_s[self.s_o.id] = self.s_o 532 | self.open_list.add(self.s_o.g + self.GetHeuristic(self.s_o), self.s_o.id) 533 | return 534 | 535 | def InitSearch_ml(self, occupylist): 536 | """ 537 | override parent method 538 | """ 539 | # start state 540 | self.s_o = SippState(1, self.sy*self.nxt+self.sx, 0.0, self.t0, np.inf, occupylist) 541 | if (self.s_o.loc in self.node_constr) and len(self.node_constr[self.s_o.loc]) > 0: 542 | self.s_o.tb = min(self.node_constr[self.s_o.loc]) - 1 # leave the start node earlier than the min time in node constraints. 543 | # goal state 544 | self.s_f = SippState(2, self.gy*self.nxt+self.gx, -1.0, 0, np.inf) 545 | if (self.s_f.loc in self.node_constr) and len(self.node_constr[self.s_f.loc]) > 0: 546 | self.s_f.t = max(self.node_constr[self.s_f.loc]) + 1 # arrive at destination later than the max time in node constraints. 547 | if DEBUG_MOSTASTAR: 548 | print(" s_o = ", self.s_o, " self.node_constr = ", self.node_constr) 549 | print(" s_f = ", self.s_f, " self.node_constr = ", self.node_constr) 550 | self.all_visited_s[self.s_o.id] = self.s_o 551 | #self.open_list.add(self.s_o.g + self.GetHeuristic(self.s_o), self.s_o.id) 552 | self.open_list.add(self.s_o.g + self.GetHeuristic_ml(self.s_o), self.s_o.id) 553 | return 554 | 555 | def Pruning(self, s): 556 | """ 557 | """ 558 | cfg_t = self.GetStateIdentifier(s) 559 | if cfg_t not in self.frontier_map: 560 | return False # this je is never visited before, should not prune 561 | elif self.frontier_map[cfg_t] <= s.g: 562 | return True 563 | return False # should not be pruned 564 | 565 | 566 | def Pruning_ml(self, s): 567 | """ 568 | """ 569 | cfg_t = self.GetStateIdentifier_ml(s) 570 | if cfg_t not in self.frontier_map: 571 | return False # this je is never visited before, should not prune 572 | elif self.frontier_map[cfg_t] <= s.g: 573 | #print(self.frontier_map[cfg_t],s.g) 574 | return True 575 | return False # should not be pruned 576 | 577 | def Search(self, time_limit=10): 578 | if DEBUG_MOSTASTAR: 579 | print(" SIPP Search begin ") 580 | self.time_limit = time_limit 581 | tstart = time.perf_counter() 582 | self.InitSearch() 583 | search_success = True 584 | rd = 0 585 | while(True): 586 | tnow = time.perf_counter() 587 | rd = rd + 1 588 | if (tnow - tstart > self.time_limit): 589 | print(" SIPP Fail! timeout! ") 590 | search_success = False 591 | break 592 | if self.open_list.size() == 0: 593 | # print(" SIPP Fail! Open empty! ") 594 | search_success = False 595 | break 596 | pop_node = self.open_list.pop() # ( sum(f), sid ) 597 | curr_s = self.all_visited_s[pop_node[1]] 598 | if DEBUG_MOSTASTAR: 599 | print("##curr_s : ", curr_s, " g=", curr_s.g, " h=", self.GetHeuristic(curr_s)) 600 | if DEBUG_MOSTASTAR: 601 | if rd % 1000 == 0: 602 | print(" search round = ", rd, " open_list sz = ", self.open_list.size(), \ 603 | " time used = ", tnow - tstart ) 604 | if self.CheckReachGoal(curr_s): # check if reach goal(and robot can stay there!) 605 | self.reached_goal_state_id = curr_s.id 606 | search_success = True 607 | break 608 | # get neighbors 609 | ngh_ss, ngh_success = self.GetNeighbors(curr_s, tnow) # neighboring states 610 | if not ngh_success: 611 | search_success = False 612 | break 613 | # loop over neighbors 614 | for idx in range(len(ngh_ss)): 615 | ngh_s = ngh_ss[idx] 616 | if DEBUG_MOSTASTAR: 617 | print (" -- loop ngh ", ngh_s) 618 | if (not self.Pruning(ngh_s)): 619 | self.frontier_map[self.GetStateIdentifier(ngh_s)] = ngh_s.g 620 | self.backtrack_dict[ngh_s.id] = curr_s.id 621 | fval = ngh_s.g + self.weight * self.GetHeuristic(ngh_s) 622 | self.open_list.add(fval, ngh_s.id) 623 | self.all_visited_s[ngh_s.id] = ngh_s 624 | else: 625 | if DEBUG_MOSTASTAR: 626 | print(" XX dom pruned XX ") 627 | if search_success: 628 | # output jpath is in reverse order, from goal to start 629 | sol_path = self.ReconstructPath(self.reached_goal_state_id) 630 | search_success = 1 631 | output_res = ( int(rd), int(search_success), float(time.perf_counter()-tstart) ) 632 | return sol_path, output_res 633 | else: 634 | output_res = ( int(rd), int(search_success), float(time.perf_counter()-tstart) ) 635 | return list(), output_res 636 | 637 | def Search_ml(self, time_limit=10, s_occupylist=[], gseq=[]): 638 | if DEBUG_MOSTASTAR: 639 | print(" multi label Search begin ") 640 | self.time_limit = time_limit 641 | tstart = time.perf_counter() 642 | self.InitSearch_ml(s_occupylist) 643 | search_success = True 644 | rd = 0 645 | while (True): 646 | tnow = time.perf_counter() 647 | rd = rd + 1 648 | if (tnow - tstart > self.time_limit): 649 | print(" multi label Search Fail! timeout! ") 650 | search_success = False 651 | break 652 | if self.open_list.size() == 0: 653 | # print(" SIPP Fail! Open empty! ") 654 | search_success = False 655 | break 656 | pop_node = self.open_list.pop() # ( sum(f), sid ) 657 | curr_s = self.all_visited_s[pop_node[1]] 658 | if DEBUG_MOSTASTAR: 659 | print("##curr_s : ", curr_s, " g=", curr_s.g, " h=", self.GetHeuristic_ml(curr_s)) 660 | print(curr_s.occupylist) 661 | if DEBUG_MOSTASTAR: 662 | if rd % 1000 == 0: 663 | print(" search round = ", rd, " open_list sz = ", self.open_list.size(), \ 664 | " time used = ", tnow - tstart) 665 | if self.CheckReachGoal_ml(curr_s) and curr_s.carttag == len( 666 | self.gseq) - 2: # check if reach goal(and robot can stay there!) 667 | self.reached_goal_state_id = curr_s.id 668 | search_success = True 669 | break 670 | 671 | # get neighbors 672 | ngh_ss, ngh_success = self.GetNeighbors_ml(curr_s, tnow) # neighboring states 673 | if not ngh_success: 674 | search_success = False 675 | break 676 | # loop over neighbors 677 | for idx in range(len(ngh_ss)): 678 | ngh_s = ngh_ss[idx] 679 | 680 | if DEBUG_MOSTASTAR: 681 | print (" -- loop ngh ", ngh_s) 682 | #if self.conflict_ml(curr_s, ngh_s): # avoid the conflict 683 | if (not self.Pruning_ml(ngh_s)): 684 | self.frontier_map[self.GetStateIdentifier_ml(ngh_s)] = ngh_s.g 685 | self.backtrack_dict[ngh_s.id] = curr_s.id 686 | fval = ngh_s.g + self.weight * self.GetHeuristic_ml(ngh_s) 687 | self.open_list.add(fval, ngh_s.id) 688 | self.all_visited_s[ngh_s.id] = ngh_s 689 | else: 690 | 691 | if DEBUG_MOSTASTAR: 692 | print(" XX dom pruned XX ", self.GetStateIdentifier_ml(ngh_s)) 693 | if search_success: 694 | # output jpath is in reverse order, from goal to start 695 | sol_path = self.ReconstructPath_ml(self.reached_goal_state_id) 696 | search_success = 1 697 | output_res = (int(rd), int(search_success), float(time.perf_counter() - tstart)) 698 | return sol_path, output_res 699 | else: 700 | output_res = (int(rd), int(search_success), float(time.perf_counter() - tstart)) 701 | return list(), output_res 702 | 703 | def AddNodeConstrBase(self, nid, t): 704 | """ 705 | This one is borrowed from MOSTA*, may be redundant... 706 | """ 707 | # a new node id 708 | t = int(t) # make sure int! 709 | if nid not in self.node_constr: 710 | self.node_constr[nid] = list() 711 | self.node_constr[nid].append(t) 712 | # print(" AddVertexConstr - self.node_constr[",nid,"]=",self.node_constr[nid]) 713 | return 714 | # locate the index for t 715 | idx = 0 716 | while idx < len(self.node_constr[nid]): 717 | if t <= self.node_constr[nid][idx]: 718 | break 719 | idx = idx + 1 720 | # if just put at the end 721 | if idx == len(self.node_constr[nid]): 722 | self.node_constr[nid].append(t) 723 | # print(" AddVertexConstr - self.node_constr[",nid,"]=",self.node_constr[nid]) 724 | return 725 | # avoid duplication 726 | if t == self.node_constr[nid][idx]: 727 | # print(" AddVertexConstr - self.node_constr[",nid,"]=",self.node_constr[nid]) 728 | return 729 | # update list 730 | tlist = list() 731 | for idy in range(len(self.node_constr[nid])): 732 | if idy == idx: 733 | tlist.append(t) 734 | tlist.append(self.node_constr[nid][idy]) 735 | self.node_constr[nid] = tlist 736 | # print(" AddVertexConstr - self.node_constr[",nid,"]=",self.node_constr[nid]) 737 | return 738 | 739 | def AddNodeConstr(self, nid, t): 740 | """ 741 | robot is forbidden from entering nid at time t. 742 | This is a naive implementation using list. 743 | There is space for improvement on data-structure and sorting but expect to be a minor one. 744 | """ 745 | if DEBUG_MOSTASTAR: 746 | print("*** AddNodeCstr node ", nid, " t ", t) 747 | self.AddNodeConstrBase(nid, t) 748 | self.sipp_space.AddNodeCstr(nid, t) 749 | 750 | return 751 | 752 | def AddSwapConstr(self, nid1, nid2, t): 753 | """ 754 | robot is forbidden from transfering from (nid1,t) to (nid2,t+1). 755 | """ 756 | # if nid1 is new to self.swap_constr[] 757 | if nid1 not in self.swap_constr: 758 | self.swap_constr[nid1] = dict() 759 | if t not in self.swap_constr[nid1]: 760 | self.swap_constr[nid1][t] = set() 761 | self.swap_constr[nid1][t].add(nid2) # just add 762 | # print("...AddSwapConstr, self.swap_constr[",nid1,"][",t,"]=", self.swap_constr[nid1][t]) 763 | self.sipp_space.AddEdgeCstr(nid1, nid2, t) 764 | return 765 | 766 | def EnforceUnitTimePath(lv,lt): 767 | """ 768 | Given a path (without the final node with infinite timestamp), 769 | insert missing (v,t) to ensure every pair of adjacent (v,t) 770 | has time difference of one. 771 | """ 772 | dt = 1 773 | nlv = list() 774 | nlt = list() 775 | for ix in range(len(lt)-1): 776 | nlv.append(lv[ix]) 777 | nlt.append(lt[ix]) 778 | if lt[ix+1]-lt[ix] > 1.001: 779 | ct = lt[ix] 780 | while lt[ix+1] - ct > 1.001: 781 | nlv.append(lv[ix]) 782 | nlt.append(ct+1) 783 | ct = ct + 1 784 | # end for 785 | nlv.append(lv[-1]) 786 | nlt.append(lt[-1]) 787 | return nlv, nlt 788 | 789 | def EnforceUnitTimePath_ml(lv,lt,locp): 790 | """ 791 | Given a path (without the final node with infinite timestamp), 792 | insert missing (v,t) to ensure every pair of adjacent (v,t) 793 | has time difference of one. 794 | """ 795 | dt = 1 796 | nlv = list() 797 | nlt = list() 798 | nlocp = list() 799 | for ix in range(len(lt)-1): 800 | if lt[ix]!=lt[ix+1]: 801 | nlv.append(lv[ix]) 802 | nlt.append(lt[ix]) 803 | nlocp.append(locp[ix]) 804 | if lt[ix+1]-lt[ix] > 1.001: 805 | ct = lt[ix] 806 | while lt[ix+1] - ct > 1.001: 807 | nlv.append(lv[ix]) 808 | nlt.append(ct+1) 809 | nlocp.append(locp[ix])#execute the task 810 | ct = ct + 1 811 | # end for 812 | nlv.append(lv[-1]) 813 | nlt.append(lt[-1]) 814 | nlocp.append(locp[-1]) 815 | return nlv, nlt, nlocp 816 | 817 | def RunSipp(grids, sx, sy, gx, gy, t0, ignore_goal_cstr, w, eps, time_limit, node_cstrs=[], swap_cstrs=[]): 818 | """ 819 | TODO[Done@2021-05-26], from implementation perspective, may need to consider igonring the node constraint at goal. 820 | Agent does not need to stay at goal after reaching it if this goal is not the destination in a MSMP problem. 821 | """ 822 | if DEBUG_MOSTASTAR: 823 | print("...RunSipp... ") 824 | print("sx:", sx, " sy:", sy, " gx:", gx, " gy:", gy, " ignore_goal_cstr:",ignore_goal_cstr) 825 | print("node_cstrs:", node_cstrs, " swap_cstrs:", swap_cstrs) 826 | 827 | sipp = SIPP(grids, sx,sy,gx,gy, t0, ignore_goal_cstr, w, eps) 828 | for node_cstr in node_cstrs: 829 | sipp.AddNodeConstr(node_cstr[0], node_cstr[1]) 830 | for swap_cstr in swap_cstrs: 831 | sipp.AddSwapConstr(swap_cstr[0], swap_cstr[1], swap_cstr[2]) 832 | sol_path, stats = sipp.Search(time_limit) 833 | if len(sol_path) != 0: 834 | unit_time_path = EnforceUnitTimePath(sol_path[0], sol_path[1]) 835 | return unit_time_path, stats 836 | else: 837 | return sol_path, stats 838 | 839 | def RunSipp_ml(grids, gseq, t0, ignore_goal_cstr, w, eps, time_limit, node_cstrs=[], swap_cstrs=[]): 840 | """ 841 | run multi label A star 842 | """ 843 | 844 | (nyt, nxt) = grids.shape 845 | sx = gseq[0] % nxt 846 | sy = int(np.floor(gseq[0] / nxt)) 847 | gx = gseq[-1] % nxt 848 | gy = int(np.floor(gseq[-1] / nxt)) 849 | if DEBUG_MOSTASTAR: 850 | print(gseq) 851 | print("...Run multi a star... ") 852 | print("sx:", sx, " sy:", sy, " gx:", gx, " gy:", gy, " ignore_goal_cstr:",ignore_goal_cstr) 853 | print("node_cstrs:", node_cstrs, " swap_cstrs:", swap_cstrs) 854 | 855 | sipp = SIPP(grids, sx,sy,gx,gy, t0, ignore_goal_cstr, w, eps,[-1,0,1,0], [0,-1,0,1], gseq) 856 | for node_cstr in node_cstrs: 857 | sipp.AddNodeConstr(node_cstr[0], node_cstr[1]) 858 | for swap_cstr in swap_cstrs: 859 | sipp.AddSwapConstr(swap_cstr[0], swap_cstr[1], swap_cstr[2]) 860 | 861 | occupylist = [gseq[0]] 862 | sol_path, stats = sipp.Search_ml(time_limit, occupylist, gseq) #mulit_Search_dl_ML_sipp mulit_Search_dl 863 | #print(sol_path) 864 | 865 | if len(sol_path) != 0: 866 | unit_time_path = EnforceUnitTimePath_ml(sol_path[0], sol_path[1], sol_path[2]) 867 | return unit_time_path, stats 868 | else: 869 | return sol_path, stats 870 | 871 | # def Test1(): 872 | # """ 873 | # """ 874 | # grids = np.zeros((5, 5)) 875 | # gseq = [2,4,10] 876 | # 877 | # t0 = 0 878 | # ignore_goal_cstr = False 879 | # w = 3.0 880 | # eps = 0.0 881 | # tlimit = 60 882 | # ncs = [(4,2)] 883 | # ecs = [] 884 | # print(grids) 885 | # 886 | # sol_path, stats = RunSipp_ml(grids, gseq, t0, ignore_goal_cstr, w, eps, tlimit, ncs,ecs) 887 | # print(sol_path) 888 | # print(stats) 889 | # return 890 | 891 | if __name__ == "__main__": 892 | Test1() 893 | 894 | 895 | -------------------------------------------------------------------------------- /libmcpf/cbss_mcpf.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | Version@2021-07 4 | All Rights Reserved 5 | ABOUT: this file constains CBSS-MCPF-AC, which is derived from CBSS (framework) and aim to solve MCPF-AC problems. 6 | """ 7 | 8 | import cbss 9 | import seq_mcpf 10 | 11 | class CbssMCPF(cbss.CbssFramework) : 12 | """ 13 | """ 14 | def __init__(self, grids, starts, goals, dests, ac_dict, configs): 15 | """ 16 | """ 17 | # mtsp_solver = msmp_seq.BridgeLKH_MSMP(grids, starts, goals, dests) 18 | # mtsp_solver = mcpf_seq.BridgeLKH_MCPF(grids, starts, goals, dests, ac_dict) # NOTE that ac_dict is only used in mtsp_solver, not in CBSS itself. 19 | mtsp_solver = seq_mcpf.SeqMCPF(grids, starts, goals, dests, ac_dict, configs) # NOTE that ac_dict is only used in mtsp_solver, not in CBSS itself. 20 | super(CbssMCPF, self).__init__(mtsp_solver, grids, starts, goals, dests, dict(), configs) 21 | return 22 | 23 | def RunCbssMCPF(grids, starts, targets, dests, ac_dict, configs): 24 | """ 25 | starts, targets and dests are all node ID. 26 | heu_weight and prune_delta are not in use. @2021-05-26 27 | """ 28 | ccbs_planner = CbssMCPF(grids, starts, targets, dests, ac_dict, configs) 29 | path_set, search_res = ccbs_planner.Search_ml() 30 | # print(path_set) 31 | # print(res_dict) 32 | res_dict = dict() 33 | res_dict["path_set"] = path_set 34 | res_dict["round"] = search_res[0] # = num of high level nodes closed. 35 | res_dict["best_g_value"] = search_res[1] 36 | res_dict["open_list_size"] = search_res[3] 37 | res_dict["num_low_level_expanded"] = search_res[4] 38 | res_dict["search_success"] = search_res[5] 39 | res_dict["search_time"] = search_res[6] 40 | res_dict["n_tsp_call"] = search_res[7] 41 | res_dict["n_tsp_time"] = search_res[8] 42 | res_dict["n_roots"] = search_res[9] 43 | 44 | return res_dict 45 | -------------------------------------------------------------------------------- /libmcpf/cbss_msmp.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | Version@2021-07 4 | ABOUT: this file constains CBSS-MSMP, which is derived from CBSS (framework). 5 | Oeffentlich fuer: RSS22 6 | """ 7 | 8 | import cbss 9 | import seq_msmp 10 | 11 | class CbssMSMP(cbss.CbssFramework) : 12 | """ 13 | """ 14 | def __init__(self, grids, starts, goals, dests, configs): 15 | """ 16 | """ 17 | mtsp_solver = seq_msmp.SeqMSMP(grids, starts, goals, dests, configs) 18 | super(CbssMSMP, self).__init__(mtsp_solver, grids, starts, goals, dests, dict(), configs) 19 | return 20 | 21 | def RunCbssMSMP(grids, starts, goals, dests, configs): 22 | """ 23 | starts, goals and dests are all node ID. 24 | heu_weight and prune_delta are not in use. @2021-05-26 25 | """ 26 | ccbs_planner = CbssMSMP(grids, starts, goals, dests, configs) 27 | path_set, search_res = ccbs_planner.Search_ml() 28 | res_dict = dict() 29 | res_dict["path_set"] = path_set 30 | res_dict["round"] = search_res[0] # = num of high level nodes closed. 31 | res_dict["best_g_value"] = search_res[1] 32 | res_dict["open_list_size"] = search_res[3] 33 | res_dict["num_low_level_expanded"] = search_res[4] 34 | res_dict["search_success"] = search_res[5] 35 | res_dict["search_time"] = search_res[6] 36 | res_dict["n_tsp_call"] = search_res[7] 37 | res_dict["n_tsp_time"] = search_res[8] 38 | res_dict["n_roots"] = search_res[9] 39 | return res_dict 40 | 41 | 42 | -------------------------------------------------------------------------------- /libmcpf/common.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | ABOUT: Utility. 5 | Oeffentlich fuer: RSS22 6 | """ 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | import heapq as hpq 11 | import matplotlib.cm as cm 12 | import json 13 | 14 | class PrioritySet(object): 15 | """ 16 | priority queue, min-heap 17 | """ 18 | def __init__(self): 19 | """ 20 | no duplication allowed 21 | """ 22 | self.heap_ = [] 23 | self.set_ = set() 24 | def add(self, pri, d): 25 | """ 26 | will check for duplication and avoid. 27 | """ 28 | if not d in self.set_: 29 | hpq.heappush(self.heap_, (pri, d)) 30 | self.set_.add(d) 31 | def pop(self): 32 | """ 33 | impl detail: return the first(min) item that is in self.set_ 34 | """ 35 | pri, d = hpq.heappop(self.heap_) 36 | while d not in self.set_: 37 | pri, d = hpq.heappop(self.heap_) 38 | self.set_.remove(d) 39 | return pri, d 40 | def size(self): 41 | return len(self.set_) 42 | def print(self): 43 | print(self.heap_) 44 | print(self.set_) 45 | return 46 | def remove(self, d): 47 | """ 48 | implementation: only remove from self.set_, not remove from self.heap_ list. 49 | """ 50 | if not d in self.set_: 51 | return False 52 | self.set_.remove(d) 53 | return True 54 | 55 | 56 | def gridAstar(grids, start, goal, w=1.0): 57 | """ 58 | Four-connected Grid 59 | Return a path (in REVERSE order!) 60 | a path is a list of node ID (not x,y!) 61 | """ 62 | output = list() 63 | (nyt, nxt) = grids.shape # nyt = ny total, nxt = nx total 64 | action_set_x = [-1,0,1,0] 65 | action_set_y = [0,-1,0,1] 66 | open_list = [] 67 | hpq.heappush( open_list, (0, start) ) 68 | close_set = dict() 69 | parent_dict = dict() 70 | parent_dict[start] = -1 71 | g_dict = dict() 72 | g_dict[start] = 0 73 | gx = goal % nxt 74 | gy = int(np.floor(goal/nxt)) 75 | search_success = True 76 | while True: 77 | if len(open_list) == 0: 78 | search_success = False; 79 | break; 80 | cnode = hpq.heappop(open_list) 81 | cid = cnode[1] 82 | curr_cost = g_dict[cid] 83 | if cid in close_set: 84 | continue 85 | close_set[cid] = 1 86 | if cid == goal: 87 | break 88 | # get neighbors 89 | # action_idx_seq = np.random.permutation(5) 90 | cx = cid % nxt 91 | cy = int(np.floor(cid / nxt)) 92 | for action_idx in range(len(action_set_x)): 93 | nx = cx + action_set_x[action_idx] 94 | ny = cy + action_set_y[action_idx] 95 | if ny < 0 or ny >= nyt or nx < 0 or nx >= nxt: 96 | continue 97 | if grids[ny,nx] > 0.5: 98 | continue 99 | nid = ny*nxt+nx 100 | heu = np.abs(gx-nx) + np.abs(gy-ny) # manhattan heu 101 | gnew = curr_cost+1 102 | if (nid) not in close_set: 103 | if (nid) not in g_dict: 104 | hpq.heappush(open_list, (gnew+w*heu, nid)) 105 | g_dict[nid] = gnew 106 | parent_dict[nid] = cid 107 | else: 108 | if (gnew < g_dict[nid]): 109 | hpq.heappush(open_list, (gnew+w*heu, nid)) 110 | g_dict[nid] = gnew 111 | parent_dict[nid] = cid 112 | # end of while 113 | 114 | # reconstruct path 115 | if search_success: 116 | cid = goal 117 | output.append(cid) 118 | while parent_dict[cid] != -1 : 119 | cid = parent_dict[cid] 120 | output.append(cid) 121 | else: 122 | # do nothing 123 | print(" fail to plan !") 124 | return output 125 | 126 | def getTargetGraph(grids,Vo,Vt,Vd): 127 | """ 128 | Return a cost matrix of size |Vo|+|Vt|+|Vd| (|Vd|=|Vo|) 129 | to represent a fully connected graph. 130 | The returned spMat use node index (in list Vo+Vt+Vd) instead of node ID. 131 | """ 132 | N = len(Vo) 133 | M = len(Vt) 134 | nn = N+M+N 135 | V = Vo + Vt + Vd 136 | spMat = np.zeros((nn,nn)) 137 | for i in range(nn): 138 | for j in range(i+1,nn): 139 | spMat[i,j] = len( gridAstar(grids,V[i],V[j]) ) - 1 140 | spMat[j,i] = spMat[i,j] 141 | return spMat 142 | 143 | def ItvOverlap(ita,itb,jta,jtb): 144 | """ 145 | check if two time interval are overlapped or not. 146 | """ 147 | if ita >= jtb or jta >= itb: # non-overlap 148 | return False, -1.0, -1.0 149 | # must overlap now 150 | tlb = jta # find the larger value among ita and jta, serve as lower bound 151 | if ita >= jta: 152 | tlb = ita 153 | tub = jtb # find the smaller value among itb and jtb, serve as upper bound 154 | if itb <= jtb: 155 | tub = itb 156 | return True, tlb, tub 157 | 158 | 159 | 160 | -------------------------------------------------------------------------------- /libmcpf/kbtsp.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | ABOUT: this file contains K-best partition. 4 | Oeffentlich fuer: RSS22 5 | """ 6 | 7 | import copy 8 | import numpy as np 9 | import sys 10 | import time 11 | 12 | import common as cm 13 | 14 | DEBUG_KBESTTSP = False 15 | 16 | class RestrictedTSP: 17 | """ 18 | A restricted TSP instance. 19 | A tuple of (setI, setO, solution) and other auxiliary variables. 20 | """ 21 | def __init__(self, setI=set(), setO=set()): 22 | """ 23 | """ 24 | self.node_id = -1 25 | self.setI = setI # must include 26 | self.setO = setO # must exclude 27 | self.sol = list() # solution, a dict that encodes a joint sequence 28 | self.cost = np.inf # tour cost. 29 | self.cost_dict = dict() # cost of each individual sequence 30 | return 31 | def __str__(self): 32 | return "" 34 | 35 | class KBestMTSP: 36 | """ 37 | Compute K cheapest solutions for a mTSP. 38 | Note: The auxilliary edge in the transformation can be skipped when doing partition. 39 | Which is equivalent as doing partition directly on this mTSP solution. 40 | Note: Edges are considered to be directed, since after transformation the transformed 41 | graph is directed and corresponds to an ATSP. 42 | """ 43 | def __init__(self, mtsp): 44 | """ 45 | tsp = an object of mTSP solver. 46 | the following API of self.tsp: 47 | - InitMat() 48 | - Solve() 49 | - AddIe(), AddOe() 50 | """ 51 | self.tsp = mtsp # mTSP solver. 52 | self.open_list = cm.PrioritySet() 53 | self.all_nodes = dict() 54 | self.node_id_gen = 1 55 | self.last_nid = -1 # this stores the k-th solution, which is not expanded yet. 56 | self.kbest_node = list() # the k-best nodes containing solutions. 57 | self.n_tsp_call = 0 58 | self.n_tsp_time = 0 59 | self.num_vertices = -1 # to be set in ComputeNextBest, will be used in a check. 60 | return 61 | 62 | def _Init(self): 63 | """ 64 | Generate the initial solution (a node) for a TSP problem. Insert into OPEN. 65 | """ 66 | 67 | ### generate initial restricted TSP problem instance. 68 | self.tsp.InitMat() 69 | tnow = time.perf_counter() 70 | flag, lb, seqs_dict, cost_dict = self.tsp.Solve() 71 | if flag == False: 72 | print("[ERROR] infeasible case? KBestTSP._Init fails to get a feasible joint sequence!") 73 | dt = time.perf_counter() - tnow 74 | self.n_tsp_call = self.n_tsp_call + 1 75 | self.n_tsp_time = self.n_tsp_time + dt # this is total time. 76 | 77 | ### generate a restricted TSP instance 78 | cval = np.sum(list(cost_dict.values())) 79 | rtsp = RestrictedTSP(set(), set()) 80 | rtsp.sol = seqs_dict 81 | rtsp.cost_dict = cost_dict 82 | rtsp.cost = cval 83 | rtsp.node_id = self.node_id_gen 84 | self.node_id_gen = self.node_id_gen + 1 85 | 86 | ### insert into OPEN. 87 | self.all_nodes[rtsp.node_id] = rtsp 88 | self.open_list.add(cval, rtsp.node_id) 89 | self.last_nid = rtsp.node_id 90 | return rtsp 91 | 92 | def _SolveRTSP(self, rtsp): 93 | """ 94 | modify distance matrix based on setI, setO, solve RTSP. 95 | """ 96 | 97 | if DEBUG_KBESTTSP: 98 | print(" >> _SolveRTSP:", rtsp) 99 | 100 | ### copy and generate a new instance 101 | temp_tsp = copy.deepcopy(self.tsp) 102 | for ek in rtsp.setI: 103 | temp_tsp.AddIe(ek[0], ek[1], ek[2]) 104 | for ek in rtsp.setO: 105 | temp_tsp.AddOe(ek[0], ek[1], ek[2]) 106 | 107 | ## solve the RTSP instance 108 | tnow = time.perf_counter() 109 | success, cost_lb, seqs_dict, cost_dict = temp_tsp.Solve() 110 | if not success: 111 | # print(" temp_tsp solve success = ", success) 112 | return success, [], [], [] 113 | 114 | dt = time.perf_counter() - tnow 115 | self.n_tsp_call = self.n_tsp_call + 1 116 | self.n_tsp_time = self.n_tsp_time + dt # this is total time. 117 | cval = np.sum(list(cost_dict.values())) 118 | 119 | ### verify against Ie, Oe. 120 | flag = self._VerifySol(temp_tsp, rtsp.setI, rtsp.setO, seqs_dict) 121 | if DEBUG_KBESTTSP: 122 | print("[INFO] kbtsp._SolveRTSP seqs_dict = ", seqs_dict, " cost_dict = ", cost_dict) 123 | print("[INFO] kbtsp._VerifySol returns = ", flag) 124 | return flag, cval, seqs_dict, cost_dict 125 | 126 | def _VerifySol(self, temp_tsp, setI, setO, seqs_dict): 127 | """ 128 | verify whether the solution satisfies the requirement imposed by set I and O. 129 | """ 130 | tempI = copy.deepcopy(setI) 131 | seq_counts = {key: len(value) for key, value in seqs_dict.items()} 132 | seq_num = 0 133 | for key in seq_counts: 134 | # Add the value corresponding to the key to the total sum 135 | seq_num += seq_counts[key] 136 | if seq_num != self.num_vertices: 137 | if DEBUG_KBESTTSP: 138 | print("[INFO] kbtsp._VerifySol sequence not complete") 139 | return False 140 | for ri in seqs_dict: 141 | seq = seqs_dict[ri] 142 | for idx in range(1,len(seq)): 143 | ek = tuple([seq[idx-1], seq[idx], ri]) 144 | if ek in setO: 145 | if DEBUG_KBESTTSP: 146 | print("[INFO] kbtsp._VerifySol edge ", ek, " violates Oe ", setO) 147 | return False 148 | if ek in tempI: 149 | tempI.remove(ek) 150 | if len(tempI) > 0: 151 | if DEBUG_KBESTTSP: 152 | print("[INFO] kbtsp._VerifySol subset of Ie ", tempI, " not included") 153 | return False 154 | if hasattr(self.tsp, "verify"): 155 | if not temp_tsp.verify(): 156 | return False 157 | return True 158 | 159 | def _Expand(self, nid, tlimit): 160 | """ 161 | Partition over the input node (represented by its ID), 162 | generate successor restricted TSP problems and insert each solution (node) into OPEN. 163 | """ 164 | nk = self.all_nodes[nid] 165 | setI = copy.deepcopy(nk.setI) 166 | flag = True 167 | for ri in nk.sol: # nk.sol is a joint sequence. loop over each agent. 168 | seq = nk.sol[ri] 169 | for idx in range(1,len(seq)): 170 | ek = tuple([seq[idx-1], seq[idx], ri]) 171 | ### Solve RTSP, setI = {e1,e2,...e(k-1)}, setO = {ek} 172 | setO = copy.deepcopy(nk.setO) 173 | setO.add(ek) 174 | rtsp = RestrictedTSP(copy.deepcopy(setI), setO) 175 | rtsp.node_id = self.node_id_gen 176 | self.node_id_gen = self.node_id_gen + 1 177 | if (time.perf_counter() - self.tstart) > tlimit: 178 | return False 179 | if not self._FeasibilityCheck1(rtsp): 180 | continue # the generated rtsp is obviously infeasible 181 | # Note: if reach here, the rtsp is not guaranteed to be feasible. 182 | flag, cval, seqs_dict, cost_dict = self._SolveRTSP(rtsp) 183 | if DEBUG_KBESTTSP: 184 | print("[INFO] kbtsp._Expand, RTSP get lag = ", flag, ", cost = ", cval, ", Ie = ", setI, ", Oe = ", setO) 185 | if flag == True: # there is such a solution. 186 | rtsp.sol = seqs_dict 187 | rtsp.cost = cval 188 | rtsp.cost_dict = cost_dict 189 | ### insert into OPEN 190 | self.all_nodes[rtsp.node_id] = rtsp 191 | self.open_list.add(cval, rtsp.node_id) 192 | if DEBUG_KBESTTSP: 193 | print("[INFO] kbtsp._Expand, add OPEN, rtsp id = ", rtsp.node_id, ", cval = ", cval) 194 | else: 195 | if DEBUG_KBESTTSP: 196 | print("[INFO] kbtsp._Expand, not add OPEN, rtsp id = ") 197 | # update setI 198 | setI.add(ek) # for next iteration 199 | return True 200 | 201 | def _FeasibilityCheck1(self, rtsp): 202 | """ 203 | """ 204 | for ek in rtsp.setO: 205 | if ek in rtsp.setI: 206 | if DEBUG_KBESTTSP: 207 | print("[INFO] kbtsp._FeaCheck1, filtered ! ", ek, rtsp.node_id) 208 | return False 209 | return True 210 | 211 | def ComputeNextBest(self, tlimit, num_vertices): 212 | """ 213 | compute the (self.k_count+1)-th best solution. 214 | """ 215 | self.num_vertices = num_vertices 216 | self.tstart = time.perf_counter() 217 | if len(self.kbest_node) == 0: 218 | self._Init() 219 | else: 220 | flag = self._Expand(self.last_nid, tlimit) 221 | if not flag: # timeout ! 222 | print("[INFO] K-Best, ComputeNextBest Timeout!") 223 | return False 224 | if self.open_list.size() > 0: 225 | (ck, nid) = self.open_list.pop() 226 | # update the kth best solution and related data. 227 | self.the_kth_node = self.all_nodes[nid] 228 | self.kbest_node.append(copy.deepcopy(self.the_kth_node)) 229 | self.last_nid = nid 230 | # print("[INFO] K-Best, return a next-best solution with cost = ", self.the_kth_node.cost) 231 | return True 232 | else: 233 | # print("[TODO] ComputeNextBest, OPEN List empty!!") 234 | return False 235 | 236 | def ComputeKBest(self, k): 237 | """ 238 | compute the k-best solution from scratch. 239 | Invoke NextBest for k times. 240 | """ 241 | # invoke NextBest() 242 | # TODO 243 | return 244 | 245 | def GetKBestList(self): 246 | """ 247 | """ 248 | return self.kbest_node 249 | 250 | def GetKthBestSol(self): 251 | """ 252 | """ 253 | return self.kbest_node[-1] 254 | 255 | def GetTotalTime(self): 256 | """ 257 | return the total run time of TSP. 258 | """ 259 | return self.n_tsp_time 260 | 261 | def GetTotalCalls(self): 262 | """ 263 | return the total number of TSP calls. 264 | """ 265 | return self.n_tsp_call 266 | -------------------------------------------------------------------------------- /libmcpf/seq_mcpf.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | Oeffentlich fuer: RSS22 5 | """ 6 | 7 | import numpy as np 8 | import time 9 | import copy 10 | 11 | import os, sys 12 | sys.path.append(os.path.abspath('../')) # to import context 13 | 14 | import context 15 | import tsp_wrapper 16 | import tf_mtsp 17 | import common as cm 18 | 19 | DEBUG_SEQ_MCPF = 0 20 | 21 | class SeqMCPF(): 22 | """ 23 | This class does the transformation that converts the target sequencing 24 | problem in MCPF to an ATSP and call ATSP solver. 25 | Note that, this implementation assumes starts, targets and destinations 26 | are disjoint sets to each other. 27 | """ 28 | 29 | def __init__(self, grid, starts, goals, dests, ac_dict, configs): 30 | """ 31 | """ 32 | ### make a copy 33 | self.grid = grid 34 | self.starts = starts # input is a list of length N, N = #agents. 35 | self.goals = goals # i.e. tasks, waypoints. 36 | self.dests = dests # N destinations. 37 | self.ac_dict = ac_dict # assignment constraints. 38 | # self.lkh_file_name = lkh_file_name 39 | self.configs = configs 40 | self.tsp_exe = configs["tsp_exe"] 41 | 42 | ### aux vars 43 | self.setStarts = set(self.starts) 44 | self.setGoals = set(self.goals) 45 | self.setDests = set(self.dests) 46 | (self.nyt, self.nxt) = self.grid.shape 47 | self.num_robot = len(starts) 48 | 49 | self.index2agent = list() 50 | self.index2nodeId = list() 51 | self.agentNode2index = dict() # map a tuple of (agent_id, node_id) to a node index. 52 | self.endingIdxGoal = -1 # after InitNodes, index [self.endingIdxGoal-1] in self.index2* is the last goal. 53 | self.cost_mat = [] # size is known after invoking InitNodes 54 | self.setIe = set() 55 | self.setOe = set() 56 | 57 | def InitTargetGraph(self): 58 | self.spMat = cm.getTargetGraph(self.grid,self.starts,self.goals,self.dests) # target graph, fully connected. 59 | self.V = self.starts + self.goals + self.dests 60 | self.n2i = dict() # node ID to node index in spMat. 61 | for i in range(len(self.V)): 62 | self.n2i[self.V[i]] = i 63 | self.bigM = (len(self.starts)+len(self.goals))*np.max(self.spMat) 64 | self.infM = 999999 65 | return 66 | 67 | def InitMat(self): 68 | """ 69 | @2021-07 an API added for K-best TSP. 70 | """ 71 | self.InitTargetGraph() 72 | self.InitNodes() 73 | self.InitEdges() 74 | return 75 | 76 | def InitNodes(self): 77 | """ 78 | init nodes in the transformed graph 79 | """ 80 | self.index2agent = list() 81 | self.index2nodeId = list() 82 | 83 | ### starts of agents 84 | idx = 0 85 | for ri in range(self.num_robot): 86 | self.index2agent.append(ri) 87 | self.index2nodeId.append(self.starts[ri]) 88 | self.agentNode2index[(ri,self.starts[ri])] = idx 89 | idx = idx + 1 90 | 91 | ### goals allowed to be visited by each agent 92 | for vg in self.goals: 93 | agent_set = self._GetEligibleAgents(vg) 94 | for ri in agent_set: 95 | self.index2agent.append(ri) 96 | self.index2nodeId.append(vg) 97 | self.agentNode2index[(ri,vg)] = idx 98 | idx = idx + 1 99 | self.endingIdxGoal = idx 100 | 101 | ### dests allowed to be visited by each agent 102 | for vd in self.dests: 103 | agent_set = self._GetEligibleAgents(vd) 104 | for ri in agent_set: 105 | self.index2agent.append(ri) 106 | self.index2nodeId.append(vd) 107 | self.agentNode2index[(ri,vd)] = idx 108 | idx = idx + 1 109 | self.cost_mat = np.zeros((len(self.index2agent),len(self.index2agent))) 110 | return 111 | 112 | def IsStart(self, nid): 113 | """ 114 | """ 115 | if nid in self.setStarts: 116 | return True 117 | return False 118 | def IsGoal(self, nid): 119 | """ 120 | """ 121 | if nid in self.setGoals: 122 | return True 123 | return False 124 | def IsDest(self, nid): 125 | """ 126 | """ 127 | if nid in self.setDests: 128 | return True 129 | return False 130 | 131 | def _GetEligibleAgents(self, nid): 132 | if nid not in self.ac_dict: 133 | return range(self.num_robot) 134 | else: 135 | return self.ac_dict[nid] 136 | 137 | 138 | def GetDist(self, nid1, nid2): 139 | """ 140 | """ 141 | return self.spMat[self.n2i[nid1],self.n2i[nid2]] 142 | 143 | def _NextAgent(self, ri, nid): 144 | """ 145 | Return the next agent after agent-ri w.r.t node nid 146 | Note that assignment constraint need to be taken into consideration! 147 | """ 148 | ## for starts 149 | if nid in self.setStarts: 150 | return ri 151 | ## for goals and dests 152 | if nid not in self.ac_dict: 153 | if ri + 1 >= self.num_robot: 154 | return 0 155 | else: 156 | return ri + 1 157 | else: 158 | for k in range(ri+1, self.num_robot): 159 | if k in self.ac_dict[nid]: 160 | return k 161 | for k in range(ri+1): 162 | if k in self.ac_dict[nid]: 163 | return k 164 | 165 | def _PrevAgent(self, ri, nid): 166 | """ 167 | similar to _NextAgent(), inverse function. 168 | """ 169 | ## for starts 170 | if nid in self.setStarts: 171 | return ri 172 | ## for goals and dests 173 | if nid not in self.ac_dict: 174 | if ri - 1 < 0: 175 | return self.num_robot-1 176 | else: 177 | return ri - 1 178 | else: 179 | for k in range(ri-1, -1, -1): 180 | if k in self.ac_dict[nid]: 181 | return k 182 | for k in range(self.num_robot, ri-1, -1): 183 | if k in self.ac_dict[nid]: 184 | return k 185 | 186 | def InitEdges(self): 187 | """ 188 | compute edge costs between pair of nodes. 189 | """ 190 | 191 | ### Compute big-M, an over-estimate of the optimal tour cost. 192 | 193 | ### PART-1, between starts to all others. 194 | for idx in range(self.num_robot): 195 | # directed, from nid1 to nid2 196 | nid1 = self.index2nodeId[idx] # must be a start 197 | ## from nid1 to another start 198 | for idy in range(self.num_robot): 199 | nid2 = self.index2nodeId[idy] # another start 200 | self.cost_mat[idx,idy] = self.infM # inf 201 | ## from nid1 to a goal, need to set both (idx,idy) and (idy,idx) 202 | for idy in range(self.num_robot, self.endingIdxGoal): 203 | nid2 = self.index2nodeId[idy] # a goal 204 | if self.index2agent[idy] != idx: # agent idx is only allowed to visit its own copy of goals/dests. 205 | self.cost_mat[idx,idy] = self.infM # inf 206 | self.cost_mat[idy,idx] = self.infM # inf 207 | continue 208 | else: # nid2 is a goal within agent idx's copy. 209 | self.cost_mat[idx,idy] = self.GetDist(nid1, nid2) + self.bigM 210 | self.cost_mat[idy,idx] = self.infM # inf., from agent's goal to agent's start. 211 | # from nid1 to a dest 212 | for idy in range(self.endingIdxGoal, len(self.index2agent)): 213 | nid2 = self.index2nodeId[idy] # a dest 214 | if self.index2agent[idy] != idx: # agent idx is only allowed to visit its own copy of goals/dests. 215 | self.cost_mat[idx,idy] = self.infM # infinity 216 | self.cost_mat[idy,idx] = 0 # zero-cost edge, from agent-idy's dest to agent-idx's start. 217 | continue 218 | else: 219 | self.cost_mat[idx,idy] = self.GetDist(nid1, nid2) + self.bigM 220 | self.cost_mat[idy,idx] = 0 221 | 222 | ### PART-2, from goals to another goals/dests 223 | for idx in range(self.num_robot, self.endingIdxGoal): # loop over goals 224 | nid1 = self.index2nodeId[idx] # must be a goal 225 | # from nid1 to a goal 226 | for idy in range(self.num_robot, self.endingIdxGoal): 227 | nid2 = self.index2nodeId[idy] # another goal 228 | if (self._NextAgent(self.index2agent[idx],nid1) == self.index2agent[idy]): 229 | # agent-i's goal to agent-(i+1)'s goal or dest. 230 | if (nid1 == nid2): 231 | # same goal node, but for diff agents 232 | self.cost_mat[idx,idy] = 0 233 | else: 234 | self.cost_mat[idx,idy] = self.GetDist(nid1, nid2) + self.bigM 235 | else: 236 | # agent-i's goal is only connected to agent-(i+1)'s goal or dest. 237 | self.cost_mat[idx,idy] = self.infM 238 | 239 | # from nid1 to a dest, need to set both (idx,idy) and (idy,idx) 240 | for idy in range(self.endingIdxGoal,len(self.index2agent)): 241 | nid2 = self.index2nodeId[idy] # a destination 242 | if (self._NextAgent(self.index2agent[idx],nid1) != self.index2agent[idy]): 243 | # agent-i's goal is only connected to agent-(i+1)'s goal or dest. 244 | self.cost_mat[idx,idy] = self.infM 245 | self.cost_mat[idy,idx] = self.infM 246 | else: 247 | self.cost_mat[idx,idy] = self.GetDist(nid1, nid2) + self.bigM 248 | self.cost_mat[idy,idx] = self.infM # cannot move from dest to a goal 249 | 250 | ### STEP-3, from dests to another dests 251 | for idx in range(self.endingIdxGoal,len(self.index2agent)): # loop over dests 252 | nid1 = self.index2nodeId[idx] # a destination 253 | for idy in range(self.endingIdxGoal,len(self.index2agent)): 254 | nid2 = self.index2nodeId[idy] # another destination 255 | 256 | if (self._NextAgent(self.index2agent[idx],nid1) == self.index2agent[idy]): 257 | # agent-i's dest to the next agent's dest. 258 | if (nid1 == nid2): 259 | # same dest node, but for diff agents 260 | self.cost_mat[idx,idy] = 0 261 | else: 262 | self.cost_mat[idx,idy] = self.infM # self.GetDist(nid1, nid2) + self.const_tour_ub # the latter one is wrong, agent is not allowed to move from one dest to another dest. 263 | else: 264 | # agent-i's dest is only connected to the next agent's dest. 265 | self.cost_mat[idx,idy] = self.infM 266 | # print(self.cost_mat) 267 | ### 268 | return 269 | 270 | def Solve(self): 271 | """ 272 | solve the instance and return the results. 273 | """ 274 | if ("mtsp_fea_check" in self.configs) and (self.configs["mtsp_fea_check"]==1): 275 | print("[ERROR] not implemented") 276 | sys.exit("[ERROR]") 277 | 278 | if_atsp = True 279 | problem_str = "runtime_files/mcpf" 280 | if problem_str in self.configs: 281 | problem_str = self.configs["problem_str"] 282 | tsp_wrapper.gen_tsp_file(problem_str, self.cost_mat, if_atsp) 283 | res = tsp_wrapper.invoke_lkh(self.tsp_exe, problem_str) 284 | # print("LKH res:", res) 285 | flag, seqs_dict, cost_dict = self.SeqsFromTour(res[0]) 286 | if DEBUG_SEQ_MCPF > 3: 287 | print("[INFO] mtsp SeqsFromTour is ", flag) 288 | return flag, res[2], seqs_dict, cost_dict 289 | 290 | def SeqsFromTour(self, tour): 291 | """ 292 | break a tour down into task sequences. 293 | """ 294 | seqs = list() 295 | seqs_dict = dict() 296 | cost_dict = dict() # the cost of each agent's goal sequences 297 | 298 | this_agent = -1 299 | curr_cost = 0 300 | for ix in range(len(tour)): 301 | index = tour[ix] 302 | curr_agent = self.index2agent[index] 303 | curr_nid = self.index2nodeId[index] 304 | 305 | ### for debug 306 | if DEBUG_SEQ_MCPF: 307 | print("ix(",ix,"),index(",index,"),node(",curr_nid,"),agent(",curr_agent,")") 308 | 309 | if self.IsStart(curr_nid): 310 | ## start a new sequence for agent "this_agent". 311 | seq = list() 312 | seq.append(curr_nid) 313 | this_agent = curr_agent 314 | curr_cost = 0 315 | # print(" start a new seq, curr seq = ", seq) 316 | else: 317 | # print(" else ") 318 | if curr_agent == this_agent: # skip other agents' goals 319 | last_nid = seq[-1] 320 | seq.append(curr_nid) 321 | curr_cost = curr_cost + self.GetDist(last_nid, curr_nid) 322 | # print(" else if, append curr seq, seq = ", seq) 323 | if self.IsDest(curr_nid): 324 | ## end a sequence for "this_agent" 325 | seqs_dict[this_agent] = seq 326 | cost_dict[this_agent] = curr_cost 327 | # print(" else if if, end seq = ", seq) 328 | 329 | if len(seqs_dict) != self.num_robot: 330 | # It is possible that after adding Ie and Oe, 331 | # the instance becomes infeasible and thus the 332 | # tour can not be splitted. 333 | # E.g. the Ie and Oe do not respect the one-in-a-set rules. 334 | return 0, dict(), dict() 335 | 336 | return 1, seqs_dict, cost_dict 337 | 338 | def ChangeCost(self, v1, v2, d, ri): 339 | """ 340 | ri = robot ID. 341 | v1,v2 are workspace graph node ID. 342 | d is the enforced distance value between them. 343 | This modifies the cost of edges in the transformed graph G_TF! 344 | """ 345 | 346 | ## get indices 347 | rj = self._PrevAgent(ri,v1) 348 | index1 = self.agentNode2index[(rj,v1)] 349 | index2 = self.agentNode2index[(ri,v2)] 350 | 351 | # an edge case @2021-07-10 352 | if d == np.inf: 353 | self.cost_mat[index1,index2] = self.infM # un-traversable! 354 | return True 355 | else: 356 | self.cost_mat[index1,index2] = d 357 | return True 358 | 359 | return False # unknown error 360 | 361 | def AddIe(self, v1, v2, ri): 362 | """ 363 | ri is used. Ie is imposed on the transformed graph. 364 | 365 | Basically, each edge in a single-agent TSP tour corresponds to either an 366 | auxiliary edge or an edge in the mTSP solution. 367 | Also note that, when do partition based on the mTSP solution, the Ie and Oe 368 | are still added in the transformed graph (i.e. in that ATSP problem). As an 369 | example, look at the HMDMTSP implementation, the AddIe and AddOe interfaces 370 | considers the prevAgent and nextAgent and all the Ie and Oe are imposed on 371 | the transformed graph (represented by cost_mat in SeqMCPF class.) 372 | 373 | """ 374 | rj = self._PrevAgent(ri,v1) 375 | index1 = self.agentNode2index[(rj,v1)] 376 | index2 = self.agentNode2index[(ri,v2)] 377 | # print("AddIe(",index1,index2,")") 378 | self.cost_mat[index1,index2] = -self.bigM # directed 379 | self.setIe.add(tuple([v1,v2,ri])) 380 | return 1 381 | 382 | def AddOe(self, v1, v2, ri): 383 | """ 384 | input ri is used. Oe is imposed on the transformed graph. 385 | """ 386 | rj = self._PrevAgent(ri,v1) 387 | index1 = self.agentNode2index[(rj,v1)] 388 | index2 = self.agentNode2index[(ri,v2)] 389 | # print("AddOe(",index1,index2,")") 390 | self.cost_mat[index1,index2] = self.infM # directed 391 | self.setOe.add(tuple([v1,v2,ri])) 392 | return 1 393 | -------------------------------------------------------------------------------- /libmcpf/seq_msmp.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | Oeffentlich fuer: RSS22 5 | """ 6 | 7 | import numpy as np 8 | import time 9 | import copy 10 | import os, sys 11 | sys.path.append(os.path.abspath('../')) # to import context 12 | 13 | import context 14 | import tsp_wrapper 15 | import tf_mtsp 16 | import common as cm 17 | 18 | class SeqMSMP(object): 19 | """ 20 | The target sequencing procedure for MSMP. 21 | 22 | APIs required by the kbestTSP.py are 23 | - InitMat() 24 | - GenFile() 25 | - CallLKH() 26 | - ResultFromFile() 27 | - ChangeCost() 28 | 29 | """ 30 | def __init__(self, grid, Vo, Vt, Vd, configs): 31 | super(SeqMSMP, self).__init__() 32 | self.grid = grid 33 | self.Vo = Vo 34 | self.Vt = Vt 35 | self.Vd = Vd 36 | self.V = self.Vo + self.Vt + self.Vd 37 | self.N = len(self.Vo) 38 | self.M = len(self.Vt) 39 | self.n2i = dict() # node ID to node index 40 | for i in range(len(self.V)): 41 | self.n2i[self.V[i]] = i 42 | self.infM = 999999 43 | self.configs = configs 44 | self.tsp_exe = configs["tsp_exe"] 45 | self.setIe = set() 46 | self.setOe = set() 47 | return 48 | 49 | def InitMat(self): 50 | self.spMat = cm.getTargetGraph(self.grid,self.Vo,self.Vt,self.Vd) # target graph, fully connected. 51 | self.original_spMat = copy.deepcopy(self.spMat) 52 | self.bigM = np.max(self.spMat)*(self.N + self.M) # totally N+M edges in a mTSP solution, not 2N+M. 53 | # print("bigM set to ", self.bigM) 54 | return 55 | 56 | def ChangeCost(self, v1, v2, d, ri): 57 | """ 58 | input ri is not in use in MSMP. 59 | A generic interface for changing (directed) edge cost. 60 | """ 61 | i = self.n2i[v1] 62 | j = self.n2i[v2] 63 | if d > self.bigM: 64 | self.spMat[i,j] = self.bigM # directed 65 | else: 66 | self.spMat[i,j] = d # directed 67 | return 1 68 | 69 | def AddIe(self, v1, v2, ri): 70 | """ 71 | input ri is not in use in MSMP. 72 | """ 73 | self.setIe.add(tuple([v1,v2,ri])) 74 | i = self.n2i[v1] 75 | j = self.n2i[v2] 76 | self.spMat[i,j] = -self.bigM # directed 77 | return 1 78 | 79 | def AddOe(self, v1, v2, ri): 80 | """ 81 | input ri is not in use in MSMP. 82 | """ 83 | self.setOe.add(tuple([v1,v2,ri])) 84 | i = self.n2i[v1] 85 | j = self.n2i[v2] 86 | self.spMat[i,j] = self.infM # directed 87 | return 1 88 | 89 | def Solve(self): 90 | """ 91 | solve the instance and return the results. 92 | """ 93 | tf_mat = tf_mtsp.tf_MDMTHPP(self.spMat, self.N, self.M, self.bigM, self.infM) 94 | 95 | if_atsp = True 96 | problem_str = "runtime_files/msmp" 97 | if problem_str in self.configs: 98 | problem_str = self.configs["problem_str"] 99 | tsp_wrapper.gen_tsp_file(problem_str, tf_mat, if_atsp) 100 | res = tsp_wrapper.invoke_lkh(self.tsp_exe, problem_str) 101 | mtsp_tours = tf_mtsp.tf_MDMTHPP_tours(res[0], self.N, self.M) 102 | seqs_dict = dict() 103 | for k in mtsp_tours: 104 | seqs_dict[k] = list() 105 | for node_idx in mtsp_tours[k]: 106 | seqs_dict[k].append(self.V[node_idx]) # from index to node ID. 107 | mtsp_costs, total_cost = tf_mtsp.tf_MDMTHPP_costs(mtsp_tours, self.original_spMat) # use the original spMat 108 | return 1, res[2], seqs_dict, mtsp_costs 109 | -------------------------------------------------------------------------------- /pytspbridge/.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 98 | __pypackages__/ 99 | 100 | # Celery stuff 101 | celerybeat-schedule 102 | celerybeat.pid 103 | 104 | # SageMath parsed files 105 | *.sage.py 106 | 107 | # Environments 108 | .env 109 | .venv 110 | env/ 111 | venv/ 112 | ENV/ 113 | env.bak/ 114 | venv.bak/ 115 | 116 | # Spyder project settings 117 | .spyderproject 118 | .spyproject 119 | 120 | # Rope project settings 121 | .ropeproject 122 | 123 | # mkdocs documentation 124 | /site 125 | 126 | # mypy 127 | .mypy_cache/ 128 | .dmypy.json 129 | dmypy.json 130 | 131 | # Pyre type checker 132 | .pyre/ 133 | 134 | # pytype static type analyzer 135 | .pytype/ 136 | 137 | # Cython debug symbols 138 | cython_debug/ 139 | -------------------------------------------------------------------------------- /pytspbridge/tf_mtsp.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren. 3 | All Rights Reserved. 4 | Note: Transformation-based method for Multiple Traveling Salesman Problem (mTSP). 5 | Include both MDMTSP and HMDMTSP. 6 | """ 7 | 8 | import numpy as np 9 | 10 | 11 | def tf_MDMTHPP(spMat, N, M, bigM, infM): 12 | """ 13 | Multi-Depot Multi-Terminal Hamiltonian Path Problem. 14 | All terminals are unassigned! 15 | In other words, there is no agent-target assignment constraints. 16 | Based on the tf algorithm for the MDMTSP problem. 17 | 18 | Input: 19 | N = #agents 20 | M = #targets 21 | spMat = the shortest path distance between any pair of nodes in depots U targets U destinations. 22 | and there should be N depots (i.e. starts), M targets, N destinations. 23 | The ID of depots should be 0 ~ N-1; 24 | The ID of targets should be N ~ N+M-1; 25 | The ID of destinations should be N+M ~ N+M+N-1. 26 | bigM = an upper bound of any possible tour cost. the lower the better. 27 | infM = the large integer that marks infinity. the large the better (avoid overflow). 28 | Ie, edges that must be included. 29 | Oe, edges that must be excluded. 30 | Ie and Oe are args that support K-best-TSP. 31 | 32 | Output: 33 | A cost matrix after transformation. This matrix is of size (N+M+N)x(N+M+N). 34 | """ 35 | nn = 2*N+M 36 | cmat = np.zeros((nn, nn)) 37 | for ix in range(nn): 38 | for iy in range(nn): 39 | # dest to others 40 | if ix >= N+M: # ix is a destination 41 | if iy < N: # iy is a start 42 | # dest to start, zero-cost edge 43 | cmat[ix,iy] = 0 44 | continue 45 | else: 46 | # dest to others, inf cost 47 | cmat[ix,iy]= infM 48 | continue 49 | elif iy < N: # iy is start 50 | if ix < N+M: # ix is not a destination 51 | cmat[ix,iy] = infM 52 | continue 53 | else: # all other cases 54 | cmat[ix,iy] = spMat[ix,iy] 55 | # end for iy 56 | # end for ix 57 | 58 | # for ie in Ie: # all edges that must be included. 59 | # cmat[ie[0], ie[1]] = -bigM 60 | # for oe in Oe: 61 | # cmat[oe[0], oe[1]] = infM 62 | 63 | return cmat 64 | 65 | def tf_MDMTHPP_tours(s, N, M): 66 | """ 67 | transform a single-agent TSP tour back to multiple agent tours. 68 | 69 | Input: 70 | s = single agent tour 71 | N = #agents 72 | M = #targets 73 | cost = cost of single-agent solution s. 74 | 75 | Output: 76 | a dict about each agent's tour, agent ID 0~(N-1), 77 | each agent's tour = list of node indices in the cost matrix for LKH. 78 | """ 79 | seq = list() 80 | seqs_dict = dict() 81 | first = True 82 | for ix in range(len(s)): 83 | n = s[ix] 84 | if (n < N) and (not first): 85 | # break here, start a new sequence of targets 86 | if len(seq) > 0: 87 | seqs_dict[seq[0]] = seq 88 | seq = list() 89 | seq.append(n) 90 | first = False 91 | # end if 92 | if len(seq) > 0: 93 | seqs_dict[seq[0]] = seq 94 | return seqs_dict 95 | 96 | def tf_MDMTHPP_costs(seqs_dict, spMat): 97 | """ 98 | recover each agent's cost based on each agent's tour (seqs_dict) 99 | """ 100 | cost_dict = dict() 101 | total_cost = 0 102 | for k in seqs_dict: 103 | cost_dict[k] = 0 104 | for idx in range(len(seqs_dict[k])-1): 105 | cost_dict[k] += spMat[seqs_dict[k][idx], seqs_dict[k][idx+1]] 106 | total_cost += spMat[seqs_dict[k][idx], seqs_dict[k][idx+1]] 107 | return cost_dict, total_cost 108 | -------------------------------------------------------------------------------- /pytspbridge/tsp_solver/readme.txt: -------------------------------------------------------------------------------- 1 | This folder should contain the LKH solver. 2 | 3 | (executable) ./LKH-2.0.9/LKH -------------------------------------------------------------------------------- /pytspbridge/tsp_wrapper.py: -------------------------------------------------------------------------------- 1 | 2 | import subprocess 3 | import numpy as np 4 | import sys 5 | import os 6 | 7 | def gen_tsp_file(problem_str, cmat, if_atsp): 8 | """ 9 | tsp_fn = tsp file name, the output path. 10 | cmat = cost matrix 11 | if_atsp = If the instance is ATSP (1 or True) or TSP (0 or False) 12 | """ 13 | nx,ny = cmat.shape 14 | if nx != ny or nx == 0: 15 | sys.exit("[ERROR] _gen_tsp_file, input cmat is not a square matrix or of size 0!") 16 | with open(problem_str+".tsp", mode="w+") as ftsp: 17 | ### generate file headers 18 | if if_atsp: 19 | ftsp.writelines(["NAME : mtspf\n", "COMMENT : file for mtspf test\n", "TYPE : ATSP\n"]) 20 | else: 21 | ftsp.writelines(["NAME : mtspf\n", "COMMENT : file for mtspf test\n", "TYPE : TSP\n"]) 22 | ftsp.write("DIMENSION : "+str(nx)+ "\n") 23 | ftsp.writelines(["EDGE_WEIGHT_TYPE : EXPLICIT\n", "EDGE_WEIGHT_FORMAT : FULL_MATRIX\n", "EDGE_WEIGHT_SECTION\n"]) 24 | ### generate cost matrix 25 | for ix in range(nx): 26 | nline = "" 27 | for iy in range(nx): 28 | nline = nline + str( int(cmat[(ix,iy)]) ) + " " 29 | ftsp.write(nline+"\n") 30 | ftsp.close() 31 | # end with 32 | return 0 33 | 34 | def gen_par_file(problem_str): 35 | with open(problem_str+".par", mode="w+") as fpar: 36 | fpar.writelines(["PROBLEM_FILE = "+problem_str+".tsp\n"]) 37 | fpar.writelines(["MOVE_TYPE = 5\n"]) 38 | fpar.writelines(["PATCHING_C = 3\n"]) 39 | fpar.writelines(["PATCHING_A = 2\n"]) 40 | fpar.writelines(["RUNS = 10\n"]) 41 | fpar.writelines(["OUTPUT_TOUR_FILE = "+problem_str+".tour\n"]) 42 | fpar.close() 43 | return 44 | 45 | def invoke_lkh(exe_path, problem_str): 46 | """ 47 | call LKH executable at exe_path 48 | problem_str = the path the identifies the problem, without any suffix. 49 | E.g. path/to/file/pr2392 (instead of pr2392.tsp) 50 | """ 51 | 52 | ### generate par file for LKH if it does not exist. 53 | if not os.path.isfile(problem_str+".par"): 54 | print("[INFO] invoke_lkh, generate par file...") 55 | gen_par_file(problem_str) 56 | 57 | ### call LKH 58 | lb_cost = np.inf # meaningless number 59 | cmd = [exe_path, problem_str+".par"] 60 | process = subprocess.Popen(cmd, stdout=subprocess.PIPE) 61 | for line in process.stdout: 62 | # print(line) 63 | line_str = line.decode('UTF-8') 64 | if line_str[0:11] == "Lower bound": 65 | temp1 = line_str.split(",") 66 | temp2 = temp1[0].split("=") 67 | lb_cost = float(temp2[1]) # lower bound from LKH. 68 | process.wait() # otherwise, subprocess run concurrently... 69 | 70 | ### get result 71 | res_file = problem_str+".tour" 72 | with open(res_file, mode="r") as fres: 73 | lines = fres.readlines() 74 | l1all = lines[1].split("=") 75 | tour_cost = int(l1all[1]) 76 | ix = 6 77 | val = int(lines[ix]) 78 | tour = [] 79 | while val != -1: 80 | tour.append(val-1) 81 | # tour.append(val) # LKH node index start from 1 while cmat index in python start from 0. 82 | ix = ix + 1 83 | val = int(lines[ix]) 84 | return tour, tour_cost, lb_cost 85 | 86 | def invoke_concorde(exe_path, problem_str): 87 | """ 88 | TODO 89 | """ 90 | sys.exit("[ERROR] Not Implemented!") 91 | return 92 | 93 | 94 | def reorderTour(tour, s): 95 | """ 96 | find s in the tour [AAA, s, BBB] and reorder the tour to make s be the first node. 97 | [s,BBB,AAA] 98 | """ 99 | found = 0 100 | for i,v in enumerate(tour): 101 | if v == s: 102 | found = 1 103 | break 104 | if not found: 105 | print("[ERROR] s = ", s, " is not in the tour ", tour) 106 | sys.exit("[ERROR] reorderTour fail to find s!") 107 | return tour[i:]+tour[:i] -------------------------------------------------------------------------------- /run_example_cbss.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Zhongqiang (Richard) Ren 3 | All Rights Reserved. 4 | ABOUT: Entrypoint to the code. 5 | Oeffentlich fuer: RSS22 6 | """ 7 | 8 | import context 9 | import time 10 | import numpy as np 11 | import random 12 | import cbss_msmp 13 | import cbss_mcpf 14 | 15 | import common as cm 16 | 17 | def run_CBSS_MSMP(): 18 | """ 19 | fully anonymous case, no assignment constraints. 20 | """ 21 | print("------run_CBSS_MSMP------") 22 | ny = 10 23 | nx = 10 24 | grids = np.zeros((ny,nx)) 25 | # Image coordinate is used. Think about the matrix as a 2d image with the origin at the upper left corner. 26 | # Row index is y and column index is x. 27 | # For example, in the matrix, grids[3,4] means the vertex with coordinate y=3,x=4 (row index=3, col index=4). 28 | grids[5,3:7] = 1 # obstacles 29 | 30 | # The following are vertex IDs. 31 | # For a vertex v with (x,y) coordinate in a grid of size (Lx,Ly), the ID of v is y*Lx+x. 32 | starts = [11,22,33,88,99] 33 | targets = [40,38,27,66,72,81,83] 34 | dests = [19,28,37,46,69] 35 | 36 | configs = dict() 37 | configs["problem_str"] = "msmp" 38 | configs["tsp_exe"] = "./pytspbridge/tsp_solver/LKH-2.0.9/LKH" 39 | configs["time_limit"] = 60 40 | configs["eps"] = 0.0 41 | res_dict = cbss_msmp.RunCbssMSMP(grids, starts, targets, dests, configs) 42 | 43 | print(res_dict) 44 | 45 | return 46 | 47 | def run_CBSS_MCPF(): 48 | """ 49 | With assignment constraints. 50 | """ 51 | print("------run_CBSS_MCPF------") 52 | ny = 10 53 | nx = 10 54 | grids = np.zeros((ny,nx)) 55 | grids[5,3:7] = 1 # obstacles 56 | 57 | starts = [11,22,33,88,99] 58 | targets = [72,81,83,40,38,27,66] 59 | dests = [46,69,19,28,37] 60 | 61 | ac_dict = dict() 62 | ri = 0 63 | for k in targets: 64 | ac_dict[k] = set([ri,ri+1]) 65 | ri += 1 66 | if ri >= len(starts)-1: 67 | break 68 | ri = 0 69 | for k in dests: 70 | ac_dict[k] = set([ri]) 71 | ri += 1 72 | print("Assignment constraints : ", ac_dict) 73 | 74 | configs = dict() 75 | configs["problem_str"] = "msmp" 76 | configs["tsp_exe"] = "./pytspbridge/tsp_solver/LKH-2.0.9/LKH" 77 | configs["time_limit"] = 60 78 | configs["eps"] = 0.0 79 | 80 | res_dict = cbss_mcpf.RunCbssMCPF(grids, starts, targets, dests, ac_dict, configs) 81 | 82 | print(res_dict) 83 | 84 | return 85 | 86 | 87 | if __name__ == '__main__': 88 | print("begin of main") 89 | 90 | run_CBSS_MSMP() 91 | 92 | run_CBSS_MCPF() 93 | 94 | print("end of main") 95 | -------------------------------------------------------------------------------- /runtime_files/mcpf.par: -------------------------------------------------------------------------------- 1 | PROBLEM_FILE = runtime_files/mcpf.tsp 2 | MOVE_TYPE = 5 3 | PATCHING_C = 3 4 | PATCHING_A = 2 5 | RUNS = 10 6 | OUTPUT_TOUR_FILE = runtime_files/mcpf.tour 7 | -------------------------------------------------------------------------------- /runtime_files/mcpf.tour: -------------------------------------------------------------------------------- 1 | NAME : mtspf.2367.tour 2 | COMMENT : Length = 2367 3 | COMMENT : Found by LKH [Keld Helsgaun] Tue Jul 23 19:20:56 2024 4 | TYPE : TOUR 5 | DIMENSION : 33 6 | TOUR_SECTION 7 | 1 8 | 29 9 | 2 10 | 7 11 | 6 12 | 8 13 | 9 14 | 25 15 | 26 16 | 27 17 | 28 18 | 24 19 | 30 20 | 5 21 | 18 22 | 14 23 | 15 24 | 16 25 | 17 26 | 33 27 | 4 28 | 11 29 | 10 30 | 12 31 | 13 32 | 32 33 | 3 34 | 21 35 | 22 36 | 23 37 | 19 38 | 20 39 | 31 40 | -1 41 | EOF 42 | -------------------------------------------------------------------------------- /runtime_files/mcpf.tsp: -------------------------------------------------------------------------------- 1 | NAME : mtspf 2 | COMMENT : file for mtspf test 3 | TYPE : ATSP 4 | DIMENSION : 33 5 | EDGE_WEIGHT_TYPE : EXPLICIT 6 | EDGE_WEIGHT_FORMAT : FULL_MATRIX 7 | EDGE_WEIGHT_SECTION 8 | 999999 999999 999999 999999 999999 199 999999 999999 999999 999999 999999 999999 999999 201 999999 999999 999999 999999 199 999999 999999 999999 999999 202 999999 999999 999999 999999 200 999999 999999 999999 999999 9 | 999999 999999 999999 999999 999999 999999 197 199 999999 999999 999999 999999 999999 999999 199 999999 999999 999999 999999 197 999999 999999 999999 999999 200 999999 999999 999999 999999 203 999999 999999 999999 10 | 999999 999999 999999 999999 999999 999999 999999 999999 199 199 999999 999999 999999 999999 999999 197 999999 999999 999999 999999 197 999999 999999 999999 999999 200 999999 999999 999999 999999 200 999999 999999 11 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 197 204 999999 999999 999999 999999 197 999999 999999 999999 999999 199 999999 999999 999999 999999 196 999999 999999 999999 999999 198 999999 12 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 206 999999 999999 999999 999999 199 999999 999999 999999 999999 201 999999 999999 999999 999999 198 999999 999999 999999 999999 200 13 | 999999 999999 999999 999999 999999 999999 0 194 999999 999999 999999 999999 999999 999999 202 999999 999999 999999 999999 202 999999 999999 999999 999999 197 999999 999999 999999 999999 200 999999 999999 999999 14 | 999999 999999 999999 999999 999999 0 999999 999999 999999 999999 999999 999999 999999 202 999999 999999 999999 999999 202 999999 999999 999999 999999 197 999999 999999 999999 999999 199 999999 999999 999999 999999 15 | 999999 999999 999999 999999 999999 999999 999999 999999 0 194 999999 999999 999999 999999 999999 204 999999 999999 999999 999999 204 999999 999999 999999 999999 199 999999 999999 999999 999999 207 999999 999999 16 | 999999 999999 999999 999999 999999 999999 194 0 999999 999999 999999 999999 999999 999999 204 999999 999999 999999 999999 204 999999 999999 999999 999999 199 999999 999999 999999 999999 202 999999 999999 999999 17 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 199 999999 999999 999999 999999 202 999999 999999 999999 999999 202 999999 999999 999999 999999 197 999999 999999 999999 999999 203 999999 18 | 999999 999999 999999 999999 999999 999999 999999 999999 194 0 999999 999999 999999 999999 999999 202 999999 999999 999999 999999 202 999999 999999 999999 999999 197 999999 999999 999999 999999 205 999999 999999 19 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 999999 999999 999999 999999 201 999999 999999 999999 999999 201 999999 999999 999999 999999 200 999999 999999 999999 999999 200 20 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 199 0 999999 999999 999999 999999 201 999999 999999 999999 999999 201 999999 999999 999999 999999 200 999999 999999 999999 999999 202 999999 21 | 999999 999999 999999 999999 999999 999999 202 204 999999 999999 999999 999999 999999 999999 0 999999 999999 999999 999999 194 999999 999999 999999 999999 197 999999 999999 999999 999999 196 999999 999999 999999 22 | 999999 999999 999999 999999 999999 999999 999999 999999 204 202 999999 999999 999999 999999 999999 0 999999 999999 999999 999999 194 999999 999999 999999 999999 197 999999 999999 999999 999999 195 999999 999999 23 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 202 201 999999 999999 999999 999999 0 999999 999999 999999 999999 194 999999 999999 999999 999999 197 999999 999999 999999 999999 193 999999 24 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 201 999999 999999 999999 999999 0 999999 999999 999999 999999 194 999999 999999 999999 999999 197 999999 999999 999999 999999 193 25 | 999999 999999 999999 999999 999999 202 999999 999999 999999 999999 999999 999999 999999 0 999999 999999 999999 999999 194 999999 999999 999999 999999 197 999999 999999 999999 999999 195 999999 999999 999999 999999 26 | 999999 999999 999999 999999 999999 999999 202 204 999999 999999 999999 999999 999999 999999 194 999999 999999 999999 999999 0 999999 999999 999999 999999 197 999999 999999 999999 999999 198 999999 999999 999999 27 | 999999 999999 999999 999999 999999 999999 999999 999999 204 202 999999 999999 999999 999999 999999 194 999999 999999 999999 999999 0 999999 999999 999999 999999 197 999999 999999 999999 999999 195 999999 999999 28 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 202 201 999999 999999 999999 999999 194 999999 999999 999999 999999 0 999999 999999 999999 999999 197 999999 999999 999999 999999 193 999999 29 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 201 999999 999999 999999 999999 194 999999 999999 999999 999999 0 999999 999999 999999 999999 197 999999 999999 999999 999999 193 30 | 999999 999999 999999 999999 999999 202 999999 999999 999999 999999 999999 999999 999999 194 999999 999999 999999 999999 0 999999 999999 999999 999999 197 999999 999999 999999 999999 195 999999 999999 999999 999999 31 | 999999 999999 999999 999999 999999 999999 197 199 999999 999999 999999 999999 999999 999999 197 999999 999999 999999 999999 197 999999 999999 999999 999999 0 999999 999999 999999 999999 195 999999 999999 999999 32 | 999999 999999 999999 999999 999999 999999 999999 999999 199 197 999999 999999 999999 999999 999999 197 999999 999999 999999 999999 197 999999 999999 999999 999999 0 999999 999999 999999 999999 200 999999 999999 33 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 197 200 999999 999999 999999 999999 197 999999 999999 999999 999999 197 999999 999999 999999 999999 0 999999 999999 999999 999999 198 999999 34 | 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 200 999999 999999 999999 999999 197 999999 999999 999999 999999 197 999999 999999 999999 999999 0 999999 999999 999999 999999 196 35 | 999999 999999 999999 999999 999999 197 999999 999999 999999 999999 999999 999999 999999 197 999999 999999 999999 999999 197 999999 999999 999999 999999 0 999999 999999 999999 999999 196 999999 999999 999999 999999 36 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 999999 999999 999999 999999 37 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 999999 999999 999999 38 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 999999 999999 39 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 999999 40 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 0 41 | -------------------------------------------------------------------------------- /runtime_files/msmp.par: -------------------------------------------------------------------------------- 1 | PROBLEM_FILE = runtime_files/msmp.tsp 2 | MOVE_TYPE = 5 3 | PATCHING_C = 3 4 | PATCHING_A = 2 5 | RUNS = 10 6 | OUTPUT_TOUR_FILE = runtime_files/msmp.tour 7 | -------------------------------------------------------------------------------- /runtime_files/msmp.tour: -------------------------------------------------------------------------------- 1 | NAME : mtspf.43.tour 2 | COMMENT : Length = 43 3 | COMMENT : Found by LKH [Keld Helsgaun] Tue Jul 23 19:20:56 2024 4 | TYPE : TOUR 5 | DIMENSION : 17 6 | TOUR_SECTION 7 | 1 8 | 6 9 | 10 10 | 11 11 | 12 12 | 9 13 | 15 14 | 3 15 | 16 16 | 2 17 | 8 18 | 13 19 | 4 20 | 7 21 | 14 22 | 5 23 | 17 24 | -1 25 | EOF 26 | -------------------------------------------------------------------------------- /runtime_files/msmp.tsp: -------------------------------------------------------------------------------- 1 | NAME : mtspf 2 | COMMENT : file for mtspf test 3 | TYPE : ATSP 4 | DIMENSION : 17 5 | EDGE_WEIGHT_TYPE : EXPLICIT 6 | EDGE_WEIGHT_FORMAT : FULL_MATRIX 7 | EDGE_WEIGHT_SECTION 8 | 999999 999999 999999 999999 999999 4 9 7 10 7 7 9 8 8 8 8 13 9 | 999999 999999 999999 999999 999999 4 7 5 8 5 7 7 8 6 6 6 11 10 | 999999 999999 999999 999999 999999 4 5 5 8 5 7 7 8 6 4 4 9 11 | 999999 999999 999999 999999 999999 12 5 7 4 7 7 5 8 6 6 6 3 12 | 999999 999999 999999 999999 999999 14 7 9 6 9 9 7 8 8 8 8 3 13 | 999999 999999 999999 999999 999999 0 9 9 8 5 5 7 12 10 8 6 11 14 | 999999 999999 999999 999999 999999 9 0 2 5 10 12 10 3 1 1 3 4 15 | 999999 999999 999999 999999 999999 9 2 0 5 10 12 10 3 1 1 3 6 16 | 999999 999999 999999 999999 999999 8 5 5 0 5 7 5 8 6 4 4 3 17 | 999999 999999 999999 999999 999999 5 10 10 5 0 2 2 13 11 9 7 8 18 | 999999 999999 999999 999999 999999 5 12 12 7 2 0 2 15 13 11 9 10 19 | 999999 999999 999999 999999 999999 7 10 10 5 2 2 0 13 11 9 9 8 20 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 21 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 22 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 23 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 24 | 0 0 0 0 0 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 999999 25 | -------------------------------------------------------------------------------- /runtime_files/placeholder.txt: -------------------------------------------------------------------------------- 1 | placeholder --------------------------------------------------------------------------------