├── doc ├── source │ ├── README.rst │ ├── tutorial.rst │ ├── index.rst │ ├── api.rst │ ├── README.md │ └── conf.py ├── Makefile └── make.bat ├── setup.cfg ├── MANIFEST.in ├── robotarium ├── __init__.py ├── tests │ └── test_graph.py ├── misc.py ├── controllers.py ├── graph.py ├── transformations.py └── Robotarium.py ├── .gitignore ├── setup.py ├── LICENSE.txt ├── examples ├── consensusStatic.py ├── barrierCertificates.py ├── leaderFollowerStatic.py └── formationControl.py └── README.md /doc/source/README.rst: -------------------------------------------------------------------------------- 1 | ../../README.md -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [aliases] 2 | test=pytest 3 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include LICENSE.txt 2 | include examples/*.py 3 | -------------------------------------------------------------------------------- /robotarium/__init__.py: -------------------------------------------------------------------------------- 1 | from .Robotarium import Robotarium 2 | from . import controllers, misc, graph, transformations 3 | -------------------------------------------------------------------------------- /doc/source/tutorial.rst: -------------------------------------------------------------------------------- 1 | Tutorial 2 | ======== 3 | 4 | Consensus 5 | --------- 6 | 7 | hey, here is how to run an example 8 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | .idea/workspace.xml 3 | 4 | .idea/codeStyleSettings.xml 5 | 6 | .idea/dictionaries/zmk5.xml 7 | 8 | .idea/misc.xml 9 | 10 | .idea/modules.xml 11 | 12 | .idea/robotarium-python-simulator.iml 13 | 14 | .idea/vcs.xml 15 | 16 | *.pyc 17 | build 18 | dist 19 | *egg-info 20 | .eggs 21 | *.cache 22 | *.p 23 | -------------------------------------------------------------------------------- /robotarium/tests/test_graph.py: -------------------------------------------------------------------------------- 1 | import robotarium 2 | from numpy.testing import assert_allclose 3 | 4 | def test_complete_gl(): 5 | n = 5 6 | graph = robotarium.graph.complete_gl(n) 7 | print(graph) 8 | assert graph.sum() == 0 9 | assert_allclose(graph.diagonal(), n - 1) 10 | assert_allclose(graph.sum(axis=1), 0) 11 | -------------------------------------------------------------------------------- /robotarium/misc.py: -------------------------------------------------------------------------------- 1 | def is_initialized(input_args): 2 | """ 3 | SUMMARY OF THIS FUNCTION GOES HERE. 4 | 5 | Parameters 6 | ---------- 7 | 8 | """ 9 | pass 10 | 11 | 12 | def unique_filename(file_name): 13 | """ 14 | Returns a time-stamped file name. 15 | Appends the current time information to the passed argument. 16 | 17 | Parameters 18 | ---------- 19 | file_name : str 20 | SOMETHING 21 | 22 | """ 23 | pass 24 | -------------------------------------------------------------------------------- /doc/source/index.rst: -------------------------------------------------------------------------------- 1 | .. robotarium documentation master file, created by 2 | sphinx-quickstart on Wed Sep 7 22:18:19 2016. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to robotarium's documentation! 7 | ====================================== 8 | 9 | Contents: 10 | 11 | 12 | .. toctree:: 13 | :maxdepth: 2 14 | 15 | README 16 | tutorial 17 | api 18 | 19 | 20 | Indices and tables 21 | ================== 22 | 23 | * :ref:`genindex` 24 | * :ref:`modindex` 25 | * :ref:`search` 26 | 27 | -------------------------------------------------------------------------------- /doc/source/api.rst: -------------------------------------------------------------------------------- 1 | API 2 | === 3 | 4 | Robotarium 5 | ---------- 6 | 7 | .. autoclass:: robotarium.Robotarium 8 | :members: 9 | :undoc-members: 10 | 11 | Controllers 12 | ----------- 13 | 14 | .. automodule:: robotarium.controllers 15 | :members: 16 | :undoc-members: 17 | 18 | Graph 19 | ----- 20 | 21 | .. automodule:: robotarium.graph 22 | :members: 23 | :undoc-members: 24 | 25 | Transformations 26 | --------------- 27 | 28 | .. automodule:: robotarium.transformations 29 | :members: 30 | :undoc-members: 31 | 32 | Misc 33 | ---- 34 | 35 | .. automodule:: robotarium.misc 36 | :members: 37 | :undoc-members: 38 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | with open('README.md') as f: 4 | long_description = f.read() 5 | 6 | setup( 7 | name='robotarium', 8 | version='0.3.0', 9 | author='Zahi Kakish', 10 | url='https://github.com/zmk5/robotarium-python-simulator', 11 | description='simulator for the Georgia Tech Robotarium', 12 | long_description=long_description, 13 | license='MIT', 14 | keywords=['robotics simulation'], 15 | install_requires=['numpy', 'matplotlib', 'sphinx', 'numpydoc'], 16 | include_package_data=True, 17 | packages=find_packages(), 18 | test_suite='tests', 19 | setup_requires=['pytest-runner'], 20 | tests_require=['pytest'], 21 | classifiers=['Programming Language :: Python :: 2', 22 | 'Programming Language :: Python :: 3', 23 | 'Intended Audience :: Science/Research', 24 | 'License :: OSI Approved :: MIT License', 25 | 'Topic :: Scientific/Engineering'] 26 | ) 27 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Original work Copyright (c) 2016 Spring Berman, Zahi Kakish 4 | Modified work Copyright 2016 Eric Squires, Magnus Egerstedt (GRITS Lab) 5 | 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | -------------------------------------------------------------------------------- /examples/consensusStatic.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from robotarium import Robotarium, transformations, graph 3 | import json 4 | 5 | # Get Robotarium object used to communicate with the robots/simulator 6 | r = Robotarium() 7 | 8 | # Get the number of available agents from the Robotarium. We don't need a 9 | # specific value for this algorithm. 10 | n = r.get_available_agents() 11 | 12 | # Initialize the Robotarium object with the desired number of agents. 13 | r.initialize(n) 14 | 15 | # Generate a cyclic graph Laplacian from our handy utilities. For this 16 | # algorithm, any connected graph will yield consensus. 17 | lap = graph.cycle_gl(n) 18 | 19 | # Gain for the diffeomorphism transformation between single-integrator and 20 | # unicycle dynamics. 21 | diffeomorphism_gain = 0.25 22 | 23 | # Select the number of iterations for the experiment. This value is arbitrary. 24 | iterations = 1000 25 | 26 | # Initialize velocity vector for agents. Each agent expects a 2x1 velocity 27 | # vector containing the linear and angular velocity, respectively. 28 | dx = np.zeros((2, n)) 29 | 30 | # Iterate for the previously specified number of iterations 31 | for _ in range(0, iterations): 32 | # Retrieve the most recent poses from the Robotarium. This time delay is 33 | # approximately 0.033 seconds. 34 | x = r.get_poses() 35 | x_ = x[0, :] 36 | y_ = x[1, :] 37 | 38 | # ALGORITHM 39 | for i in range(0, n): 40 | # Initialize velocity to zero for each agent. This allows us to sum 41 | # over agent i's neighbors 42 | dx[:, [i]] = np.zeros((2, 1)) 43 | 44 | # Get the topological neighbors of agent i based on the graph. 45 | neighbors = r.get_top_neighbors(i, lap) 46 | 47 | # Iterate through agent i's neighbors 48 | for j in neighbors: 49 | # For each neighbor, calculate appropriate consensus term and 50 | # add it to the total velocity. 51 | dx[:, i] += (x[0:2, j] - x[0:2, i]) 52 | 53 | # END ALGORITHM 54 | dx = transformations.barrier_certificate(dx, x, ds=0.1) 55 | dx = transformations.int_to_uni3(dx, x, diffeomorphism_gain) 56 | 57 | # Set velocities of agents 1,...,n 58 | r.set_velocities(range(0, n), dx) 59 | 60 | # Send the previously set velocities to the agents. 61 | # This function must be called. 62 | r.step() 63 | 64 | with open('log.json', 'w') as f: 65 | json.dump(r.log, f) 66 | -------------------------------------------------------------------------------- /examples/barrierCertificates.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from robotarium import Robotarium, transformations, controllers 3 | 4 | 5 | # Get Robotarium object used to communicate with the robots/simulator. 6 | r = Robotarium() 7 | 8 | # Get the number of available agents from the Robotarium. We don't need a 9 | # specific value from this algorithm. 10 | n = r.get_available_agents() 11 | 12 | # Number of iterations. 13 | iterations = 20000 14 | 15 | # Initialize the Robotarium object with the desired number of agents. 16 | r.initialize(n) 17 | 18 | # Initialize velocity vector for agents. Each agent expects a 2x1 velocity 19 | # vector containing the linear and angular velocity, respectively. 20 | dx = np.zeros((2, n)) 21 | 22 | xy_bound = np.array([-0.5, 0.5, -0.3, 0.3]) 23 | p_theta = (np.arange(1, 2*n, 2)) / (2 * n) * 2 * np.pi 24 | p_circ = np.vstack( 25 | [np.hstack([xy_bound[1] * np.cos(p_theta), 26 | xy_bound[1] * np.cos(p_theta + np.pi)]), 27 | np.hstack([xy_bound[3] * np.sin(p_theta), 28 | xy_bound[3] * np.sin(p_theta + np.pi)])]) 29 | x_goal = p_circ[:, 0:n] 30 | flag = 0 # Flag of task completion 31 | 32 | # iterate for the previously specified number of iterations. 33 | for _ in range(0, iterations): 34 | # Retrieve teh most recent poses from teh Robotarium. The time delay is 35 | # approximately 0.033 seconds. 36 | x = r.get_poses() 37 | x_temp = x[0:2, :] 38 | 39 | # ALGORITHM 40 | 41 | # Nominal controller, go2goal 42 | if np.linalg.norm(x_goal-x_temp, ord=1) < 0.08: 43 | flag = 1 - flag 44 | 45 | if flag == 0: 46 | x_goal = p_circ[:, 0:n] 47 | 48 | else: 49 | x_goal = p_circ[:, n:2*n] 50 | 51 | # Use different go-to-goal 52 | dx = controllers.position_int(x, x_goal, 0.05) 53 | 54 | # Saturation of controls 55 | dx_max = 0.1 56 | for i in range(0, n): 57 | if np.linalg.norm(dx[:, i]) > dx_max: 58 | dx[:, i] = dx[:, i] / np.linalg.norm(dx[:, i]) * dx_max 59 | 60 | # END ALGORITHM 61 | 62 | # Ensure the robots don't collide 63 | dx = transformations.barrier_certificate(dx, x, ds=0.1) 64 | 65 | # Transform the single-integrator dynamics to unicycle dynamics using a 66 | # diffeomorphism, which can be found in the utilities. 67 | dx = transformations.int_to_uni2(dx, x, 0.75, np.pi) 68 | 69 | # Set velocities of agents 1,...,n 70 | r.set_velocities(range(0, n), dx) 71 | 72 | # Send the previously set velocities to the agents. 73 | # This function must be called. 74 | r.step() 75 | -------------------------------------------------------------------------------- /examples/leaderFollowerStatic.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from robotarium import Robotarium, transformations, controllers, graph 3 | 4 | 5 | if __name__ == '__main__': 6 | # Run for 300 Iterations 7 | iterations = 2000 8 | u_h = None 9 | 10 | # Get Robotarium object and set the save parameters 11 | r = Robotarium() 12 | n = r.get_available_agents() 13 | r.initialize(n) 14 | 15 | # Graph Laplacian 16 | followers = -1 * graph.complete_gl(n-1) 17 | lap = np.zeros((n, n)) 18 | lap[1:n, 1:n] = followers 19 | lap[1, 1] += 1 20 | lap[1, 0] = -1 21 | 22 | # Initialize velocity vector 23 | dx = np.zeros((2, n)) 24 | 25 | # State for leader 26 | state = 0 27 | 28 | # Collision_avoidance_gain = 0.001 29 | formation_control_gain = 10 30 | desired_distance = 0.09 31 | 32 | for _ in range(0, 300): 33 | # Retrieve the most recent poses from the Robotarium. The time 34 | # delay is approximately 0.033 seconds. 35 | x = r.get_poses() 36 | 37 | # ALGORITHM 38 | for i in range(1, n): 39 | # Zero velocity and get the topological neighbors of agent i 40 | dx[:, [i]] = np.zeros((2, 1)) 41 | neighbors = r.get_top_neighbors(i, lap) 42 | 43 | for j in neighbors: 44 | dx[:, i] += formation_control_gain * \ 45 | (np.power(np.linalg.norm(x[0:2, int(j)] 46 | - x[0:2, i]), 2) 47 | - np.power(desired_distance, 2)) * \ 48 | np.subtract(x[0:2, int(j)], x[0:2, i]) 49 | 50 | # END ALGORITHM 51 | if state == 3: 52 | dx[:, [0]] = controllers.position_int(x[:, 0], 53 | np.array([[0.25], [-0.15]]), 54 | 0.05) 55 | if np.linalg.norm(x[0:2, 0] - np.array([[0.25], [-0.15]])) < 0.05: 56 | state = 4 57 | 58 | if state == 2: 59 | dx[:, [0]] = controllers.position_int(x[:, 0], 60 | np.array([[-0.25], [-0.15]]), 61 | 0.05) 62 | if np.linalg.norm(x[0:2, 0] - np.array([[-0.25], [-0.15]])) < 0.05: 63 | state = 3 64 | 65 | if state == 1: 66 | dx[:, [0]] = controllers.position_int(x[:, 0], 67 | np.array([[-0.25], [0.15]]), 68 | 0.05) 69 | if np.linalg.norm(x[0:2, 0] - np.array([[-0.25], [0.15]])) < 0.05: 70 | state = 2 71 | 72 | if state == 0: 73 | dx[:, [0]] = controllers.position_int(x[:, [0]], 74 | np.array([[0.25], [0.15]]), 75 | 0.05) 76 | if np.linalg.norm(x[0:2, 0] - np.array([[0.25], [0.15]])) < 0.05: 77 | state = 1 78 | 79 | dx[:, [0]] = (dx[:, [0]] / np.linalg.norm(dx[:, 0])) * 0.05 80 | dx = transformations.barrier_certificate(dx, x, ds=0.1) 81 | dx = transformations.int_to_uni2(dx, x, 1, 2) 82 | 83 | # Set velocities 84 | r.set_velocities(range(0, n), dx) 85 | 86 | # Iterate experiment 87 | r.step() 88 | -------------------------------------------------------------------------------- /examples/formationControl.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from robotarium import Robotarium, transformations 3 | 4 | 5 | # Get Robotarium object used to communicate with the robots/simulator 6 | r = Robotarium() 7 | 8 | # Get the number of available agents from the Robotarium. We don't need 9 | # a specific value for this algorithm. 10 | n = 6 11 | 12 | # Initialize the Robotarium object with the desired number of agents. 13 | r.initialize(n) 14 | 15 | # Gains for the transformation from single-integrator to unicycle 16 | # dynamics 17 | linear_velocity_gain = 1 18 | angular_velocity_gain = np.pi / 2 19 | formation_control_gain = 4 20 | 21 | # Select the number of iterations for the experiment. This value is 22 | # arbitrary 23 | iterations = 500 24 | 25 | # Communication topology for desired formation. We need 2 * N - 3 = 9 26 | # edges to ensure that the formation is rigid. 27 | lap = np.array([ 28 | [3, -1, 0, -1, 0, -1], 29 | [-1, 3, -1, 0, -1, 0], 30 | [0, -1, 3, -1, 0, -1], 31 | [-1, 0, -1, 3, -1, 0], 32 | [0, -1, 0, -1, 3, -1], 33 | [-1, 0, -1, 0, -1, 3]]) 34 | 35 | # The desired inter-agent distance for the formation. 36 | d = 0.2 37 | 38 | # Pre-compute diagonal values for the rectangular formation 39 | d_diag = np.sqrt(np.power(2*d, 2) + np.power(d, 2)) 40 | 41 | # Weight matrix 42 | weights = np.array([ 43 | [0, d, 0, d, 0, d_diag], 44 | [d, 0, d, 0, d, 0], 45 | [0, d, 0, d_diag, 0, d], 46 | [d, 0, d_diag, 0, d, 0], 47 | [0, d, 0, d, 0, d], 48 | [d_diag, 0, d, 0, d, 0]]) 49 | 50 | # Initialize velocity vector for agents. Each agents expects a 2x1 51 | # velocity vector containing the linear and angular velocity, respectively. 52 | dx = np.zeros((2, n)) 53 | 54 | # Iterate for the previously specified number of iterations. 55 | for _ in range(0, iterations): 56 | # Retrieve the most recent poses from the Robotarium. The time delay is 57 | # approximately 0.033 seconds. 58 | x = r.get_poses() 59 | 60 | # ALGORITHM 61 | # This section contains the actual algorithm for formation control! 62 | 63 | # Calculate single integrator control inputs using edge-energy consensus 64 | for i in range(0, n): 65 | # Initialize velocity to zero for each agent. this allows us to sum 66 | # over agent i's neighbors 67 | dx[:, [i]] = np.zeros((2, 1)) 68 | 69 | # Get the topological neighbors of agent i form the communication 70 | # topology 71 | for j in r.get_top_neighbors(i, lap): 72 | # For each neighbor, calculate appropriate formation control 73 | # term and add it to the total velocity. 74 | 75 | # FORMATION CONTROL 76 | dx[:, [i]] += \ 77 | formation_control_gain * \ 78 | (np.power(np.linalg.norm(x[0:2, [i]] - x[0:2, [j]]), 2) - 79 | np.power(weights[i, j], 2)) * \ 80 | np.subtract(x[0:2, [j]], x[0:2, [i]]) 81 | 82 | # END FORMATION CONTROL 83 | 84 | # END ALGORITHM 85 | # Transform the single-integrator dynamics to unicycle dynamics using a 86 | # provided utility. 87 | dx = transformations.int_to_uni2(dx, x, linear_velocity_gain, 88 | angular_velocity_gain) 89 | 90 | # Set velocities of agents 1 to n. 91 | r.set_velocities(range(0, n), dx) 92 | 93 | # Send the previously set velocities to the agents. This function must be 94 | # called. 95 | r.step() 96 | -------------------------------------------------------------------------------- /robotarium/controllers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def park(states, poses, gamma, k, h): 5 | """ 6 | SUMMARY OF THIS FUNCTION GOES HERE. 7 | 8 | Parameters 9 | ---------- 10 | states : SOMETHING 11 | SOMETHING 12 | poses : SOMETHING 13 | SOMETHING 14 | gamma : SOMETHING 15 | SOMETHING 16 | k : SOMETHING 17 | SOMETHING 18 | h : SOMETHING 19 | SOMETHING 20 | 21 | """ 22 | n = states.shape[1] 23 | dxu = np.zeros((2, n)) 24 | 25 | for i in range(0, n): 26 | translate = rot_mat(-1*poses[2, i]) * (poses[0:2, i] - states[0:2, i]) 27 | e = np.linalg.norm(translate) 28 | theta = np.arctan2(translate[1], translate[0]) 29 | alpha = theta - (states[2, i] - poses[2, i]) 30 | alpha = np.arctan2(np.sin(alpha), np.cos(alpha)) 31 | 32 | ca = np.cos(alpha) 33 | sa = np.sin(alpha) 34 | 35 | dxu[0, i] = gamma * e * ca 36 | dxu[1, i] = k * alpha + gamma * ((ca*sa)/alpha) * (alpha + h * theta) 37 | 38 | return dxu 39 | 40 | 41 | def rot_mat(x): 42 | """ Create Rotation Matrix to rotate a 2d numpy array by the angle x 43 | 44 | Parameters 45 | ---------- 46 | x : float 47 | angle defining the rotation (rotation is counter clockwise for positive 48 | values) 49 | 50 | Returns 51 | ------- 52 | mat : np.ndarray 53 | a 2 x 2 matrix that can be used to rotate a 2d array 54 | 55 | Examples 56 | -------- 57 | >>> import numpy as np 58 | >>> from robotarium.controllers import rot_mat 59 | >>> rot = rot_mat(np.pi / 2) 60 | >>> rot.dot(np.array([1, 0])) 61 | array([ 6.12323400e-17, 1.00000000e+00]) 62 | """ 63 | return np.array([[np.cos(x), -1*np.sin(x)], [np.sin(x), np.cos(x)]]) 64 | 65 | 66 | def position_clf(states, poses): 67 | """ 68 | SUMMARY OF THIS FUNCTION GOES HERE. 69 | 70 | Parameters 71 | ---------- 72 | states : SOMETHING 73 | SOMETHING 74 | poses : SOMETHING 75 | SOMETHING 76 | 77 | """ 78 | n = states.shape[1] 79 | dx = np.zeros((2, n)) 80 | 81 | for i in range(0, n): 82 | dx_temp = poses[0, i] - states[0, i] 83 | dy = poses[1, i] - states[1, i] 84 | dt = np.arctan2(dy, dx_temp) 85 | 86 | dist = np.sqrt(np.power(dx_temp, 2) + np.power(dy, 2)) 87 | 88 | dx[0, i] = np.cos(dt - states[2, i]) 89 | dx[1, i] = np.sin(dt - states[2, i]) 90 | 91 | return dx 92 | 93 | 94 | def position_int(states, poses, k=1): 95 | """ 96 | Position controller via single integrator dynamics. 97 | 98 | Parameters 99 | ---------- 100 | states : np.ndarray 101 | a 5 x n numpy array with rows representing x position, y position, 102 | angular position, forward velocity, and angular velocity. The kth 103 | column represents the state for the kth vehicle. 104 | 105 | poses : np.ndarray 106 | a 3 x n numpy array with rows representing desired x position, y 107 | position, and angular position. 108 | 109 | k : float (default = 1) 110 | proportional gain 111 | 112 | """ 113 | n = poses.shape[1] 114 | dx = np.zeros((2, n)) 115 | 116 | for i in range(0, n): 117 | dx[:, [i]] = k * (poses[0:2, [i]] - states[0:2, [i]]) 118 | 119 | return dx 120 | -------------------------------------------------------------------------------- /doc/source/README.md: -------------------------------------------------------------------------------- 1 | robotarium-python-simulator 2 | =========================== 3 | 4 | This is a Python simulator for Robotarium! The Robotarium is a project started by Georgia Tech Professor Magnus Egerstedt allowing public, remote access to a state-of-the-art multi-robot testbed. 5 | 6 | This project, however, is NOT associated with the Robotarium. This is an open-source, re-implementation of the [MATLAB simulator] (https://github.com/robotarium/robotarium-matlab-simulator) in Python (provided by Magnus Egerstedt, Daniel Pickem, and pglotfel). The purpose of this project is to allow people to further experiment with the Robotarium simulator in the following ways: 7 | 8 | 1. Unable to have access to MATLAB. 9 | 2. Want the flexibility of Python. 10 | 3. Educational purposes, such as learning about multi-agent systems. 11 | 12 | NOTE: A Script/Code generated with this simulator is not usable with the remote client set up by Magnus Egerstedt. This is ONLY for simulation purposes. To have a physical implementation code run, you must re-implement your code into MATLAB. The way this simulator is written should allow for painless re-implementation. 13 | 14 | Credit is given to Magnus Egerstedt and Daniel Pickem for the [code] (https://github.com/robotarium/robotarium-matlab-simulator) from which this was derived (MIT License). 15 | 16 | This work is done in association with the Autonomous Collective Systems Lab at Arizona State University. 17 | 18 | ## Installation 19 | 20 | The simulator can run on all major platforms (Windows, Linux, and macOS). All that is required is cloning the repository and installing some necessary dependencies. 21 | 22 | ``` 23 | git clone https://github.com/zmk5/robotarium-python-simulator.git 24 | ``` 25 | 26 | The following dependencies are required for utilization of the simulator: 27 | - [NumPy] (http://www.numpy.org) 28 | - [matplotlib] (http://matplotlib.org/index.html) 29 | - [CVXOPT] (http://cvxopt.org/index.html) 30 | 31 | NOTE: The SciPy stack and matplotlib can be difficult to install on Windows. However, [this] (http://www.lfd.uci.edu/~gohlke/pythonlibs/) link provides .whl files necessary for installation. Make sure to install all the dependencies for each version part of the SciPy and matplotlib stack! 32 | 33 | ## Usage 34 | To utilize the simulator, the following libraries will need to be imported: 35 | 36 | ``` 37 | import Robotarium 38 | 39 | # If you need specific utlities, import the following: 40 | from utilities import controllers 41 | from utilities import graph 42 | from utilities import misc 43 | from utilities import transformation 44 | ``` 45 | 46 | Once the files are imported, the Robotarium object must be instantantiated: 47 | 48 | ``` 49 | r = Robotarium() 50 | ``` 51 | 52 | This call will initialize default values that may be changed after this point. Once complete, the creation of the robots is done through the following method call: 53 | 54 | ``` 55 | r.initialize(n) 56 | ``` 57 | 58 | where 'n' is a number of grits bots desired. Once these lines are called, a user may begin formulating algorithms for use. 59 | 60 | ## Python to MATLAB 61 | Currently, the Robotarium exclusively uses MATLAB for running user scripts for remote-connection sessions. This means that scripts generated using this code will not work with the remote session tools. The code will need to be rewritten into MATLAB syntax to work with the Robotarium system. Another issue is that the code contained within this repository was made PEP8 compliant. Therefore, method and function calls need to be converted to their equivalent MATLAB calls. These changes are shown below: 62 | 63 | ``` 64 | # Python Version # MATLAB Version 65 | 66 | # --- For the Robotarium class --- # 67 | # Methods 68 | r.initialize() ---> r.initialize() 69 | r.step() ---> r.step() 70 | 71 | # Mutators 72 | r.set_position_controller() ---> r.setPositionController() 73 | r.set_velocities() ---> r.setVelocities() 74 | r.set_positions() ---> r.setPositions() 75 | r.set_save_parameters() ---> r.setSaveParameters() 76 | 77 | # Accessors 78 | r.get_d_disk_neighbors() ---> r.getDDiskNeighbors() 79 | r.get_top_neighbors() ---> r.getTopNeighbors() 80 | r.get_poses() ---> r.getPoses() 81 | r.get_available_agents() ---> r.getAvailableAgents() 82 | 83 | # --- utilities/controller folder --- # 84 | park() ---> park() 85 | position_clf() ---> positionCLF() 86 | position_int() ---> positionInt() 87 | 88 | # --- utilities/graph folder --- # 89 | complete_gl() ---> completeGL() 90 | cycle_gl() ---> cycleGL() 91 | line_gl() ---> lineGL() 92 | random_connected_gl() ---> randomConnectedGL() 93 | random_gl() ---> randomGL() 94 | 95 | # --- utilities/misc folder --- # 96 | is_initialized() ---> isInitialized() 97 | unique_filename() ---> uniqueFilename() 98 | 99 | # --- utilities/transformations folder --- # 100 | int_to_uni() ---> int2uni() 101 | int_to_uni2() ---> int2uni2() 102 | int_to_uni3() ---> int2uni3() 103 | uni_to_int() ---> uni2int() 104 | 105 | ``` 106 | 107 | Methods or functions not in this chart are not located within MATLAB version of this simulator. 108 | -------------------------------------------------------------------------------- /robotarium/graph.py: -------------------------------------------------------------------------------- 1 | ''' a set of utilities to create matrix representations (Laplacians) of graphs 2 | ''' 3 | import numpy as np 4 | 5 | 6 | def complete_gl(n): 7 | """ 8 | return the Laplacian of a complete graph (all nodes are connected to all 9 | edges) 10 | 11 | Parameters 12 | ---------- 13 | n : int 14 | number of nodes in the graph 15 | 16 | Examples 17 | -------- 18 | >>> from robotarium.graph import complete_gl 19 | >>> complete_gl(4) 20 | array([[ 3., -1., -1., -1.], 21 | [-1., 3., -1., -1.], 22 | [-1., -1., 3., -1.], 23 | [-1., -1., -1., 3.]]) 24 | 25 | """ 26 | return n * np.eye(n) - np.ones((n, n)) 27 | 28 | 29 | def cycle_gl(n): 30 | """ 31 | return the Laplacian of a cycle graph (The order is assumed to be 32 | 1->2->3->...->n) 33 | 34 | Parameters 35 | ---------- 36 | n : int 37 | number of nodes in the graph 38 | 39 | Examples 40 | -------- 41 | >>> from robotarium.graph import cycle_gl 42 | >>> cycle_gl(4) 43 | array([[ 2., -1., 0., -1.], 44 | [-1., 2., -1., 0.], 45 | [ 0., -1., 2., -1.], 46 | [-1., 0., -1., 2.]]) 47 | """ 48 | laplacian = 2 * np.eye(n) - np.diag([1] * (n-1), 1) - \ 49 | np.diag([1] * (n-1), -1) 50 | laplacian[n-1, 0] = -1 51 | laplacian[0, n-1] = -1 52 | 53 | return laplacian 54 | 55 | 56 | def line_gl(n): 57 | """ 58 | return the Laplacian of a line graph 59 | 60 | Parameters 61 | ---------- 62 | n : int 63 | number of nodes in the graph 64 | 65 | Examples 66 | -------- 67 | >>> from robotarium.graph import line_gl 68 | >>> line_gl(4) 69 | array([[ 1., -1., 0., 0.], 70 | [-1., 2., -1., 0.], 71 | [ 0., -1., 2., -1.], 72 | [ 0., 0., -1., 1.]]) 73 | """ 74 | laplacian = 2 * np.eye(n) - np.diag(np.ones(n-1), 1) - \ 75 | np.diag(np.ones(n-1), -1) 76 | laplacian[0, 0] = 1 77 | laplacian[n-1, n-1] = 1 78 | 79 | return laplacian 80 | 81 | 82 | def random_connected_gl(v, e): 83 | """ 84 | Outputs a randomly generated, undirected, connected graph. 85 | Laplacian with v - 1 + e edges 86 | 87 | Parameters 88 | ---------- 89 | v : int 90 | number of nodes 91 | e : int 92 | number of edges 93 | 94 | Examples 95 | -------- 96 | 97 | """ 98 | 99 | laplacian = np.zeros((v, v)) 100 | 101 | for i in range(1, v): 102 | edge = np.random.randint(i) 103 | 104 | # Update adjacency relations. 105 | laplacian[i, edge] = -1 106 | laplacian[edge, i] = -1 107 | 108 | # Update node degrees 109 | laplacian[i, i] += 1 110 | laplacian[edge, edge] += 1 111 | 112 | # This works because all nodes have at least 1 degree. Choose from only 113 | # upper diagonal portion. 114 | temp = np.where(np.triu(laplacian).reshape(v*v) == 1) 115 | pot_edges = temp[0] 116 | sz = laplacian.shape 117 | 118 | # num_edges = min(e, len(pot_edges)) 119 | num_edges = np.where(e <= len(pot_edges), e, len(pot_edges)) 120 | 121 | if num_edges <= 0: 122 | return 123 | 124 | # Indices of randomly chosen extra edges. 125 | temp = np.random.permutation(len(pot_edges)) 126 | edge_indices = temp[0:num_edges] 127 | 128 | i, j = ind_to_sub(sz, pot_edges[edge_indices]) 129 | 130 | # Update adjacency relation 131 | laplacian[i, j] = -1 132 | laplacian[j, i] = -1 133 | 134 | # Update degree relation 135 | laplacian[i, i] += 1 136 | laplacian[j, j] += 1 137 | 138 | return laplacian 139 | 140 | 141 | def random_gl(v, e): 142 | """ 143 | Outputs a randomly generated, undirected, connected graph Laplacian with 144 | 'n' nodes. 145 | 146 | Parameters 147 | ---------- 148 | v : SOMETHING 149 | SOMETHING 150 | e : SOMETHING 151 | SOMETHING 152 | 153 | """ 154 | laplacian = np.tril(np.ones((v, v))) 155 | 156 | # This works because I can't select diagonals 157 | temp = np.where(np.triu(laplacian).reshape(v*v) == 0) 158 | pot_edges = temp[0] 159 | sz = laplacian.shape 160 | 161 | # Rest to zeros 162 | laplacian = np.zeros((v, v)) 163 | 164 | num_edges = np.where(e <= len(pot_edges), e, len(pot_edges)) 165 | 166 | # Indices of randomly chosen extra edges. 167 | temp = np.random.permutation(len(pot_edges)) 168 | edge_indices = temp[0:num_edges] 169 | 170 | i, j = ind_to_sub(sz, pot_edges[edge_indices]) 171 | 172 | # Update adjacency relation 173 | laplacian[i, j] = -1 174 | laplacian[j, i] = -1 175 | 176 | # Update degree relation 177 | laplacian[i, i] += 1 178 | laplacian[j, j] += 1 179 | 180 | return laplacian 181 | 182 | 183 | def ind_to_sub(siz, ind): 184 | """ 185 | Subscripts from linear index. 186 | 187 | This is a python formulation of the function ind2sub(). 188 | The original function can be found here: 189 | https://www.mathworks.com/help/matlab/ref/ind2sub.html 190 | 191 | The function provided below is a modification of a function provided here: 192 | https://stackoverflow.com/questions/28995146/matlab-ind2sub-equivalent-in-python 193 | 194 | The subtraction by one in the 'rows' variable is to keep index changes 195 | consistent with a 0 index start compared to MATLAB's 1 start. 196 | 197 | Parameters 198 | ---------- 199 | siz : int tuple 200 | contains the size of the matrix that is passed through. 201 | 202 | ind : np.ndarray 203 | the matrix that the linear index subscripts will be derived. 204 | 205 | Returns 206 | ------- 207 | rows : np.ndarray 208 | vector containing the equivalent row subscripts corresponding to each 209 | linear index from the original matrix ind. 210 | 211 | columns : np.ndarray 212 | vector containing the equivalent column subscripts corresponding to each 213 | linear index from the original matrix ind. 214 | 215 | """ 216 | ind[ind < 0] = -1 217 | ind[ind >= siz[0] * siz[1]] = -1 218 | rows = np.asarray((np.ceil(ind.astype('int') / siz[0]) - 1), dtype=int) 219 | columns = (ind % siz[1]) 220 | return rows, columns 221 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = build 9 | 10 | # Internal variables. 11 | PAPEROPT_a4 = -D latex_paper_size=a4 12 | PAPEROPT_letter = -D latex_paper_size=letter 13 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source 14 | # the i18n builder cannot share the environment and doctrees with the others 15 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source 16 | 17 | .PHONY: help 18 | help: 19 | @echo "Please use \`make ' where is one of" 20 | @echo " html to make standalone HTML files" 21 | @echo " dirhtml to make HTML files named index.html in directories" 22 | @echo " singlehtml to make a single large HTML file" 23 | @echo " pickle to make pickle files" 24 | @echo " json to make JSON files" 25 | @echo " htmlhelp to make HTML files and a HTML help project" 26 | @echo " qthelp to make HTML files and a qthelp project" 27 | @echo " applehelp to make an Apple Help Book" 28 | @echo " devhelp to make HTML files and a Devhelp project" 29 | @echo " epub to make an epub" 30 | @echo " epub3 to make an epub3" 31 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 32 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 33 | @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" 34 | @echo " text to make text files" 35 | @echo " man to make manual pages" 36 | @echo " texinfo to make Texinfo files" 37 | @echo " info to make Texinfo files and run them through makeinfo" 38 | @echo " gettext to make PO message catalogs" 39 | @echo " changes to make an overview of all changed/added/deprecated items" 40 | @echo " xml to make Docutils-native XML files" 41 | @echo " pseudoxml to make pseudoxml-XML files for display purposes" 42 | @echo " linkcheck to check all external links for integrity" 43 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 44 | @echo " coverage to run coverage check of the documentation (if enabled)" 45 | @echo " dummy to check syntax errors of document sources" 46 | 47 | .PHONY: clean 48 | clean: 49 | rm -rf $(BUILDDIR)/* 50 | 51 | .PHONY: html 52 | html: 53 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 54 | @echo 55 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 56 | 57 | .PHONY: dirhtml 58 | dirhtml: 59 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 60 | @echo 61 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 62 | 63 | .PHONY: singlehtml 64 | singlehtml: 65 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 66 | @echo 67 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 68 | 69 | .PHONY: pickle 70 | pickle: 71 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 72 | @echo 73 | @echo "Build finished; now you can process the pickle files." 74 | 75 | .PHONY: json 76 | json: 77 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 78 | @echo 79 | @echo "Build finished; now you can process the JSON files." 80 | 81 | .PHONY: htmlhelp 82 | htmlhelp: 83 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 84 | @echo 85 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 86 | ".hhp project file in $(BUILDDIR)/htmlhelp." 87 | 88 | .PHONY: qthelp 89 | qthelp: 90 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 91 | @echo 92 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 93 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 94 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/robotarium.qhcp" 95 | @echo "To view the help file:" 96 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/robotarium.qhc" 97 | 98 | .PHONY: applehelp 99 | applehelp: 100 | $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp 101 | @echo 102 | @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." 103 | @echo "N.B. You won't be able to view it unless you put it in" \ 104 | "~/Library/Documentation/Help or install it in your application" \ 105 | "bundle." 106 | 107 | .PHONY: devhelp 108 | devhelp: 109 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 110 | @echo 111 | @echo "Build finished." 112 | @echo "To view the help file:" 113 | @echo "# mkdir -p $$HOME/.local/share/devhelp/robotarium" 114 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/robotarium" 115 | @echo "# devhelp" 116 | 117 | .PHONY: epub 118 | epub: 119 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 120 | @echo 121 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 122 | 123 | .PHONY: epub3 124 | epub3: 125 | $(SPHINXBUILD) -b epub3 $(ALLSPHINXOPTS) $(BUILDDIR)/epub3 126 | @echo 127 | @echo "Build finished. The epub3 file is in $(BUILDDIR)/epub3." 128 | 129 | .PHONY: latex 130 | latex: 131 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 132 | @echo 133 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 134 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 135 | "(use \`make latexpdf' here to do that automatically)." 136 | 137 | .PHONY: latexpdf 138 | latexpdf: 139 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 140 | @echo "Running LaTeX files through pdflatex..." 141 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 142 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 143 | 144 | .PHONY: latexpdfja 145 | latexpdfja: 146 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 147 | @echo "Running LaTeX files through platex and dvipdfmx..." 148 | $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja 149 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 150 | 151 | .PHONY: text 152 | text: 153 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 154 | @echo 155 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 156 | 157 | .PHONY: man 158 | man: 159 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 160 | @echo 161 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 162 | 163 | .PHONY: texinfo 164 | texinfo: 165 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 166 | @echo 167 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 168 | @echo "Run \`make' in that directory to run these through makeinfo" \ 169 | "(use \`make info' here to do that automatically)." 170 | 171 | .PHONY: info 172 | info: 173 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 174 | @echo "Running Texinfo files through makeinfo..." 175 | make -C $(BUILDDIR)/texinfo info 176 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 177 | 178 | .PHONY: gettext 179 | gettext: 180 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 181 | @echo 182 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 183 | 184 | .PHONY: changes 185 | changes: 186 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 187 | @echo 188 | @echo "The overview file is in $(BUILDDIR)/changes." 189 | 190 | .PHONY: linkcheck 191 | linkcheck: 192 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 193 | @echo 194 | @echo "Link check complete; look for any errors in the above output " \ 195 | "or in $(BUILDDIR)/linkcheck/output.txt." 196 | 197 | .PHONY: doctest 198 | doctest: 199 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 200 | @echo "Testing of doctests in the sources finished, look at the " \ 201 | "results in $(BUILDDIR)/doctest/output.txt." 202 | 203 | .PHONY: coverage 204 | coverage: 205 | $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage 206 | @echo "Testing of coverage in the sources finished, look at the " \ 207 | "results in $(BUILDDIR)/coverage/python.txt." 208 | 209 | .PHONY: xml 210 | xml: 211 | $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml 212 | @echo 213 | @echo "Build finished. The XML files are in $(BUILDDIR)/xml." 214 | 215 | .PHONY: pseudoxml 216 | pseudoxml: 217 | $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml 218 | @echo 219 | @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." 220 | 221 | .PHONY: dummy 222 | dummy: 223 | $(SPHINXBUILD) -b dummy $(ALLSPHINXOPTS) $(BUILDDIR)/dummy 224 | @echo 225 | @echo "Build finished. Dummy builder generates no files." 226 | -------------------------------------------------------------------------------- /doc/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | REM Command file for Sphinx documentation 4 | 5 | if "%SPHINXBUILD%" == "" ( 6 | set SPHINXBUILD=sphinx-build 7 | ) 8 | set BUILDDIR=build 9 | set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% source 10 | set I18NSPHINXOPTS=%SPHINXOPTS% source 11 | if NOT "%PAPER%" == "" ( 12 | set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% 13 | set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% 14 | ) 15 | 16 | if "%1" == "" goto help 17 | 18 | if "%1" == "help" ( 19 | :help 20 | echo.Please use `make ^` where ^ is one of 21 | echo. html to make standalone HTML files 22 | echo. dirhtml to make HTML files named index.html in directories 23 | echo. singlehtml to make a single large HTML file 24 | echo. pickle to make pickle files 25 | echo. json to make JSON files 26 | echo. htmlhelp to make HTML files and a HTML help project 27 | echo. qthelp to make HTML files and a qthelp project 28 | echo. devhelp to make HTML files and a Devhelp project 29 | echo. epub to make an epub 30 | echo. epub3 to make an epub3 31 | echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter 32 | echo. text to make text files 33 | echo. man to make manual pages 34 | echo. texinfo to make Texinfo files 35 | echo. gettext to make PO message catalogs 36 | echo. changes to make an overview over all changed/added/deprecated items 37 | echo. xml to make Docutils-native XML files 38 | echo. pseudoxml to make pseudoxml-XML files for display purposes 39 | echo. linkcheck to check all external links for integrity 40 | echo. doctest to run all doctests embedded in the documentation if enabled 41 | echo. coverage to run coverage check of the documentation if enabled 42 | echo. dummy to check syntax errors of document sources 43 | goto end 44 | ) 45 | 46 | if "%1" == "clean" ( 47 | for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i 48 | del /q /s %BUILDDIR%\* 49 | goto end 50 | ) 51 | 52 | 53 | REM Check if sphinx-build is available and fallback to Python version if any 54 | %SPHINXBUILD% 1>NUL 2>NUL 55 | if errorlevel 9009 goto sphinx_python 56 | goto sphinx_ok 57 | 58 | :sphinx_python 59 | 60 | set SPHINXBUILD=python -m sphinx.__init__ 61 | %SPHINXBUILD% 2> nul 62 | if errorlevel 9009 ( 63 | echo. 64 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 65 | echo.installed, then set the SPHINXBUILD environment variable to point 66 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 67 | echo.may add the Sphinx directory to PATH. 68 | echo. 69 | echo.If you don't have Sphinx installed, grab it from 70 | echo.http://sphinx-doc.org/ 71 | exit /b 1 72 | ) 73 | 74 | :sphinx_ok 75 | 76 | 77 | if "%1" == "html" ( 78 | %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html 79 | if errorlevel 1 exit /b 1 80 | echo. 81 | echo.Build finished. The HTML pages are in %BUILDDIR%/html. 82 | goto end 83 | ) 84 | 85 | if "%1" == "dirhtml" ( 86 | %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml 87 | if errorlevel 1 exit /b 1 88 | echo. 89 | echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. 90 | goto end 91 | ) 92 | 93 | if "%1" == "singlehtml" ( 94 | %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml 95 | if errorlevel 1 exit /b 1 96 | echo. 97 | echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. 98 | goto end 99 | ) 100 | 101 | if "%1" == "pickle" ( 102 | %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle 103 | if errorlevel 1 exit /b 1 104 | echo. 105 | echo.Build finished; now you can process the pickle files. 106 | goto end 107 | ) 108 | 109 | if "%1" == "json" ( 110 | %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json 111 | if errorlevel 1 exit /b 1 112 | echo. 113 | echo.Build finished; now you can process the JSON files. 114 | goto end 115 | ) 116 | 117 | if "%1" == "htmlhelp" ( 118 | %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp 119 | if errorlevel 1 exit /b 1 120 | echo. 121 | echo.Build finished; now you can run HTML Help Workshop with the ^ 122 | .hhp project file in %BUILDDIR%/htmlhelp. 123 | goto end 124 | ) 125 | 126 | if "%1" == "qthelp" ( 127 | %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp 128 | if errorlevel 1 exit /b 1 129 | echo. 130 | echo.Build finished; now you can run "qcollectiongenerator" with the ^ 131 | .qhcp project file in %BUILDDIR%/qthelp, like this: 132 | echo.^> qcollectiongenerator %BUILDDIR%\qthelp\robotarium.qhcp 133 | echo.To view the help file: 134 | echo.^> assistant -collectionFile %BUILDDIR%\qthelp\robotarium.ghc 135 | goto end 136 | ) 137 | 138 | if "%1" == "devhelp" ( 139 | %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp 140 | if errorlevel 1 exit /b 1 141 | echo. 142 | echo.Build finished. 143 | goto end 144 | ) 145 | 146 | if "%1" == "epub" ( 147 | %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub 148 | if errorlevel 1 exit /b 1 149 | echo. 150 | echo.Build finished. The epub file is in %BUILDDIR%/epub. 151 | goto end 152 | ) 153 | 154 | if "%1" == "epub3" ( 155 | %SPHINXBUILD% -b epub3 %ALLSPHINXOPTS% %BUILDDIR%/epub3 156 | if errorlevel 1 exit /b 1 157 | echo. 158 | echo.Build finished. The epub3 file is in %BUILDDIR%/epub3. 159 | goto end 160 | ) 161 | 162 | if "%1" == "latex" ( 163 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 164 | if errorlevel 1 exit /b 1 165 | echo. 166 | echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. 167 | goto end 168 | ) 169 | 170 | if "%1" == "latexpdf" ( 171 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 172 | cd %BUILDDIR%/latex 173 | make all-pdf 174 | cd %~dp0 175 | echo. 176 | echo.Build finished; the PDF files are in %BUILDDIR%/latex. 177 | goto end 178 | ) 179 | 180 | if "%1" == "latexpdfja" ( 181 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 182 | cd %BUILDDIR%/latex 183 | make all-pdf-ja 184 | cd %~dp0 185 | echo. 186 | echo.Build finished; the PDF files are in %BUILDDIR%/latex. 187 | goto end 188 | ) 189 | 190 | if "%1" == "text" ( 191 | %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text 192 | if errorlevel 1 exit /b 1 193 | echo. 194 | echo.Build finished. The text files are in %BUILDDIR%/text. 195 | goto end 196 | ) 197 | 198 | if "%1" == "man" ( 199 | %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man 200 | if errorlevel 1 exit /b 1 201 | echo. 202 | echo.Build finished. The manual pages are in %BUILDDIR%/man. 203 | goto end 204 | ) 205 | 206 | if "%1" == "texinfo" ( 207 | %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo 208 | if errorlevel 1 exit /b 1 209 | echo. 210 | echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. 211 | goto end 212 | ) 213 | 214 | if "%1" == "gettext" ( 215 | %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale 216 | if errorlevel 1 exit /b 1 217 | echo. 218 | echo.Build finished. The message catalogs are in %BUILDDIR%/locale. 219 | goto end 220 | ) 221 | 222 | if "%1" == "changes" ( 223 | %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes 224 | if errorlevel 1 exit /b 1 225 | echo. 226 | echo.The overview file is in %BUILDDIR%/changes. 227 | goto end 228 | ) 229 | 230 | if "%1" == "linkcheck" ( 231 | %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck 232 | if errorlevel 1 exit /b 1 233 | echo. 234 | echo.Link check complete; look for any errors in the above output ^ 235 | or in %BUILDDIR%/linkcheck/output.txt. 236 | goto end 237 | ) 238 | 239 | if "%1" == "doctest" ( 240 | %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest 241 | if errorlevel 1 exit /b 1 242 | echo. 243 | echo.Testing of doctests in the sources finished, look at the ^ 244 | results in %BUILDDIR%/doctest/output.txt. 245 | goto end 246 | ) 247 | 248 | if "%1" == "coverage" ( 249 | %SPHINXBUILD% -b coverage %ALLSPHINXOPTS% %BUILDDIR%/coverage 250 | if errorlevel 1 exit /b 1 251 | echo. 252 | echo.Testing of coverage in the sources finished, look at the ^ 253 | results in %BUILDDIR%/coverage/python.txt. 254 | goto end 255 | ) 256 | 257 | if "%1" == "xml" ( 258 | %SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml 259 | if errorlevel 1 exit /b 1 260 | echo. 261 | echo.Build finished. The XML files are in %BUILDDIR%/xml. 262 | goto end 263 | ) 264 | 265 | if "%1" == "pseudoxml" ( 266 | %SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml 267 | if errorlevel 1 exit /b 1 268 | echo. 269 | echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml. 270 | goto end 271 | ) 272 | 273 | if "%1" == "dummy" ( 274 | %SPHINXBUILD% -b dummy %ALLSPHINXOPTS% %BUILDDIR%/dummy 275 | if errorlevel 1 exit /b 1 276 | echo. 277 | echo.Build finished. Dummy builder generates no files. 278 | goto end 279 | ) 280 | 281 | :end 282 | -------------------------------------------------------------------------------- /doc/source/conf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | # 4 | # robotarium documentation build configuration file, created by 5 | # sphinx-quickstart on Wed Sep 7 22:18:19 2016. 6 | # 7 | # This file is execfile()d with the current directory set to its 8 | # containing dir. 9 | # 10 | # Note that not all possible configuration values are present in this 11 | # autogenerated file. 12 | # 13 | # All configuration values have a default; values that are commented out 14 | # serve to show the default. 15 | 16 | # If extensions (or modules to document with autodoc) are in another directory, 17 | # add these directories to sys.path here. If the directory is relative to the 18 | # documentation root, use os.path.abspath to make it absolute, like shown here. 19 | # 20 | # import os 21 | # import sys 22 | # sys.path.insert(0, os.path.abspath('.')) 23 | 24 | # -- General configuration ------------------------------------------------ 25 | 26 | # If your documentation needs a minimal Sphinx version, state it here. 27 | # 28 | # needs_sphinx = '1.0' 29 | 30 | # Add any Sphinx extension module names here, as strings. They can be 31 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 32 | # ones. 33 | extensions = [ 34 | 'sphinx.ext.autodoc', 35 | 'sphinx.ext.intersphinx', 36 | 'sphinx.ext.todo', 37 | 'sphinx.ext.coverage', 38 | 'sphinx.ext.mathjax', 39 | 'sphinx.ext.ifconfig', 40 | #'sphinx.ext.viewcode', 41 | 'sphinx.ext.githubpages', 42 | 'numpydoc', 43 | ] 44 | 45 | # Add any paths that contain templates here, relative to this directory. 46 | templates_path = ['_templates'] 47 | 48 | # The suffix(es) of source filenames. 49 | # You can specify multiple suffix as a list of string: 50 | # 51 | # source_suffix = ['.rst', '.md'] 52 | source_suffix = '.rst' 53 | 54 | # The encoding of source files. 55 | # 56 | # source_encoding = 'utf-8-sig' 57 | 58 | # The master toctree document. 59 | master_doc = 'index' 60 | 61 | # General information about the project. 62 | project = 'robotarium' 63 | copyright = '2016, Zahi Kakish' 64 | author = 'Zahi Kakish' 65 | 66 | # The version info for the project you're documenting, acts as replacement for 67 | # |version| and |release|, also used in various other places throughout the 68 | # built documents. 69 | # 70 | # The short X.Y version. 71 | version = '0.3' 72 | # The full version, including alpha/beta/rc tags. 73 | release = '0.3' 74 | 75 | # The language for content autogenerated by Sphinx. Refer to documentation 76 | # for a list of supported languages. 77 | # 78 | # This is also used if you do content translation via gettext catalogs. 79 | # Usually you set "language" from the command line for these cases. 80 | language = None 81 | 82 | # There are two options for replacing |today|: either, you set today to some 83 | # non-false value, then it is used: 84 | # 85 | # today = '' 86 | # 87 | # Else, today_fmt is used as the format for a strftime call. 88 | # 89 | # today_fmt = '%B %d, %Y' 90 | 91 | # List of patterns, relative to source directory, that match files and 92 | # directories to ignore when looking for source files. 93 | # This patterns also effect to html_static_path and html_extra_path 94 | exclude_patterns = [] 95 | 96 | # The reST default role (used for this markup: `text`) to use for all 97 | # documents. 98 | # 99 | # default_role = None 100 | 101 | # If true, '()' will be appended to :func: etc. cross-reference text. 102 | # 103 | # add_function_parentheses = True 104 | 105 | # If true, the current module name will be prepended to all description 106 | # unit titles (such as .. function::). 107 | # 108 | # add_module_names = True 109 | 110 | # If true, sectionauthor and moduleauthor directives will be shown in the 111 | # output. They are ignored by default. 112 | # 113 | # show_authors = False 114 | 115 | # The name of the Pygments (syntax highlighting) style to use. 116 | pygments_style = 'sphinx' 117 | 118 | # A list of ignored prefixes for module index sorting. 119 | # modindex_common_prefix = [] 120 | 121 | # If true, keep warnings as "system message" paragraphs in the built documents. 122 | # keep_warnings = False 123 | 124 | # If true, `todo` and `todoList` produce output, else they produce nothing. 125 | todo_include_todos = True 126 | 127 | 128 | # -- Options for HTML output ---------------------------------------------- 129 | 130 | # The theme to use for HTML and HTML Help pages. See the documentation for 131 | # a list of builtin themes. 132 | # 133 | html_theme = 'bizstyle' 134 | 135 | # Theme options are theme-specific and customize the look and feel of a theme 136 | # further. For a list of options available for each theme, see the 137 | # documentation. 138 | # 139 | # html_theme_options = {} 140 | 141 | # Add any paths that contain custom themes here, relative to this directory. 142 | # html_theme_path = [] 143 | 144 | # The name for this set of Sphinx documents. 145 | # " v documentation" by default. 146 | # 147 | # html_title = 'robotarium v0.3' 148 | 149 | # A shorter title for the navigation bar. Default is the same as html_title. 150 | # 151 | # html_short_title = None 152 | 153 | # The name of an image file (relative to this directory) to place at the top 154 | # of the sidebar. 155 | # 156 | # html_logo = None 157 | 158 | # The name of an image file (relative to this directory) to use as a favicon of 159 | # the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 160 | # pixels large. 161 | # 162 | # html_favicon = None 163 | 164 | # Add any paths that contain custom static files (such as style sheets) here, 165 | # relative to this directory. They are copied after the builtin static files, 166 | # so a file named "default.css" will overwrite the builtin "default.css". 167 | html_static_path = ['_static'] 168 | 169 | # Add any extra paths that contain custom files (such as robots.txt or 170 | # .htaccess) here, relative to this directory. These files are copied 171 | # directly to the root of the documentation. 172 | # 173 | # html_extra_path = [] 174 | 175 | # If not None, a 'Last updated on:' timestamp is inserted at every page 176 | # bottom, using the given strftime format. 177 | # The empty string is equivalent to '%b %d, %Y'. 178 | # 179 | # html_last_updated_fmt = None 180 | 181 | # If true, SmartyPants will be used to convert quotes and dashes to 182 | # typographically correct entities. 183 | # 184 | # html_use_smartypants = True 185 | 186 | # Custom sidebar templates, maps document names to template names. 187 | # 188 | # html_sidebars = {} 189 | 190 | # Additional templates that should be rendered to pages, maps page names to 191 | # template names. 192 | # 193 | # html_additional_pages = {} 194 | 195 | # If false, no module index is generated. 196 | # 197 | # html_domain_indices = True 198 | 199 | # If false, no index is generated. 200 | # 201 | # html_use_index = True 202 | 203 | # If true, the index is split into individual pages for each letter. 204 | # 205 | # html_split_index = False 206 | 207 | # If true, links to the reST sources are added to the pages. 208 | # 209 | # html_show_sourcelink = True 210 | 211 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 212 | # 213 | # html_show_sphinx = True 214 | 215 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 216 | # 217 | # html_show_copyright = True 218 | 219 | # If true, an OpenSearch description file will be output, and all pages will 220 | # contain a tag referring to it. The value of this option must be the 221 | # base URL from which the finished HTML is served. 222 | # 223 | # html_use_opensearch = '' 224 | 225 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 226 | # html_file_suffix = None 227 | 228 | # Language to be used for generating the HTML full-text search index. 229 | # Sphinx supports the following languages: 230 | # 'da', 'de', 'en', 'es', 'fi', 'fr', 'h', 'it', 'ja' 231 | # 'nl', 'no', 'pt', 'ro', 'r', 'sv', 'tr', 'zh' 232 | # 233 | # html_search_language = 'en' 234 | 235 | # A dictionary with options for the search language support, empty by default. 236 | # 'ja' uses this config value. 237 | # 'zh' user can custom change `jieba` dictionary path. 238 | # 239 | # html_search_options = {'type': 'default'} 240 | 241 | # The name of a javascript file (relative to the configuration directory) that 242 | # implements a search results scorer. If empty, the default will be used. 243 | # 244 | # html_search_scorer = 'scorer.js' 245 | 246 | # Output file base name for HTML help builder. 247 | htmlhelp_basename = 'robotariumdoc' 248 | 249 | # -- Options for LaTeX output --------------------------------------------- 250 | 251 | latex_elements = { 252 | # The paper size ('letterpaper' or 'a4paper'). 253 | # 254 | # 'papersize': 'letterpaper', 255 | 256 | # The font size ('10pt', '11pt' or '12pt'). 257 | # 258 | # 'pointsize': '10pt', 259 | 260 | # Additional stuff for the LaTeX preamble. 261 | # 262 | # 'preamble': '', 263 | 264 | # Latex figure (float) alignment 265 | # 266 | # 'figure_align': 'htbp', 267 | } 268 | 269 | # Grouping the document tree into LaTeX files. List of tuples 270 | # (source start file, target name, title, 271 | # author, documentclass [howto, manual, or own class]). 272 | latex_documents = [ 273 | (master_doc, 'robotarium.tex', 'robotarium Documentation', 274 | 'Zahi Kakish', 'manual'), 275 | ] 276 | 277 | # The name of an image file (relative to this directory) to place at the top of 278 | # the title page. 279 | # 280 | # latex_logo = None 281 | 282 | # For "manual" documents, if this is true, then toplevel headings are parts, 283 | # not chapters. 284 | # 285 | # latex_use_parts = False 286 | 287 | # If true, show page references after internal links. 288 | # 289 | # latex_show_pagerefs = False 290 | 291 | # If true, show URL addresses after external links. 292 | # 293 | # latex_show_urls = False 294 | 295 | # Documents to append as an appendix to all manuals. 296 | # 297 | # latex_appendices = [] 298 | 299 | # It false, will not define \strong, \code, itleref, \crossref ... but only 300 | # \sphinxstrong, ..., \sphinxtitleref, ... To help avoid clash with user added 301 | # packages. 302 | # 303 | # latex_keep_old_macro_names = True 304 | 305 | # If false, no module index is generated. 306 | # 307 | # latex_domain_indices = True 308 | 309 | 310 | # -- Options for manual page output --------------------------------------- 311 | 312 | # One entry per manual page. List of tuples 313 | # (source start file, name, description, authors, manual section). 314 | man_pages = [ 315 | (master_doc, 'robotarium', 'robotarium Documentation', 316 | [author], 1) 317 | ] 318 | 319 | # If true, show URL addresses after external links. 320 | # 321 | # man_show_urls = False 322 | 323 | 324 | # -- Options for Texinfo output ------------------------------------------- 325 | 326 | # Grouping the document tree into Texinfo files. List of tuples 327 | # (source start file, target name, title, author, 328 | # dir menu entry, description, category) 329 | texinfo_documents = [ 330 | (master_doc, 'robotarium', 'robotarium Documentation', 331 | author, 'robotarium', 'One line description of project.', 332 | 'Miscellaneous'), 333 | ] 334 | 335 | # Documents to append as an appendix to all manuals. 336 | # 337 | # texinfo_appendices = [] 338 | 339 | # If false, no module index is generated. 340 | # 341 | # texinfo_domain_indices = True 342 | 343 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 344 | # 345 | # texinfo_show_urls = 'footnote' 346 | 347 | # If true, do not generate a @detailmenu in the "Top" node's menu. 348 | # 349 | # texinfo_no_detailmenu = False 350 | 351 | 352 | # Example configuration for intersphinx: refer to the Python standard library. 353 | intersphinx_mapping = {'https://docs.python.org/': None} 354 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | DEPRECATION WARNING 2 | =================== 3 | 4 | This repository is now deprecated! Check out the official python simulator at https://github.com/robotarium/robotarium_python_simulator. 5 | 6 | Introduction 7 | ============ 8 | 9 | This is a Python simulator for Robotarium! The Robotarium is a project started by Georgia Tech Professor Magnus Egerstedt allowing public, remote access to a state-of-the-art multi-robot testbed. 10 | 11 | This is an open-source, re-implementation of the [MATLAB simulator] (https://github.com/robotarium/robotarium-matlab-simulator) in Python (provided by Magnus Egerstedt, Daniel Pickem, and pglotfel). The purpose of this project is to allow people to further experiment with the Robotarium simulator in the following ways: 12 | 13 | 1. Unable to have access to MATLAB. 14 | 2. Want the flexibility of Python. 15 | 3. Educational purposes, such as learning about multi-agent systems. 16 | 17 | NOTE: A Script/Code generated with this simulator is not usable with the remote client set up by Magnus Egerstedt. This is ONLY for simulation purposes. To have a physical implementation code run, you must re-implement your code into MATLAB. The way this simulator is written should allow for painless re-implementation. 18 | 19 | Credit is given to Magnus Egerstedt and Daniel Pickem for the [code] (https://github.com/robotarium/robotarium-matlab-simulator) from which this was derived (MIT License). 20 | 21 | This work is done in association with the Autonomous Collective Systems Lab at Arizona State University. 22 | 23 | ## Installation 24 | The simulator can run on all major platforms (Windows, Linux, and macOS). All that is required is cloning the repository and installing some necessary dependencies. 25 | 26 | ``` 27 | git clone https://github.com/zmk5/robotarium-python-simulator.git 28 | ``` 29 | 30 | The following dependencies are required for utilization of the simulator: 31 | - [NumPy] (http://www.numpy.org) 32 | - [matplotlib] (http://matplotlib.org/index.html) 33 | - [CVXOPT] (http://cvxopt.org/index.html) 34 | 35 | NOTE: The SciPy stack and matplotlib can be difficult to install on Windows. However, [this] (http://www.lfd.uci.edu/~gohlke/pythonlibs/) link provides .whl files necessary for installation. Make sure to install all the dependencies for each version part of the SciPy and matplotlib stack! 36 | 37 | ## Dependency Installation 38 | 39 | The guide below will show you how to install the necessary dependencies. Since the simulator can work on both Python 2.7.x and Python 3.5.x versions, the installation is given for both options. You do not need to install both! 40 | 41 | ### Linux 42 | To install the simulator on linux requires the installation of the dependencies labeled above. The installation varies depending on the distribution used. The easiest way to install CVXOPT is to use pip, which is typically installed with the default python installation. 43 | 44 | #### Ubuntu, Debian, and other Ubuntu/Debian based distributions. 45 | ``` 46 | # Python 2.7.x 47 | sudo apt-get install python-numpy python-scipy python-matplotlib python-pip 48 | pip install cvxopt --user 49 | 50 | # Python 3.5.x 51 | sudo apt-get install python3-numpy python3-scipy python3-matplotlib python3-pip 52 | pip3 install cvxopt --user 53 | ``` 54 | 55 | #### Fedora, CentOS, and other RPM based distributions. 56 | ``` 57 | # Python 2.7.x 58 | sudo yum install numpy scipy python-matplotlib python-pip # For YUM package manager. 59 | sudo dnf install numpy scipy python-matplotlib python-pip # For DNF package manager. 60 | pip install cvxopt --user 61 | 62 | # Python 3.5.x 63 | sudo yum install numpy scipy python3-matplotlib python3-pip # For YUM package manager. 64 | sudo dnf install numpy scipy python3-matplotlib python3-pip # For DNF package manager. 65 | pip3 install cvxopt --user 66 | ``` 67 | 68 | #### pip 69 | If you are already using python with (or without) pip installed and configured, the installation can be done simply with the following commands: 70 | 71 | ``` 72 | # For Python 2.7.x 73 | sudo apt-get install python-pip # Ubuntu/Debian 74 | sudo yum install python-pip # Fedora/CentOS (RPM based YUM) 75 | sudo dnf install python-pip # Fedora/CentOS (RPM based DNF) 76 | 77 | pip install scipy 78 | pip install numpy 79 | pip install matplotlib 80 | pip install cvxopt --user 81 | 82 | # For Python 3.5.x 83 | sudo apt-get install python3-pip # Ubuntu/Debian based 84 | sudo yum install python3-pip # Fedora/CentOS based (RPM Yum based) 85 | sudo dnf install python3-pip # Fedora/CentOS based (RPM dnf based) 86 | 87 | pip3 install scipy 88 | pip3 install numpy 89 | pip3 install matplotlib 90 | pip3 install cvxopt --user 91 | ``` 92 | 93 | ### macOS 94 | To install the simulator on macOS, it is recommended to install a package manager for easy installation. CVXOPT will have to be installed using PIP. 95 | 96 | #### Homebrew 97 | To use [Homebrew] (http://brew.sh) for dependency installation requires a bit of extra work due to the scipy stack not being a part of the main repository. You can then install the dependencies labeled above using the following work around (Requires PIP). A more detailed explanation can be found [here] (https://penandpants.com/2012/02/24/install-python/). 98 | 99 | ``` 100 | # Install Python (Choose Python 2.7.x or 3.5.x) 101 | brew install python 102 | brew install python3 103 | 104 | # Restart terminal to allow the path to python to be updated. 105 | # make sure "which python" command returns "/usr/local/bin/python" 106 | 107 | # Install pip 108 | easy_install pip 109 | 110 | # Install NumPy 111 | pip install numpy 112 | 113 | # Install SciPy 114 | brew install gfortran # Install to prevent an error inherent in SciPy. 115 | pip install scipy 116 | 117 | # Install matplotlib 118 | brew install pkg-config 119 | pip install matplotlib 120 | 121 | # Install CVXOPT 122 | pip install cvxopt --user 123 | ``` 124 | 125 | #### Macports 126 | To use [Macports] (https://www.macports.org/), use the following commands to install the scipy stack. At the time of writing, a Python 3.5.x version for the NumPy stack do not exist. 127 | ``` 128 | # For Python 2.7+ 129 | sudo port install py27-numpy py27-scipy py27-matplotlib 130 | 131 | # Install pip 132 | easy_install pip 133 | 134 | # Install CVXOPT 135 | pip install cvxopt --user 136 | ``` 137 | 138 | ### Windows 139 | Of the three installations, this one will be the most difficult due to the fact that Windows does not come with a native or easily installable package manager. To circumvent these problems, it will be necessary to install the packages using pip. The issue with using pip, however, is that NumPy, SciPy, and matplotlib require the packages to be installed without compiling. Therefore, each wheel must be installed individually. This is a simple process using pip 8.x version. The following commands are for python installations that are using PIP 8.x version. The wheel files used here can be found [here] (http://www.lfd.uci.edu/~gohlke/pythonlibs/). 140 | 141 | NOTE: The following files installed are for 64-bit architectures. If you have a 32-bit CPU, download the corresponding 32-bit and python versions of the files specified below. 142 | 143 | #### Install NumPy 144 | ``` 145 | # Install NumPy (64-bit) 146 | pip install numpy-1.11.1+mkl-cp27-cp27m-win_amd64.whl # Python 2.7.x Version 147 | pip install numpy-1.11.1+mkl-cp34-cp34m-win_amd64.whl # Python 3.4.x Version 148 | pip install numpy-1.11.1+mkl-cp35-cp35m-win_amd64.whl # Python 3.5.x Version 149 | 150 | # Install NumPy (32-bit) 151 | pip install numpy-1.11.1+mkl-cp27-cp27m-win32.whl # Python 2.7.x Version 152 | pip install numpy-1.11.1+mkl-cp34-cp34m-win32.whl # Python 3.4.x Version 153 | pip install numpy-1.11.1+mkl-cp35-cp35m-win32.whl # Python 3.5.x Version 154 | ``` 155 | 156 | #### Install SciPy 157 | ``` 158 | # Install SciPy (64-bit) 159 | pip install scipy-0.18.0-cp27-cp27m-win_amd64.whl # Python 2.7.x Version 160 | pip install scipy-0.18.0-cp34-cp34m-win_amd64.whl # Python 3.4.x Version 161 | pip install scipy-0.18.0-cp35-cp35m-win_amd64.whl # Python 3.5.x Version 162 | 163 | # Install SciPy (32-bit) 164 | pip install scipy-0.18.0-cp27-cp27m-win32.whl # Python 2.7.x Version 165 | pip install scipy-0.18.0-cp34-cp34m-win32.whl # Python 3.4.x Version 166 | pip install scipy-0.18.0-cp35-cp35m-win32.whl # Python 3.5.x Version 167 | ``` 168 | 169 | #### Install matplotlib 170 | Installation of matplotlib requires extra dependencies to be installed first. 171 | 172 | ``` 173 | # Install dateutil 174 | pip install python_dateutil-2.5.3-py2.py3-none-any.whl 175 | 176 | # Install pytz 177 | pip install pytz-2016.6.1-py2.py3-none-any.whl 178 | 179 | # Install pyparsing 180 | pip install pyparsing-2.1.8-py2.py3-none-any.whl 181 | 182 | # Install cycler 183 | pip install cycler-0.10.0-py2.py3-none-any.whl 184 | 185 | # Install setuptools 186 | pip install setuptools-25.2.0-py2.py3-none-any.whl 187 | 188 | # Install matplotlib (64-bit) 189 | pip install matplotlib-1.5.2-cp27-cp27m-win_amd64.whl # Python 2.7.x Version 190 | pip install matplotlib-1.5.2-cp34-cp34m-win_amd64.whl # Python 3.4.x Version 191 | pip install matplotlib-1.5.2-cp35-cp35m-win_amd64.whl # Python 3.5.x Version 192 | 193 | # Install matplotlib (32-bit) 194 | pip install matplotlib-1.5.2-cp27-cp27m-win32.whl # Python 2.7.x Version 195 | pip install matplotlib-1.5.2-cp34-cp34m-win32.whl # Python 3.4.x Version 196 | pip install matplotlib-1.5.2-cp35-cp35m-win32.whl # Python 3.5.x Version 197 | ``` 198 | 199 | ## Usage 200 | To utilize the simulator, the following libraries will need to be imported: 201 | 202 | ``` 203 | from robotarium.Robotarium import Robotarium 204 | 205 | # If you need specific utlities, import the following: 206 | import robotarium.controllers 207 | import robotarium.graph 208 | import robotarium.transformation 209 | ``` 210 | 211 | Once the files are imported, the Robotarium object must be instantantiated: 212 | 213 | ``` 214 | r = Robotarium() 215 | ``` 216 | 217 | This call will initialize default values that may be changed after this point. Once complete, the creation of the robots is done through the following method call: 218 | 219 | ``` 220 | r.initialize(n) 221 | ``` 222 | 223 | where 'n' is a number of grits bots desired. Once these lines are called, a user may begin formulating algorithms for use. 224 | 225 | To run one of the examples: 226 | 227 | ``` 228 | python examples/formationControl.py 229 | ``` 230 | 231 | ## Issues 232 | 233 | Please enter a ticket in the [issue tracker](https://github.com/robotarium/robotarium-python-simulator/issues). 234 | 235 | ## Python to MATLAB 236 | Currently, the Robotarium exclusively uses MATLAB for running user scripts for remote-connection sessions. This means that scripts generated using this code will not work with the remote session tools. The code will need to be rewritten into MATLAB syntax to work with the Robotarium system. Another issue is that the code contained within this repository was made PEP8 compliant. Therefore, method and function calls need to be converted to their equivalent MATLAB calls. These changes are shown below: 237 | 238 | ``` 239 | # Python Version # MATLAB Version 240 | 241 | # --- For the Robotarium class --- # 242 | # Methods 243 | r.initialize() ---> r.initialize() 244 | r.step() ---> r.step() 245 | 246 | # Mutators 247 | r.set_position_controller() ---> r.setPositionController() 248 | r.set_velocities() ---> r.setVelocities() 249 | r.set_positions() ---> r.setPositions() 250 | r.set_save_parameters() ---> r.setSaveParameters() 251 | 252 | # Accessors 253 | r.get_d_disk_neighbors() ---> r.getDDiskNeighbors() 254 | r.get_top_neighbors() ---> r.getTopNeighbors() 255 | r.get_poses() ---> r.getPoses() 256 | r.get_available_agents() ---> r.getAvailableAgents() 257 | 258 | # --- utilities/controller folder --- # 259 | park() ---> park() 260 | position_clf() ---> positionCLF() 261 | position_int() ---> positionInt() 262 | 263 | # --- utilities/graph folder --- # 264 | complete_gl() ---> completeGL() 265 | cycle_gl() ---> cycleGL() 266 | line_gl() ---> lineGL() 267 | random_connected_gl() ---> randomConnectedGL() 268 | random_gl() ---> randomGL() 269 | 270 | # --- utilities/transformations folder --- # 271 | int_to_uni() ---> int2uni() 272 | int_to_uni2() ---> int2uni2() 273 | int_to_uni3() ---> int2uni3() 274 | uni_to_int() ---> uni2int() 275 | 276 | ``` 277 | 278 | Methods or functions not in this chart are not located within MATLAB version of this simulator. 279 | -------------------------------------------------------------------------------- /robotarium/transformations.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxopt 3 | 4 | 5 | def barrier_certificate(dxi, x, dx_max=0.1, ds=0.08): 6 | """ Wraps single-integrator dynamics in safety barrier certificates. 7 | 8 | This function accepts single-integrator dynamics and wraps them in barrier 9 | certificates to ensure that collisions do not occur. Note that this 10 | algorithm bounds the magnitude of the generated output to 0.1. 11 | 12 | Parameters 13 | ---------- 14 | dxi : SOMETHING 15 | Single-integrator dynamics. 16 | x : SOMETHING 17 | States of the agents. 18 | dx_max : float 19 | SOMETHING 20 | ds : float 21 | Safety radius, size of the agents (or desired separation distance). 22 | 23 | Returns 24 | ------- 25 | dx_out : SOMETHING 26 | Generated safe, single-integrator inputs. 27 | 28 | """ 29 | cvxopt.solvers.options['show_progress'] = False 30 | n = dxi.shape[1] 31 | gamma = 1e4 32 | x = x[0:2, :] 33 | dx_out = dxi.copy() 34 | 35 | a_mat = np.empty((0, 2*n)) 36 | b_mat = np.empty((0, 1)) 37 | 38 | for i in range(0, n-1): 39 | for j in range(i+1, n): 40 | h_mat = np.linalg.norm(np.power(x[0:2, i] - x[0:2, j], 2) - 41 | np.power(ds, 2)) 42 | a_new = np.zeros((1, 2*n)) 43 | a_new[0, (2*i):(2*i+2)] = -2 * np.transpose(x[:, [i]] - x[:, [j]]) 44 | a_new[0, (2*j):(2*j+2)] = 2 * np.transpose(x[:, [i]] - x[:, [j]]) 45 | a_mat = np.vstack((a_mat, a_new)) 46 | b_mat = np.vstack((b_mat, gamma * np.power(h_mat, 3))) 47 | 48 | # Add velocity bounds to all robots 49 | g_mat = np.vstack([a_mat, -1 * np.eye(2*n), np.eye(2*n)]) 50 | h_mat = np.vstack([b_mat, dx_max * np.ones((2*n, 1)), 51 | dx_max * np.ones((2*n, 1))]) 52 | h_cap = 2 * np.eye(2*n) 53 | f_mat = -2 * np.reshape(np.transpose(dxi), (2*n, 1)) 54 | 55 | # Solve LP to check for feasibility 56 | m_0, n_0 = a_mat.shape 57 | m, n = g_mat.shape 58 | aa = np.hstack([g_mat, -np.ones((m, 1))]) 59 | bb = h_mat 60 | cc = np.zeros(n+1) 61 | cc[-1] = 1 62 | sol = cvxopt.solvers.lp(cvxopt.matrix(cc), 63 | cvxopt.matrix(aa), 64 | cvxopt.matrix(bb)) 65 | x = sol['x'] 66 | lp_tol = 0.05 67 | 68 | if x[-1] < 0: # feasible 69 | sol = cvxopt.solvers.qp(cvxopt.matrix(h_cap), 70 | cvxopt.matrix(f_mat), 71 | cvxopt.matrix(g_mat), 72 | cvxopt.matrix(h_mat), 73 | cvxopt.matrix(np.empty((0, n))), 74 | cvxopt.matrix(np.empty((0, 1)))) 75 | x = sol['x'] 76 | 77 | else: 78 | print('QP not feasible, LP result is used instead!') 79 | 80 | for i in range(0, int(n/2)): 81 | dx_out[:, i] = np.transpose(x[2*i:2*i+2]) 82 | 83 | return dx_out 84 | 85 | 86 | def safety_barrier_subset(x, dx, indices=list(), dx_max=0.1, ds=0.08): 87 | """ 88 | 89 | Parameters 90 | ---------- 91 | x : 92 | State vector containing the states of all agents in the system. 93 | dx : 94 | Velocity input vector for the subset of agents indicated by indices. 95 | indices : 96 | SOMETHING 97 | dx_max : 98 | SOMETHING 99 | ds : 100 | SOMETHING 101 | 102 | Returns 103 | ------- 104 | dx_out : 105 | SOMETHING 106 | 107 | """ 108 | 109 | # Ensure that dx and indices has the same dimensionality. 110 | m = len(indices) 111 | n = max(x.shape) 112 | assert dx.shape == (2, m) 113 | 114 | # Suppress solver command line output. 115 | cvxopt.solvers.options['show_progress'] = False 116 | dx_out = dx.copy() 117 | gamma = 1e4 118 | 119 | # Add inter-agent constraints between agents in the index set. 120 | a_mat = np.empty((0, 2*m)) 121 | b_mat = np.empty((0, 1)) 122 | for i in range(len(indices)): 123 | for j in range(i+1, len(indices)): 124 | d = np.linalg.norm(x[:, indices[i]] - x[:, indices[j]]) 125 | # Only consider agents closer than 2 * ds to current agent i. 126 | if d < (1.5 * ds): 127 | h_mat = np.power(d, 2) - np.power(ds, 2) 128 | a_new = np.zeros((2*m, 1)) 129 | a_new[2*i:2*i+2] = -2 * np.transpose(x[:, indices[i]] - 130 | x[:, indices[j]]) 131 | a_new[2*i:2*i+2] = 2 * np.transpose(x[:, indices[i]] - 132 | x[:, indices[j]]) 133 | a_mat = np.vstack([a_mat, a_new]) 134 | b_mat = np.vstack([b_mat, gamma * np.power(h_mat, 3)]) 135 | 136 | # Add inter-agent constraints between agents in the index set and agents 137 | # over which we don't have control. Treat agents not in the index set as 138 | # obstacles. 139 | for i in range(len(indices)): 140 | for j in range(n): 141 | if j not in indices: 142 | d = np.linalg.norm(x[:, indices[i]] - x[:, j]) 143 | # Only consider agents closer than 2.5 * ds to current agent i. 144 | if d < (2.5 * ds): 145 | h_mat = np.power(d, 2) - np.power(ds, 2) 146 | a_new = np.zeros((2*m, 1)) 147 | a_new[2*i:2*i+2] = -2 * np.transpose(x[:, indices[i]] - 148 | x[:, j]) 149 | a_mat = np.vstack([a_mat, a_new]) 150 | b_mat = np.vstack([b_mat, gamma * np.power(h_mat, 3)]) 151 | 152 | # Add velocity bounds to all robots in index set (indices) 153 | g_mat = np.vstack([a_mat, -1 * np.eye(2 * m), np.eye(2*m)]) 154 | h_mat = np.vstack([b_mat, dx_max * np.ones((2*m, 1)), 155 | dx_max * np.ones((2*m, 1))]) 156 | h_cap = 2 * np.eye(2*m) 157 | f_mat = -2 * np.reshape(dx.T, (2*m, 1)) 158 | 159 | # Solve LP to check for feasibility. 160 | m0, n0 = a_mat.shape 161 | m, n = g_mat.shape 162 | aa = np.hstack([g_mat, -1 * np.ones((m, 1))]) 163 | bb = h_mat 164 | cc = np.zeros(n+1) 165 | sol = cvxopt.solvers.lp(cvxopt.matrix(cc), 166 | cvxopt.matrix(aa), 167 | cvxopt.matrix(bb)) 168 | x = sol['x'] 169 | lp_tol = 0.05 170 | 171 | if x[-1] < 0: # Feasible 172 | sol = cvxopt.solvers.qp(cvxopt.matrix(h_mat), 173 | cvxopt.matrix(f_mat), 174 | cvxopt.matrix(g_mat), 175 | cvxopt.matrix(h_mat), 176 | cvxopt.matrix(np.empty((0, 2*m))), 177 | cvxopt.matrix(np.empty((0, 1)))) 178 | x = sol['x'] 179 | 180 | else: 181 | print('QP not feasible, LP result is used instead!') 182 | 183 | for i in range(m): 184 | dx_out[:, i] = np.transpose(x[2*i:2*i+2]) 185 | 186 | return dx_out 187 | 188 | 189 | def saturate_velocities(dxu, v_max=0.1, rps_max=4): 190 | """ 191 | 192 | Parameters 193 | ---------- 194 | dxu : 195 | v_max : 196 | rps_max : 197 | 198 | Returns 199 | ------- 200 | dx_out : 201 | 202 | """ 203 | if dxu.shape == (2,) or dxu.shape == (2, 1): 204 | # 1. Map [v, w] to rps values. 205 | # 2. Rate limit rps values to rps_max. 206 | # 3. Map [rps_L, rps_R] back to saturated [v, w] values. 207 | rps = np.array([rps_l(dxu[0], dxu[1]), rps_r(dxu[0], dxu[1])]) 208 | rps_scale = np.max(np.absolute(rps)) / np.absolute(rps_max) 209 | if rps_scale > 1: 210 | rps /= rps_scale 211 | 212 | return np.array([v(rps[0], rps[1]), w(rps[0], rps[1])]) 213 | 214 | else: 215 | m = dxu.shape[1] 216 | dx_out = np.zeros((2, m)) 217 | for i in range(m): 218 | rps = np.array([rps_l(dxu[0, i], dxu[1, i]), 219 | rps_r(dxu[0, i], dxu[1, i])]) 220 | rps_scale = np.max(np.absolute(rps)) / np.absolute(rps_max) 221 | 222 | if rps_scale > 1: 223 | rps /= rps_scale 224 | 225 | dx_out[:, i] = np.array([v(rps[0], rps[1]), w(rps[0], rps[1])]) 226 | 227 | return dx_out 228 | 229 | 230 | def barrier_unicycle(dxu, x, safety_radius, lambda_val): 231 | """ 232 | 233 | Parameters 234 | ---------- 235 | dxu : SOMETHING 236 | Single-integrator dynamics. 237 | x : SOMETHING 238 | States of the agent. 239 | safety_radius : int 240 | Size of the agents (or desired separation distance). 241 | lambda_val : SOMETHING 242 | SOMETHING 243 | 244 | Returns 245 | ------- 246 | dx : SOMETHING 247 | Generated safe, single-integrator inputs. 248 | 249 | """ 250 | n = dxu.shape[1] 251 | gamma = 1e4 252 | 253 | # Shift to single integrator 254 | xi = x[0:2, :] + lambda_val * np.vstack((np.cos(x[2, :]), np.sin(x[2, :]))) 255 | dxi = uni_to_int(dxu, x, lambda_val) 256 | 257 | # Determine dimension of constraints 258 | count = 0 259 | for i in range(0, n-1): 260 | for j in range(i+1, n): 261 | count += 1 262 | 263 | # Generate constraints for barrier certificates based on the size of 264 | # the safety radius. 265 | a_mat = np.zeros((count, 2*n)) 266 | b_mat = np.zeros((count, 1)) 267 | for i in range(0, n-1): 268 | for j in range(i+1, n): 269 | h = np.power(np.linalg.norm(xi[0:2, i] - xi[0:2, j]), 2) - \ 270 | np.power(safety_radius + 2 * lambda_val, 2) 271 | a_new = np.zeros(1, 2 * n) 272 | a_new[0, (2*i-1):(2*i)] = -2 * np.transpose(xi[:, i] - xi[:, j]) 273 | a_new[0, (2*j-1):(2*j)] = 2 * np.transpose(xi[:, i] - xi[:, j]) 274 | a_mat = np.concatenate((a_mat, a_new), axis=0) 275 | b_mat = np.concatenate((b_mat, gamma * np.power(h, 3)), axis=0) 276 | 277 | # Solve QP program generated earlier 278 | v_hat = np.reshape(dxi, (2*n, 1)) 279 | k_relax = 1 280 | h_cap = 2 * np.eye(2*n) 281 | f = -2 * v_hat 282 | relaxation_var = k_relax * 1 * np.ones((1, 2*n)) 283 | v_new = cvxopt.solvers.qp() 284 | 285 | # Set robot velocities to new velocities 286 | dx = int_to_uni(np.reshape(v_new, (2, n)), x, lambda_val) 287 | return dx 288 | 289 | 290 | def int_to_uni(dxi, x, lambda_val): 291 | """ 292 | Translates from single integrator to unicycle dynamics. 293 | 294 | Parameters 295 | ---------- 296 | dxi : something 297 | SOMETHING 298 | x : something 299 | Unicycle states 3 x N 300 | lambda_val : 301 | 302 | 303 | Returns 304 | ------- 305 | dx : SOMETHING 306 | SOMETHING 307 | 308 | """ 309 | n = dxi.shape[1] 310 | dx = np.zeros((2, n)) 311 | t = np.array([[1, 0], [0, 1/lambda_val]]) 312 | 313 | for i in range(0, n): 314 | temp = np.array([[np.cos(x[2, i]), np.sin(x[2, i])], 315 | [-1*np.sin(x[2, i]), np.cos(x[2, i])]]) 316 | dx[:, i] = t * np.dot(temp, dxi[:, i]) 317 | 318 | return dx 319 | 320 | 321 | def int_to_uni2(dxi, states, kv, kw): 322 | """ 323 | 324 | Parameters 325 | ---------- 326 | dxi : 327 | Single-integrator dynamics. 328 | states : 329 | unicycle states (3 x N) 330 | kv : 331 | Linear velocity gain. 332 | kw : 333 | Rotational velocity gain. Due to normalization, the w value will be 334 | in the range -kw to kw. 335 | 336 | Returns 337 | ------- 338 | dx 339 | 340 | """ 341 | n = dxi.shape[1] 342 | dx = np.zeros((2, n)) 343 | 344 | for i in range(0, n): 345 | temp_1 = np.array([[np.cos(states[2, i]), np.sin(states[2, i])]]) 346 | temp_2 = np.array([[-1 * np.sin(states[2, i]), np.cos(states[2, i])]]) 347 | dx[0, i] = kv * np.dot(temp_1, dxi[:, i]) 348 | 349 | # Normalizing the output of atan2 to between -kw and kw 350 | dx[1, i] = kw * np.arctan2(np.dot(temp_2, dxi[:, i]), 351 | np.dot(temp_1, dxi[:, i])/(np.pi/2)) 352 | 353 | return dx 354 | 355 | 356 | def int_to_uni3(dxi, x, lambda_val): 357 | """ 358 | Translates from single integrator to unicycle dynamics. 359 | 360 | Parameters 361 | ---------- 362 | dxi : 363 | Single integrator control input. 364 | x : 365 | Unicycle states (3 x N) 366 | lambda_val : float 367 | Something 368 | 369 | Returns 370 | ------- 371 | dx : 372 | 373 | 374 | """ 375 | n = dxi.shape[1] 376 | dx = np.zeros((2, n)) 377 | t = np.array([[1, 0], [0, 1/lambda_val]]) 378 | 379 | for i in range(0, n): 380 | temp = np.array([[np.cos(x[2, i]), np.sin(x[2, i])], 381 | [-1 * np.sin(x[2, i]), np.cos(x[2, i])]]) 382 | dx[:, i] = np.dot(t, np.dot(temp, dxi[:, i])) 383 | if dx[0, i] < 0: 384 | dx[1, i] *= -1 385 | 386 | return dx 387 | 388 | 389 | def uni_to_int(dxu, x, lambda_val): 390 | """ 391 | Translates from single integrator to unicycle dynamics. 392 | 393 | Parameters 394 | ---------- 395 | dxu : 396 | Single integrator control input. 397 | x : 398 | Unicycle states (3 x N) 399 | lambda_val : 400 | 401 | 402 | Returns 403 | ------- 404 | dx : 405 | 406 | 407 | """ 408 | n = dxu.shape[1] 409 | dx = np.zeros((2, n)) 410 | 411 | for i in range(0, n): 412 | temp = np.array([[np.cos(x[2, i]), -lambda_val * np.sin(x[2, i])], 413 | [np.sin(x[2, i]), lambda_val * np.cos(x[2, i])]]) 414 | dx[:, i] = np.dot(temp, dxu[:, i]) 415 | 416 | return dx 417 | 418 | 419 | def rps_r(v1, w1, r=0.005, r_track=0.0175): 420 | """ 421 | 422 | Parameters 423 | ---------- 424 | v1 : float 425 | Something 426 | w1 : float 427 | Something 428 | r : float (Default: 0.005) 429 | Wheel radius. 430 | r_track : float (Default: 0.0175) 431 | Wheel base radius. 432 | 433 | Returns 434 | ------- 435 | 436 | """ 437 | return v1 / (2 * np.pi * r) + w1 * r_track / r 438 | 439 | 440 | def rps_l(v1, w1, l=0.03, r=0.005, r_track=0.0175): 441 | """ 442 | 443 | Parameters 444 | ---------- 445 | v1 : float 446 | Something 447 | w1 : float 448 | Something 449 | l : float 450 | Something 451 | r : float (Default: 0.005) 452 | Wheel radius. 453 | r_track : float (Default: 0.0175) 454 | Wheel base radius. 455 | 456 | Returns 457 | ------- 458 | 459 | """ 460 | return v1 / (2 * np.pi * r) - w1 * r_track / r 461 | 462 | 463 | def v(wl, wr, r=0.005): 464 | """ 465 | 466 | Parameters 467 | ---------- 468 | wl : float 469 | Something 470 | wr : float 471 | Something 472 | r : float (Default: 0.005) 473 | Wheel radius. 474 | 475 | Returns 476 | ------- 477 | 478 | """ 479 | return np.pi * r * (wr + wl) 480 | 481 | 482 | def w(wl, wr, r=0.005, r_track=0.0175): 483 | """ 484 | 485 | Parameters 486 | ---------- 487 | wl : float 488 | Something 489 | wr : float 490 | Something 491 | r : float (Default: 0.005) 492 | Wheel radius. 493 | r_track : float (Default: 0.0175) 494 | Wheel base radius. 495 | 496 | Returns 497 | ------- 498 | 499 | """ 500 | return (r/r_track) * (wr - wl) / 2 501 | -------------------------------------------------------------------------------- /robotarium/Robotarium.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.text import Annotation 4 | from matplotlib import animation 5 | 6 | 7 | class Robotarium(object): 8 | """ Simulator Interface for the Robotarium. 9 | 10 | NOTE: A position controller is not applied by default, therefore it must 11 | be added through the set_position_controller() method. 12 | 13 | Parameters 14 | ---------- 15 | ion : bool (Default=True) 16 | Bool for displaying the matplotlib plot for dynamic animation. THIS 17 | SHOULD NOT BE CHANGED unless you just want a quick visualization of 18 | agent placement. 19 | anim_speed : float (Default=0.05) 20 | The speed at which matplotlib will update animations. 21 | low_quality : bool (Default=False) 22 | This mode reduces the number of points used to generate the plt.Polygon 23 | object, which allows for faster rendering of behaviors. Use this mode 24 | for slower computers or very large numbers of agents. 25 | 26 | Attributes 27 | ---------- 28 | time_step : float 29 | The increment of time for calculating velocity. 30 | figure_handle : matplotlib figure object 31 | This handle contains the figure for which the grits bots will be 32 | displayed. 33 | 34 | Methods 35 | ------- 36 | initialize(n) 37 | Initialize the state of 'n' robots and start visualization. 38 | step() 39 | Update the state of agents and display. 40 | time_to_iters(time) 41 | Returns the number of iterations from the current time. 42 | 43 | set_position_controller(controller) 44 | Sets the position controller to use the simulation. 45 | set_velocities(ids, vs) 46 | Sets the velocities of the current agents. 47 | set_positions(ids, ps) 48 | Sets the velocities for the agents via the position controller. 49 | set_save_parameters(file_path, length, every) 50 | Sets the state saving parameters for the simulation. 51 | set_robot_color(new_color) 52 | Sets a matplotlib compliant color for the robot. 53 | 54 | get_d_disk_neighbors(ids, r) 55 | Gets the neighbors of a particular agent within r distance in the 2 56 | norm. 57 | get_top_neighbors(ids, laplacian) 58 | Gets teh topological neighbors of an agent, given a graph Laplacian. 59 | get_poses() 60 | Gets the (x, y, theta) poses of the robots. 61 | get_available_agents() 62 | Returns the number of available agents. 63 | """ 64 | 65 | def __init__(self, ion=True, anim_speed=0.05, low_quality=False): 66 | # --- Public Attributes --- # 67 | self.time_step = 0.033 68 | self.figure_handle = None 69 | 70 | # --- Private Attributes --- # 71 | # Data Logging Parameters 72 | self.__states = None 73 | self.__temp_states = [] 74 | self.__save_length = 0 75 | self.__save_every = 0 76 | self.__prev_iters = 1 77 | self.__iters = 1 78 | self.__file_path = None 79 | self.__previous_time_step = None 80 | self.__time = 0 81 | self.log = [] 82 | 83 | # Dynamics and Parameters 84 | self.__num_agents = 4 85 | self.__linear_velocity_coef = 1 86 | self.__angular_velocity_coef = 1 87 | self.__max_linear_velocity = 0.1 88 | self.__max_angular_velocity = 2 * np.pi 89 | 90 | # Controllers (functions) 91 | self.__position_controller = None 92 | 93 | # Visualization 94 | self.__robot_handle = {} 95 | self.__boundaries = [-0.6, 0.6, -0.35, 0.35] 96 | self.__boundary_points = [[-0.6, 0.6, 0.6, -0.6], 97 | [-0.35, -0.35, 0.35, 0.35]] 98 | self.__robot_body = None 99 | self.__offset = 0.05 100 | self.__ion = ion 101 | self.__anim_speed = anim_speed 102 | self.__robot_number = {} 103 | self.__pos_bias = 0.01 # Bias for the annotation position. 104 | self.__robot_color = '#9521F6' 105 | self.__low_quality = low_quality 106 | 107 | # Barrier Certificates 108 | self.__gamma = 1e4 109 | self.__safety_radius = 0.1 110 | self.__diff_morphism_gain = 0.05 111 | 112 | def initialize(self, n): 113 | """ Initialize the state of 'n' robots and start visualization. 114 | 115 | Parameters 116 | ---------- 117 | n : int 118 | Number of agents desired. 119 | """ 120 | self.__num_agents = n 121 | self.__states = np.zeros((5, n)) 122 | 123 | num_x = int(np.floor(1.2 / self.__safety_radius)) 124 | num_y = int(np.floor(0.7 / self.__safety_radius)) 125 | values = np.random.permutation(num_x * num_y) 126 | values = np.resize(values, (1, n)) 127 | 128 | # Create a random X, Y, and Theta start position. 129 | for i in range(0, n): 130 | x, y = np.unravel_index(values[0, i], (num_x, num_y)) 131 | x = x * self.__safety_radius - 0.6 132 | y = y * self.__safety_radius - 0.35 133 | self.__states[0, i] = x 134 | self.__states[1, i] = y 135 | self.__states[2, i] = np.random.rand() * 2 * np.pi 136 | 137 | # Start Visualization 138 | self.__init_robot_visualize() 139 | 140 | def set_position_controller(self, controller): 141 | """ Sets the position controller to use the simulation. 142 | 143 | Parameters 144 | ---------- 145 | controller : obj 146 | Position (go-to-goal) controller. 147 | """ 148 | 149 | self.__position_controller = controller 150 | 151 | def set_velocities(self, ids, vs): 152 | """ Sets the velocities of the current agents. 153 | 154 | Parameters 155 | ---------- 156 | ids : array of int 157 | Identities of agents whose velocities to set. 158 | vs : array of ints 159 | Velocities to set. 160 | """ 161 | n = vs.shape[1] 162 | 163 | for i in range(0, n): 164 | if np.absolute(vs[0, i]) > self.__max_linear_velocity: 165 | vs[0, i] = self.__max_linear_velocity * np.sign(vs[0, i]) 166 | 167 | if np.absolute(vs[1, i]) > self.__max_angular_velocity: 168 | vs[1, i] = self.__max_angular_velocity * np.sign(vs[1, i]) 169 | 170 | # Change the state of agents within the ids array 171 | self.__states[3:5, ids] = vs 172 | 173 | def set_positions(self, ids, ps): 174 | """ Sets the velocities for the agents via the position controller. 175 | 176 | Parameters 177 | ---------- 178 | ids : array of int 179 | Identities of agent positions to set. 180 | ps : matrix of ints 181 | Goal positions of agents 2xN. 182 | """ 183 | if self.__position_controller is None: 184 | print("ERROR: No position controller set. Use ", 185 | "set_position_controller() to set one.") 186 | 187 | else: 188 | self.set_velocities(ids, self.__position_controller( 189 | self.__states[:, ids], ps)) 190 | 191 | def set_save_parameters(self, file_path, length, every): 192 | """ Sets the state saving parameters for the simulation. 193 | 194 | Parameters 195 | ---------- 196 | file_path : str 197 | Path to save file. 198 | length : int 199 | Number of state iterations to save. 200 | every : int 201 | Save per 'every' iterations. 202 | """ 203 | 204 | self.__save_length = length 205 | self.__save_every = every 206 | self.__temp_states = np.zeros((5 * self.__num_agents, every)) 207 | self.__file_path = file_path 208 | # robot_states = np.zeros((5 * self.__num_agents, length)) 209 | 210 | # Save to an external file. 211 | 212 | def set_robot_color(self, new_color): 213 | """ Sets the color of the grits bots rendered on the plot. 214 | 215 | Parameters 216 | ---------- 217 | new_color : str 218 | These are matplotlib standard color schemes and can take the 219 | following types 220 | eg. new_color = 'w' # White 221 | eg. new_color = '#eeefff' # Hex string color 222 | eg. new_color = '0.75' # Gray-scale color (0 - 1) 223 | 224 | """ 225 | self.__robot_color = new_color 226 | 227 | def get_d_disk_neighbors(self, ids, rad): 228 | """ Gets the neighbors of a particular agent within r distance in the 2 norm. 229 | 230 | Parameters 231 | ---------- 232 | ids : int 233 | Identity of agent whose neighbors to get_poses. 234 | rad : int 235 | Radius of delta disk. 236 | """ 237 | neighbors = np.zeros((1, self.__num_agents)) 238 | count = 0 239 | 240 | for i in range(0, self.__num_agents): 241 | if (i is not id) and (np.linalg.norm(self.__states[0:2, ids] - 242 | self.__states[0:2, i]) < rad): 243 | count += 1 244 | neighbors[:, count] = i 245 | 246 | if count == 0: 247 | neighbors = [] 248 | 249 | else: 250 | neighbors = neighbors[0, :] 251 | 252 | return neighbors 253 | 254 | def get_top_neighbors(self, ids, laplacian): 255 | """ Gets the topological neighbors of an agent, given a graph Laplacian. 256 | 257 | Parameters 258 | ---------- 259 | ids : array of int 260 | Identity of agents whose neighbors to get. 261 | laplacian : array of int 262 | Graph Laplacian of the communication topology. 263 | """ 264 | neighbors = np.zeros((1, self.__num_agents), dtype=np.int) 265 | count = 0 266 | 267 | for i in range(0, self.__num_agents): 268 | if (i is not ids) and (laplacian[ids, i] is not 0): 269 | count += 1 270 | neighbors[:, count] = i 271 | 272 | if count == 0: 273 | neighbors = [] 274 | 275 | else: 276 | neighbors = neighbors[0, :] 277 | 278 | return neighbors 279 | 280 | def get_poses(self): 281 | """ Gets the (x, y, theta) poses of the robots. """ 282 | return self.__states[0:3, :] 283 | 284 | def get_available_agents(self): 285 | """ Return the number of available agents. """ 286 | return self.__num_agents 287 | 288 | def step(self): 289 | """ Update the state of agents. 290 | 291 | Using an agents velocities, the X, Y, and theta of each agent is 292 | updated for eventual rendering by the __draw_robots() private method. 293 | """ 294 | # Update velocities using unicycle dynamics 295 | for i in range(0, self.__num_agents): 296 | self.log.append([self.__time, i] + list(self.__states[:, i])) 297 | self.__states[0, i] += self.__linear_velocity_coef * \ 298 | self.time_step * np.multiply(self.__states[3, i], 299 | np.cos(self.__states[2, i])) 300 | self.__states[1, i] += self.__linear_velocity_coef * \ 301 | self.time_step * np.multiply(self.__states[3, i], 302 | np.sin(self.__states[2, i])) 303 | self.__states[2, i] += self.__angular_velocity_coef * \ 304 | self.time_step * self.__states[4, i] 305 | 306 | # Ensure we're in the right range. 307 | self.__states[2, i] = np.arctan2(np.sin(self.__states[2, i]), 308 | np.cos(self.__states[2, i])) 309 | 310 | self.__time += self.time_step 311 | 312 | # self.__save() 313 | self.__draw_robots() 314 | 315 | # ADD PART ABOUT PAUSE AND PREVIOUS TIME STEP. 316 | 317 | def time_to_iters(self, time): 318 | """ Return the number of iterations from the current time. 319 | 320 | Parameters 321 | ---------- 322 | time : SOMETHING 323 | ADD SOMETHING 324 | """ 325 | return np.ceil(time / self.time_step) 326 | 327 | def __save(self): 328 | """ Save data. NEED TO FIGURE OUT A WAY To SAVE THIS DATA. """ 329 | if np.all(self.__temp_states): 330 | if self.__prev_iters <= self.__save_length: 331 | # Write to mat-file. 332 | pass 333 | 334 | if (self.__iters - 1) == self.__save_every: 335 | pass 336 | 337 | else: 338 | pass 339 | 340 | def __draw_robots(self): 341 | """ Animates the motion of agents. 342 | 343 | Updated state values are applied to the matplotlib Polygon objects 344 | within the __robot_handle dictionary. Annotations containing the 345 | agent's number are updated to reflect the agents motion. 346 | """ 347 | 348 | def init(): 349 | return self.__robot_handle 350 | 351 | def animate(i): 352 | """ Draw motion of robots. """ 353 | for j in range(0, self.__num_agents): 354 | x = self.__states[0, j] 355 | y = self.__states[1, j] 356 | th = self.__states[2, j] 357 | pose_transformation_mat = np.array([ 358 | [np.cos(th), -1*np.sin(th), x], 359 | [np.sin(th), np.cos(th), y], 360 | [0, 0, 1]]) 361 | robot_body_transformed = np.dot(self.__robot_body, 362 | pose_transformation_mat.T) 363 | # New robot location. 364 | self.__robot_handle[j].set_xy(robot_body_transformed[:, 0:2]) 365 | 366 | # New Annotation location. 367 | self.__robot_number[j].remove() 368 | self.__robot_number[j] = Annotation( 369 | str(j + 1), 370 | xy=(x - self.__pos_bias, y - self.__pos_bias), 371 | xytext=(x - self.__pos_bias, y - self.__pos_bias)) 372 | self.ax.add_artist(self.__robot_number[j]) 373 | 374 | return self.__robot_handle 375 | 376 | # Animate the movement 377 | anim = animation.FuncAnimation( 378 | self.figure_handle, animate, frames=1, interval=0, init_func=init, 379 | repeat=False, blit=False) 380 | plt.draw() 381 | plt.pause(self.__anim_speed) 382 | 383 | def __init_robot_visualize(self): 384 | """ Initialize visualization for robots. 385 | 386 | The body of each robot consists of different parts, namely: 387 | grits_bot_left_wheel --> X, Y, Z points for left wheel 388 | grits_bot_right_wheel --> X, Y, Z points for right wheel 389 | grits_bot_tail_pin --> X, Y, Z points for tail pin 390 | grits_bot_base --> X, Y, Z points for robot base 391 | 392 | Each array consists for points on the graph that MATLAB/matplotlib 393 | use to form shapes. For example: 394 | grits_bot_base = [ 0, 0, 1; 395 | 1, 0, 1; 396 | 1, 1, 1; 397 | 0, 1, 1]; 398 | 399 | When all points are connected by lines, the grits bot base is 400 | generated. 401 | 402 | All robots are stored in the __robot_handle dictionary. Each robot 403 | is a matplotlib Polygon object and can be accessed with the 404 | appropriate index value. 405 | 406 | """ 407 | # Initialize variables. 408 | robot_diameter = 0.03 409 | num_robots = self.__num_agents 410 | 411 | # Scale Factor (max value of single gaussian) 412 | # scale_factor = 0.5 413 | fig_phi = plt.figure(1) 414 | self.figure_handle = fig_phi 415 | self.ax = self.figure_handle.add_subplot(111) 416 | 417 | # Plot Robotarium boundaries 418 | self.ax.spines['top'].set_color('c') 419 | self.ax.spines['bottom'].set_color('c') 420 | self.ax.spines['left'].set_color('c') 421 | self.ax.spines['right'].set_color('c') 422 | self.ax.set_xlim([self.__boundaries[0] - self.__offset, 423 | self.__boundaries[1] + self.__offset]) 424 | self.ax.xaxis.set_visible(False) 425 | self.ax.set_ylim([self.__boundaries[2] - self.__offset, 426 | self.__boundaries[3] + self.__offset]) 427 | self.ax.yaxis.set_visible(False) 428 | 429 | # Define custom patch variables 430 | """ Creating Grits Bot Base Matrix. """ 431 | val = np.sqrt(2) / 2 432 | 433 | grits_bot_base = np.array([[(-1 * val), (-1 * val), 1], 434 | [val, (-1 * val), 1], 435 | [val, val, 1], 436 | [(-1 * val), val, 1], 437 | [(-1 * val), (-1 * val), 1], 438 | [val, (-1 * val), 1]]) 439 | grits_bot_base[:, 0:2] = grits_bot_base[:, 0:2] * robot_diameter 440 | 441 | """ Creating Grits Bot Left and Right Wheel Matrix """ 442 | grits_bot_wheel = np.array([[(-1 * val), (-1 * val), 1], 443 | [val, (-1 * val), 1], 444 | [val, val, 1], 445 | [(-1 * val), val, 1]]) 446 | grits_bot_wheel[:, 0:2] = np.dot(grits_bot_wheel[:, 0:2], 447 | np.diag([(robot_diameter / 3), 448 | (robot_diameter / 6)])) 449 | grits_bot_left_wheel = grits_bot_wheel + \ 450 | np.array([0, (7 / 6 * val * robot_diameter), 0]) 451 | grits_bot_right_wheel = grits_bot_wheel + \ 452 | np.array([0, (-7 / 6 * val * robot_diameter), 0]) 453 | 454 | """ Creating Grits Bot Tail Pin Matrix """ 455 | grits_bot_tail_pin_angle = np.arange((np.pi * 8 / 9), 456 | (np.pi * 10 / 9), 457 | (np.pi / 18))[:, np.newaxis] 458 | 459 | grits_bot_tail_pin = np.vstack( 460 | [np.array([[(-1 * val), np.sin(grits_bot_tail_pin_angle[0]), 1]]), 461 | np.hstack([np.cos(grits_bot_tail_pin_angle), 462 | np.sin(grits_bot_tail_pin_angle), 463 | np.ones(grits_bot_tail_pin_angle.shape)]), 464 | np.hstack([0.95 * np.cos(grits_bot_tail_pin_angle[::-1]), 465 | 0.95 * np.sin(grits_bot_tail_pin_angle[::-1]), 466 | np.ones(grits_bot_tail_pin_angle.shape)]), 467 | np.array([[(-1 * val), 468 | (0.95 * np.sin(grits_bot_tail_pin_angle[0])), 1]]) 469 | ]) 470 | 471 | grits_bot_tail_pin[:, 0:2] = grits_bot_tail_pin[:, 0:2] * \ 472 | robot_diameter 473 | 474 | """ Create Low Quality Version """ 475 | grits_bot_low_quality = np.array([[(-1 * val), (-1 * val), 1], 476 | [val, (-1 * val) + val/2, 1], 477 | [val, val - val/2, 1], 478 | [(-1 * val), val, 1], 479 | [(-1 * val), (-1 * val), 1], 480 | [val, (-1 * val) + val/2, 1]]) 481 | 482 | # Define common patch variables 483 | if not self.__low_quality: 484 | self.__robot_body = np.vstack([grits_bot_left_wheel, 485 | grits_bot_right_wheel, 486 | grits_bot_tail_pin, 487 | grits_bot_base]) 488 | 489 | else: 490 | self.__robot_body = grits_bot_low_quality 491 | self.__robot_body[:, 0:2] = self.__robot_body[:, 0:2] * \ 492 | robot_diameter 493 | 494 | # Create empty dictionary to place plt.Polygon objects. 495 | for i in range(0, num_robots): 496 | self.__robot_handle[i] = None 497 | 498 | # Set initial placement of agents on graph. 499 | for j in range(0, num_robots): 500 | x = self.__states[0, j] 501 | y = self.__states[1, j] 502 | th = self.__states[2, j] 503 | 504 | pose_transformation_mat = np.array([ 505 | [np.cos(th), -1*np.sin(th), x], 506 | [np.sin(th), np.cos(th), y], 507 | [0, 0, 1]]) 508 | 509 | robot_body_transformed = np.dot(self.__robot_body, 510 | pose_transformation_mat.T) 511 | 512 | # Visualize each robot. 513 | self.__robot_handle[j] = plt.Polygon( 514 | robot_body_transformed[:, 0:2]) 515 | self.__robot_handle[j].set_fill(True) 516 | self.__robot_handle[j].set_visible(True) 517 | self.__robot_handle[j].set_color(self.__robot_color) 518 | self.ax.add_patch(self.__robot_handle[j]) 519 | 520 | # Annotate each new robot. 521 | self.__robot_number[j] = Annotation(str(j+1), xy=(x-.01, y-.01), 522 | xytext=(x-.01, y-.01)) 523 | self.ax.add_artist(self.__robot_number[j]) 524 | 525 | # Show plot. 526 | if self.__ion: 527 | plt.ion() 528 | plt.show() 529 | 530 | 531 | if __name__ == '__main__': 532 | # Get Robotarium object used to communicate with the robots/simulator 533 | r = Robotarium(ion=False) 534 | 535 | # Get the number of available agents from the Robotarium. We don't need a 536 | # specific value for this algorithm 537 | N = r.get_available_agents() 538 | 539 | # Initialize the Robotarium object with the desired number of agents 540 | r.initialize(N) 541 | --------------------------------------------------------------------------------