├── .gitignore ├── LICENSE ├── README.md ├── RVO.py ├── __init__.py ├── data ├── .DS_Store ├── mkmovie.sh └── snapshots.png ├── example.py └── vis.py /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *.pyc 3 | *.avi 4 | snap*.png 5 | data/*.jpg 6 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | RVO_Py_MAS 2 | ======== 3 | 4 | Python Implementation of Reciprocal Velocity Obstacle (RVO) for Multi-agent Systems 5 | 6 | ``` 7 | @ARTICLE{8361450, 8 | author={M. {Guo} and M. M. {Zavlanos}}, 9 | journal={IEEE Transactions on Robotics}, 10 | title={Multirobot Data Gathering Under Buffer Constraints and Intermittent Communication}, 11 | year={2018}, 12 | volume={34}, 13 | number={4}, 14 | pages={1082-1097}, 15 | doi={10.1109/TRO.2018.2830370}} 16 | ``` 17 | 18 | ----- 19 | Description 20 | ----- 21 | This package contains a **_plug-and-play_** Python package for collision-avoidance in multi-agent system, based on reciprocal velocity obstacles ([RVO](https://www.cs.unc.edu/~geom/RVO/icra2008.pdf)) and hybrid reciprocal velocity obstacles ([HRVO](https://www.cs.unc.edu/~geom/RVO/icra2008.pdf)). 22 | 23 | It has _minimal impact_ on your control objective and requires _minimal integration_. 24 | 25 | 26 | 27 |

28 | 29 |

30 | 31 | ----- 32 | Features 33 | ----- 34 | * Takes a 2D workspace with _any number_ of non-overlaping circular or square obstacles 35 | * _Any number_ of dynamic agents with non-zero volume. 36 | * Allow the choice of VO, RVO, HRVO. 37 | * **Direct plug-and-play** and **fully integrate-able with your control objective**, i.e., the output velocity is a minimal modification of the desired velocity. 38 | 39 | ```python 40 | from your_module import compute_desired_V, Update_V 41 | from RVO import RVO_update 42 | 43 | # your control objective here 44 | V_desired = compute_desired_V(X, control_objective, V_max) 45 | 46 | # plug in the RVO controller from this package 47 | V = RVO_update(X, V_desired, workspace_model) 48 | 49 | # let the robot move 50 | X = Update_X(X, V, step) 51 | ``` 52 | 53 | * Scalable and fast, see examples below. 54 | * See [example.py](https://github.com/MengGuo/RVO_Py_MAS/blob/master/example.py) for test run. [[Video1]](https://vimeo.com/185405407), [[Video2]](https://vimeo.com/185408368) 55 | 56 | 57 | ---- 58 | References 59 | ---- 60 | * Papers on [RVO](https://www.cs.unc.edu/~geom/RVO/icra2008.pdf), [HRVO](https://www.cs.unc.edu/~geom/RVO/icra2008.pdf) 61 | * There are [Python bindings](https://github.com/sybrenstuvel/Python-RVO2) of the C++ implementation from the algorithm developers. For my purposes, the formality is too _heavy_ to be integrated into my own project, so I have my own try. 62 | * This package does _not_ depend on the [Clearpath geometric package](http://pcl.intel-research.net/publications/clearpath_sca2009.pdf) to compute velocity obstacles. 63 | 64 | 65 | ---- 66 | Discussion 67 | ---- 68 | * For **very** clustered workspace with a large number of robots, you may need to limit the `maximal velocity` and use very `small step size`. 69 | * You may add additional constraints in `RVO_update` such as the change rate of `V`, the lower bound of `V`. 70 | * When applying this module to experimental robot control, you may need to set the **step size** higher due to hardware constraints. 71 | * In most practical experiments, this scheme should still work by limiting the _maximal velocity_. 72 | 73 | -------------------------------------------------------------------------------- /RVO.py: -------------------------------------------------------------------------------- 1 | from math import ceil, floor, sqrt 2 | import copy 3 | import numpy 4 | 5 | from math import cos, sin, tan, atan2, asin 6 | 7 | from math import pi as PI 8 | 9 | 10 | 11 | def distance(pose1, pose2): 12 | """ compute Euclidean distance for 2D """ 13 | return sqrt((pose1[0]-pose2[0])**2+(pose1[1]-pose2[1])**2)+0.001 14 | 15 | 16 | def RVO_update(X, V_des, V_current, ws_model): 17 | """ compute best velocity given the desired velocity, current velocity and workspace model""" 18 | ROB_RAD = ws_model['robot_radius']+0.1 19 | V_opt = list(V_current) 20 | for i in range(len(X)): 21 | vA = [V_current[i][0], V_current[i][1]] 22 | pA = [X[i][0], X[i][1]] 23 | RVO_BA_all = [] 24 | for j in range(len(X)): 25 | if i!=j: 26 | vB = [V_current[j][0], V_current[j][1]] 27 | pB = [X[j][0], X[j][1]] 28 | # use RVO 29 | transl_vB_vA = [pA[0]+0.5*(vB[0]+vA[0]), pA[1]+0.5*(vB[1]+vA[1])] 30 | # use VO 31 | #transl_vB_vA = [pA[0]+vB[0], pA[1]+vB[1]] 32 | dist_BA = distance(pA, pB) 33 | theta_BA = atan2(pB[1]-pA[1], pB[0]-pA[0]) 34 | if 2*ROB_RAD > dist_BA: 35 | dist_BA = 2*ROB_RAD 36 | theta_BAort = asin(2*ROB_RAD/dist_BA) 37 | theta_ort_left = theta_BA+theta_BAort 38 | bound_left = [cos(theta_ort_left), sin(theta_ort_left)] 39 | theta_ort_right = theta_BA-theta_BAort 40 | bound_right = [cos(theta_ort_right), sin(theta_ort_right)] 41 | # use HRVO 42 | # dist_dif = distance([0.5*(vB[0]-vA[0]),0.5*(vB[1]-vA[1])],[0,0]) 43 | # transl_vB_vA = [pA[0]+vB[0]+cos(theta_ort_left)*dist_dif, pA[1]+vB[1]+sin(theta_ort_left)*dist_dif] 44 | RVO_BA = [transl_vB_vA, bound_left, bound_right, dist_BA, 2*ROB_RAD] 45 | RVO_BA_all.append(RVO_BA) 46 | for hole in ws_model['circular_obstacles']: 47 | # hole = [x, y, rad] 48 | vB = [0, 0] 49 | pB = hole[0:2] 50 | transl_vB_vA = [pA[0]+vB[0], pA[1]+vB[1]] 51 | dist_BA = distance(pA, pB) 52 | theta_BA = atan2(pB[1]-pA[1], pB[0]-pA[0]) 53 | # over-approximation of square to circular 54 | OVER_APPROX_C2S = 1.5 55 | rad = hole[2]*OVER_APPROX_C2S 56 | if (rad+ROB_RAD) > dist_BA: 57 | dist_BA = rad+ROB_RAD 58 | theta_BAort = asin((rad+ROB_RAD)/dist_BA) 59 | theta_ort_left = theta_BA+theta_BAort 60 | bound_left = [cos(theta_ort_left), sin(theta_ort_left)] 61 | theta_ort_right = theta_BA-theta_BAort 62 | bound_right = [cos(theta_ort_right), sin(theta_ort_right)] 63 | RVO_BA = [transl_vB_vA, bound_left, bound_right, dist_BA, rad+ROB_RAD] 64 | RVO_BA_all.append(RVO_BA) 65 | vA_post = intersect(pA, V_des[i], RVO_BA_all) 66 | V_opt[i] = vA_post[:] 67 | return V_opt 68 | 69 | 70 | def intersect(pA, vA, RVO_BA_all): 71 | # print '----------------------------------------' 72 | # print 'Start intersection test' 73 | norm_v = distance(vA, [0, 0]) 74 | suitable_V = [] 75 | unsuitable_V = [] 76 | for theta in numpy.arange(0, 2*PI, 0.1): 77 | for rad in numpy.arange(0.02, norm_v+0.02, norm_v/5.0): 78 | new_v = [rad*cos(theta), rad*sin(theta)] 79 | suit = True 80 | for RVO_BA in RVO_BA_all: 81 | p_0 = RVO_BA[0] 82 | left = RVO_BA[1] 83 | right = RVO_BA[2] 84 | dif = [new_v[0]+pA[0]-p_0[0], new_v[1]+pA[1]-p_0[1]] 85 | theta_dif = atan2(dif[1], dif[0]) 86 | theta_right = atan2(right[1], right[0]) 87 | theta_left = atan2(left[1], left[0]) 88 | if in_between(theta_right, theta_dif, theta_left): 89 | suit = False 90 | break 91 | if suit: 92 | suitable_V.append(new_v) 93 | else: 94 | unsuitable_V.append(new_v) 95 | new_v = vA[:] 96 | suit = True 97 | for RVO_BA in RVO_BA_all: 98 | p_0 = RVO_BA[0] 99 | left = RVO_BA[1] 100 | right = RVO_BA[2] 101 | dif = [new_v[0]+pA[0]-p_0[0], new_v[1]+pA[1]-p_0[1]] 102 | theta_dif = atan2(dif[1], dif[0]) 103 | theta_right = atan2(right[1], right[0]) 104 | theta_left = atan2(left[1], left[0]) 105 | if in_between(theta_right, theta_dif, theta_left): 106 | suit = False 107 | break 108 | if suit: 109 | suitable_V.append(new_v) 110 | else: 111 | unsuitable_V.append(new_v) 112 | #---------------------- 113 | if suitable_V: 114 | # print 'Suitable found' 115 | vA_post = min(suitable_V, key = lambda v: distance(v, vA)) 116 | new_v = vA_post[:] 117 | for RVO_BA in RVO_BA_all: 118 | p_0 = RVO_BA[0] 119 | left = RVO_BA[1] 120 | right = RVO_BA[2] 121 | dif = [new_v[0]+pA[0]-p_0[0], new_v[1]+pA[1]-p_0[1]] 122 | theta_dif = atan2(dif[1], dif[0]) 123 | theta_right = atan2(right[1], right[0]) 124 | theta_left = atan2(left[1], left[0]) 125 | else: 126 | # print 'Suitable not found' 127 | tc_V = dict() 128 | for unsuit_v in unsuitable_V: 129 | tc_V[tuple(unsuit_v)] = 0 130 | tc = [] 131 | for RVO_BA in RVO_BA_all: 132 | p_0 = RVO_BA[0] 133 | left = RVO_BA[1] 134 | right = RVO_BA[2] 135 | dist = RVO_BA[3] 136 | rad = RVO_BA[4] 137 | dif = [unsuit_v[0]+pA[0]-p_0[0], unsuit_v[1]+pA[1]-p_0[1]] 138 | theta_dif = atan2(dif[1], dif[0]) 139 | theta_right = atan2(right[1], right[0]) 140 | theta_left = atan2(left[1], left[0]) 141 | if in_between(theta_right, theta_dif, theta_left): 142 | small_theta = abs(theta_dif-0.5*(theta_left+theta_right)) 143 | if abs(dist*sin(small_theta)) >= rad: 144 | rad = abs(dist*sin(small_theta)) 145 | big_theta = asin(abs(dist*sin(small_theta))/rad) 146 | dist_tg = abs(dist*cos(small_theta))-abs(rad*cos(big_theta)) 147 | if dist_tg < 0: 148 | dist_tg = 0 149 | tc_v = dist_tg/distance(dif, [0,0]) 150 | tc.append(tc_v) 151 | tc_V[tuple(unsuit_v)] = min(tc)+0.001 152 | WT = 0.2 153 | vA_post = min(unsuitable_V, key = lambda v: ((WT/tc_V[tuple(v)])+distance(v, vA))) 154 | return vA_post 155 | 156 | def in_between(theta_right, theta_dif, theta_left): 157 | if abs(theta_right - theta_left) <= PI: 158 | if theta_right <= theta_dif <= theta_left: 159 | return True 160 | else: 161 | return False 162 | else: 163 | if (theta_left <0) and (theta_right >0): 164 | theta_left += 2*PI 165 | if theta_dif < 0: 166 | theta_dif += 2*PI 167 | if theta_right <= theta_dif <= theta_left: 168 | return True 169 | else: 170 | return False 171 | if (theta_left >0) and (theta_right <0): 172 | theta_right += 2*PI 173 | if theta_dif < 0: 174 | theta_dif += 2*PI 175 | if theta_left <= theta_dif <= theta_right: 176 | return True 177 | else: 178 | return False 179 | 180 | def compute_V_des(X, goal, V_max): 181 | V_des = [] 182 | for i in range(len(X)): 183 | dif_x = [goal[i][k]-X[i][k] for k in range(2)] 184 | norm = distance(dif_x, [0, 0]) 185 | norm_dif_x = [dif_x[k]*V_max[k]/norm for k in range(2)] 186 | V_des.append(norm_dif_x[:]) 187 | if reach(X[i], goal[i], 0.1): 188 | V_des[i][0] = 0 189 | V_des[i][1] = 0 190 | return V_des 191 | 192 | def reach(p1, p2, bound=0.5): 193 | if distance(p1,p2)< bound: 194 | return True 195 | else: 196 | return False 197 | 198 | 199 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MengGuo/RVO_Py_MAS/e656e88294e98171aead6f07591cbb78ffffadd9/__init__.py -------------------------------------------------------------------------------- /data/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MengGuo/RVO_Py_MAS/e656e88294e98171aead6f07591cbb78ffffadd9/data/.DS_Store -------------------------------------------------------------------------------- /data/mkmovie.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | ffmpeg -r 3 -f image2 -i snap%d.png -s 1000x1000 -y simulation.avi 4 | -------------------------------------------------------------------------------- /data/snapshots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MengGuo/RVO_Py_MAS/e656e88294e98171aead6f07591cbb78ffffadd9/data/snapshots.png -------------------------------------------------------------------------------- /example.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | 4 | from RVO import RVO_update, reach, compute_V_des, reach 5 | from vis import visualize_traj_dynamic 6 | 7 | 8 | #------------------------------ 9 | #define workspace model 10 | ws_model = dict() 11 | #robot radius 12 | ws_model['robot_radius'] = 0.2 13 | #circular obstacles, format [x,y,rad] 14 | # no obstacles 15 | ws_model['circular_obstacles'] = [] 16 | # with obstacles 17 | # ws_model['circular_obstacles'] = [[-0.3, 2.5, 0.3], [1.5, 2.5, 0.3], [3.3, 2.5, 0.3], [5.1, 2.5, 0.3]] 18 | #rectangular boundary, format [x,y,width/2,heigth/2] 19 | ws_model['boundary'] = [] 20 | 21 | #------------------------------ 22 | #initialization for robot 23 | # position of [x,y] 24 | X = [[-0.5+1.0*i, 0.0] for i in range(7)] + [[-0.5+1.0*i, 5.0] for i in range(7)] 25 | # velocity of [vx,vy] 26 | V = [[0,0] for i in range(len(X))] 27 | # maximal velocity norm 28 | V_max = [1.0 for i in range(len(X))] 29 | # goal of [x,y] 30 | goal = [[5.5-1.0*i, 5.0] for i in range(7)] + [[5.5-1.0*i, 0.0] for i in range(7)] 31 | 32 | #------------------------------ 33 | #simulation setup 34 | # total simulation time (s) 35 | total_time = 15 36 | # simulation step 37 | step = 0.01 38 | 39 | #------------------------------ 40 | #simulation starts 41 | t = 0 42 | while t*step < total_time: 43 | # compute desired vel to goal 44 | V_des = compute_V_des(X, goal, V_max) 45 | # compute the optimal vel to avoid collision 46 | V = RVO_update(X, V_des, V, ws_model) 47 | # update position 48 | for i in range(len(X)): 49 | X[i][0] += V[i][0]*step 50 | X[i][1] += V[i][1]*step 51 | #---------------------------------------- 52 | # visualization 53 | if t%10 == 0: 54 | visualize_traj_dynamic(ws_model, X, V, goal, time=t*step, name='data/snap%s.png'%str(t/10)) 55 | #visualize_traj_dynamic(ws_model, X, V, goal, time=t*step, name='data/snap%s.png'%str(t/10)) 56 | t += 1 57 | 58 | -------------------------------------------------------------------------------- /vis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import matplotlib 3 | import matplotlib.pyplot as pyplot 4 | from matplotlib.path import Path 5 | import matplotlib.patches as patches 6 | from matplotlib.patches import Polygon 7 | import matplotlib.cm as cmx 8 | import matplotlib.colors as colors 9 | 10 | from math import pi as PI 11 | from math import atan2, sin, cos, sqrt 12 | 13 | 14 | 15 | def visualize_traj_dynamic(ws_model, X, U, goal, time = None, name=None): 16 | figure = pyplot.figure() 17 | ax = figure.add_subplot(1,1,1) 18 | cmap = get_cmap(len(X)) 19 | # plot obstacles 20 | for hole in ws_model['circular_obstacles']: 21 | srec = matplotlib.patches.Rectangle( 22 | (hole[0]-hole[2], hole[1]-hole[2]), 23 | 2*hole[2], 2*hole[2], 24 | facecolor= 'red', 25 | fill = True, 26 | alpha=1) 27 | ax.add_patch(srec) 28 | # ---plot traj--- 29 | for i in range(0,len(X)): 30 | #-------plot car 31 | robot = matplotlib.patches.Circle( 32 | (X[i][0],X[i][1]), 33 | radius = ws_model['robot_radius'], 34 | facecolor=cmap(i), 35 | edgecolor='black', 36 | linewidth=1.0, 37 | ls='solid', 38 | alpha=1, 39 | zorder=2) 40 | ax.add_patch(robot) 41 | #----------plot velocity 42 | ax.arrow(X[i][0], X[i][1], U[i][0], U[i][1], head_width=0.05, head_length=0.1, fc=cmap(i), ec=cmap(i)) 43 | ax.text(X[i][0]-0.1, X[i][1]-0.1, r'$%s$' %i, fontsize=15, fontweight = 'bold',zorder=3) 44 | ax.plot([goal[i][0]], [goal[i][1]], '*', color=cmap(i), markersize =15,linewidth=3.0) 45 | if time: 46 | ax.text(2,5.5,'$t=%.1f s$' %time, 47 | fontsize=20, fontweight ='bold') 48 | # ---set axes --- 49 | ax.set_aspect('equal') 50 | ax.set_xlim(-1.0, 6.0) 51 | ax.set_ylim(-1.0, 6.0) 52 | ax.set_xlabel(r'$x (m)$') 53 | ax.set_ylabel(r'$y (m)$') 54 | ax.grid(True) 55 | if name: 56 | pyplot.savefig(name, dpi = 200) 57 | #pyplot.savefig(name,bbox_inches='tight') 58 | pyplot.cla() 59 | pyplot.close(figure) 60 | return figure 61 | 62 | def get_cmap(N): 63 | '''Returns a function that maps each index in 0, 1, ... N-1 to a distinct RGB color.''' 64 | color_norm = colors.Normalize(vmin=0, vmax=N-1) 65 | scalar_map = cmx.ScalarMappable(norm=color_norm, cmap='hsv') 66 | def map_index_to_rgb_color(index): 67 | return scalar_map.to_rgba(index) 68 | return map_index_to_rgb_color 69 | --------------------------------------------------------------------------------