├── .DS_Store ├── MASAR ├── Crazyflie.JPG ├── LICENSE ├── README.md ├── TurtleBot.png ├── action_selection.py ├── agent.py ├── data10.json ├── env1.1.py ├── gridworld_multi_agent1_1.py ├── maps.py ├── multi_agent_Q_learning_1R.hdf5 ├── multi_agent_Q_learning_2R.hdf5 ├── multi_agent_Q_learning_2R_.hdf5 ├── multi_agent_Q_learning_4RS_1000episodes.hdf5 ├── multi_agent_Q_learning_4RS_1000episodes_old.hdf5 ├── multi_agent_Q_learning_FindSurvivors.hdf5 ├── multiagent_search.py ├── network.py ├── search_algorithms.py ├── typhoon.jpg ├── victim.png ├── visualizer1.1.py └── wall.png ├── MRTA_new ├── Clustering.py ├── Crazyflie.JPG ├── MRTA.hdf5 ├── MRTA_FindSurvivors_env_map1_2R20V.hdf5 ├── MRTA_FindSurvivors_env_map1_3R30V.hdf5 ├── MRTA_FindSurvivors_env_map1_4R40V.hdf5 ├── MRTA_FindSurvivors_env_map1_5R50V.hdf5 ├── PerfAnalysis.py ├── ReqmentAnalysis.py ├── Times_msmrta_FindSurvivors_env_map0_30R100V.hdf5 ├── Times_msmrta_FindSurvivors_env_map1_30R100V.hdf5 ├── Times_msmrta_FindSurvivors_env_map2_30R100V.hdf5 ├── Times_msmrta_FindSurvivors_env_map3_30R100V.hdf5 ├── TurtleBot.png ├── a_star.py ├── gridworld_multi_agent1_1.py ├── main.py ├── maps.py ├── robot.py ├── typhoon.jpg ├── victim.png ├── victim.py └── wall.png ├── PathPlanning ├── Crazyflie.JPG ├── TurtleBot.png ├── a_star.py ├── gridworld_multi_agent1_1.py ├── typhoon.jpg ├── victim.png └── wall.png └── README.md /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/.DS_Store -------------------------------------------------------------------------------- /MASAR/Crazyflie.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/Crazyflie.JPG -------------------------------------------------------------------------------- /MASAR/LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /MASAR/README.md: -------------------------------------------------------------------------------- 1 | # Multi-Agent-Search-and-Rescue 2 | Multi-Agent Search and Rescue using Q-learning 3 | -------------------------------------------------------------------------------- /MASAR/TurtleBot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/TurtleBot.png -------------------------------------------------------------------------------- /MASAR/action_selection.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def greedy(q): 5 | return np.random.choice(np.flatnonzero(q == np.max(q))) 6 | 7 | 8 | def eps_greedy(q, actions, epsilon=0.05): 9 | if np.random.random() < epsilon: 10 | idx = np.random.randint(len(actions)) 11 | else: 12 | idx = greedy(q) 13 | return idx 14 | 15 | 16 | def ucb(q, c, step, N): 17 | ucb_eq = q + c * np.sqrt(np.log(step) / N) 18 | return greedy(ucb_eq) 19 | 20 | def Boltzmann(q, t=0.4): 21 | return np.exp(q / t) / np.sum(np.exp(q / t)) -------------------------------------------------------------------------------- /MASAR/agent.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from search_algorithms import SearchAlgorithms 3 | 4 | 5 | class Agent(SearchAlgorithms): 6 | def __init__(self, agent_id, role, vfd, max_vfd, speed, init_pos, num_actions, num_rows, num_cols): 7 | super(Agent, self).__init__(max_vfd, init_pos, num_actions, num_rows, num_cols) 8 | self.Role = role # can be 'r': rescuer, 's': scout, 'rs': rescuer and scout, 'v': victim 9 | self.id = agent_id # an identification for the agent 10 | self.VisualField = vfd 11 | self.max_VisualField = max_vfd 12 | self.vfd_status = np.ones((2 * self.VisualField + 1, 2 * self.VisualField + 1), dtype=bool) 13 | 14 | self.curr_Sensation = [np.nan, np.nan] 15 | self.old_Sensation = self.curr_Sensation 16 | 17 | self.curr_Index = None 18 | self.old_Index = self.curr_Index 19 | 20 | self.CanSeeIt = False 21 | self.Finish = False 22 | self.Convergence = False 23 | self.First = True 24 | self.wereHere = np.ones((num_rows, num_cols)) 25 | self.Speed = speed # is the number of cells the agent can move in one time-step 26 | 27 | self.init_pos = init_pos 28 | self.curr_Pos = self.init_pos 29 | self.old_Pos = self.curr_Pos 30 | 31 | self.Traj = [] # Trajectory of the agent locations 32 | self.VFD_status_history = [] 33 | self.RewHist = [] 34 | self.RewHist_seen = [] 35 | self.RewSum = [] # Keeps track of the rewards in each step 36 | self.RewSum_seen = [] # Keeps track of the rewards after receiving first data 37 | self.Steps = [] # Keeps track of the steps in each step 38 | self.Steps_seen = [] # Keeps track of the steps after receiving first data 39 | 40 | self.num_actions = num_actions 41 | self.num_rows = num_rows 42 | self.num_cols = num_cols 43 | self.t_step_seen = 0 44 | self.action = None 45 | self.reward = None 46 | self.probs = np.nan 47 | self.Q = np.zeros(((2 * self.max_VisualField + 1) ** 2 + 1, self.num_actions)) 48 | self.Q_hist = self.Q 49 | 50 | def reset(self): 51 | self.old_Pos = self.init_pos 52 | self.curr_Pos = self.init_pos 53 | self.old_Sensation = [np.nan, np.nan] 54 | self.curr_Sensation = [np.nan, np.nan] 55 | self.CanSeeIt = False 56 | self.Finish = False 57 | self.Convergence = False 58 | self.First = True 59 | self.t_step_seen = 0 60 | self.RewHist = [] 61 | self.RewHist_seen = [] 62 | self.Traj = [] 63 | self.VFD_status_history = [] 64 | self.wereHere = np.ones_like(self.wereHere) 65 | self.vfd_status = np.ones((2 * self.VisualField + 1, 2 * self.VisualField + 1), dtype=bool) 66 | 67 | def update_vfd(self, env_map): 68 | # rescuer visual field depth 69 | self.vfd_status = np.ones((2 * self.VisualField + 1, 2 * self.VisualField + 1), dtype=bool) 70 | vfd_j = 0 71 | for j in range(int(max(self.old_Pos[1] - self.VisualField, 0)), 72 | int(min(self.num_cols, self.old_Pos[1] + self.VisualField + 1))): 73 | vfd_i = 0 74 | for i in range(int(max(self.old_Pos[0] - self.VisualField, 0)), 75 | int(min(self.num_rows, self.old_Pos[0] + self.VisualField + 1))): 76 | if env_map[i, j] == 1: 77 | self.vfd_status[vfd_i, vfd_j] = False 78 | # Down or Up 79 | if i - self.old_Pos[0] > 0 and j - self.old_Pos[1] == 0: 80 | self.vfd_status[vfd_i:, vfd_j] = False 81 | elif i - self.old_Pos[0] < 0 and j - self.old_Pos[1] == 0: 82 | self.vfd_status[:min(vfd_i+1, self.VisualField), vfd_j] = False 83 | # Right or Left 84 | elif i - self.old_Pos[0] == 0 and j - self.old_Pos[1] > 0: 85 | self.vfd_status[vfd_i, vfd_j:] = False 86 | elif i - self.old_Pos[0] == 0 and j - self.old_Pos[1] < 0: 87 | self.vfd_status[vfd_i, :min(vfd_j+1, self.VisualField)] = False 88 | # Northeast or Northwest 89 | elif i - self.old_Pos[0] < 0 and j - self.old_Pos[1] > 0: 90 | self.vfd_status[:min(vfd_i+1, self.VisualField), vfd_j:] = False 91 | elif i - self.old_Pos[0] < 0 and j - self.old_Pos[1] < 0: 92 | self.vfd_status[:min(vfd_i+1, self.VisualField), :min(vfd_j+1, self.VisualField)] = False 93 | # Southeast or Southwest 94 | elif i - self.old_Pos[0] > 0 and j - self.old_Pos[1] > 0: 95 | self.vfd_status[vfd_i:, vfd_j:] = False 96 | elif i - self.old_Pos[0] > 0 and j - self.old_Pos[1] < 0: 97 | self.vfd_status[vfd_i:, :min(vfd_j+1, self.VisualField)] = False 98 | 99 | vfd_i += 1 100 | vfd_j += 1 101 | 102 | def update_sensation(self, index, raw_sensation, sensation_evaluate, pos2pos, net_adj_mat, adj_mat): 103 | 104 | next_sensation = [np.nan, np.nan] 105 | self.CanSeeIt = False 106 | 107 | if any(sensation_evaluate[index, :]): 108 | which_victim = np.argwhere(sensation_evaluate[index, :])[0][0] 109 | for victim in np.argwhere(sensation_evaluate[index, :])[0]: 110 | if (np.linalg.norm(raw_sensation[index, victim, :]) < 111 | np.linalg.norm(raw_sensation[index, which_victim, :])): 112 | which_victim = victim 113 | next_sensation = raw_sensation[index, which_victim, :] 114 | self.CanSeeIt = True 115 | 116 | elif not all(np.isnan(net_adj_mat[index, :])): 117 | temp_sensation = next_sensation.copy() 118 | num_scouts = np.sum(adj_mat[index, :]) 119 | for ns in range(int(num_scouts)): 120 | curr_scout = np.argwhere(adj_mat[index, :])[ns] 121 | if any(sensation_evaluate[curr_scout, :][0].tolist()): 122 | which_victim = np.argwhere(sensation_evaluate[curr_scout, :][0])[0] 123 | for victim in np.argwhere(sensation_evaluate[curr_scout, :][0]): 124 | if (np.linalg.norm(raw_sensation[curr_scout, victim, :]) < 125 | np.linalg.norm(raw_sensation[curr_scout, which_victim, :])): 126 | which_victim = victim 127 | 128 | next_sensation[0] = (pos2pos[curr_scout, index][0][0] + 129 | raw_sensation[curr_scout, which_victim, :][0][0]) 130 | next_sensation[1] = (pos2pos[curr_scout, index][0][1] + 131 | raw_sensation[curr_scout, which_victim, :][0][1]) 132 | self.CanSeeIt = True 133 | 134 | if np.linalg.norm(temp_sensation) < np.linalg.norm(next_sensation): 135 | next_sensation = temp_sensation.copy() 136 | else: 137 | temp_sensation = next_sensation.copy() 138 | 139 | return next_sensation 140 | 141 | def sensation2index(self, sensation, max_vfd): 142 | if self.CanSeeIt: 143 | index = ((sensation[0] + max_vfd) * (2 * max_vfd + 1) + (sensation[1] + max_vfd)) 144 | else: 145 | index = (2 * max_vfd + 1) ** 2 146 | 147 | return int(index) 148 | 149 | def rescue_accomplished(self, rescue_team_Hist, agent, adj_mat): 150 | if (((self.old_Sensation[0] == 0 and self.old_Sensation[1] == 0) or 151 | (self.curr_Sensation[0] == 0 and self.curr_Sensation[1] == 0)) and 152 | 'r' in self.Role): 153 | self.Finish = True 154 | adj_mat = np.delete(adj_mat, rescue_team_Hist.index(agent), 0) 155 | adj_mat = np.delete(adj_mat, rescue_team_Hist.index(agent), 1) 156 | 157 | rescue_team_Hist.remove(agent) 158 | 159 | return rescue_team_Hist, adj_mat 160 | 161 | def victim_rescued(self, rescue_team_old_pos_list, rescue_team_curr_pos_list, 162 | rescue_team_role_list, victim, victims_Hist): 163 | for idx, rescuer_old_pos in enumerate(rescue_team_old_pos_list): 164 | if (((rescuer_old_pos[0] == self.old_Pos[0] and 165 | rescuer_old_pos[1] == self.old_Pos[1]) or 166 | (rescue_team_curr_pos_list[idx][0] == self.old_Pos[0] and 167 | rescue_team_curr_pos_list[idx][1] == self.old_Pos[1])) and 168 | 'r' in rescue_team_role_list[idx]): 169 | self.Finish = True 170 | victims_Hist.remove(victim) 171 | break # You already removed this victim, no need to check the rest of the list 172 | 173 | return victims_Hist 174 | 175 | def convergence_check(self, accuracy): 176 | 177 | if (np.abs(np.sum(self.Q - self.Q_hist) / 178 | (np.shape(self.Q)[0] * np.shape(self.Q)[1])) <= accuracy): 179 | self.Convergence = True 180 | 181 | return self.Convergence 182 | -------------------------------------------------------------------------------- /MASAR/env1.1.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import numpy as np 4 | import h5py 5 | import json 6 | 7 | from action_selection import eps_greedy 8 | from network import Network 9 | from agent import Agent 10 | 11 | NUM_EPISODES = 500 12 | NUM_RUNS = 100 13 | Multi_Runs = False 14 | # Actions 15 | FORWARD = 0 16 | BACKWARD = 1 17 | RIGHT = 2 18 | LEFT = 3 19 | ACTIONS = [FORWARD, BACKWARD, RIGHT, LEFT] 20 | num_Acts = len(ACTIONS) 21 | 22 | # Environment dimensions 23 | Row_num = 20 24 | Col_num = 20 25 | row_lim = Row_num - 1 26 | col_lim = Col_num - 1 27 | 28 | # r1 r2 29 | adj_mat_prior = np.array([[0, 0], 30 | [0, 0]], dtype=float) 31 | exp_name = 'FindSurvivors' 32 | 33 | # make the map from json file 34 | # with open('data10.json') as f: 35 | # data = json.load(f) 36 | # test = data['map'][0] 37 | # dim = data['dimensions'] 38 | # rows = dim[0]['rows'] 39 | # columns = dim[0]['columns'] 40 | # 41 | # env_map = np.zeros((rows, columns)) 42 | # 43 | # for cell in data['map']: 44 | # if cell['isWall'] == 'true': 45 | # env_map[cell['x'], cell['y']] = 1 46 | 47 | env_mat = np.zeros((Row_num, Col_num)) 48 | global env_map 49 | env_map = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 50 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 51 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 52 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 53 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 54 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 55 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 56 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 57 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 58 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 59 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 60 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 61 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 62 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 63 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 64 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 65 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 66 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 67 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 68 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]) 69 | # env_map=np.zeros((20, 20)) 70 | # Transition function (avoid walls) 71 | def movement(pos, action, speed): 72 | global env_map 73 | row = pos[0] 74 | col = pos[1] 75 | next_pos = pos.copy() 76 | if action == 0: # up 77 | next_pos = [max(row - speed, 0), col] 78 | elif action == 1: # down 79 | next_pos = [min(row + speed, row_lim), col] 80 | elif action == 2: # right 81 | next_pos = [row, min(col + speed, col_lim)] 82 | elif action == 3: # left 83 | next_pos = [row, max(col - speed, 0)] 84 | if env_map[next_pos[0], next_pos[1]] == 0: 85 | return next_pos 86 | else: 87 | return pos 88 | 89 | 90 | def reward_func(sensation_prime): 91 | if sensation_prime[0] == 0 and sensation_prime[1] == 0: 92 | re = 1 93 | else: 94 | re = -.1 95 | return re 96 | 97 | 98 | def q_learning(q, old_idx, curr_idx, re, act, alpha=0.8, gamma=0.9): 99 | q[old_idx, act] += alpha * (re + gamma * np.max(q[curr_idx, :]) - q[old_idx, act]) 100 | return q 101 | 102 | 103 | def env(accuracy=1e-15): 104 | global adj_mat_prior 105 | # Define the Network and the agent objects 106 | network = Network 107 | agent = Agent 108 | 109 | # Define the rescue team 110 | r1 = agent(0, 'r', 5, 5, 1, np.argwhere(env_map == 0)[np.random.randint(len(np.argwhere(env_map == 0)))], 111 | num_Acts, Row_num, Col_num) 112 | r2 = agent(1, 'r', 3, 3, 1, np.argwhere(env_map == 0)[np.random.randint(len(np.argwhere(env_map == 0)))], 113 | num_Acts, Row_num, Col_num) 114 | # s3 = agent(2, 's', 4, 4, 1, [row_lim, 0], num_Acts, Row_num, Col_num) 115 | # s4 = agent(3, 's', 4, 4, 1, [0, col_lim], num_Acts, Row_num, Col_num) 116 | # rs5 = agent(4, 'r', 4, Row_num, 1, [row_lim, col_lim], num_Acts, Row_num, Col_num) 117 | 118 | # Define the victims 119 | v1 = agent(0, 'v', 0, 0, 1, np.argwhere(env_map == 0)[np.random.randint(len(np.argwhere(env_map == 0)))], 120 | num_Acts, Row_num, Col_num) 121 | v2 = agent(1, 'v', 0, 0, 1, np.argwhere(env_map == 0)[np.random.randint(len(np.argwhere(env_map == 0)))], 122 | num_Acts, Row_num, Col_num) 123 | # v3 = agent(2, 'v', 0, 0, 1, [int(Row_num / 2) - 2, int(Col_num / 2) - 2], num_Acts, Row_num, Col_num) 124 | # v4 = agent(3, 'v', 0, 0, 1, [int(Row_num / 2) + 4, int(Col_num / 2) + 4], num_Acts, Row_num, Col_num) 125 | # v5 = agent(4, 'v', 0, 0, 1, [int(Row_num / 2) - 4, int(Col_num / 2) - 4], num_Acts, Row_num, Col_num) 126 | 127 | # List of objects 128 | rescue_team = [r1, r2] 129 | victims = [v1, v2] 130 | VFD_list = [] 131 | 132 | num_just_scouts = 0 133 | rescue_team_roles = [] 134 | 135 | 136 | for agent in rescue_team: 137 | rescue_team_roles.append(agent.Role) 138 | # List of the Visual Fields 139 | VFD_list.append(agent.VisualField) 140 | 141 | # Count the number of just scouts 142 | if agent.Role == 's': 143 | num_just_scouts += 1 144 | # eps = -1 145 | tic = time.time() 146 | # while True: 147 | for eps in range(NUM_EPISODES): 148 | rescue_team_Hist = rescue_team.copy() 149 | victims_Hist = victims.copy() 150 | adj_mat = adj_mat_prior.copy() 151 | 152 | agents_idx = [] 153 | for agent in rescue_team: 154 | agents_idx.append(agent.id) 155 | 156 | victims_idx = [] 157 | for victim in victims: 158 | victims_idx.append(victim.id) 159 | rescue_team_roles = np.array(rescue_team_roles, dtype=list) 160 | # eps += 1 161 | 162 | # Reset the agents flags, positions, etc 163 | for agent in rescue_team: 164 | agent.reset() 165 | # Reset the victims flags, positions, etc 166 | for victim in victims: 167 | victim.reset() 168 | 169 | t_step = 0 170 | # for _ in range(100): 171 | while True: 172 | num_rescue_team = len(rescue_team_Hist) 173 | num_victims = len(victims_Hist) 174 | 175 | net = network(adj_mat, num_rescue_team, num_victims) 176 | 177 | t_step += 1 178 | 179 | rescue_team_VFD_list = [] 180 | team_VFD_status = [] 181 | for agent in rescue_team_Hist: 182 | # List of the Visual Fields 183 | rescue_team_VFD_list.append(agent.VisualField) 184 | 185 | # Count the steps that agent could see a victim 186 | if agent.CanSeeIt: 187 | agent.t_step_seen += 1 188 | 189 | # Keeping track of the rescue team positions 190 | agent.Traj.append(agent.old_Pos) 191 | 192 | # Update VFD status 193 | agent.update_vfd(env_map) 194 | 195 | # Keep track of VFD status 196 | agent.VFD_status_history.append(agent.vfd_status) 197 | 198 | # VFD status for the team 199 | team_VFD_status.append(agent.vfd_status) 200 | 201 | # History of Q 202 | agent.Q_hist = agent.Q.copy() 203 | 204 | rescue_team_VFD_list = np.asarray(rescue_team_VFD_list) 205 | 206 | # Keep track of the victims positions 207 | # Make a list of the victims old positions 208 | victims_old_pos_list = [] 209 | for victim in victims_Hist: 210 | victim.Traj.append(victim.old_Pos) 211 | victims_old_pos_list.append(victim.old_Pos) 212 | victims_old_pos_list = np.asarray(victims_old_pos_list) 213 | 214 | # Make a list of the agents old positions 215 | rescue_team_old_pos_list = [] 216 | for agent in rescue_team_Hist: 217 | rescue_team_old_pos_list.append(agent.old_Pos) 218 | rescue_team_old_pos_list = np.asarray(rescue_team_old_pos_list) 219 | 220 | # Calculation of the distance between the agents 221 | old_scouts2rescuers = net.pos2pos(rescue_team_old_pos_list) 222 | 223 | # Calculation of the raw sensations for the rescue team 224 | old_raw_sensations = net.sensed_pos(victims_old_pos_list, rescue_team_old_pos_list) 225 | 226 | # Check to see if the sensations are in the agents visual fields 227 | eval_old_sensations = net.is_seen(rescue_team_VFD_list, old_raw_sensations, team_VFD_status) 228 | 229 | rescue_team_curr_pos_list = [] 230 | rescue_team_role_list = [] 231 | 232 | for agent in rescue_team_Hist: 233 | # Calculation of the sensations for the rescue team 234 | agent.old_Sensation = agent.update_sensation(rescue_team_Hist.index(agent), 235 | old_raw_sensations, eval_old_sensations, 236 | old_scouts2rescuers, net.adj_mat, adj_mat) 237 | # Calculation of the indices for the rescue team 238 | agent.old_Index = agent.sensation2index(agent.old_Sensation, agent.max_VisualField) 239 | 240 | # Actions for the rescue team 241 | agent.action = eps_greedy(agent.Q[agent.old_Index, :], ACTIONS) 242 | 243 | # Next positions for the rescue team 244 | agent.curr_Pos = movement(agent.old_Pos, agent.action, agent.Speed) 245 | 246 | # Search algorithm 247 | # agent.straight_move(agent.old_Index, agent.wereHere, env_map) 248 | # agent.random_walk(agent.old_Index, agent.old_Pos, agent.Speed, env_map) 249 | agent.ant_colony_move(env_mat, agent.old_Index, env_map) 250 | # agent.levy_walk(env_map) 251 | # List of the current positions for the rescue team members 252 | rescue_team_curr_pos_list.append(agent.curr_Pos) 253 | 254 | # List of the roles for the rescue team members 255 | rescue_team_role_list.append(agent.Role) 256 | 257 | rescue_team_curr_pos_list = np.asarray(rescue_team_curr_pos_list) 258 | 259 | # Calculation of the distance between agents (after their movement) 260 | curr_scouts2rescuers = net.pos2pos(rescue_team_curr_pos_list) 261 | 262 | # Calculation of the new raw sensations for the rescue team (after their movement) 263 | curr_raw_sensations = net.sensed_pos(victims_old_pos_list, rescue_team_curr_pos_list) 264 | 265 | # Check to see if the sensations are in the agents visual fields 266 | eval_curr_sensations = net.is_seen(rescue_team_VFD_list, curr_raw_sensations, team_VFD_status) 267 | 268 | # Calculation of the new sensations for the rescue team (after their movement) 269 | for agent in rescue_team_Hist: 270 | agent.curr_Sensation = agent.update_sensation(rescue_team_Hist.index(agent), 271 | curr_raw_sensations, eval_curr_sensations, 272 | curr_scouts2rescuers, net.adj_mat, adj_mat) 273 | # Calculation of the indices for the rescue team (after their movement) 274 | agent.curr_Index = agent.sensation2index(agent.curr_Sensation, agent.max_VisualField) 275 | 276 | # Rewarding the rescue team 277 | agent.reward = reward_func(agent.curr_Sensation) 278 | 279 | # Q learning for the rescue team 280 | agent.Q = q_learning(agent.Q, agent.old_Index, agent.curr_Index, agent.reward, agent.action, alpha=0.8) 281 | 282 | # Check to see if the team rescued any victim 283 | if not agent.Finish: 284 | rescue_team_Hist, adj_mat = agent.rescue_accomplished(rescue_team_Hist, agent, adj_mat) 285 | # Keeping track of the rewards 286 | agent.RewHist.append(agent.reward) 287 | if agent.CanSeeIt: 288 | agent.RewHist_seen.append(agent.reward) 289 | if agent.Finish and agent.First: 290 | agent.Steps.append(t_step) 291 | agent.Steps_seen.append(agent.t_step_seen) 292 | agent.RewSum.append(np.sum(agent.RewHist)) 293 | agent.RewSum_seen.append(np.sum(agent.RewHist_seen)) 294 | rescue_team[agent.id] = agent 295 | agent.First = False 296 | for victim in victims_Hist: 297 | # Check to see if the victim rescued by the team 298 | # Keep track of the steps 299 | # Remove the victim from the list 300 | if not victim.Finish: 301 | victims[victim.id] = victim 302 | victims_Hist = victim.victim_rescued(rescue_team_old_pos_list, 303 | rescue_team_curr_pos_list, 304 | rescue_team_role_list, 305 | victim, victims_Hist) 306 | if victim.Finish and victim.First: 307 | victim.Steps.append(t_step) 308 | victim.First = False 309 | break # Rescue more than one victim by an agent 310 | if len(rescue_team_Hist) == num_just_scouts and len(victims_Hist) == 0: 311 | print(f'In episode {eps+1}, all of the victims were rescued in {t_step} steps') 312 | break 313 | 314 | # Update the rescue team positions 315 | for agent in rescue_team_Hist: 316 | agent.old_Pos = agent.curr_Pos 317 | 318 | # Victims' actions and positions 319 | for victim in victims_Hist: 320 | # Actions for the victims 321 | victim.action = np.random.choice(ACTIONS) 322 | # Victims next positions 323 | victim.curr_Pos = movement(victim.old_Pos, victim.action, victim.Speed) 324 | # Update the victims position 325 | victim.old_Pos = victim.curr_Pos 326 | 327 | # Check for the proper number of episodes 328 | # convergence_flag = [] 329 | # for agent in rescue_team: 330 | # convergence_flag.append(agent.convergence_check(accuracy)) 331 | # if all(convergence_flag): 332 | # break 333 | 334 | # Add agents last pos in the trajectory 335 | for agent in rescue_team: 336 | for victim in victims: 337 | if agent.curr_Pos[0] == victim.old_Pos[0] and agent.curr_Pos[1] == victim.old_Pos[1]: 338 | agent.Traj.append(agent.curr_Pos) 339 | agent.VFD_status_history.append(agent.vfd_status) 340 | 341 | rescue_team_Traj = [] 342 | VFD_status_list = [] 343 | rescue_team_RewSum = [] 344 | rescue_team_Steps = [] 345 | rescue_team_RewSum_seen = [] 346 | rescue_team_Steps_seen = [] 347 | rescue_team_Q = [] 348 | largest = len(rescue_team[0].Traj) 349 | for agent in rescue_team: 350 | if len(agent.Traj) > largest: 351 | largest = len(agent.Traj) 352 | rescue_team_RewSum.append(agent.RewSum) 353 | rescue_team_Steps.append(agent.Steps) 354 | rescue_team_RewSum_seen.append(agent.RewSum_seen) 355 | rescue_team_Steps_seen.append(agent.Steps_seen) 356 | rescue_team_Q.append(agent.Q) 357 | for agent in rescue_team: 358 | while len(agent.Traj) < largest: 359 | agent.Traj.append(agent.Traj[-1]) 360 | agent.VFD_status_history.append((agent.vfd_status)) 361 | rescue_team_Traj.append(agent.Traj) 362 | # List of the VFD status 363 | VFD_status_list.append(agent.VFD_status_history) 364 | 365 | victims_Traj = [] 366 | for victim in victims: 367 | while len(victim.Traj) < largest: 368 | victim.Traj.append(victim.Traj[-1]) 369 | victims_Traj.append(victim.Traj) 370 | print(f'This experiment took {time.time() - tic} seconds') 371 | return (rescue_team_Traj, 372 | rescue_team_RewSum, rescue_team_Steps, 373 | rescue_team_RewSum_seen, rescue_team_Steps_seen, 374 | rescue_team_Q, victims_Traj, VFD_list, VFD_status_list, rescue_team_roles) 375 | 376 | 377 | if Multi_Runs: 378 | # Multi Runs 379 | rescue_team_RewSum_Run = [] 380 | rescue_team_Steps_Run = [] 381 | rescue_team_RewSum_seen_Run = [] 382 | rescue_team_Steps_seen_Run = [] 383 | for run in range(NUM_RUNS): 384 | print(f'Run {run + 1} of {NUM_RUNS}') 385 | (rescue_team_Traj, 386 | rescue_team_RewSum, rescue_team_Steps, 387 | rescue_team_RewSum_seen, rescue_team_Steps_seen, 388 | rescue_team_Q, victims_Traj, VFD_list, VFD_status_list, rescue_team_roles) = env(accuracy=1e-7) 389 | 390 | rescue_team_RewSum_Run.append(list(filter(None, rescue_team_RewSum))) 391 | rescue_team_Steps_Run.append(list(filter(None, rescue_team_Steps))) 392 | rescue_team_RewSum_seen_Run.append(list(filter(None, rescue_team_RewSum_seen))) 393 | rescue_team_Steps_seen_Run.append(list(filter(None, rescue_team_Steps_seen))) 394 | 395 | rescue_team_RewSum_Run = np.mean(np.asarray(rescue_team_RewSum_Run), axis=0) 396 | rescue_team_Steps_Run = np.mean(np.asarray(rescue_team_Steps_Run), axis=0) 397 | rescue_team_RewSum_seen_Run = np.mean(np.asarray(rescue_team_RewSum_seen_Run), axis=0) 398 | rescue_team_Steps_seen_Run = np.mean(np.asarray(rescue_team_Steps_seen_Run), axis=0) 399 | 400 | with h5py.File(f'multi_agent_Q_learning_{exp_name}_{str(NUM_RUNS)}Runs.hdf5', 'w') as f: 401 | for idx, rew_sum in enumerate(rescue_team_RewSum_Run): 402 | f.create_dataset(f'RS{idx}_reward', data=rew_sum) 403 | for idx, steps in enumerate(rescue_team_Steps_Run): 404 | f.create_dataset(f'RS{idx}_steps', data=steps) 405 | for idx, rew_sum_seen in enumerate(rescue_team_RewSum_seen_Run): 406 | f.create_dataset(f'RS{idx}_reward_seen', data=rew_sum_seen) 407 | for idx, steps_seen in enumerate(rescue_team_Steps_seen_Run): 408 | f.create_dataset(f'RS{idx}_steps_seen', data=steps_seen) 409 | f.create_dataset('RS_VFD', data=VFD_list) 410 | 411 | else: 412 | # Single Run 413 | (rescue_team_Traj, 414 | rescue_team_RewSum, rescue_team_Steps, 415 | rescue_team_RewSum_seen, rescue_team_Steps_seen, 416 | rescue_team_Q, victims_Traj, VFD_list, VFD_status_list, rescue_team_roles) = env(accuracy=1e-7) 417 | 418 | with h5py.File(f'multi_agent_Q_learning_{exp_name}.hdf5', 'w') as f: 419 | for idx, traj in enumerate(rescue_team_Traj): 420 | f.create_dataset(f'RS{idx}_trajectory', data=traj) 421 | for idx, vfd_sts in enumerate(VFD_status_list): 422 | f.create_dataset(f'RS{idx}_VFD_status', data=vfd_sts) 423 | for idx, rew_sum in enumerate(rescue_team_RewSum): 424 | f.create_dataset(f'RS{idx}_reward', data=rew_sum) 425 | for idx, steps in enumerate(rescue_team_Steps): 426 | f.create_dataset(f'RS{idx}_steps', data=steps) 427 | for idx, rew_sum_seen in enumerate(rescue_team_RewSum_seen): 428 | f.create_dataset(f'RS{idx}_reward_seen', data=rew_sum_seen) 429 | for idx, steps_seen in enumerate(rescue_team_Steps_seen): 430 | f.create_dataset(f'RS{idx}_steps_seen', data=steps_seen) 431 | for idx, q in enumerate(rescue_team_Q): 432 | f.create_dataset(f'RS{idx}_Q', data=q) 433 | for idx, victim_traj in enumerate(victims_Traj): 434 | f.create_dataset(f'victim{idx}_trajectory', data=victim_traj) 435 | f.create_dataset('victims_num', data=[len(victims_Traj)]) 436 | f.create_dataset('RS_VFD', data=VFD_list) 437 | f.create_dataset('RS_ROLES', data=rescue_team_roles) 438 | -------------------------------------------------------------------------------- /MASAR/gridworld_multi_agent1_1.py: -------------------------------------------------------------------------------- 1 | import pygame as pg 2 | import numpy as np 3 | import pygame 4 | import time 5 | 6 | 7 | pygame.init() 8 | 9 | # Constants 10 | WIDTH = 800 # width of the environment (px) 11 | HEIGHT = 800 # height of the environment (px) 12 | TS = 10 # delay in msec 13 | Col_num = 20 # number of columns 14 | Row_num = 20 # number of rows 15 | 16 | # define colors 17 | bg_color = pg.Color(255, 255, 255) 18 | line_color = pg.Color(128, 128, 128) 19 | vfdr_color = pg.Color(8, 136, 8, 128) 20 | vfds_color = pg.Color(255, 165, 0, 128) 21 | vfdrs_color = pg.Color(173, 216, 230, 128) 22 | 23 | def draw_grid(scr): 24 | '''a function to draw gridlines and other objects''' 25 | # Horizontal lines 26 | for j in range(Row_num + 1): 27 | pg.draw.line(scr, line_color, (0, j * HEIGHT // Row_num), (WIDTH, j * HEIGHT // Row_num), 2) 28 | # # Vertical lines 29 | for i in range(Col_num + 1): 30 | pg.draw.line(scr, line_color, (i * WIDTH // Col_num, 0), (i * WIDTH // Col_num, HEIGHT), 2) 31 | 32 | for x1 in range(0, WIDTH, WIDTH // Col_num): 33 | for y1 in range(0, HEIGHT, HEIGHT // Row_num): 34 | rect = pg.Rect(x1, y1, WIDTH // Col_num, HEIGHT // Row_num) 35 | pg.draw.rect(scr, bg_color, rect, 1) 36 | 37 | 38 | def animate(rescue_team_traj, victims_traj, rescue_team_vfd, rescue_team_vfd_status, rescue_team_roles, env_map, wait_time): 39 | 40 | font = pg.font.SysFont('arial', 20) 41 | 42 | num_rescue_team = len(rescue_team_traj) 43 | num_victims = len(victims_traj) 44 | 45 | pg.init() # initialize pygame 46 | screen = pg.display.set_mode((WIDTH + 2, HEIGHT + 2)) # set up the screen 47 | pg.display.set_caption("gridworld") # add a caption 48 | bg = pg.Surface(screen.get_size()) # get a background surface 49 | bg = bg.convert() 50 | 51 | img_rescuer = pg.image.load('TurtleBot.png') 52 | img_mdf_r = pg.transform.scale(img_rescuer, (WIDTH // Col_num, HEIGHT // Row_num)) 53 | 54 | img_rescuer_scout = pg.image.load('typhoon.jpg') 55 | img_mdf_rs = pg.transform.scale(img_rescuer_scout, (WIDTH // Col_num, HEIGHT // Row_num)) 56 | 57 | img_scout = pg.image.load('Crazyflie.JPG') 58 | img_mdf_s = pg.transform.scale(img_scout, (WIDTH // Col_num, HEIGHT // Row_num)) 59 | 60 | img_victim = pg.image.load('victim.png') 61 | img_mdf_victim = pg.transform.scale(img_victim, (WIDTH // Col_num, HEIGHT // Row_num)) 62 | 63 | img_wall = pg.image.load('wall.png') 64 | img_mdf_wall = pg.transform.scale(img_wall, (WIDTH // Col_num, HEIGHT // Row_num)) 65 | 66 | bg.fill(bg_color) 67 | screen.blit(bg, (0, 0)) 68 | clock = pg.time.Clock() 69 | pg.display.flip() 70 | run = True 71 | while run: 72 | clock.tick(60) 73 | for event in pg.event.get(): 74 | if event.type == pg.QUIT: 75 | run = False 76 | step = -1 77 | list_victims = np.arange(num_victims).tolist() 78 | list_rescue_team = np.arange(num_rescue_team).tolist() 79 | 80 | for rescue_team_stt, victims_stt in zip(np.moveaxis(rescue_team_traj, 0, -1), 81 | np.moveaxis(victims_traj, 0, -1)): 82 | 83 | for row in range(Row_num): 84 | for col in range(Col_num): 85 | if env_map[row, col] == 1: 86 | screen.blit(img_mdf_wall, 87 | (col * (WIDTH // Col_num), 88 | row * (HEIGHT // Row_num))) 89 | step += 1 90 | for num in list_rescue_team: 91 | if str(rescue_team_roles[num]) == "b'rs'": 92 | vfd_color = vfdrs_color 93 | elif str(rescue_team_roles[num]) == "b'r'": 94 | vfd_color = vfdr_color 95 | elif str(rescue_team_roles[num]) == "b's'": 96 | vfd_color = vfds_color 97 | 98 | # rescuer visual field depth 99 | vfd_j = 0 100 | for j in range(int(max(rescue_team_stt[1, num] - rescue_team_vfd[num], 0)), 101 | int(min(Col_num, rescue_team_stt[1, num] + rescue_team_vfd[num] + 1))): 102 | vfd_i = 0 103 | for i in range(int(max(rescue_team_stt[0, num] - rescue_team_vfd[num], 0)), 104 | int(min(Row_num, rescue_team_stt[0, num] + rescue_team_vfd[num] + 1))): 105 | if rescue_team_vfd_status[num][step][vfd_i, vfd_j]: 106 | rect = pg.Rect(j * (WIDTH // Col_num), i * (HEIGHT // Row_num), 107 | (WIDTH // Col_num), (HEIGHT // Row_num)) 108 | pg.draw.rect(screen, vfd_color, rect) 109 | vfd_i += 1 110 | vfd_j += 1 111 | 112 | # agents 113 | for num in list_rescue_team: 114 | if str(rescue_team_roles[num]) == "b'rs'": 115 | img_mdf = img_mdf_rs 116 | elif str(rescue_team_roles[num]) == "b'r'": 117 | img_mdf = img_mdf_r 118 | elif str(rescue_team_roles[num]) == "b's'": 119 | img_mdf = img_mdf_s 120 | screen.blit(img_mdf, 121 | (rescue_team_stt[1, num] * (WIDTH // Col_num), 122 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 123 | screen.blit(font.render(str(num+1), True, (0, 0, 0)), 124 | (rescue_team_stt[1, num] * (WIDTH // Col_num), 125 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 126 | 127 | # Stop showing finished agents 128 | # if (step >= 1 and 129 | # (rescue_team_stt[:, num][0] == rescue_team_history[:, num][0] == rescue_team_traj[num, -1, 0] and 130 | # rescue_team_stt[:, num][1] == rescue_team_history[:, num][1] == rescue_team_traj[num, -1, 1])): 131 | # list_rescue_team.remove(num) 132 | 133 | for num in list_victims: 134 | screen.blit(img_mdf_victim, (victims_stt[1, num] * (WIDTH // Col_num), 135 | victims_stt[0, num] * (HEIGHT // Row_num))) 136 | screen.blit(font.render(str(num+1), True, (0, 0, 0)), 137 | (victims_stt[1, num] * (WIDTH // Col_num), victims_stt[0, num] * (HEIGHT // Row_num))) 138 | 139 | # Stop showing rescued victims 140 | # if step >= 1 and (victims_stt[:, num][0] == victims_history[:, num][0] == victims_traj[num, -1, 0] and 141 | # victims_stt[:, num][1] == victims_history[:, num][1] == victims_traj[num, -1, 1]): 142 | # list_victims.remove(num) 143 | 144 | draw_grid(screen) 145 | pg.display.flip() 146 | pg.display.update() 147 | time.sleep(wait_time) # wait between the shows 148 | 149 | for num in list_victims: 150 | screen.blit(bg, (victims_stt[1, num] * (WIDTH // Col_num), 151 | victims_stt[0, num] * (HEIGHT // Row_num))) 152 | 153 | for num in list_rescue_team: 154 | screen.blit(bg, (rescue_team_stt[1, num] * (WIDTH // Col_num), 155 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 156 | 157 | # rescuer visual field depths 158 | for j in range(int(max(rescue_team_stt[1, num] - rescue_team_vfd[num], 0)), 159 | int(min(Row_num, rescue_team_stt[1, num] + rescue_team_vfd[num] + 1))): 160 | for i in range(int(max(rescue_team_stt[0, num] - rescue_team_vfd[num], 0)), 161 | int(min(Col_num, rescue_team_stt[0, num] + rescue_team_vfd[num] + 1))): 162 | rect = pg.Rect(j * (WIDTH // Col_num), i * (HEIGHT // Row_num), 163 | (WIDTH // Col_num), (HEIGHT // Row_num)) 164 | pg.draw.rect(screen, bg_color, rect) 165 | 166 | victims_history = victims_stt 167 | rescue_team_history = rescue_team_stt 168 | 169 | for num in list_rescue_team: 170 | if str(rescue_team_roles[num]) == "b'rs'": 171 | img_mdf = img_mdf_rs 172 | elif str(rescue_team_roles[num]) == "b'r'": 173 | img_mdf = img_mdf_r 174 | elif str(rescue_team_roles[num]) == "b's'": 175 | img_mdf = img_mdf_s 176 | screen.blit(img_mdf, (rescue_team_traj[num, -1, 1] * (WIDTH // Col_num), 177 | rescue_team_traj[num, -1, 0] * (HEIGHT // Row_num))) 178 | for num in list_victims: 179 | screen.blit(img_mdf_victim, (victims_traj[num, -1, 1] * (WIDTH // Col_num), 180 | victims_traj[num, -1, 0] * (HEIGHT // Row_num))) 181 | 182 | draw_grid(screen) 183 | pg.display.flip() 184 | pg.display.update() 185 | run = False 186 | pg.quit() 187 | -------------------------------------------------------------------------------- /MASAR/maps.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | env_map0 = np.zeros((20, 20)) 5 | env_map0_entrances = [[0, 0], [0, 1], [0, 2], [1, 0], [2, 0]] 6 | 7 | 8 | env_map1 = np.array([[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 9 | [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 10 | [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 11 | [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 12 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 13 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 14 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 15 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], 16 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], 17 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], 18 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 19 | [1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 20 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 21 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 22 | [1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 23 | [1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 24 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 25 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 26 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 27 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]) 28 | env_map1_entrances = [[1, 1], [1, 2], [1, 3], [2, 1], [3, 1]] 29 | 30 | 31 | env_map2 = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 32 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 33 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 34 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 35 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 36 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 37 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 38 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 39 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 40 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 41 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 42 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 43 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 44 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 45 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 46 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 47 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 48 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 49 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 50 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]) 51 | env_map2_entrances = [[0, 0], [0, 1], [0, 2], [1, 0], [2, 0]] 52 | 53 | 54 | env_map3 = np.array([[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 55 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 56 | [1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1], 57 | [1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1], 58 | [1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1], 59 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 60 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 61 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 62 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 63 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 64 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 65 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 66 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 67 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 68 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 69 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 70 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], 71 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], 72 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1], 73 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]) 74 | env_map3_entrances = [[2, 3], [2, 4], [2, 5], [3, 3], [4, 3]] -------------------------------------------------------------------------------- /MASAR/multi_agent_Q_learning_1R.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/multi_agent_Q_learning_1R.hdf5 -------------------------------------------------------------------------------- /MASAR/multi_agent_Q_learning_2R.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/multi_agent_Q_learning_2R.hdf5 -------------------------------------------------------------------------------- /MASAR/multi_agent_Q_learning_2R_.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/multi_agent_Q_learning_2R_.hdf5 -------------------------------------------------------------------------------- /MASAR/multi_agent_Q_learning_4RS_1000episodes.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/multi_agent_Q_learning_4RS_1000episodes.hdf5 -------------------------------------------------------------------------------- /MASAR/multi_agent_Q_learning_4RS_1000episodes_old.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/multi_agent_Q_learning_4RS_1000episodes_old.hdf5 -------------------------------------------------------------------------------- /MASAR/multi_agent_Q_learning_FindSurvivors.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/multi_agent_Q_learning_FindSurvivors.hdf5 -------------------------------------------------------------------------------- /MASAR/multiagent_search.py: -------------------------------------------------------------------------------- 1 | from random import sample, choice, choices, seed 2 | from scipy.stats import wrapcauchy, levy_stable 3 | import numpy as np 4 | import os 5 | import sys 6 | import decimal 7 | from tqdm import tqdm 8 | from record_vis import RecordVis 9 | 10 | class Cell: 11 | def __init__(self, row, col, isboundary): 12 | self.visitors = [] # list of agents who have visited the cell 13 | self.notified = dict() # list of agents who recognize the marking of that cell 14 | self.row = row 15 | self.col = col 16 | self.isboundary = isboundary 17 | 18 | def __repr__(self): 19 | return f"({self.row}, {self.col})" 20 | 21 | class Map: 22 | ###### UPDATE WHATEVER IS DOING MAP CHECK STUFF ###### 23 | def __init__(self, map_array): #map_size: int 24 | self.side_len = int(map_array.shape[0]) # Map array should be square 25 | self.map_size = self.side_len**2 # Map array should be square 26 | self.map = [[Cell(row=i, col=j, isboundary= map_array[i][j]) 27 | for j in range(self.side_len)] 28 | for i in range(self.side_len)] 29 | self.cells_to_search = [self.map[i][j] 30 | for i in range(self.side_len) 31 | for j in range(self.side_len) 32 | if self.map[i][j].isboundary == False] 33 | # self.map = np.array(self.map) 34 | self.visited_cells = 0 35 | 36 | class Agent: 37 | def __init__(self, id_: int, v: float, seed_=None, fov="N/A"): 38 | self.id = id_ 39 | self.v = v 40 | self.period = round(1 / v, 1) 41 | self.seed = seed_ if seed_ is not None else choice(range(sys.maxsize)) 42 | self.search_time = 0 43 | self.fov = "N/A" 44 | 45 | 46 | # column where agents are indices and the value is the probability 47 | # that the agent will mark for them 48 | self.rel_row = None 49 | 50 | def __repr__(self): 51 | return f"Agent {self.id}" 52 | 53 | def place_in_map(self, map_): 54 | starting_cell = np.random.choice(map_.cells_to_search) 55 | return starting_cell.row, starting_cell.col 56 | 57 | 58 | ### Agent Acts on Map - Define Random Walk Agent 59 | class RandomWalkAgent(Agent): 60 | def __init__(self, id_: int, v: float, fov=12, seed_=None, alpha = 2, rho =.02): 61 | super(RandomWalkAgent, self).__init__(id_, v, seed, fov=None) 62 | 63 | self.seed = seed_ if seed_ is not None else choice(range(sys.maxsize)) 64 | self.fov = fov # side length of observable square around the agent 65 | 66 | self.up, self.down, self.left, self.right = (-1, 0), (1, 0), (0, -1), (0, 1) 67 | self.actions = [self.up, self.down, self.left, self.right] 68 | self.curr_pos = None 69 | self.prev_action = None 70 | 71 | # Levy Parameters 72 | self.alpha = alpha 73 | self.beta = 0 74 | self.scale_to_var = 1/(np.sqrt(2)) 75 | self.levy_dist = levy_stable(self.alpha, self.beta, scale = self.scale_to_var) 76 | self.decision_wait_time = 0 # Relevent for Levy Walk 77 | 78 | # Turning Angle Parameters 79 | self.rho = rho # 0 is completely fair 1 is biased to previous action 80 | self.wrap_dist = wrapcauchy(self.rho) 81 | self.curr_angle = np.random.random()*np.pi*2 82 | self.dest_comb = 20 83 | self.path_to_take = None 84 | self.path_taken_counter = 0 85 | 86 | 87 | def account_for_boundary(self, map_): 88 | row, col = self.curr_pos 89 | actions = self.actions.copy() 90 | 91 | if row == 0: 92 | actions[0] = actions[1] 93 | elif row == map_.side_len - 1: 94 | actions[1] = actions[0] 95 | if col == 0: 96 | actions[2] = actions[3] 97 | elif col == map_.side_len - 1: 98 | actions[3] = actions[2] 99 | 100 | check_cells = [ (drow+row,dcol+col) for drow, dcol in [list(action) for action in actions]] 101 | 102 | for n, cell_ in enumerate(check_cells): 103 | m = (1,0,3,2) # Theres probably a better way to get opposite action 104 | if map_.map[cell_[0]][cell_[1]].isboundary: 105 | actions[n] = actions[m[n]] 106 | 107 | return actions 108 | 109 | def get_next_step(self, map_): 110 | """ 111 | Randomly gets next step based on set of actions. 112 | Boundary conditions are reflective 113 | """ 114 | prev_action = self.prev_action 115 | 116 | #Levy Walk each decision step continuos otherwise repeat last action 117 | if prev_action is None or self.decision_wait_time == 0: 118 | r = self.levy_dist.rvs() 119 | r = np.round(np.abs(r)) 120 | self.decision_wait_time = int(r) 121 | 122 | r_angle = self.wrap_dist.rvs() 123 | self.curr_angle = (self.curr_angle + r_angle) % (2*np.pi) 124 | px = np.cos(self.curr_angle) 125 | py = np.sin(self.curr_angle) 126 | # print(px,py, r_angle) 127 | 128 | possible_actions = (int(-1*int(np.sign(py))),0), (0,int(np.sign(px))) 129 | px1 = abs(px) 130 | py1 = abs(py) 131 | # print(px1, py1, r_angle, self.curr_angle, self.decision_wait_time) 132 | self.path_to_take = [ possible_actions[i] for i in np.random.choice( [0,1], size = self.dest_comb, p = [py1**2, px1**2])] 133 | else: 134 | self.decision_wait_time -= 1 135 | 136 | 137 | actions_in_boundary = self.account_for_boundary(map_) 138 | 139 | desired_action = self.path_to_take[self.path_taken_counter] 140 | self.path_taken_counter = (1+self.path_taken_counter) % self.dest_comb 141 | 142 | if desired_action in actions_in_boundary: 143 | return desired_action 144 | else: 145 | # self.path_to_take = [( int(desired_action[0]*-1), int(desired_action[1]*-1)) if desired_action==x else x for x in self.path_to_take] 146 | if np.abs(desired_action[1]) == 1: 147 | dtheta = self.curr_angle - np.pi/2 148 | if dtheta>0: 149 | self.curr_angle = np.pi/2 - dtheta 150 | else: 151 | self.curr_angle = np.pi/2 + np.abs(dtheta) 152 | 153 | elif np.abs(desired_action[0]) == 1: 154 | dtheta = self.curr_angle - np.pi 155 | if dtheta>0: 156 | self.curr_angle = np.pi - dtheta 157 | else: 158 | self.curr_angle = np.pi + np.abs(dtheta) 159 | self.path_to_take = [ ( int(desired_action[0]*-1), int(desired_action[1]*-1)) if x == desired_action else x for x in self.path_to_take] 160 | # print("boundary hit", (self.curr_angle), np.cos(self.curr_angle), np.sin(self.curr_angle)) 161 | # self.path_to_take = [ possible_actions[i] for i in np.random.choice( [0,1], size = self.dest_comb, p = [py1/(py1+px1),px1/(py1+px1)])] 162 | return ( int(desired_action[0]*-1) , int(desired_action[1]*-1) ) 163 | 164 | def take_step(self, map_): 165 | # seed(self.seed) 166 | # prev_step = self.prev_pos 167 | # curr_step = self.curr_pos 168 | while True: 169 | if self.curr_pos is None: 170 | # This is the Initial step 171 | next_step = self.place_in_map(map_) 172 | row, col = next_step 173 | else: 174 | action = self.get_next_step(map_) 175 | # if action not in self.path_to_take: 176 | # print(action, self.path_to_take,self.curr_angle) 177 | 178 | row, col = self.curr_pos[0] + action[0], self.curr_pos[1] + action[1] 179 | self.prev_action = action 180 | 181 | self.curr_pos= (row, col) 182 | 183 | cell = map_.map[row][col] 184 | if not cell.notified.get(self.id): 185 | cell.visitors.append(self.id) 186 | update = True 187 | else: 188 | update = False 189 | # self.update_cell(cell) 190 | yield cell, update 191 | 192 | 193 | class MultiAgentSearch: 194 | def __init__(self, map_, agents, map_array,time_step=0.1, vis=False, sim_time = None, vis_outpath =None): 195 | self.map = map_ 196 | self.agents = agents 197 | self.time_step = time_step 198 | self.vis = vis 199 | self.vis_outpath = vis_outpath 200 | self.sim_time = sim_time 201 | self.map_array = map_array 202 | 203 | self.period_dict = self.get_period_dict() 204 | self.steps_generators = [agent.take_step(map_) for agent in agents] 205 | 206 | self.record = {} 207 | 208 | def get_period_dict(self): 209 | period_dict = {} 210 | # every rate has a list of agents with that rate (1/v) 211 | for agent in self.agents: 212 | try: 213 | period_dict[agent.period].append(agent.id) 214 | except KeyError: 215 | period_dict[agent.period] = [agent.id] 216 | return period_dict 217 | 218 | def update_map(self, cell): 219 | global FINISHED 220 | # print(self.map.visited_cells) 221 | if self.map.visited_cells > len(self.map.cells_to_search)-1: 222 | FINISHED = True 223 | 224 | if len(cell.visitors) == 1: 225 | self.map.visited_cells += 1 226 | 227 | 228 | def divisible(self, num, den): 229 | if num < den: 230 | return False 231 | return round(float(decimal.Decimal(str(num)) % decimal.Decimal(str(den))), 1) == 0 232 | 233 | def search(self): 234 | global FINISHED 235 | self.counter = self.time_step 236 | while not FINISHED: 237 | 238 | if (self.sim_time != None) and (self.counter> self.sim_time): 239 | FINISHED = True 240 | 241 | # get ids of agents who will provide cells. These are indices in the steps generators object 242 | searchers_ids = [] 243 | for period, agents_ids in self.period_dict.items(): 244 | if self.divisible(self.counter, period): 245 | searchers_ids.extend(agents_ids) 246 | 247 | searchers_ids = sample(searchers_ids, len(searchers_ids)) # shuffle 248 | 249 | for agent_id in searchers_ids: 250 | cell, update = next(self.steps_generators[agent_id]) 251 | info = (agent_id, cell.row, cell.col) 252 | 253 | if self.vis: 254 | try: 255 | self.record[self.counter].append(info) 256 | except KeyError: 257 | self.record[self.counter] = [info] 258 | 259 | if update: 260 | self.update_map(cell) 261 | self.counter = round(self.counter + self.time_step, 1) 262 | 263 | # if FINISHED: 264 | if self.vis: 265 | # Should instead write to file 266 | self.vis_record() 267 | return 268 | 269 | 270 | def vis_record(self): 271 | if self.vis_outpath == None: 272 | RecordVis().vis_record(self.record, self.map.side_len, self.map_array, 'vids/'+uuid.uuid1().hex) 273 | else: 274 | RecordVis().vis_record(self.record, self.map.side_len, self.map_array, 'vids/'+self.vis_outpath) 275 | 276 | 277 | def get_agents(agent_class, num_agents, speeds, adj_matrix, fov, alpha, rho): 278 | """ 279 | Get a number of agents and agents speeds and care adjacency matrix. 280 | Update each agent's care dict according to adj matrix 281 | return list of agents 282 | """ 283 | agents = [agent_class(id_=i, v=speeds[i], fov=fov, seed_=i + 10, alpha = alpha, rho = rho) for i in range(num_agents)] 284 | # for agent in agents: 285 | # agent.rel_row = adj_matrix[agent.id] 286 | return agents 287 | 288 | 289 | def get_adj_str(adj_matrix): 290 | num_agents = len(adj_matrix) 291 | row_len = num_agents * 11 292 | top_row_len = num_agents * 11 - len(" Adjacency Matrix ") 293 | bot_row_len = row_len 294 | 295 | string = "*" * (top_row_len // 2) + " Adjacency Matrix " + "*" * (top_row_len // 2) + "\n" 296 | string += "\t\t" + "\t".join([f"Agent_{i}" for i in range(num_agents)]) 297 | string += "\n" 298 | for idx, row in enumerate(adj_matrix): 299 | string += f"Agent_{idx}\t" 300 | string += "\t\t".join([str(n) for n in row]) 301 | string += "\n\n" 302 | string += "*" * bot_row_len 303 | return string 304 | 305 | 306 | def get_header(agents): 307 | header = "" 308 | for agent in agents: 309 | header += agent.__repr__() 310 | header += f". Speed: {agent.v} sq/s" 311 | header += "\n" 312 | header += "\n\n" 313 | return header 314 | 315 | 316 | def output_results(round_, agents, adj_matrix, map_, out_path="results.csv"): 317 | """ 318 | output a csv file with columns: 319 | search_time, Pi_1, Pi_2, Pi_3, etc 320 | """ 321 | if not os.path.exists(out_path): 322 | header = get_header(agents) 323 | with open(out_path, "w") as out: 324 | out.write(header) 325 | cols = ",".join([f"Pi_{agent.id}" for agent in agents] + ["Search Time (s)\n"]) 326 | out.write(cols) 327 | 328 | result = {agent.id: 0 for agent in agents} 329 | for i, row in enumerate(map_): 330 | for j, cell in enumerate(row): 331 | try: 332 | result[cell.visitors[0]] += 1 333 | except: 334 | # collapsed models 335 | continue 336 | 337 | with open(out_path, "a") as out: 338 | row = [] 339 | search_time = round_.counter 340 | for agent_id in sorted(result.keys()): 341 | # search_time = round(agents[agent_id].search_time, 1) 342 | pi = round(result[agent_id] / search_time, 1) 343 | row.append(str(pi)) 344 | row.append(str(search_time)) 345 | out.write(",".join(row)) 346 | out.write("\n") 347 | 348 | 349 | # def main(agent_class, num_agents, fov, speeds, adj_matrix, map_size, out_path, vis): 350 | def main(agent_class, num_agents, fov, speeds, adj_matrix, map_array, out_path, vis, sim_time, alpha, rho,vis_outpath): 351 | global FINISHED 352 | FINISHED = False 353 | agent_class = {0: Agent, 1: RandomWalkAgent}[agent_class] 354 | 355 | out_path = 'data/'+ out_path +".csv" 356 | agents = get_agents(agent_class, num_agents, speeds, adj_matrix, fov, alpha, rho) 357 | # test_map = np.zeros((int(map_size**0.5), int(map_size**0.5)), dtype= int) 358 | map_ = Map(map_array) 359 | 360 | round_ = MultiAgentSearch(map_=map_, agents=agents, map_array=map_array, time_step=0.1, vis=vis, sim_time = sim_time, vis_outpath = vis_outpath) 361 | round_.search() 362 | output_results(round_, agents, adj_matrix, map_.map, out_path=out_path) 363 | 364 | 365 | if __name__ == '__main__': 366 | import argparse 367 | import uuid 368 | 369 | description = """ 370 | simulate a multiagent map coverage using an input numpy adjacency matrix. 371 | get simulation results as a csv file for all agents' performance and final search time 372 | """ 373 | id_ = uuid.uuid1().hex 374 | parser = argparse.ArgumentParser(description=description) 375 | parser.add_argument('--agent_type', type=int, default=1, 376 | help="type of agent: 0: teleporting; 1: Random Walk") 377 | parser.add_argument('--fov', type=int, default=3, 378 | help="agent field of view in Manhattan distance. Only for guided random walk agent") 379 | parser.add_argument('--adj_matrix', type=str, default='networks/2/C.npy', 380 | help="path to .npy square adjacency matrix") 381 | parser.add_argument('--map_array_path', type=str, default=None, 382 | help='Path to Map in .py form, defaults to 10x10 array') 383 | parser.add_argument('--map_array', type= str, default=None, 384 | help='Custom map array') 385 | parser.add_argument('--num_rounds', type=int, default=1, 386 | help='number of rounds played') 387 | parser.add_argument('--out_path', type=str, default=f"{id_}", 388 | help='path of output csv file') 389 | parser.add_argument('--visualize', type=int, default=0, 390 | help='1 to generate a video for the search') 391 | parser.add_argument('--vis_outpath', type=str, default=None, 392 | help='outpath for video') 393 | parser.add_argument('--sim_time', type=float, default=None, 394 | help='Total time for Simulation (agent_speed = 1 sq/s)') 395 | parser.add_argument('--num_agents', type=int, default=2, 396 | help="Number of Agents") 397 | parser.add_argument('--alpha', type=float, default=2, 398 | help="Levy Walk alpha") 399 | parser.add_argument('--rho', type=float, default=.01, 400 | help="Persistance turning angle rho") 401 | 402 | 403 | 404 | args = parser.parse_args() 405 | agent_type = args.agent_type 406 | fov = args.fov 407 | agent_class = agent_type#{0: Agent, 1: RandomWalkAgent}[agent_type] 408 | adj_matrix = np.load(args.adj_matrix) 409 | num_agents = args.num_agents #adj_matrix.shape[0] 410 | if args.map_array_path is not None: 411 | map_array = np.load(args.map_array_path) 412 | elif args.map_array is None: 413 | map_array = np.zeros((10,10)) 414 | else: 415 | map_array = eval('np.array(' + args.map_array + ')') 416 | 417 | 418 | num_rounds = args.num_rounds 419 | out_path = args.out_path 420 | vis = args.visualize 421 | sim_time = args.sim_time 422 | alpha = args.alpha 423 | rho = args.rho 424 | vis_outpath = args.vis_outpath 425 | 426 | speeds = np.ones(num_agents) 427 | for _ in tqdm(range(num_rounds)): 428 | main(agent_class=agent_class, 429 | num_agents=num_agents, 430 | fov=fov, 431 | speeds=speeds, 432 | adj_matrix=adj_matrix, 433 | map_array=map_array, 434 | out_path=out_path, 435 | vis=vis, 436 | sim_time=sim_time, 437 | alpha = alpha, 438 | rho = rho, 439 | vis_outpath= vis_outpath) 440 | -------------------------------------------------------------------------------- /MASAR/network.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Network: 5 | def __init__(self, adj_mat, num_agents, num_victims): 6 | self.adj_mat = adj_mat.copy() 7 | self.adj_mat[self.adj_mat == 0] = np.nan 8 | self.num_agents = num_agents 9 | self.num_victims = num_victims 10 | 11 | def pos2pos(self, pos_list): 12 | 13 | pos_array = np.empty((self.num_agents, self.num_agents, 2)) 14 | pos_array[:, :, 0] = np.tile(pos_list[:, 0].reshape(self.num_agents, 1), (1, self.num_agents)) 15 | pos_array[:, :, 1] = np.tile(pos_list[:, 1].reshape(self.num_agents, 1), (1, self.num_agents)) 16 | 17 | pos2pos = np.subtract(pos_array, np.transpose(pos_array, (1, 0, 2))) 18 | return pos2pos 19 | 20 | def sensed_pos(self, victim_pos_list, rs_pos_list): 21 | self.num_victims = len(victim_pos_list) 22 | 23 | rs_pos_array = np.empty((self.num_agents, self.num_victims, 2)) 24 | rs_pos_array[:, :, 0] = np.tile(rs_pos_list[:, 0].reshape(self.num_agents, 1), (1, self.num_victims)) 25 | rs_pos_array[:, :, 1] = np.tile(rs_pos_list[:, 1].reshape(self.num_agents, 1), (1, self.num_victims)) 26 | 27 | victim_pos_array = np.empty((self.num_agents, self.num_victims, 2)) 28 | 29 | victim_pos_array[:, :, 0] = np.tile(victim_pos_list[:, 0].reshape(1, self.num_victims), (self.num_agents, 1)) 30 | victim_pos_array[:, :, 1] = np.tile(victim_pos_list[:, 1].reshape(1, self.num_victims), (self.num_agents, 1)) 31 | 32 | return np.subtract(victim_pos_array, rs_pos_array) 33 | 34 | def is_seen(self, vfd_list, raw_sensation, vfd_status): 35 | vfd_mat = np.tile(vfd_list.reshape(self.num_agents, 1), (1, self.num_victims)) 36 | in_vfd_condition = np.zeros_like(raw_sensation) 37 | in_vfd_condition[:, :, 0] = in_vfd_condition[:, :, 1] = vfd_mat 38 | tuple_cond = np.abs(raw_sensation) <= in_vfd_condition 39 | in_vfd = np.logical_and(tuple_cond[:, :, 0], tuple_cond[:, :, 1]) 40 | 41 | idx_vfd_status = in_vfd_condition + raw_sensation 42 | 43 | vfd_status_condition = in_vfd.copy() 44 | for agent_id in range(len(vfd_list)): 45 | for victim_id, first_cond in enumerate(in_vfd[agent_id]): 46 | if first_cond: 47 | if vfd_status[agent_id][int(idx_vfd_status[agent_id, victim_id][0]), 48 | int(idx_vfd_status[agent_id, victim_id][1])]: 49 | vfd_status_condition[agent_id, victim_id] = True 50 | return vfd_status_condition 51 | -------------------------------------------------------------------------------- /MASAR/search_algorithms.py: -------------------------------------------------------------------------------- 1 | from scipy.stats import wrapcauchy, levy_stable 2 | import numpy as np 3 | 4 | 5 | class SearchAlgorithms: 6 | def __init__(self, max_vfd, init_pos, num_actions, num_rows, num_cols): 7 | self.action = None 8 | self.max_VisualField = max_vfd 9 | self.wereHere = np.ones((num_rows, num_cols)) 10 | 11 | self.init_pos = init_pos 12 | self.curr_Pos = self.init_pos 13 | self.old_Pos = self.curr_Pos 14 | 15 | self.num_actions = num_actions 16 | self.num_rows = num_rows 17 | self.num_cols = num_cols 18 | 19 | self.up, self.down, self.left, self.right = (-1, 0), (1, 0), (0, -1), (0, 1) 20 | self.actions = [self.up, self.down, self.left, self.right] 21 | # Levy Parameters 22 | self.prev_action = None 23 | self.alpha = 0.8 24 | self.beta = 0 25 | self.scale_to_var = 1/(np.sqrt(2)) 26 | self.levy_dist = levy_stable(self.alpha, self.beta, scale=self.scale_to_var) 27 | self.decision_wait_time = 0 28 | 29 | # Turning Angle Parameters 30 | self.rho = 0.1 # 0 is completely fair 1 is biased to previous action 31 | self.wrap_dist = wrapcauchy(self.rho) 32 | self.curr_angle = np.random.random()*np.pi*2 33 | self.dest_comb = 20 34 | self.path_to_take = None 35 | self.path_taken_counter = 0 36 | 37 | def straight_move(self, idx, were_here, env_map): 38 | """ 39 | takes index to see if the agent is in search mode 40 | wereHere is a matrix that tracks the visited cells 41 | """ 42 | if idx == (2 * self.max_VisualField + 1) ** 2: 43 | if len(np.argwhere(were_here)) > 0: 44 | for loc in np.argwhere(were_here): 45 | if np.sqrt((loc[0] - self.old_Pos[0]) ** 2 + (loc[1] - self.old_Pos[1]) ** 2) == 1: 46 | next_loc = loc 47 | self.wereHere[self.curr_Pos[0], self.curr_Pos[1]] = 0 48 | 49 | if env_map[next_loc[0], next_loc[1]] == 0: 50 | self.curr_Pos = next_loc 51 | break 52 | else: 53 | continue 54 | 55 | def random_walk(self, idx, pos, speed, env_map): 56 | """ 57 | takes current location and its relevant index in the Q table 58 | as well as agents speed 59 | If the index is for the search mode, 60 | it randomly selects one of the 4 possible directions 61 | """ 62 | row_lim = self.num_rows - 1 63 | col_lim = self.num_cols - 1 64 | row = pos[0] 65 | col = pos[1] 66 | if idx == (2 * self.max_VisualField + 1) ** 2: 67 | self.action = np.random.randint(self.num_actions) 68 | 69 | if self.action == 0: # up 70 | next_loc = [max(row - speed, 0), col] 71 | elif self.action == 1: # down 72 | next_loc = [min(row + speed, row_lim), col] 73 | elif self.action == 2: # right 74 | next_loc = [row, min(col + speed, col_lim)] 75 | elif self.action == 3: # left 76 | next_loc = [row, max(col - speed, 0)] 77 | 78 | if env_map[next_loc[0], next_loc[1]] == 0: 79 | self.curr_Pos = next_loc 80 | 81 | # The following three methods were developed by Mohamed Martini 82 | def get_nearby_location_visits(self, grid_cells): 83 | """ takes the grid of cells (represented by a 2D numpy array) 84 | returns a of the locations (as x,y tuples) which are 1 unit away 85 | (ie: is UP, DOWN, LEFT, RIGHT of current agent position) together with their 86 | visit count which is an integer representing the number of times the cell has been visited 87 | """ 88 | nearby_cell_visits = list() 89 | for row in range(0, len(grid_cells)): 90 | for col in range(0, len(grid_cells[row, :])): 91 | visit_num = grid_cells[row, col] 92 | loc = [row, col] 93 | if np.linalg.norm(np.subtract(loc, self.old_Pos)) == 1: 94 | loc_visits = [loc, visit_num] 95 | nearby_cell_visits.append(loc_visits) 96 | return nearby_cell_visits 97 | 98 | def get_minimum_visited_cells(self, location_visits): 99 | """ takes a list of tuples whose elements represent locations in the grid world together 100 | with their visit counts and returns an array of locations which have the minimum number 101 | of visits 102 | """ 103 | min_visits = np.inf # or any very large number (greater than any expected visit count) 104 | min_visited_locations = [] 105 | # find the minimum visited number for cells corresponding with the passed locations 106 | for loc_visits in location_visits: 107 | times_visited = loc_visits[1] 108 | if times_visited < min_visits: 109 | min_visits = times_visited 110 | # filter the locations corresponding with this minimum visit number 111 | for loc in location_visits: 112 | if loc[1] == min_visits: 113 | min_visited_locations.append(loc) 114 | return min_visited_locations 115 | 116 | def ant_colony_move(self, cells_visited, idx, env_map): 117 | """ takes a 2D array representing the visit count for cells in the grid world 118 | and increments the current agents position toward the least visited neighboring cell 119 | """ 120 | # increment the cell visit number 121 | cells_visited[self.old_Pos[0], self.old_Pos[1]] += 1 122 | if idx == (2 * self.max_VisualField + 1) ** 2: 123 | nearby_location_visits = self.get_nearby_location_visits(cells_visited) 124 | least_visited_locations = self.get_minimum_visited_cells(nearby_location_visits) 125 | # select a random location from the least visit locations nearby 126 | next_loc_ind = np.random.randint(0, len(least_visited_locations)) 127 | next_loc = least_visited_locations[next_loc_ind][0] 128 | if env_map[next_loc[0], next_loc[1]] == 0: 129 | self.curr_Pos = next_loc 130 | 131 | # The following method was developed by Fernando Mazzoni 132 | def account_for_boundary(self): 133 | row, col = self.curr_Pos 134 | actions = self.actions.copy() 135 | 136 | if row == 0: 137 | actions[0] = actions[1] 138 | elif row == self.num_rows - 1: 139 | actions[1] = actions[0] 140 | if col == 0: 141 | actions[2] = actions[3] 142 | elif col == self.num_cols - 1: 143 | actions[3] = actions[2] 144 | 145 | check_cells = [[drow+row, dcol+col] for drow, dcol in [list(action) for action in actions]] 146 | 147 | for n, cell_ in enumerate(check_cells): 148 | m = [1, 0, 3, 2] # get the opposite action 149 | if ((cell_[0] == 0 or cell_[0] == self.num_cols - 1) or 150 | (cell_[1] == 0 or cell_[1] == self.num_rows - 1)): 151 | actions[n] = actions[m[n]] 152 | 153 | return actions 154 | 155 | def levy_walk(self, env_map): 156 | """ 157 | Randomly gets next step based on set of actions. 158 | Boundary conditions are reflective 159 | """ 160 | prev_action = self.prev_action 161 | 162 | # Levy Walk each decision step continues otherwise repeat last action 163 | if prev_action is None or self.decision_wait_time == 0: 164 | r = self.levy_dist.rvs() 165 | r = np.round(np.abs(r)) 166 | self.decision_wait_time = int(r) 167 | 168 | r_angle = self.wrap_dist.rvs() 169 | self.curr_angle = (self.curr_angle + r_angle) % (2*np.pi) 170 | px = np.cos(self.curr_angle) 171 | py = np.sin(self.curr_angle) 172 | 173 | possible_actions = [[int(-1*int(np.sign(py))), 0], [0, int(np.sign(px))]] 174 | px1 = abs(px) 175 | py1 = abs(py) 176 | self.path_to_take = [possible_actions[i] for i in np.random.choice([0, 1], 177 | size=self.dest_comb, 178 | p=[py1**2, px1**2])] 179 | else: 180 | self.decision_wait_time -= 1 181 | 182 | actions_in_boundary = self.account_for_boundary() 183 | desired_action = self.path_to_take[self.path_taken_counter] 184 | self.path_taken_counter = (1+self.path_taken_counter) % self.dest_comb 185 | 186 | if desired_action in actions_in_boundary: 187 | action = desired_action 188 | else: 189 | if np.abs(desired_action[1]) == 1: 190 | dtheta = self.curr_angle - np.pi/2 191 | if dtheta > 0: 192 | self.curr_angle = np.pi/2 - dtheta 193 | else: 194 | self.curr_angle = np.pi/2 + np.abs(dtheta) 195 | elif np.abs(desired_action[0]) == 1: 196 | dtheta = self.curr_angle - np.pi 197 | if dtheta > 0: 198 | self.curr_angle = np.pi - dtheta 199 | else: 200 | self.curr_angle = np.pi + np.abs(dtheta) 201 | self.path_to_take = [(int(desired_action[0]*-1), int(desired_action[1]*-1)) 202 | if x == desired_action else x for x in self.path_to_take] 203 | 204 | action = [int(desired_action[0]*-1), int(desired_action[1]*-1)] 205 | 206 | next_loc = [self.curr_Pos[0] + action[0], self.curr_Pos[1] + action[1]] 207 | self.prev_action = action 208 | if env_map[next_loc[0], next_loc[1]] == 0: 209 | self.curr_Pos = next_loc 210 | 211 | -------------------------------------------------------------------------------- /MASAR/typhoon.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/typhoon.jpg -------------------------------------------------------------------------------- /MASAR/victim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/victim.png -------------------------------------------------------------------------------- /MASAR/visualizer1.1.py: -------------------------------------------------------------------------------- 1 | from gridworld_multi_agent1_1 import animate 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import h5py 5 | env_map = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 6 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 7 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 8 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 9 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 10 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 11 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 12 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 13 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 14 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 15 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 16 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 17 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 18 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 19 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 20 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 21 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 22 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 23 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 24 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]) 25 | exp_name = '4RS_1000episodes' 26 | # exp_name = '2R_NS' 27 | # exp_name = '2R_2S' 28 | # exp_name = '2R_2S_A2A' 29 | # exp_name = '2R_NS_1V' 30 | # exp_name = '2R_2S_1V' 31 | # exp_name = '2R_2S_A2A_1V' 32 | # exp_name = '5R_5S' 33 | 34 | plt.rcParams.update({'font.size': 22}) 35 | file_name = f'multi_agent_Q_learning_{exp_name}.hdf5' 36 | 37 | run_animate = False 38 | 39 | rescue_team_Traj = [] 40 | rescue_team_VFD_status = [] 41 | rescue_team_RewSum = [] 42 | rescue_team_Steps = [] 43 | rescue_team_RewSum_seen = [] 44 | rescue_team_Steps_seen = [] 45 | rescue_team_Q = [] 46 | victims_Traj = [] 47 | 48 | with h5py.File(file_name, 'r') as f: 49 | 50 | for idx in range(len(f['RS_VFD'])): 51 | 52 | rescue_team_Traj.append(f[f'RS{idx}_trajectory']) 53 | rescue_team_VFD_status.append(np.asarray(f[f'RS{idx}_VFD_status'])) 54 | rescue_team_RewSum.append(f[f'RS{idx}_reward']) 55 | rescue_team_Steps.append(f[f'RS{idx}_steps']) 56 | rescue_team_RewSum_seen.append(f[f'RS{idx}_reward_seen']) 57 | rescue_team_Steps_seen.append(f[f'RS{idx}_steps_seen']) 58 | rescue_team_Q.append(f[f'RS{idx}_Q']) 59 | for idx in range(f['victims_num'][0]): 60 | victims_Traj.append(f[f'victim{idx}_trajectory']) 61 | 62 | if run_animate: 63 | animate(np.asarray(rescue_team_Traj), np.asarray(victims_Traj), 64 | np.asarray(f['RS_VFD']), rescue_team_VFD_status, f['RS_ROLES'], env_map, wait_time=.01) 65 | 66 | rescue_team_legends = [] 67 | 68 | plt.figure('reward') 69 | for idx in range(len(f['RS_VFD'])): 70 | plt.plot(np.asarray(rescue_team_RewSum[idx])[::10]) 71 | rescue_team_legends.append(f'Agent {idx+1}') 72 | plt.xlabel('Number of episodes') 73 | plt.ylabel('Rescue Team Total Rewards') 74 | plt.legend(rescue_team_legends) 75 | 76 | plt.figure('reward_seen') 77 | for idx in range(len(f['RS_VFD'])): 78 | plt.plot(np.asarray(rescue_team_RewSum_seen[idx])) 79 | plt.xlabel('Number of Episodes') 80 | plt.ylabel('Rescue Team Rewards During Victim Visit') 81 | plt.legend(rescue_team_legends) 82 | 83 | plt.figure('steps') 84 | for idx in range(len(f['RS_VFD'])): 85 | plt.plot(np.asarray(rescue_team_Steps[idx])) 86 | plt.xlabel('Number of Episodes') 87 | plt.ylabel('Rescue Team Total Steps') 88 | plt.legend(rescue_team_legends) 89 | 90 | plt.figure('steps_seen') 91 | for idx in range(len(f['RS_VFD'])): 92 | plt.plot(np.asarray(rescue_team_Steps_seen[idx])) 93 | plt.xlabel('Number of Episodes') 94 | plt.ylabel('Rescue Team Steps During Victim Visit') 95 | plt.legend(rescue_team_legends) 96 | 97 | plt.show() 98 | -------------------------------------------------------------------------------- /MASAR/wall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MASAR/wall.png -------------------------------------------------------------------------------- /MRTA_new/Clustering.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.cluster.vq import kmeans, vq 3 | from scipy.optimize import linear_sum_assignment 4 | from a_star import AStarPlanner 5 | import collections 6 | 7 | def Clustering(num_clusters, task_list, walls_x, walls_y): 8 | """ 9 | :param num_clusters: is equal to the number of regions/rooms in the environment 10 | :param task_list: list of the tasks in the environment 11 | :return: 12 | """ 13 | num_rows = 20 14 | num_cols = 20 15 | tasks_pos = [] 16 | for task in task_list: 17 | tasks_pos.append(task.pos) 18 | 19 | # k-means 20 | clusters_coord = kmeans(tasks_pos, num_clusters)[0] 21 | clusters = vq(tasks_pos, clusters_coord)[0] 22 | 23 | for cluster_idx, cluster in enumerate(clusters_coord): 24 | 25 | int_clusters_coord = [int(cluster[0]), int(cluster[1])] 26 | random_neighbor = [[min(num_rows-1, int_clusters_coord[0] + 1), int_clusters_coord[1]], 27 | [max(0, int_clusters_coord[0] - 1), int_clusters_coord[1]], 28 | [int_clusters_coord[0], min(num_cols-1, int_clusters_coord[1] + 1)], 29 | [int_clusters_coord[0], max(0, int_clusters_coord[1] - 1)], 30 | [min(num_rows-1, int_clusters_coord[0] + 1), min(num_cols-1, int_clusters_coord[1] + 1)], 31 | [max(0, int_clusters_coord[0] - 1), max(0, int_clusters_coord[1] - 1)], 32 | [min(num_rows-1, int_clusters_coord[0] + 1), max(0, int_clusters_coord[1] - 1)], 33 | [max(0, int_clusters_coord[0] - 1), min(num_cols-1, int_clusters_coord[1] + 1)]] 34 | 35 | for idx in range(len(random_neighbor)): 36 | if tuple(random_neighbor[idx]) in list(zip(walls_x, walls_y)): 37 | continue 38 | else: 39 | int_clusters_coord = random_neighbor[idx] 40 | break 41 | 42 | clusters_coord[cluster_idx] = int_clusters_coord 43 | 44 | grid_size = 1 # [m] 45 | robot_radius = .5 # [m] 46 | a_star = AStarPlanner(walls_x, walls_y, grid_size, robot_radius) 47 | a_star_NW = AStarPlanner([], [], grid_size, robot_radius) 48 | # assign robot into the relevant cluster 49 | for idx, cluster_id in enumerate(clusters): 50 | task_list[idx].cluster_id = cluster_id 51 | rx, ry = a_star.planning(clusters_coord[cluster_id][0], clusters_coord[cluster_id][1], 52 | task_list[idx].pos[0], task_list[idx].pos[1]) 53 | rx_NW, ry_NW = a_star_NW.planning(clusters_coord[cluster_id][0], clusters_coord[cluster_id][1], 54 | task_list[idx].pos[0], task_list[idx].pos[1]) 55 | task_list[idx].cluster_dist = [[x, y] for x, y in zip(rx[::-1], ry[::-1])] 56 | task_list[idx].cluster_dist_NW = [[x, y] for x, y in zip(rx_NW[::-1], ry_NW[::-1])] 57 | return clusters, clusters_coord 58 | 59 | 60 | def weight_calc(robot_list, task_list, clusters, num_clusters, psi=.95): 61 | ''' 62 | :param robot_list: list of the robot objects (rescue team) 63 | :param task_list: 64 | :param clusters: list of the clusters calculated by Clustering 65 | :param psi: Coefficient for fulfilled tasks 66 | :return: the cost that is the opposite of the number of tasks each robot can save at each cluster 67 | ''' 68 | cost = np.zeros((len(robot_list), num_clusters)) 69 | for robot in robot_list: 70 | for task_id, cluster_id in enumerate(clusters): 71 | 72 | if task_id in robot.tasks: 73 | caps = [cap in robot.capabilities for cap in task_list[task_id].capabilities] 74 | robot.w_rc[cluster_id] += (1-psi) * sum(caps) 75 | if task_id in robot.tasks_full: 76 | robot.w_rc[cluster_id] += psi * len(task_list[task_id].capabilities) 77 | 78 | cost[robot.id, :] = -robot.w_rc 79 | return cost 80 | 81 | 82 | def ClstrAsgn(robot_list, task_list, clusters, clusters_coord, num_clusters): 83 | cost = weight_calc(robot_list, task_list, clusters, num_clusters) 84 | robots_opt, clusters_opt = linear_sum_assignment(cost) 85 | task_list_update = task_list.copy() 86 | for robot_idx, robot in enumerate(robot_list): 87 | for cluster_idx, cluster in enumerate(clusters): 88 | if cluster == clusters_opt[robot_idx]: 89 | if task_list[cluster_idx].id in robot.tasks: # check for the ability to rescue 90 | # Assign the tasks to the robot for rescue 91 | # check for the wall in cluster 92 | if len(task_list[cluster_idx].cluster_dist) <= len(task_list[cluster_idx].cluster_dist_NW): 93 | robot.tasks_init.append(task_list[cluster_idx].id) 94 | robot.tasks_init_dist.append(len(task_list[cluster_idx].cluster_dist)) 95 | caps = [cap in robot.capabilities for cap in task_list[cluster_idx].capabilities] 96 | 97 | for cap_id, cap in enumerate(caps): 98 | if cap: 99 | task_list[cluster_idx].rescued[cap_id] = True 100 | 101 | # Update the list of the remaining tasks and set the task's rescue flag True 102 | task_list_update.remove(task_list[cluster_idx]) 103 | 104 | # Send the robot to cluster's center 105 | robot.pos = clusters_coord[cluster] 106 | # Sort the assignment on the basis of distance to the cluster coordination 107 | robot.tasks_init = [task_id for _, task_id in sorted(zip(robot.tasks_init_dist, robot.tasks_init))] 108 | # check for the wall in cluster 109 | for task_id, task in enumerate(robot.tasks_init): 110 | if len(task_list[task].cluster_dist) > len(task_list[task].cluster_dist_NW): 111 | robot.tasks_init.remove(task) 112 | del robot.tasks_init_dist[task_id] 113 | 114 | return task_list_update 115 | -------------------------------------------------------------------------------- /MRTA_new/Crazyflie.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/Crazyflie.JPG -------------------------------------------------------------------------------- /MRTA_new/MRTA.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/MRTA.hdf5 -------------------------------------------------------------------------------- /MRTA_new/MRTA_FindSurvivors_env_map1_2R20V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/MRTA_FindSurvivors_env_map1_2R20V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/MRTA_FindSurvivors_env_map1_3R30V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/MRTA_FindSurvivors_env_map1_3R30V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/MRTA_FindSurvivors_env_map1_4R40V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/MRTA_FindSurvivors_env_map1_4R40V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/MRTA_FindSurvivors_env_map1_5R50V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/MRTA_FindSurvivors_env_map1_5R50V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/PerfAnalysis.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from a_star import AStarPlanner 3 | from scipy.optimize import linear_sum_assignment 4 | 5 | 6 | def make_span_calc(robot_list, task_list): 7 | for robot in robot_list: 8 | # robot.travel_cost() 9 | for task_id in robot.tasks_init: 10 | for cap in task_list[task_id].capabilities: 11 | if cap in robot.capabilities: 12 | # Add the time for the accomplished task to the robot's make span 13 | robot.make_span.append(task_list[task_id].make_span[cap]) 14 | else: 15 | # Accumulate the robot's abort time for the tasks he couldn't accomplish in the cluster 16 | robot.abort_time += task_list[task_id].make_span[cap] 17 | 18 | 19 | def time_cost(robot_list, tasks_new, alpha=.2, beta=.25, gamma=.55): 20 | num_robots = len(robot_list) 21 | num_rem_tasks = len(tasks_new) 22 | cost = np.ones((num_robots, num_rem_tasks)) 23 | for robot in robot_list: 24 | if len(robot.make_span) == 0: 25 | robot.make_span.append(0) 26 | for task_id, task in enumerate(tasks_new): 27 | if task.id in robot.tasks: # Check with the capability analyser output 28 | cost[robot.id, task_id] = ((gamma * np.max(robot.make_span)) + 29 | (alpha * robot.travel_time) + 30 | (beta * robot.abort_time / robot.num_sensors)) 31 | else: 32 | cost[robot.id, task_id] = 1e20 33 | 34 | return cost 35 | 36 | 37 | def VictimAssign(robot_list, task_list, tasks_new): 38 | 39 | make_span_calc(robot_list, task_list) 40 | cost = time_cost(robot_list, tasks_new, alpha=.0, beta=.99, gamma=.01) 41 | robots_opt, tasks_opt = linear_sum_assignment(cost) 42 | for task, robot in enumerate(robots_opt): 43 | robot_list[robot].tasks_final.append(tasks_new[tasks_opt[task]].id) 44 | 45 | caps = [cap in robot_list[robot].capabilities for cap in tasks_new[tasks_opt[task]].capabilities] 46 | 47 | for cap_id, cap in enumerate(caps): 48 | if cap: 49 | tasks_new[tasks_opt[task]].rescued[cap_id] = True 50 | task_list[task_list.index(tasks_new[tasks_opt[task]])].rescued[cap_id] = True 51 | for task in tasks_new: 52 | if all(task.rescued): 53 | tasks_new.remove(task) 54 | 55 | return tasks_new 56 | 57 | 58 | def RobotAssign(robot_list, task_list_new, task_list, walls_x, walls_y): 59 | grid_size = 1 # [m] 60 | robot_radius = .5 # [m] 61 | a_star = AStarPlanner(walls_x, walls_y, grid_size, robot_radius) 62 | for task in task_list: 63 | 64 | for idx, status in enumerate(task.rescued): 65 | if not status: 66 | dist = [] 67 | for candid in task.candidates[idx]: 68 | # Where is this (candidate) robot now 69 | if len(robot_list[candid].tasks_final) > 0: 70 | dist.append(task_list[robot_list[candid].tasks_final[-1]].pos) 71 | elif len(robot_list[candid].tasks_init) > 0: 72 | dist.append(task_list[robot_list[candid].tasks_init[-1]].pos) 73 | temp = np.inf 74 | id = np.nan 75 | for candid_id, d in enumerate(dist): 76 | rx, ry = a_star.planning(d[0], d[1], task.pos[0], task.pos[1]) 77 | ManhattanDist = len(rx) 78 | if ManhattanDist < temp: 79 | temp = ManhattanDist 80 | id = task.candidates[idx][candid_id] 81 | if not np.isnan(id): 82 | robot_list[id].tasks_finalized.append(task.id) 83 | if task.id in robot_list[id].tasks_full: 84 | task.rescued = np.ones_like(task.rescued, dtype=bool).tolist() 85 | break 86 | else: 87 | task.rescued[idx] = True 88 | for task in task_list_new: 89 | if all(task.rescued): 90 | task_list_new.remove(task) 91 | if len(task_list_new) and all(task_list_new[0].rescued): 92 | del task_list_new[0] 93 | return task_list_new 94 | -------------------------------------------------------------------------------- /MRTA_new/ReqmentAnalysis.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def ReqmentAnalysis(robot_list, task_list, robot_reqs, task_reqs): 5 | num_robots = len(robot_list) 6 | num_tasks = len(task_list) 7 | 8 | robot_and_task = task_reqs @ np.transpose(robot_reqs) 9 | task_reqs_sum = np.tile(np.sum(task_reqs, axis=1).reshape(num_tasks, 1), (1, num_robots)) 10 | 11 | perfect_fulfill = np.logical_and(np.zeros_like(robot_and_task) < robot_and_task, robot_and_task == task_reqs_sum) 12 | perfect_fulfill_x, perfect_fulfill_y = np.where(np.transpose(perfect_fulfill)) 13 | perfect_fulfill_list = [[b for a, b in zip(perfect_fulfill_x, perfect_fulfill_y) if a == i] for i in 14 | range(max(perfect_fulfill_x)+1)] 15 | 16 | partial_fulfill = np.logical_and(np.zeros_like(robot_and_task) < robot_and_task, robot_and_task < task_reqs_sum) 17 | partial_fulfill_x, partial_fulfill_y = np.where(np.transpose(partial_fulfill)) 18 | partial_fulfill_list = [[b for a, b in zip(partial_fulfill_x, partial_fulfill_y) if a == i] for i in 19 | range(max(partial_fulfill_x) + 1)] 20 | 21 | [setattr(obj, attribute, value) for obj, value_1, value_2 in zip(robot_list, perfect_fulfill_list, partial_fulfill_list) 22 | for attribute, value in [("tasks_full", value_1), ("tasks", value_2)]] 23 | 24 | overall_fulfillment = np.logical_or(perfect_fulfill, partial_fulfill) 25 | who_helps_who = np.argwhere(overall_fulfillment) 26 | for candidate in who_helps_who: 27 | task_id = candidate[0] 28 | robot_id = candidate[1] 29 | check_req = np.logical_and(task_reqs[task_id, :], robot_reqs[robot_id, :]) 30 | what_reqs = np.where(check_req)[0].tolist() 31 | 32 | for what_req in what_reqs: 33 | task_list[task_id].vectorized_candidates[what_req].append(robot_id) 34 | for task_id in range(num_tasks): 35 | for candidates_id in np.argwhere(task_reqs[task_id, :]): 36 | task_list[task_id].candidates.append(task_list[task_id].vectorized_candidates[candidates_id[0]]) 37 | 38 | def MissingCap(task_list, robot_reqs): 39 | unavaialable_caps = np.logical_not(np.logical_or.reduce(robot_reqs, 0)) 40 | unavaialable_caps_ids = np.argwhere(unavaialable_caps)[0] 41 | missing_cap = [] 42 | for task in task_list: 43 | for id in unavaialable_caps_ids: 44 | if task.vectorized_cap[id]: 45 | task.vectorized_rescued[id] = True 46 | missing_cap.append({task.id: id}) 47 | print(missing_cap) 48 | return missing_cap 49 | -------------------------------------------------------------------------------- /MRTA_new/Times_msmrta_FindSurvivors_env_map0_30R100V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/Times_msmrta_FindSurvivors_env_map0_30R100V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/Times_msmrta_FindSurvivors_env_map1_30R100V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/Times_msmrta_FindSurvivors_env_map1_30R100V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/Times_msmrta_FindSurvivors_env_map2_30R100V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/Times_msmrta_FindSurvivors_env_map2_30R100V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/Times_msmrta_FindSurvivors_env_map3_30R100V.hdf5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/Times_msmrta_FindSurvivors_env_map3_30R100V.hdf5 -------------------------------------------------------------------------------- /MRTA_new/TurtleBot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/TurtleBot.png -------------------------------------------------------------------------------- /MRTA_new/a_star.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | A* grid planning 4 | 5 | author: Atsushi Sakai(@Atsushi_twi) 6 | Nikos Kanargias (nkana@tee.gr) 7 | 8 | See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) 9 | 10 | """ 11 | 12 | import math 13 | import numpy as np 14 | import matplotlib.pyplot as plt 15 | import h5py 16 | from gridworld_multi_agent1_1 import animate 17 | show_animation = False 18 | from victim import Victim 19 | 20 | capabilities = ['FirstAids', 'DebrisRemover', 'OxygenCylinder', 'Defuser', 'Manipulator', 'FireExtinguisher'] 21 | make_span = {'FirstAids': 15, 'DebrisRemover': 30, 'OxygenCylinder': 20, 22 | 'Defuser': 10, 'Manipulator': 45, 'FireExtinguisher': 35} 23 | 24 | class AStarPlanner: 25 | 26 | def __init__(self, ox, oy, resolution, rr): 27 | """ 28 | Initialize grid map for a star planning 29 | 30 | ox: x position list of Obstacles [m] 31 | oy: y position list of Obstacles [m] 32 | resolution: grid resolution [m] 33 | rr: robot radius[m] 34 | """ 35 | 36 | self.resolution = resolution 37 | self.rr = rr 38 | self.min_x, self.min_y = 0, 0 39 | self.max_x, self.max_y = 0, 0 40 | self.obstacle_map = None 41 | self.x_width, self.y_width = 0, 0 42 | self.motion = self.get_motion_model() 43 | self.calc_obstacle_map(ox, oy) 44 | 45 | class Node: 46 | def __init__(self, x, y, cost, parent_index): 47 | self.x = x # index of grid 48 | self.y = y # index of grid 49 | self.cost = cost 50 | self.parent_index = parent_index 51 | 52 | def __str__(self): 53 | return str(self.x) + "," + str(self.y) + "," + str( 54 | self.cost) + "," + str(self.parent_index) 55 | 56 | def planning(self, sx, sy, gx, gy): 57 | """ 58 | A star path search 59 | 60 | input: 61 | s_x: start x position [m] 62 | s_y: start y position [m] 63 | gx: goal x position [m] 64 | gy: goal y position [m] 65 | 66 | output: 67 | rx: x position list of the final path 68 | ry: y position list of the final path 69 | """ 70 | 71 | start_node = self.Node(self.calc_xy_index(sx, self.min_x), 72 | self.calc_xy_index(sy, self.min_y), 0.0, -1) 73 | goal_node = self.Node(self.calc_xy_index(gx, self.min_x), 74 | self.calc_xy_index(gy, self.min_y), 0.0, -1) 75 | 76 | open_set, closed_set = dict(), dict() 77 | open_set[self.calc_grid_index(start_node)] = start_node 78 | 79 | while True: 80 | if len(open_set) == 0: 81 | print("Open set is empty..", start_node) 82 | break 83 | 84 | c_id = min( 85 | open_set, 86 | key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, 87 | open_set[ 88 | o])) 89 | current = open_set[c_id] 90 | 91 | # show graph 92 | if show_animation: # pragma: no cover 93 | plt.plot(self.calc_grid_position(current.x, self.min_x), 94 | self.calc_grid_position(current.y, self.min_y), "xc") 95 | # for stopping simulation with the esc key. 96 | plt.gcf().canvas.mpl_connect('key_release_event', 97 | lambda event: [exit( 98 | 0) if event.key == 'escape' else None]) 99 | if len(closed_set.keys()) % 10 == 0: 100 | plt.pause(0.001) 101 | 102 | if current.x == goal_node.x and current.y == goal_node.y: 103 | print("Find goal") 104 | goal_node.parent_index = current.parent_index 105 | goal_node.cost = current.cost 106 | break 107 | 108 | # Remove the item from the open set 109 | del open_set[c_id] 110 | 111 | # Add it to the closed set 112 | closed_set[c_id] = current 113 | 114 | # expand_grid search grid based on motion model 115 | for i, _ in enumerate(self.motion): 116 | node = self.Node(current.x + self.motion[i][0], 117 | current.y + self.motion[i][1], 118 | current.cost + self.motion[i][2], c_id) 119 | n_id = self.calc_grid_index(node) 120 | 121 | # If the node is not safe, do nothing 122 | if not self.verify_node(node): 123 | continue 124 | 125 | if n_id in closed_set: 126 | continue 127 | 128 | if n_id not in open_set: 129 | open_set[n_id] = node # discovered a new node 130 | else: 131 | if open_set[n_id].cost > node.cost: 132 | # This path is the best until now. record it 133 | open_set[n_id] = node 134 | 135 | rx, ry = self.calc_final_path(goal_node, closed_set) 136 | 137 | return rx, ry 138 | 139 | def calc_final_path(self, goal_node, closed_set): 140 | # generate final course 141 | rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ 142 | self.calc_grid_position(goal_node.y, self.min_y)] 143 | parent_index = goal_node.parent_index 144 | while parent_index != -1: 145 | n = closed_set[parent_index] 146 | rx.append(self.calc_grid_position(n.x, self.min_x)) 147 | ry.append(self.calc_grid_position(n.y, self.min_y)) 148 | parent_index = n.parent_index 149 | 150 | return rx, ry 151 | 152 | @staticmethod 153 | def calc_heuristic(n1, n2): 154 | w = 1.0 # weight of heuristic 155 | d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) 156 | return d 157 | 158 | def calc_grid_position(self, index, min_position): 159 | """ 160 | calc grid position 161 | 162 | :param index: 163 | :param min_position: 164 | :return: 165 | """ 166 | pos = index * self.resolution + min_position 167 | return pos 168 | 169 | def calc_xy_index(self, position, min_pos): 170 | return round((position - min_pos) / self.resolution) 171 | 172 | def calc_grid_index(self, node): 173 | return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) 174 | 175 | def verify_node(self, node): 176 | px = self.calc_grid_position(node.x, self.min_x) 177 | py = self.calc_grid_position(node.y, self.min_y) 178 | 179 | if px < self.min_x: 180 | return False 181 | elif py < self.min_y: 182 | return False 183 | elif px >= self.max_x: 184 | return False 185 | elif py >= self.max_y: 186 | return False 187 | 188 | # collision check 189 | if self.obstacle_map[node.x][node.y]: 190 | return False 191 | 192 | return True 193 | 194 | def calc_obstacle_map(self, ox, oy): 195 | if len(ox) == 0 and len(oy) == 0: 196 | self.min_x = 0 197 | self.min_y = 0 198 | self.max_x = 19 199 | self.max_y = 19 200 | print("min_x:", self.min_x) 201 | print("min_y:", self.min_y) 202 | print("max_x:", self.max_x) 203 | print("max_y:", self.max_y) 204 | 205 | self.x_width = round((self.max_x - self.min_x) / self.resolution) 206 | self.y_width = round((self.max_y - self.min_y) / self.resolution) 207 | print("x_width:", self.x_width) 208 | print("y_width:", self.y_width) 209 | 210 | self.obstacle_map = np.zeros((self.x_width, self.y_width), dtype=bool).tolist() 211 | 212 | for ix in range(self.x_width): 213 | x = self.calc_grid_position(ix, self.min_x) 214 | for iy in range(self.y_width): 215 | y = self.calc_grid_position(iy, self.min_y) 216 | else: 217 | self.min_x = round(min(ox)) 218 | self.min_y = round(min(oy)) 219 | self.max_x = round(max(ox)) 220 | self.max_y = round(max(oy)) 221 | print("min_x:", self.min_x) 222 | print("min_y:", self.min_y) 223 | print("max_x:", self.max_x) 224 | print("max_y:", self.max_y) 225 | 226 | self.x_width = round((self.max_x - self.min_x) / self.resolution) 227 | self.y_width = round((self.max_y - self.min_y) / self.resolution) 228 | print("x_width:", self.x_width) 229 | print("y_width:", self.y_width) 230 | 231 | # obstacle map generation 232 | self.obstacle_map = [[False for _ in range(self.y_width)] 233 | for _ in range(self.x_width)] 234 | for ix in range(self.x_width): 235 | x = self.calc_grid_position(ix, self.min_x) 236 | for iy in range(self.y_width): 237 | y = self.calc_grid_position(iy, self.min_y) 238 | for iox, ioy in zip(ox, oy): 239 | d = math.hypot(iox - x, ioy - y) 240 | if d <= self.rr: 241 | self.obstacle_map[ix][iy] = True 242 | break 243 | 244 | @staticmethod 245 | def get_motion_model(): 246 | # dx, dy, cost 247 | motion = [[1, 0, 1], 248 | [0, 1, 1], 249 | [-1, 0, 1], 250 | [0, -1, 1]]#, 251 | # [-1, -1, math.sqrt(2)], 252 | # [-1, 1, math.sqrt(2)], 253 | # [1, -1, math.sqrt(2)], 254 | # [1, 1, math.sqrt(2)]] 255 | return motion 256 | 257 | 258 | 259 | def main(): 260 | print(__file__ + " start!!") 261 | exp_name = 'FindSurvivors' 262 | 263 | plt.rcParams.update({'font.size': 22}) 264 | 265 | with h5py.File(f'MRTA.hdf5', 'r') as f: 266 | num_robots = np.asarray(f[f'RS_size']).tolist() 267 | num_victims = np.asarray(f[f'Victims_size']).tolist() 268 | starts = np.asarray(f[f'RS_starts']).tolist() 269 | travel2clusters = np.asarray(f[f'RS_travel2clusters']).tolist() 270 | travel2clusters = [[int(x), int(y)] for x, y in travel2clusters] 271 | # unnecessary floor puts cluster center in the wall!!!!!!!!!!! 272 | travel2clusters[1][1] += 1 273 | tasks = [] 274 | for i in range(num_robots): 275 | list_t = [] 276 | s1 = np.asarray(f[f'RS{i}_Step_1']).tolist() 277 | list_t.append(s1) 278 | s2 = np.asarray(f[f'RS{i}_Step_2']).tolist() 279 | list_t.append(s2) 280 | s3 = np.asarray(f[f'RS{i}_Step_3']).tolist() 281 | list_t.append(s3) 282 | tasks.append(list_t) 283 | 284 | file_name = f'../MASAR/multi_agent_Q_learning_{exp_name}.hdf5' 285 | 286 | grid_size = 1.0 # [m] 287 | robot_radius = .50 # [m] 288 | env_map = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 289 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 290 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 291 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 292 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 293 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 294 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 295 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 296 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 297 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 298 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 299 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 300 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 301 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 302 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 303 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 304 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 305 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 306 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 307 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]) 308 | 309 | # env_map = np.array([[0, 0, 0, 0, 0, 0, 0, 0], 310 | # [0, 0, 0, 0, 0, 0, 0, 0], 311 | # [0, 1, 1, 1, 1, 1, 1, 0], 312 | # [0, 1, 0, 0, 0, 0, 0, 0], 313 | # [0, 1, 0, 0, 0, 0, 0, 1], 314 | # [0, 1, 0, 0, 0, 0, 0, 1], 315 | # [0, 0, 0, 0, 0, 0, 0, 1], 316 | # [0, 0, 1, 1, 1, 1, 1, 1], 317 | # [0, 0, 0, 0, 0, 0, 0, 0], 318 | # [0, 0, 0, 0, 0, 0, 0, 0], 319 | # [0, 0, 1, 1, 1, 1, 1, 1], 320 | # [0, 0, 0, 0, 0, 0, 0, 1], 321 | # [1, 0, 0, 0, 1, 0, 0, 1], 322 | # [1, 0, 0, 0, 1, 0, 0, 1], 323 | # [0, 0, 0, 0, 1, 0, 0, 1], 324 | # [1, 1, 1, 1, 1, 0, 0, 0]]) 325 | 326 | # set obstacle positions 327 | oy, ox = np.where(env_map == 1) 328 | # oy = oy 329 | 330 | # sy, sx = starts[0] 331 | # gy, gx = travel2clusters[0] 332 | # a_star = AStarPlanner(ox, oy, grid_size, robot_radius) 333 | # rx, ry = a_star.planning(sx, sy, gx, gy) 334 | 335 | # if show_animation: # pragma: no cover 336 | # plt.plot(ox, oy, ".k") 337 | # plt.plot(sx, sy, "og") 338 | # plt.plot(gx, gy, "xb") 339 | # plt.grid(True) 340 | # plt.axis("equal") 341 | # rx, ry = a_star.planning(sx, sy, gx, gy) 342 | # 343 | a_star = AStarPlanner(ox, oy, grid_size, robot_radius) 344 | # start and goal position 345 | rescue_team_Traj = [[] for _ in range(num_robots)] 346 | Roles = [b'r' for _ in range(num_robots)] 347 | VFDs = [0 for _ in range(num_robots)] 348 | 349 | for num in range(num_robots): 350 | temp_task = [] 351 | for tsk in tasks[num]: 352 | temp_task += tsk 353 | tasks[num] = temp_task 354 | v0 = Victim(0, [16., 7.], make_span, ['DebrisRemover', 'OxygenCylinder'], capabilities) 355 | v1 = Victim(1, [15., 12.], make_span, ['Defuser', 'Manipulator'], capabilities) 356 | v2 = Victim(2, [6., 5.], make_span, ['DebrisRemover', 'FireExtinguisher'], capabilities) 357 | v3 = Victim(3, [11., 4.], make_span, ['OxygenCylinder', 'Manipulator'], capabilities) 358 | v4 = Victim(4, [0., 1.], make_span, ['FirstAids', 'DebrisRemover'], capabilities) 359 | v5 = Victim(5, [14., 14.], make_span, ['Manipulator', 'FireExtinguisher'], capabilities) 360 | v6 = Victim(6, [14., 12.], make_span, ['FirstAids', 'Manipulator'], capabilities) 361 | v7 = Victim(7, [3., 16.], make_span, ['FirstAids', 'Defuser'], capabilities) 362 | v8 = Victim(8, [10., 15.], make_span, ['DebrisRemover', 'Defuser'], capabilities) 363 | v9 = Victim(9, [0., 12.], make_span, ['FirstAids', 'Manipulator'], capabilities) 364 | victims = [v0, v1, v2, v3, v4, v5, v6, v7, v8, v9] 365 | with h5py.File(file_name, 'r') as f: 366 | for num in range(num_robots): 367 | sy, sx = starts[num] 368 | gx, gy = travel2clusters[num] 369 | for tsk in tasks[num]: 370 | 371 | rx, ry = a_star.planning(sx, sy, gx, gy) 372 | temp = [[y, x] for x, y in zip(rx[::-1], ry[::-1])] 373 | for pos in temp: 374 | rescue_team_Traj[num].append(pos) 375 | 376 | sy, sx = gy, gx 377 | gy, gx = victims[tsk].pos#f[f'victim{tsk}_trajectory'][0] # [m] 378 | 379 | len_max = len(rescue_team_Traj[0]) 380 | for num in range(num_robots): 381 | if len(rescue_team_Traj[num]) > len_max: 382 | len_max = len(rescue_team_Traj[num]) 383 | for num in range(num_robots): 384 | while len(rescue_team_Traj[num]) < len_max: 385 | rescue_team_Traj[num].append(rescue_team_Traj[num][-1]) 386 | 387 | victims_Traj = [] 388 | with h5py.File(file_name, 'r') as f: 389 | for idx in range(num_victims): 390 | victims_Traj.append([victims[idx].pos]) 391 | # victims_Traj.append(np.asarray(f[f'victim{idx}_trajectory']).tolist()) 392 | while len(victims_Traj[idx]) < len_max: 393 | victims_Traj[idx].append(victims_Traj[idx][-1]) 394 | if len(victims_Traj[idx]) > len_max: 395 | victims_Traj[idx] = victims_Traj[idx][:len_max] 396 | rescue_team_VFD_status = [np.ones((num_robots, 1, 1), dtype=bool) for _ in range(len_max)] 397 | 398 | animate(np.asarray(rescue_team_Traj), np.asarray(victims_Traj), 399 | np.asarray(VFDs), rescue_team_VFD_status, Roles, env_map, wait_time=0.5) 400 | 401 | if show_animation: # pragma: no cover 402 | plt.plot(rx, ry, "-r") 403 | plt.pause(0.001) 404 | plt.show() 405 | 406 | 407 | if __name__ == '__main__': 408 | main() 409 | -------------------------------------------------------------------------------- /MRTA_new/gridworld_multi_agent1_1.py: -------------------------------------------------------------------------------- 1 | import pygame as pg 2 | import numpy as np 3 | import pygame 4 | import time 5 | 6 | 7 | pygame.init() 8 | 9 | # Constants 10 | WIDTH = 800 # width of the environment (px) 11 | HEIGHT = 800 # height of the environment (px) 12 | TS = 10 # delay in msec 13 | Col_num = 20 # number of columns 14 | Row_num = 20 # number of rows 15 | 16 | # define colors 17 | bg_color = pg.Color(255, 255, 255) 18 | line_color = pg.Color(128, 128, 128) 19 | vfdr_color = pg.Color(8, 136, 8, 128) 20 | vfds_color = pg.Color(255, 165, 0, 128) 21 | vfdrs_color = pg.Color(173, 216, 230, 128) 22 | 23 | def draw_grid(scr): 24 | '''a function to draw gridlines and other objects''' 25 | # Horizontal lines 26 | for j in range(Row_num + 1): 27 | pg.draw.line(scr, line_color, (0, j * HEIGHT // Row_num), (WIDTH, j * HEIGHT // Row_num), 2) 28 | # # Vertical lines 29 | for i in range(Col_num + 1): 30 | pg.draw.line(scr, line_color, (i * WIDTH // Col_num, 0), (i * WIDTH // Col_num, HEIGHT), 2) 31 | 32 | for x1 in range(0, WIDTH, WIDTH // Col_num): 33 | for y1 in range(0, HEIGHT, HEIGHT // Row_num): 34 | rect = pg.Rect(x1, y1, WIDTH // Col_num, HEIGHT // Row_num) 35 | pg.draw.rect(scr, bg_color, rect, 1) 36 | 37 | 38 | def animate(rescue_team_traj, victims_traj, rescue_team_vfd, rescue_team_vfd_status, rescue_team_roles, env_map, wait_time): 39 | 40 | font = pg.font.SysFont('arial', 20) 41 | 42 | num_rescue_team = len(rescue_team_traj) 43 | num_victims = len(victims_traj) 44 | 45 | pg.init() # initialize pygame 46 | screen = pg.display.set_mode((WIDTH + 2, HEIGHT + 2)) # set up the screen 47 | pg.display.set_caption("gridworld") # add a caption 48 | bg = pg.Surface(screen.get_size()) # get a background surface 49 | bg = bg.convert() 50 | 51 | img_rescuer = pg.image.load('TurtleBot.png') 52 | img_mdf_r = pg.transform.scale(img_rescuer, (WIDTH // Col_num, HEIGHT // Row_num)) 53 | 54 | img_rescuer_scout = pg.image.load('typhoon.jpg') 55 | img_mdf_rs = pg.transform.scale(img_rescuer_scout, (WIDTH // Col_num, HEIGHT // Row_num)) 56 | 57 | img_scout = pg.image.load('Crazyflie.JPG') 58 | img_mdf_s = pg.transform.scale(img_scout, (WIDTH // Col_num, HEIGHT // Row_num)) 59 | 60 | img_victim = pg.image.load('victim.png') 61 | img_mdf_victim = pg.transform.scale(img_victim, (WIDTH // Col_num, HEIGHT // Row_num)) 62 | 63 | img_wall = pg.image.load('wall.png') 64 | img_mdf_wall = pg.transform.scale(img_wall, (WIDTH // Col_num, HEIGHT // Row_num)) 65 | 66 | bg.fill(bg_color) 67 | screen.blit(bg, (0, 0)) 68 | clock = pg.time.Clock() 69 | pg.display.flip() 70 | run = True 71 | while run: 72 | clock.tick(60) 73 | for event in pg.event.get(): 74 | if event.type == pg.QUIT: 75 | run = False 76 | step = -1 77 | list_victims = np.arange(num_victims).tolist() 78 | list_rescue_team = np.arange(num_rescue_team).tolist() 79 | 80 | for rescue_team_stt, victims_stt in zip(np.moveaxis(rescue_team_traj, 0, -1), 81 | np.moveaxis(victims_traj, 0, -1)): 82 | 83 | for row in range(Row_num): 84 | for col in range(Col_num): 85 | if env_map[row, col] == 1: 86 | screen.blit(img_mdf_wall, 87 | (col * (WIDTH // Col_num), 88 | row * (HEIGHT // Row_num))) 89 | step += 1 90 | for num in list_rescue_team: 91 | if str(rescue_team_roles[num]) == "b'rs'": 92 | vfd_color = vfdrs_color 93 | elif str(rescue_team_roles[num]) == "b'r'": 94 | vfd_color = vfdr_color 95 | elif str(rescue_team_roles[num]) == "b's'": 96 | vfd_color = vfds_color 97 | 98 | # rescuer visual field depth 99 | # vfd_j = 0 100 | # for j in range(int(max(rescue_team_stt[1, num] - rescue_team_vfd[num], 0)), 101 | # int(min(Col_num, rescue_team_stt[1, num] + rescue_team_vfd[num] + 1))): 102 | # vfd_i = 0 103 | # for i in range(int(max(rescue_team_stt[0, num] - rescue_team_vfd[num], 0)), 104 | # int(min(Row_num, rescue_team_stt[0, num] + rescue_team_vfd[num] + 1))): 105 | # if rescue_team_vfd_status[num][step][vfd_i, vfd_j]: 106 | # rect = pg.Rect(j * (WIDTH // Col_num), i * (HEIGHT // Row_num), 107 | # (WIDTH // Col_num), (HEIGHT // Row_num)) 108 | # pg.draw.rect(screen, vfd_color, rect) 109 | # vfd_i += 1 110 | # vfd_j += 1 111 | 112 | # agents 113 | for num in list_rescue_team: 114 | if str(rescue_team_roles[num]) == "b'rs'": 115 | img_mdf = img_mdf_rs 116 | elif str(rescue_team_roles[num]) == "b'r'": 117 | img_mdf = img_mdf_r 118 | elif str(rescue_team_roles[num]) == "b's'": 119 | img_mdf = img_mdf_s 120 | screen.blit(img_mdf, 121 | (rescue_team_stt[1, num] * (WIDTH // Col_num), 122 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 123 | screen.blit(font.render(str(num), True, (0, 0, 0)), 124 | (rescue_team_stt[1, num] * (WIDTH // Col_num), 125 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 126 | 127 | # Stop showing finished agents 128 | # if (step >= 1 and 129 | # (rescue_team_stt[:, num][0] == rescue_team_history[:, num][0] == rescue_team_traj[num, -1, 0] and 130 | # rescue_team_stt[:, num][1] == rescue_team_history[:, num][1] == rescue_team_traj[num, -1, 1])): 131 | # list_rescue_team.remove(num) 132 | 133 | for num in list_victims: 134 | screen.blit(img_mdf_victim, (victims_stt[1, num] * (WIDTH // Col_num), 135 | victims_stt[0, num] * (HEIGHT // Row_num))) 136 | screen.blit(font.render(str(num), True, (0, 0, 0)), 137 | (victims_stt[1, num] * (WIDTH // Col_num), victims_stt[0, num] * (HEIGHT // Row_num))) 138 | 139 | # Stop showing rescued victims 140 | # if step >= 1 and (victims_stt[:, num][0] == victims_history[:, num][0] == victims_traj[num, -1, 0] and 141 | # victims_stt[:, num][1] == victims_history[:, num][1] == victims_traj[num, -1, 1]): 142 | # list_victims.remove(num) 143 | 144 | draw_grid(screen) 145 | pg.display.flip() 146 | pg.display.update() 147 | time.sleep(wait_time) # wait between the shows 148 | 149 | for num in list_victims: 150 | screen.blit(bg, (victims_stt[1, num] * (WIDTH // Col_num), 151 | victims_stt[0, num] * (HEIGHT // Row_num))) 152 | 153 | for num in list_rescue_team: 154 | screen.blit(bg, (rescue_team_stt[1, num] * (WIDTH // Col_num), 155 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 156 | 157 | # rescuer visual field depths 158 | for j in range(int(max(rescue_team_stt[1, num] - rescue_team_vfd[num], 0)), 159 | int(min(Row_num, rescue_team_stt[1, num] + rescue_team_vfd[num] + 1))): 160 | for i in range(int(max(rescue_team_stt[0, num] - rescue_team_vfd[num], 0)), 161 | int(min(Col_num, rescue_team_stt[0, num] + rescue_team_vfd[num] + 1))): 162 | rect = pg.Rect(j * (WIDTH // Col_num), i * (HEIGHT // Row_num), 163 | (WIDTH // Col_num), (HEIGHT // Row_num)) 164 | pg.draw.rect(screen, bg_color, rect) 165 | 166 | victims_history = victims_stt 167 | rescue_team_history = rescue_team_stt 168 | 169 | for num in list_rescue_team: 170 | if str(rescue_team_roles[num]) == "b'rs'": 171 | img_mdf = img_mdf_rs 172 | elif str(rescue_team_roles[num]) == "b'r'": 173 | img_mdf = img_mdf_r 174 | elif str(rescue_team_roles[num]) == "b's'": 175 | img_mdf = img_mdf_s 176 | screen.blit(img_mdf, (rescue_team_traj[num, -1, 1] * (WIDTH // Col_num), 177 | rescue_team_traj[num, -1, 0] * (HEIGHT // Row_num))) 178 | for num in list_victims: 179 | screen.blit(img_mdf_victim, (victims_traj[num, -1, 1] * (WIDTH // Col_num), 180 | victims_traj[num, -1, 0] * (HEIGHT // Row_num))) 181 | 182 | draw_grid(screen) 183 | pg.display.flip() 184 | pg.display.update() 185 | run = False 186 | pg.quit() 187 | -------------------------------------------------------------------------------- /MRTA_new/main.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from scipy.spatial import Voronoi, voronoi_plot_2d 4 | from ReqmentAnalysis import ReqmentAnalysis, MissingCap 5 | from Clustering import Clustering, ClstrAsgn 6 | from maps import env_map2 7 | from robot import Robot 8 | from victim import Victim 9 | from PerfAnalysis import VictimAssign, RobotAssign 10 | import h5py 11 | 12 | 13 | capabilities = ['FirstAids', 'DebrisRemover', 'OxygenCylinder', 'Defuser', 'Manipulator', 'FireExtinguisher'] 14 | make_span = {'FirstAids': 15, 'DebrisRemover': 30, 'OxygenCylinder': 20, 15 | 'Defuser': 10, 'Manipulator': 45, 'FireExtinguisher': 35} 16 | num_clusters = 4 17 | v0 = Victim(0, [16., 7.], make_span, ['DebrisRemover', 'OxygenCylinder'], capabilities) 18 | v1 = Victim(1, [15., 12.], make_span, ['Defuser', 'Manipulator'], capabilities) 19 | v2 = Victim(2, [6., 5.], make_span, ['DebrisRemover', 'FireExtinguisher'], capabilities) 20 | v3 = Victim(3, [11., 4.], make_span, ['OxygenCylinder', 'Manipulator'], capabilities) 21 | v4 = Victim(4, [0., 1.], make_span, ['FirstAids', 'DebrisRemover'], capabilities) 22 | v5 = Victim(5, [14., 14.], make_span, ['Manipulator', 'FireExtinguisher'], capabilities) 23 | v6 = Victim(6, [14., 12.], make_span, ['FirstAids', 'Manipulator'], capabilities) 24 | v7 = Victim(7, [3., 16.], make_span, ['FirstAids', 'Defuser'], capabilities) 25 | v8 = Victim(8, [10., 15.], make_span, ['DebrisRemover', 'Defuser'], capabilities) 26 | v9 = Victim(9, [0., 12.], make_span, ['FirstAids', 'Manipulator'], capabilities) 27 | 28 | plt.rcParams.update({'font.size': 22}) 29 | 30 | env_map = env_map2 31 | num_rows, num_cols = np.shape(env_map) 32 | ox, oy = np.where(env_map == 1) 33 | 34 | victims = [v0, v1, v2, v3, v4, v5, v6, v7, v8, v9] 35 | 36 | 37 | r0 = Robot(0, [0, 9], 0.2, [], num_clusters, [], ['Defuser', 'DebrisRemover'], capabilities) 38 | r1 = Robot(1, [0, 10], 0.3, [], num_clusters, [], ['FirstAids', 'OxygenCylinder', 'Manipulator'], capabilities) 39 | r2 = Robot(2, [19, 9], 0.4, [], num_clusters, [], ['Manipulator'], capabilities) 40 | r3 = Robot(3, [19, 10], 0.3, [], num_clusters, [], ['FirstAids', 'Defuser'], capabilities) 41 | 42 | robots = [r0, r1, r2, r3] 43 | starts = [] 44 | for robot in robots: 45 | starts.append(robot.pos) 46 | victim_reqs = [] 47 | # Go through the list of tasks 48 | for victim in victims: 49 | victim_reqs.append(victim.vectorized_cap) 50 | victim_reqs = np.asarray(victim_reqs) 51 | 52 | robot_reqs = [] 53 | # Go through the list of robots 54 | for robot in robots: 55 | robot_reqs.append(robot.vectorized_cap) 56 | robot_reqs = np.asarray(robot_reqs) 57 | ReqmentAnalysis(robots, victims, robot_reqs, victim_reqs) 58 | MissingCap(victims, robot_reqs) 59 | print(f'Robot 0 tasks based on capabilities: {r0.tasks}\n' 60 | f'Robot 1 tasks based on capabilities: {r1.tasks}\n' 61 | f'Robot 2 tasks based on capabilities: {r2.tasks}\n' 62 | f'Robot 3 tasks based on capabilities: {r3.tasks}\n') 63 | 64 | print(f'Robot 0 tasks based on full satisfaction of the capabilities: {r0.tasks_full}\n' 65 | f'Robot 1 tasks based on full satisfaction of the capabilities: {r1.tasks_full}\n' 66 | f'Robot 2 tasks based on full satisfaction of the capabilities: {r2.tasks_full}\n' 67 | f'Robot 3 tasks based on full satisfaction of the capabilities: {r3.tasks_full}\n') 68 | 69 | clusters, clusters_coord = Clustering(num_clusters, victims, ox, oy) 70 | victims_new = ClstrAsgn(robots, victims, clusters, clusters_coord, num_clusters) 71 | travel2clusters = [] 72 | for robot in robots: 73 | travel2clusters.append(robot.pos) 74 | victims_new = VictimAssign(robots, victims, victims_new) 75 | victims_new = RobotAssign(robots, victims_new, victims, ox, oy) 76 | for robot in robots: 77 | print(f'Robot{robot.id}-->B2:{robot.tasks_init}, B3:{robot.tasks_final}, B4:{robot.tasks_finalized}') 78 | 79 | with h5py.File(f'MRTA.hdf5', 'w') as f: 80 | f.create_dataset(f'RS_size', data=len(robots)) 81 | f.create_dataset(f'Victims_size', data=len(victims)) 82 | f.create_dataset(f'RS_starts', data=starts) 83 | f.create_dataset(f'RS_travel2clusters', data=travel2clusters) 84 | for robot in robots: 85 | print(f'{robot.id} --> {robot.tasks_init} & {robot.tasks_final} & {robot.tasks_finalized}') 86 | 87 | f.create_dataset(f'RS{robot.id}_Step_1', data=robot.tasks_init) 88 | f.create_dataset(f'RS{robot.id}_Step_2', data=robot.tasks_final) 89 | f.create_dataset(f'RS{robot.id}_Step_3', data=robot.tasks_finalized) 90 | 91 | 92 | fig, ax = plt.subplots(1, 1) 93 | fig.tight_layout() 94 | plt.rcParams.update({'font.size': 50}) 95 | for idx, cluster in enumerate(clusters_coord): 96 | ax.scatter(cluster[0], cluster[1], c="red", marker="^") 97 | ax.text(cluster[0], cluster[1], f'C{idx}') 98 | 99 | for victim in victims: 100 | # print(victim.rescued) 101 | ax.scatter(victim.pos[1]-1, victim.pos[0]+1, c="blue", marker="s") 102 | ax.text(victim.pos[1]-1, victim.pos[0]+1, f'V{victim.id}') 103 | vor = Voronoi(clusters_coord) 104 | for p_id, p in enumerate(vor.points): 105 | vor.points[p_id] = p[::-1] 106 | 107 | for p_id, p in enumerate(vor.vertices): 108 | vor.vertices[p_id] = p[::-1] 109 | 110 | for p_id, p in enumerate(vor.ridge_points): 111 | vor.ridge_points[p_id] = p[::-1] 112 | 113 | for p_id, p in enumerate(vor.regions): 114 | vor.regions[p_id] = p[::-1] 115 | 116 | voronoi_plot_2d(vor, ax) 117 | plt.plot(dpi=1200) 118 | plt.xticks([]) 119 | plt.yticks([]) 120 | ax.invert_yaxis() 121 | plt.show() -------------------------------------------------------------------------------- /MRTA_new/maps.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | env_map0 = np.zeros((20, 20)) 5 | env_map0_entrances = [[0, 0], [0, 1], [0, 2], [1, 0], [2, 0]] 6 | 7 | 8 | env_map1 = np.array([[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 9 | [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 10 | [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 11 | [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 12 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 13 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 14 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 15 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], 16 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], 17 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1], 18 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 19 | [1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 20 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 21 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 22 | [1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1], 23 | [1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 24 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 25 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 26 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 27 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]) 28 | env_map1_entrances = [[1, 1], [1, 2], [1, 3], [2, 1], [3, 1]] 29 | 30 | 31 | env_map2 = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 32 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 33 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 34 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 35 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 36 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 37 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 38 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 39 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 40 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 41 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 42 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 43 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 44 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 45 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 46 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 47 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 48 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 49 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 50 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]) 51 | env_map2_entrances = [[0, 0], [0, 1], [0, 2], [1, 0], [2, 0]] 52 | 53 | 54 | env_map3 = np.array([[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 55 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1], 56 | [1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1], 57 | [1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1], 58 | [1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 1, 1], 59 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 60 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 61 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 62 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 63 | [1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 64 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 65 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 66 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 67 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 68 | [1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 69 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], 70 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], 71 | [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], 72 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1], 73 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]]) 74 | env_map3_entrances = [[2, 3], [2, 4], [2, 5], [3, 3], [4, 3]] -------------------------------------------------------------------------------- /MRTA_new/robot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Robot: 5 | def __init__(self, id, pos, speed, algorithms, num_clusters, competency, capabilities, cap_list): 6 | self.id = id 7 | self.init_pos = pos 8 | self.pos = pos 9 | self.speed = speed 10 | self.w_rc = np.zeros((num_clusters,)) 11 | self.algorithms = algorithms 12 | self.competency = competency 13 | self.capabilities = capabilities 14 | self.vectorized_cap = [1 if cap in self.capabilities else 0 for cap in cap_list] 15 | self.num_sensors = len(capabilities) 16 | 17 | self.tasks = [] # Assignment based on the capabilities 18 | self.tasks_full = [] # Assignment based on full satisfaction of the victims requirements 19 | 20 | self.tasks_init = [] # Assignment based on the number of tasks in the cluster (B2) 21 | self.tasks_init_dist = [] # Assigned task distance to the cluster center 22 | 23 | self.tasks_final = [] # Final Assignment based on the robots busy time (B3) 24 | self.tasks_finalized = [] # (B4) 25 | 26 | self.make_span = [] 27 | self.abort_time = 0.0 28 | self.travel_time = 0.0 29 | 30 | def travel_cost(self): 31 | X_i = np.linalg.norm(np.subtract(self.tasks_init_dist[0], self.init_pos)) 32 | X_f = np.linalg.norm(np.subtract(self.tasks_init_dist[-1], self.tasks_init_dist[0])) 33 | T_i = X_i / self.speed 34 | T_f = X_f / self.speed 35 | self.travel_time = T_f - T_i 36 | 37 | def reset(self): 38 | 39 | self.tasks = [] # Assignment based on the capabilities 40 | self.tasks_full = [] # Assignment based on full satisfaction of the victims requirements 41 | 42 | self.tasks_init = [] # Assignment based on the number of tasks in the cluster 43 | self.tasks_init_dist = [] # Assigned task distance to the cluster center 44 | 45 | self.tasks_final = [] # Final Assignment based on the robots busy time 46 | 47 | self.make_span = [] 48 | self.abort_time = 0.0 49 | self.travel_time = 0.0 50 | 51 | self.w_rc = np.zeros_like(self.w_rc) 52 | 53 | 54 | def cap_list(self): 55 | self.capabilities 56 | -------------------------------------------------------------------------------- /MRTA_new/typhoon.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/typhoon.jpg -------------------------------------------------------------------------------- /MRTA_new/victim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/victim.png -------------------------------------------------------------------------------- /MRTA_new/victim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Victim: 5 | def __init__(self, id, pos, make_span, requirements, cap_list): 6 | self.id = id 7 | self.pos = pos 8 | self.capabilities = requirements # Victim requirements 9 | for cap_id, cap in enumerate(self.capabilities): 10 | if isinstance(cap, bytes): 11 | self.capabilities[cap_id] = cap.decode() 12 | self.vectorized_cap = [1 if cap in self.capabilities else 0 for cap in cap_list] 13 | self.rem_req = [] # Victim remaining requirements 14 | self.vectorized_candidates = [[] for _ in range(len(self.vectorized_cap))] 15 | self.candidates = [] 16 | self.cluster_id = np.nan 17 | self.cluster_dist = np.nan 18 | self.cluster_dist_NW = np.nan 19 | self.rescued = np.zeros_like(requirements, dtype=bool) 20 | self.vectorized_rescued = np.zeros_like(self.vectorized_cap, dtype=bool) 21 | self.make_span = make_span 22 | self.health_stt = np.random.choice([1, .6, .3]) # 1: high health, .6: low health, .3:critical health 23 | -------------------------------------------------------------------------------- /MRTA_new/wall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/MRTA_new/wall.png -------------------------------------------------------------------------------- /PathPlanning/Crazyflie.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/PathPlanning/Crazyflie.JPG -------------------------------------------------------------------------------- /PathPlanning/TurtleBot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/PathPlanning/TurtleBot.png -------------------------------------------------------------------------------- /PathPlanning/a_star.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | A* grid planning 4 | 5 | author: Atsushi Sakai(@Atsushi_twi) 6 | Nikos Kanargias (nkana@tee.gr) 7 | 8 | See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) 9 | 10 | """ 11 | 12 | import math 13 | import numpy as np 14 | import matplotlib.pyplot as plt 15 | import h5py 16 | from gridworld_multi_agent1_1 import animate 17 | show_animation = False 18 | 19 | 20 | class AStarPlanner: 21 | 22 | def __init__(self, ox, oy, resolution, rr): 23 | """ 24 | Initialize grid map for a star planning 25 | 26 | ox: x position list of Obstacles [m] 27 | oy: y position list of Obstacles [m] 28 | resolution: grid resolution [m] 29 | rr: robot radius[m] 30 | """ 31 | 32 | self.resolution = resolution 33 | self.rr = rr 34 | self.min_x, self.min_y = 0, 0 35 | self.max_x, self.max_y = 0, 0 36 | self.obstacle_map = None 37 | self.x_width, self.y_width = 0, 0 38 | self.motion = self.get_motion_model() 39 | self.calc_obstacle_map(ox, oy) 40 | 41 | class Node: 42 | def __init__(self, x, y, cost, parent_index): 43 | self.x = x # index of grid 44 | self.y = y # index of grid 45 | self.cost = cost 46 | self.parent_index = parent_index 47 | 48 | def __str__(self): 49 | return str(self.x) + "," + str(self.y) + "," + str( 50 | self.cost) + "," + str(self.parent_index) 51 | 52 | def planning(self, sx, sy, gx, gy): 53 | """ 54 | A star path search 55 | 56 | input: 57 | s_x: start x position [m] 58 | s_y: start y position [m] 59 | gx: goal x position [m] 60 | gy: goal y position [m] 61 | 62 | output: 63 | rx: x position list of the final path 64 | ry: y position list of the final path 65 | """ 66 | 67 | start_node = self.Node(self.calc_xy_index(sx, self.min_x), 68 | self.calc_xy_index(sy, self.min_y), 0.0, -1) 69 | goal_node = self.Node(self.calc_xy_index(gx, self.min_x), 70 | self.calc_xy_index(gy, self.min_y), 0.0, -1) 71 | 72 | open_set, closed_set = dict(), dict() 73 | open_set[self.calc_grid_index(start_node)] = start_node 74 | 75 | while True: 76 | if len(open_set) == 0: 77 | print("Open set is empty..", start_node) 78 | break 79 | 80 | c_id = min( 81 | open_set, 82 | key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, 83 | open_set[ 84 | o])) 85 | current = open_set[c_id] 86 | 87 | # show graph 88 | if show_animation: # pragma: no cover 89 | plt.plot(self.calc_grid_position(current.x, self.min_x), 90 | self.calc_grid_position(current.y, self.min_y), "xc") 91 | # for stopping simulation with the esc key. 92 | plt.gcf().canvas.mpl_connect('key_release_event', 93 | lambda event: [exit( 94 | 0) if event.key == 'escape' else None]) 95 | if len(closed_set.keys()) % 10 == 0: 96 | plt.pause(0.001) 97 | 98 | if current.x == goal_node.x and current.y == goal_node.y: 99 | print("Find goal") 100 | goal_node.parent_index = current.parent_index 101 | goal_node.cost = current.cost 102 | break 103 | 104 | # Remove the item from the open set 105 | del open_set[c_id] 106 | 107 | # Add it to the closed set 108 | closed_set[c_id] = current 109 | 110 | # expand_grid search grid based on motion model 111 | for i, _ in enumerate(self.motion): 112 | node = self.Node(current.x + self.motion[i][0], 113 | current.y + self.motion[i][1], 114 | current.cost + self.motion[i][2], c_id) 115 | n_id = self.calc_grid_index(node) 116 | 117 | # If the node is not safe, do nothing 118 | if not self.verify_node(node): 119 | continue 120 | 121 | if n_id in closed_set: 122 | continue 123 | 124 | if n_id not in open_set: 125 | open_set[n_id] = node # discovered a new node 126 | else: 127 | if open_set[n_id].cost > node.cost: 128 | # This path is the best until now. record it 129 | open_set[n_id] = node 130 | 131 | rx, ry = self.calc_final_path(goal_node, closed_set) 132 | 133 | return rx, ry 134 | 135 | def calc_final_path(self, goal_node, closed_set): 136 | # generate final course 137 | rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ 138 | self.calc_grid_position(goal_node.y, self.min_y)] 139 | parent_index = goal_node.parent_index 140 | while parent_index != -1: 141 | n = closed_set[parent_index] 142 | rx.append(self.calc_grid_position(n.x, self.min_x)) 143 | ry.append(self.calc_grid_position(n.y, self.min_y)) 144 | parent_index = n.parent_index 145 | 146 | return rx, ry 147 | 148 | @staticmethod 149 | def calc_heuristic(n1, n2): 150 | w = 1.0 # weight of heuristic 151 | d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) 152 | return d 153 | 154 | def calc_grid_position(self, index, min_position): 155 | """ 156 | calc grid position 157 | 158 | :param index: 159 | :param min_position: 160 | :return: 161 | """ 162 | pos = index * self.resolution + min_position 163 | return pos 164 | 165 | def calc_xy_index(self, position, min_pos): 166 | return round((position - min_pos) / self.resolution) 167 | 168 | def calc_grid_index(self, node): 169 | return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) 170 | 171 | def verify_node(self, node): 172 | px = self.calc_grid_position(node.x, self.min_x) 173 | py = self.calc_grid_position(node.y, self.min_y) 174 | 175 | if px < self.min_x: 176 | return False 177 | elif py < self.min_y: 178 | return False 179 | elif px >= self.max_x: 180 | return False 181 | elif py >= self.max_y: 182 | return False 183 | 184 | # collision check 185 | if self.obstacle_map[node.x][node.y]: 186 | return False 187 | 188 | return True 189 | 190 | def calc_obstacle_map(self, ox, oy): 191 | 192 | self.min_x = round(min(ox)) 193 | self.min_y = round(min(oy)) 194 | self.max_x = round(max(ox)) 195 | self.max_y = round(max(oy)) 196 | print("min_x:", self.min_x) 197 | print("min_y:", self.min_y) 198 | print("max_x:", self.max_x) 199 | print("max_y:", self.max_y) 200 | 201 | self.x_width = round((self.max_x - self.min_x) / self.resolution) 202 | self.y_width = round((self.max_y - self.min_y) / self.resolution) 203 | print("x_width:", self.x_width) 204 | print("y_width:", self.y_width) 205 | 206 | # obstacle map generation 207 | self.obstacle_map = [[False for _ in range(self.y_width)] 208 | for _ in range(self.x_width)] 209 | for ix in range(self.x_width): 210 | x = self.calc_grid_position(ix, self.min_x) 211 | for iy in range(self.y_width): 212 | y = self.calc_grid_position(iy, self.min_y) 213 | for iox, ioy in zip(ox, oy): 214 | d = math.hypot(iox - x, ioy - y) 215 | if d <= self.rr: 216 | self.obstacle_map[ix][iy] = True 217 | break 218 | 219 | @staticmethod 220 | def get_motion_model(): 221 | # dx, dy, cost 222 | motion = [[1, 0, 1], 223 | [0, 1, 1], 224 | [-1, 0, 1], 225 | [0, -1, 1]] 226 | # [-1, -1, math.sqrt(2)], 227 | # [-1, 1, math.sqrt(2)], 228 | # [1, -1, math.sqrt(2)], 229 | # [1, 1, math.sqrt(2)] 230 | return motion 231 | 232 | 233 | def main(): 234 | print(__file__ + " start!!") 235 | exp_name = 'FindSurvivors' 236 | 237 | plt.rcParams.update({'font.size': 22}) 238 | 239 | with h5py.File(f'../MRTA_new/MRTA.hdf5', 'r') as f: 240 | num_robots = np.asarray(f[f'RS_size']).tolist() 241 | num_victims = np.asarray(f[f'Victims_size']).tolist() 242 | starts = np.asarray(f[f'RS_starts']).tolist() 243 | travel2clusters = np.asarray(f[f'RS_travel2clusters']).tolist() 244 | travel2clusters = [[int(x), int(y)] for x, y in travel2clusters] 245 | 246 | tasks = [] 247 | for i in range(num_robots): 248 | list_t = [] 249 | s1 = np.asarray(f[f'RS{i}_Step_1']).tolist() 250 | list_t.append(s1) 251 | s2 = np.asarray(f[f'RS{i}_Step_2']).tolist() 252 | list_t.append(s2) 253 | s3 = np.asarray(f[f'RS{i}_Step_3']).tolist() 254 | list_t.append(s3) 255 | tasks.append(list_t) 256 | 257 | file_name = f'../MASAR/multi_agent_Q_learning_{exp_name}.hdf5' 258 | 259 | grid_size = 1 # [m] 260 | robot_radius = .5 # [m] 261 | env_map = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 262 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 263 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 264 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 265 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 266 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 267 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 268 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 269 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 270 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 271 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 272 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 273 | [0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 274 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0], 275 | [0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 276 | [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 277 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1], 278 | [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 279 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], 280 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]) 281 | num_rows, num_cols = np.shape(env_map) 282 | # set obstacle positions 283 | ox, oy = np.where(env_map == 1) 284 | # oy = oy 285 | 286 | # sy, sx = starts[0] 287 | # gy, gx = travel2clusters[0] 288 | # a_star = AStarPlanner(ox, oy, grid_size, robot_radius) 289 | # rx, ry = a_star.planning(sx, sy, gx, gy) 290 | 291 | # if show_animation: # pragma: no cover 292 | # plt.plot(ox, oy, ".k") 293 | # plt.plot(sx, sy, "og") 294 | # plt.plot(gx, gy, "xb") 295 | # plt.grid(True) 296 | # plt.axis("equal") 297 | # rx, ry = a_star.planning(sx, sy, gx, gy) 298 | # 299 | a_star = AStarPlanner(ox, oy, grid_size, robot_radius) 300 | # start and goal position 301 | rescue_team_Traj = [[] for _ in range(num_robots)] 302 | Roles = ['r' for _ in range(num_robots)] 303 | VFDs = [0 for _ in range(num_robots)] 304 | 305 | for num in range(num_robots): 306 | temp_task = [] 307 | for tsk in tasks[num]: 308 | temp_task += tsk 309 | tasks[num] = temp_task 310 | 311 | with h5py.File(file_name, 'r') as f: 312 | 313 | # send robots from entrance to the cluster centers 314 | for num in range(num_robots): 315 | sx, sy = starts[num] 316 | gx, gy = travel2clusters[num] 317 | 318 | rx, ry = a_star.planning(sx, sy, gx, gy) 319 | temp = [[x, y] for x, y in zip(rx[::-1], ry[::-1])] 320 | for pos in temp: 321 | rescue_team_Traj[num].append(pos) 322 | # send robots from cluster centers to the tasks 323 | for num in range(num_robots): 324 | sx, sy = travel2clusters[num] 325 | 326 | for tsk in tasks[num]: 327 | gx, gy = f[f'victim{tsk}_trajectory'][0] # [m] 328 | rx, ry = a_star.planning(sx, sy, gx, gy) 329 | temp = [[x, y] for x, y in zip(rx[::-1], ry[::-1])] 330 | for pos in temp: 331 | rescue_team_Traj[num].append(pos) 332 | 333 | sx, sy = gx, gy 334 | 335 | len_max = len(rescue_team_Traj[0]) 336 | for num in range(num_robots): 337 | if len(rescue_team_Traj[num]) > len_max: 338 | len_max = len(rescue_team_Traj[num]) 339 | for num in range(num_robots): 340 | while len(rescue_team_Traj[num]) < len_max: 341 | rescue_team_Traj[num].append(rescue_team_Traj[num][-1]) 342 | 343 | victims_Traj = [] 344 | with h5py.File(file_name, 'r') as f: 345 | for idx in range(num_victims): 346 | victims_Traj.append(np.asarray(f[f'victim{idx}_trajectory']).tolist()) 347 | while len(victims_Traj[idx]) < len_max: 348 | victims_Traj[idx].append(victims_Traj[idx][-1]) 349 | if len(victims_Traj[idx]) > len_max: 350 | victims_Traj[idx] = victims_Traj[idx][:len_max] 351 | rescue_team_VFD_status = [np.ones((num_robots, 1, 1), dtype=bool) for _ in range(len_max)] 352 | 353 | animate(np.asarray(rescue_team_Traj), np.asarray(victims_Traj), 354 | np.asarray(VFDs), rescue_team_VFD_status, Roles, env_map, wait_time=0.5) 355 | 356 | if show_animation: # pragma: no cover 357 | plt.plot(rx, ry, "-r") 358 | plt.pause(0.001) 359 | plt.show() 360 | 361 | 362 | if __name__ == '__main__': 363 | main() 364 | -------------------------------------------------------------------------------- /PathPlanning/gridworld_multi_agent1_1.py: -------------------------------------------------------------------------------- 1 | import pygame as pg 2 | import numpy as np 3 | import pygame 4 | import time 5 | 6 | 7 | pygame.init() 8 | 9 | # Constants 10 | WIDTH = 800 # width of the environment (px) 11 | HEIGHT = 800 # height of the environment (px) 12 | TS = 10 # delay in msec 13 | Col_num = 20 # number of columns 14 | Row_num = 20 # number of rows 15 | 16 | # define colors 17 | bg_color = pg.Color(255, 255, 255) 18 | line_color = pg.Color(128, 128, 128) 19 | vfdr_color = pg.Color(8, 136, 8, 128) 20 | vfds_color = pg.Color(255, 165, 0, 128) 21 | vfdrs_color = pg.Color(173, 216, 230, 128) 22 | 23 | def draw_grid(scr): 24 | '''a function to draw gridlines and other objects''' 25 | # Horizontal lines 26 | for j in range(Row_num + 1): 27 | pg.draw.line(scr, line_color, (0, j * HEIGHT // Row_num), (WIDTH, j * HEIGHT // Row_num), 2) 28 | # # Vertical lines 29 | for i in range(Col_num + 1): 30 | pg.draw.line(scr, line_color, (i * WIDTH // Col_num, 0), (i * WIDTH // Col_num, HEIGHT), 2) 31 | 32 | for x1 in range(0, WIDTH, WIDTH // Col_num): 33 | for y1 in range(0, HEIGHT, HEIGHT // Row_num): 34 | rect = pg.Rect(x1, y1, WIDTH // Col_num, HEIGHT // Row_num) 35 | pg.draw.rect(scr, bg_color, rect, 1) 36 | 37 | 38 | def animate(rescue_team_traj, victims_traj, rescue_team_vfd, rescue_team_vfd_status, rescue_team_roles, env_map, wait_time): 39 | 40 | font = pg.font.SysFont('arial', 20) 41 | 42 | num_rescue_team = len(rescue_team_traj) 43 | num_victims = len(victims_traj) 44 | 45 | pg.init() # initialize pygame 46 | screen = pg.display.set_mode((WIDTH + 2, HEIGHT + 2)) # set up the screen 47 | pg.display.set_caption("gridworld") # add a caption 48 | bg = pg.Surface(screen.get_size()) # get a background surface 49 | bg = bg.convert() 50 | 51 | img_rescuer = pg.image.load('TurtleBot.png') 52 | img_mdf_r = pg.transform.scale(img_rescuer, (WIDTH // Col_num, HEIGHT // Row_num)) 53 | 54 | img_rescuer_scout = pg.image.load('typhoon.jpg') 55 | img_mdf_rs = pg.transform.scale(img_rescuer_scout, (WIDTH // Col_num, HEIGHT // Row_num)) 56 | 57 | img_scout = pg.image.load('Crazyflie.JPG') 58 | img_mdf_s = pg.transform.scale(img_scout, (WIDTH // Col_num, HEIGHT // Row_num)) 59 | 60 | img_victim = pg.image.load('victim.png') 61 | img_mdf_victim = pg.transform.scale(img_victim, (WIDTH // Col_num, HEIGHT // Row_num)) 62 | 63 | img_wall = pg.image.load('wall.png') 64 | img_mdf_wall = pg.transform.scale(img_wall, (WIDTH // Col_num, HEIGHT // Row_num)) 65 | 66 | bg.fill(bg_color) 67 | screen.blit(bg, (0, 0)) 68 | clock = pg.time.Clock() 69 | pg.display.flip() 70 | run = True 71 | while run: 72 | clock.tick(60) 73 | for event in pg.event.get(): 74 | if event.type == pg.QUIT: 75 | run = False 76 | step = -1 77 | list_victims = np.arange(num_victims).tolist() 78 | list_rescue_team = np.arange(num_rescue_team).tolist() 79 | 80 | for rescue_team_stt, victims_stt in zip(np.moveaxis(rescue_team_traj, 0, -1), 81 | np.moveaxis(victims_traj, 0, -1)): 82 | 83 | for row in range(Row_num): 84 | for col in range(Col_num): 85 | if env_map[row, col] == 1: 86 | screen.blit(img_mdf_wall, 87 | (col * (WIDTH // Col_num), 88 | row * (HEIGHT // Row_num))) 89 | step += 1 90 | for num in list_rescue_team: 91 | if str(rescue_team_roles[num]) == "b'rs'": 92 | vfd_color = vfdrs_color 93 | elif str(rescue_team_roles[num]) == "b'r'": 94 | vfd_color = vfdr_color 95 | elif str(rescue_team_roles[num]) == "b's'": 96 | vfd_color = vfds_color 97 | 98 | # rescuer visual field depth 99 | # vfd_j = 0 100 | # for j in range(int(max(rescue_team_stt[1, num] - rescue_team_vfd[num], 0)), 101 | # int(min(Col_num, rescue_team_stt[1, num] + rescue_team_vfd[num] + 1))): 102 | # vfd_i = 0 103 | # for i in range(int(max(rescue_team_stt[0, num] - rescue_team_vfd[num], 0)), 104 | # int(min(Row_num, rescue_team_stt[0, num] + rescue_team_vfd[num] + 1))): 105 | # if rescue_team_vfd_status[num][step][vfd_i, vfd_j]: 106 | # rect = pg.Rect(j * (WIDTH // Col_num), i * (HEIGHT // Row_num), 107 | # (WIDTH // Col_num), (HEIGHT // Row_num)) 108 | # pg.draw.rect(screen, vfd_color, rect) 109 | # vfd_i += 1 110 | # vfd_j += 1 111 | 112 | # agents 113 | for num in list_rescue_team: 114 | if str(rescue_team_roles[num]) == 'rs': 115 | img_mdf = img_mdf_rs 116 | elif str(rescue_team_roles[num]) == 'r': 117 | img_mdf = img_mdf_r 118 | elif str(rescue_team_roles[num]) == 's': 119 | img_mdf = img_mdf_s 120 | screen.blit(img_mdf, 121 | (rescue_team_stt[1, num] * (WIDTH // Col_num), 122 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 123 | screen.blit(font.render(str(num), True, (0, 0, 0)), 124 | (rescue_team_stt[1, num] * (WIDTH // Col_num), 125 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 126 | 127 | # Stop showing finished agents 128 | # if (step >= 1 and 129 | # (rescue_team_stt[:, num][0] == rescue_team_history[:, num][0] == rescue_team_traj[num, -1, 0] and 130 | # rescue_team_stt[:, num][1] == rescue_team_history[:, num][1] == rescue_team_traj[num, -1, 1])): 131 | # list_rescue_team.remove(num) 132 | 133 | for num in list_victims: 134 | screen.blit(img_mdf_victim, (victims_stt[1, num] * (WIDTH // Col_num), 135 | victims_stt[0, num] * (HEIGHT // Row_num))) 136 | screen.blit(font.render(str(num), True, (0, 0, 0)), 137 | (victims_stt[1, num] * (WIDTH // Col_num), victims_stt[0, num] * (HEIGHT // Row_num))) 138 | 139 | # Stop showing rescued victims 140 | # if step >= 1 and (victims_stt[:, num][0] == victims_history[:, num][0] == victims_traj[num, -1, 0] and 141 | # victims_stt[:, num][1] == victims_history[:, num][1] == victims_traj[num, -1, 1]): 142 | # list_victims.remove(num) 143 | 144 | draw_grid(screen) 145 | pg.display.flip() 146 | pg.display.update() 147 | time.sleep(wait_time) # wait between the shows 148 | 149 | for num in list_victims: 150 | screen.blit(bg, (victims_stt[1, num] * (WIDTH // Col_num), 151 | victims_stt[0, num] * (HEIGHT // Row_num))) 152 | 153 | for num in list_rescue_team: 154 | screen.blit(bg, (rescue_team_stt[1, num] * (WIDTH // Col_num), 155 | rescue_team_stt[0, num] * (HEIGHT // Row_num))) 156 | 157 | # rescuer visual field depths 158 | for j in range(int(max(rescue_team_stt[1, num] - rescue_team_vfd[num], 0)), 159 | int(min(Row_num, rescue_team_stt[1, num] + rescue_team_vfd[num] + 1))): 160 | for i in range(int(max(rescue_team_stt[0, num] - rescue_team_vfd[num], 0)), 161 | int(min(Col_num, rescue_team_stt[0, num] + rescue_team_vfd[num] + 1))): 162 | rect = pg.Rect(j * (WIDTH // Col_num), i * (HEIGHT // Row_num), 163 | (WIDTH // Col_num), (HEIGHT // Row_num)) 164 | pg.draw.rect(screen, bg_color, rect) 165 | 166 | victims_history = victims_stt 167 | rescue_team_history = rescue_team_stt 168 | 169 | for num in list_rescue_team: 170 | if str(rescue_team_roles[num]) == "b'rs'": 171 | img_mdf = img_mdf_rs 172 | elif str(rescue_team_roles[num]) == "b'r'": 173 | img_mdf = img_mdf_r 174 | elif str(rescue_team_roles[num]) == "b's'": 175 | img_mdf = img_mdf_s 176 | screen.blit(img_mdf, (rescue_team_traj[num, -1, 1] * (WIDTH // Col_num), 177 | rescue_team_traj[num, -1, 0] * (HEIGHT // Row_num))) 178 | for num in list_victims: 179 | screen.blit(img_mdf_victim, (victims_traj[num, -1, 1] * (WIDTH // Col_num), 180 | victims_traj[num, -1, 0] * (HEIGHT // Row_num))) 181 | 182 | draw_grid(screen) 183 | pg.display.flip() 184 | pg.display.update() 185 | run = False 186 | pg.quit() 187 | -------------------------------------------------------------------------------- /PathPlanning/typhoon.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/PathPlanning/typhoon.jpg -------------------------------------------------------------------------------- /PathPlanning/victim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/PathPlanning/victim.png -------------------------------------------------------------------------------- /PathPlanning/wall.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hamidosooli/Multi-Robot-Task-Assignment/e09c4644fded1365b0485b753eb7944249177057/PathPlanning/wall.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi Robot Task Assignment 2 | A Multi-Robot Task Assignment Framework for Search and Rescue with Heterogeneous Teams 3 | 4 | In the MRTA_new folder: 5 | 6 | - Running main.py assigns the tasks to the robots and saves the results in the MRTA.hdf5 file. 7 | 8 | - Running a_star.py visualizes the robot movements to the victims (tasks) 9 | 10 | 11 | In the MASAR folder: 12 | 13 | - Running env1.1.py trains the agents to find the victims and saves the results in multi_agent_Q_learnnig_{experiment name}.hdf5 file 14 | 15 | - Runnning visualizer1.1.py demonstrates the robots performance in a simulation 16 | 17 | 18 | In the PathPanning folder: 19 | 20 | - Running a_star.py visualizes the robots paths in the environment 21 | 22 | 23 | 24 | 25 | 26 | 27 | ## Citation 28 | 29 | If you found this repository useful, we would really appreciate if you could cite our work: 30 | 31 | - [1] Hamid Osooli, Paul Robinette, Kshitij Jerath, S. Reza Ahmadzadeh, "A Multi-Robot Task Assignment Framework for Search and Rescue with Heterogeneous Teams," in Advances in Multi-Agent Learning - Coordination, Communication, and Control Workshop at IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2023), Detroit, MI, USA, Oct 1-5, 2023. 32 | 33 | ```bibtex 34 | @article{osooli2023multi, 35 | title={A Multi-Robot Task Assignment Framework for Search and Rescue with Heterogeneous Teams}, 36 | author={Osooli, Hamid and Robinette, Paul and Jerath, Kshitij and Ahmadzadeh, S Reza}, 37 | journal={IROS 2023 Advances in Multi-Agent Learning - Coordination, Perception, and Control Workshop}, 38 | year={2023} 39 | } 40 | 41 | ``` 42 | --------------------------------------------------------------------------------