├── .gitignore ├── examples ├── plotting │ ├── GTLogo.png │ └── leader_follower_with_plotting.m ├── barrier_certificates │ ├── html │ │ ├── main_eq09235582099063973062.png │ │ └── main_eq17134034933542560612.png │ ├── uni_barriers.m │ └── si_barriers.m ├── go_to_goal │ ├── go_to_pose.m │ ├── go_to_point.m │ └── html │ │ └── go_to_pose.html ├── consensus_static │ ├── consensus.m │ └── consensus_fewer_errors.m ├── leader_follower_static │ └── leader_follower.m └── formation_control │ └── formation_control.m ├── utilities ├── graph │ ├── html │ │ ├── lineGL_eq12942671300991736101.png │ │ ├── cycleGL_eq12942671300991736101.png │ │ ├── randomGL_eq12942671300991736101.png │ │ ├── completeGL_eq08486233743349304198.png │ │ ├── completeGL_eq12942671300991736101.png │ │ ├── random_connectedGL_eq12942671300991736101.png │ │ ├── delta_disk_neighbors_eq14324477822824332334.png │ │ ├── topological_neighbors_eq12941163982435923632.png │ │ ├── completeGL.html │ │ ├── cycleGL.html │ │ ├── lineGL.html │ │ ├── topological_neighbors.html │ │ ├── delta_disk_neighbors.html │ │ ├── randomGL.html │ │ └── random_connectedGL.html │ ├── completeGL.m │ ├── cycleGL.m │ ├── lineGL.m │ ├── topological_neighbors.m │ ├── delta_disk_neighbors.m │ ├── randomGL.m │ └── random_connectedGL.m ├── misc │ ├── html │ │ ├── generate_initial_conditions_eq07459738348397623860.png │ │ ├── generate_initial_conditions_eq16033661748480904678.png │ │ ├── unique_filename.html │ │ ├── generate_initial_conditions.html │ │ └── create_is_initialized.html │ ├── unique_filename.m │ ├── create_is_initialized.m │ └── generate_initial_conditions.m ├── transformations │ ├── html │ │ ├── create_si_to_uni_mapping_eq03927841599078721039.png │ │ ├── create_si_to_uni_mapping_eq06303456170493080000.png │ │ ├── create_si_to_uni_mapping_eq12450574036302976132.png │ │ ├── create_si_to_uni_mapping_eq13491430810795123718.png │ │ ├── create_uni_to_si_mapping_eq12450574036302976132.png │ │ ├── create_uni_to_si_mapping_eq13491430810795123718.png │ │ ├── create_si_to_uni_mapping2_eq12450574036302976132.png │ │ ├── create_si_to_uni_mapping2.html │ │ ├── create_uni_to_si_mapping.html │ │ └── create_si_to_uni_mapping.html │ ├── create_si_to_uni_mapping.m │ ├── create_si_to_uni_dynamics.m │ ├── create_si_to_uni_dynamics_with_backwards_motion.m │ └── create_uni_to_si_mapping.m ├── controllers │ ├── create_automatic_parking_controller.m │ ├── create_si_position_controller.m │ ├── create_unicycle_position_controller.m │ ├── create_automatic_parking_controller2.m │ ├── create_parking_controller.m │ └── create_waypoint_controller.m └── barrier_certificates │ ├── create_si_barrier_certificate2.m │ ├── create_si_barrier_certificate.m │ ├── create_uni_barrier_certificate.m │ ├── create_uni_barrier_certificate_with_obstacles.m │ ├── create_si_barrier_certificate_with_boundary.m │ ├── create_uni_barrier_certificate2.m │ └── create_uni_barrier_certificate_with_boundary.m ├── RobotariumError.m ├── init.m ├── LICENSE.txt ├── simulation_skeleton.m ├── README.md ├── patch_generation └── gritsbot_patch.m └── Robotarium.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.mat 2 | -------------------------------------------------------------------------------- /examples/plotting/GTLogo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/examples/plotting/GTLogo.png -------------------------------------------------------------------------------- /utilities/graph/html/lineGL_eq12942671300991736101.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/lineGL_eq12942671300991736101.png -------------------------------------------------------------------------------- /utilities/graph/html/cycleGL_eq12942671300991736101.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/cycleGL_eq12942671300991736101.png -------------------------------------------------------------------------------- /utilities/graph/html/randomGL_eq12942671300991736101.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/randomGL_eq12942671300991736101.png -------------------------------------------------------------------------------- /utilities/graph/html/completeGL_eq08486233743349304198.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/completeGL_eq08486233743349304198.png -------------------------------------------------------------------------------- /utilities/graph/html/completeGL_eq12942671300991736101.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/completeGL_eq12942671300991736101.png -------------------------------------------------------------------------------- /examples/barrier_certificates/html/main_eq09235582099063973062.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/examples/barrier_certificates/html/main_eq09235582099063973062.png -------------------------------------------------------------------------------- /examples/barrier_certificates/html/main_eq17134034933542560612.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/examples/barrier_certificates/html/main_eq17134034933542560612.png -------------------------------------------------------------------------------- /utilities/graph/html/random_connectedGL_eq12942671300991736101.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/random_connectedGL_eq12942671300991736101.png -------------------------------------------------------------------------------- /utilities/graph/html/delta_disk_neighbors_eq14324477822824332334.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/delta_disk_neighbors_eq14324477822824332334.png -------------------------------------------------------------------------------- /utilities/graph/html/topological_neighbors_eq12941163982435923632.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/graph/html/topological_neighbors_eq12941163982435923632.png -------------------------------------------------------------------------------- /utilities/misc/html/generate_initial_conditions_eq07459738348397623860.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/misc/html/generate_initial_conditions_eq07459738348397623860.png -------------------------------------------------------------------------------- /utilities/misc/html/generate_initial_conditions_eq16033661748480904678.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/misc/html/generate_initial_conditions_eq16033661748480904678.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping_eq03927841599078721039.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_si_to_uni_mapping_eq03927841599078721039.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping_eq06303456170493080000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_si_to_uni_mapping_eq06303456170493080000.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping_eq12450574036302976132.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_si_to_uni_mapping_eq12450574036302976132.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping_eq13491430810795123718.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_si_to_uni_mapping_eq13491430810795123718.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_uni_to_si_mapping_eq12450574036302976132.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_uni_to_si_mapping_eq12450574036302976132.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_uni_to_si_mapping_eq13491430810795123718.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_uni_to_si_mapping_eq13491430810795123718.png -------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping2_eq12450574036302976132.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/HEAD/utilities/transformations/html/create_si_to_uni_mapping2_eq12450574036302976132.png -------------------------------------------------------------------------------- /RobotariumError.m: -------------------------------------------------------------------------------- 1 | classdef RobotariumError < double 2 | %ERRORS Used to contain Robotarium error enumeration. 3 | 4 | enumeration 5 | ExceededActuatorLimits (1) 6 | RobotsTooClose (2) 7 | RobotsOutsideBoundaries (3) 8 | end 9 | end 10 | 11 | -------------------------------------------------------------------------------- /utilities/graph/completeGL.m: -------------------------------------------------------------------------------- 1 | %% completeGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$ 2 | % Returns a completely connected graph Laplacian 3 | %% Example Usage 4 | % L = completeGL(5); 5 | %% Implementation 6 | function [ L ] = completeGL( n ) 7 | L = n * eye(n) - ones(n,n); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /utilities/graph/cycleGL.m: -------------------------------------------------------------------------------- 1 | %% cycleGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$ 2 | % Returns a cycle graph Laplacian 3 | %% Example Usage 4 | % L = cycleGL(4) 5 | %% Implementation 6 | function [ L ] = cycleGL( n ) 7 | L = 2*eye(n) - diag(ones(1,(n-1)), 1) - diag(ones(1,(n-1)), -1); 8 | L(n, 1) = -1; 9 | L(1, n) = -1; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /utilities/graph/lineGL.m: -------------------------------------------------------------------------------- 1 | %% lineGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$ 2 | % Returns a line graph Laplacian of size n x n 3 | %% Example Usage 4 | % L = lineGL(5) 5 | %% Implementation 6 | function [ L ] = lineGL(n) 7 | L = 2*eye(n) - diag(ones(1,(n-1)), 1) - diag(ones(1, n-1), -1); 8 | L(1, 1) = 1; 9 | L(n, n) = 1; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /utilities/misc/unique_filename.m: -------------------------------------------------------------------------------- 1 | %% unique_filename 2 | % Returns a timestamped filename with *.mat appended 3 | %% Example Usage 4 | % filename = unique_filename('filename'); 5 | function [ filePath ] = unique_filename(file_name) 6 | date = datetime('now'); 7 | filePath = strcat(file_name, '_', num2str(date.Month), '_', num2str(date.Day), '_', num2str(date.Year), '_', num2str(date.Hour), '_', num2str(date.Minute), '_', num2str(date.Second), '.mat'); 8 | end 9 | 10 | -------------------------------------------------------------------------------- /utilities/graph/topological_neighbors.m: -------------------------------------------------------------------------------- 1 | %% topological_neighbors $\mathbf{R}^{N \times N} \times \mathbf{Z}^{+} \to \mathbf{Z}^{+}$ 2 | % Returns the topological neighbors of a given agent 3 | 4 | function [neighbors] = topological_neighbors(L, agent) 5 | 6 | N = size(L, 2); 7 | 8 | assert(agent <= N && agent >= 1, 'Supplied agent (%i) must be between 1 and %i', agent, N); 9 | 10 | L_agent = L(agent, :); 11 | L_agent(agent) = 0; 12 | 13 | neighbors = find(L_agent ~= 0); 14 | end 15 | 16 | -------------------------------------------------------------------------------- /utilities/graph/delta_disk_neighbors.m: -------------------------------------------------------------------------------- 1 | %% delta_disk_neighbors $\mathbf{R}^{2 \times N} \times \mathbf{Z}^{+} \times \mathbf{R} \to \mathbf{Z}^{+}$ 2 | % Returns the agents within the 2-norm of the supplied agent 3 | %% Example Usage 4 | % neighbors = delta_disk_neighbors(robot_poses, 1, 0.5); 5 | 6 | function [ neighbors ] = delta_disk_neighbors(poses, agent, delta) 7 | 8 | N = size(poses, 2); 9 | agents = 1:N; 10 | agents(agent) = []; 11 | 12 | assert(agent<=N && agent>=1, 'Supplied agent (%i) must be between 1 and %i', agent, N); 13 | 14 | within_distance = arrayfun(@(x) norm(poses(1:2, x) - poses(1:2, agent)) <= delta, agents); 15 | neighbors = agents(within_distance); 16 | end 17 | 18 | -------------------------------------------------------------------------------- /init.m: -------------------------------------------------------------------------------- 1 | % This script initializes the Robotarium simulator, adding the required 2 | % paths to the MATLAB instance. You need to run this before doing anything 3 | % in the simulator. Also, DO NOT SUBMIT THIS FILE WITH YOUR EXPERIMENT OR 4 | % CALL IT IN YOUR SCRIPTS. All of these utilities will be automatically 5 | % included in your experiment! 6 | 7 | path = genpath('utilities'); 8 | if(isempty(path)) 9 | disp('WARNING: Cannot find utilities directory. This script should be run from the base directory.') 10 | else 11 | addpath(path) 12 | end 13 | 14 | path = genpath('patch_generation'); 15 | if(isempty(path)) 16 | disp('WARNING: Cannot find patch_generation directory. This script should be run from the base directory.') 17 | 18 | else 19 | addpath(path) 20 | end 21 | 22 | addpath('./') -------------------------------------------------------------------------------- /utilities/graph/randomGL.m: -------------------------------------------------------------------------------- 1 | %% randomGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$ 2 | % Returns a random grab laplacian with a specified number of verticies and 3 | % edges 4 | %% Example Usage 5 | % L = randomGL(5, 3); 6 | %% Implementation 7 | function [ L ] = randomGL(v, e) 8 | 9 | L = tril(ones(v, v)); 10 | 11 | %This works becuase I can't select diagonals 12 | potEdges = find(triu(L) == 0); 13 | sz = size(L); 14 | 15 | %Rest to zeros 16 | L = L - L; 17 | 18 | numEdges = min(e, length(potEdges)); 19 | edgeIndices = randperm(length(potEdges), numEdges); 20 | 21 | for index = edgeIndices 22 | 23 | [i, j] = ind2sub(sz, potEdges(index)); 24 | 25 | %Update adjacency relation 26 | L(i, j) = -1; 27 | L(j, i) = -1; 28 | 29 | %Update degree relation 30 | L(i, i) = L(i, i) + 1; 31 | L(j, j) = L(j, j) + 1; 32 | end 33 | end 34 | 35 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 Magnus Egerstedt, Daniel Pickem, Paul Glotfelter 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /simulation_skeleton.m: -------------------------------------------------------------------------------- 1 | %% Simulator Skeleton File 2 | % Paul Glotfelter 3 | % 10/04/2016 4 | % This file provides the bare-bones requirements for interacting with the 5 | % Robotarium. Note that this code won't actually run. You'll have to 6 | % insert your own algorithm! If you want to see some working code, check 7 | % out the 'examples' folder. 8 | 9 | %% Get Robotarium object used to communicate with the robots/simulator 10 | N = 12; 11 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true); 12 | 13 | % Select the number of iterations for the experiment. This value is 14 | % arbitrary 15 | iterations = 1000; 16 | 17 | % Iterate for the previously specified number of iterations 18 | for t = 1:iterations 19 | 20 | % Retrieve the most recent poses from the Robotarium. The time delay is 21 | % approximately 0.033 seconds 22 | x = r.get_poses(); 23 | 24 | %% Insert your code here! 25 | 26 | % dxu = algorithm(x); 27 | 28 | %% Send velocities to agents 29 | 30 | % Set velocities of agents 1,...,N 31 | r.set_velocities(1:N, dxu); 32 | 33 | % Send the previously set velocities to the agents. This function must be called! 34 | r.step(); 35 | end 36 | 37 | % We should call r.call_at_scripts_end() after our experiment is over! 38 | r.debug(); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # NEW VERSION 2 | 3 | The simulator has been updated to work with the brand new GRITSBot X! Gains accross the utilities have changes. **Please check out the examples to update your own code.** 4 | 5 | # MATLAB Version 6 | 7 | Currently, we know that the simulator works with MATLAB 2014b and higher. The backwards compatibility issues are mostly due to changes in the way MATLAB handles figures in newer releases. 8 | 9 | # Required Toolboxes 10 | 11 | We make heavy use of MATLAB's optimization toolbox function 'quadprog.' Though this toolbox isn't necessarily required, all of our barrier function algorithms utilize this function. 12 | 13 | # robotarium-matlab-simulator 14 | MATLAB simulator for the Robotarium! The purpose of the Robotarium simulator is to ensure that algorithms perform reasonably well before deployment onto the Robotarium. Note that scripts created for the simulator can be directly deployed onto the Robotarium. To ensure minimum modification after deployment, the simulator has been created to closely approximate the actual behavior of the Robotarium's agents. 15 | 16 | # Usage 17 | 18 | First, take a look at the "examples" folder for a few, simple examples. Note that, to run these examples, you must first run the "init.m" script to add the requisite directories. 19 | 20 | # Documentation 21 | 22 | For example mathematical documentation, FAQs, and more, visit http://www.robotarium.gatech.edu. 23 | -------------------------------------------------------------------------------- /utilities/graph/random_connectedGL.m: -------------------------------------------------------------------------------- 1 | %% random_connectedGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$ 2 | % Returns a random, connected GL with v verticies and (v-1) + e edges 3 | %% Example Usage 4 | % L = random_connectedGL(4, 3); 5 | %% Implementation 6 | function [ L ] = random_connectedGL(v, e) 7 | 8 | L = zeros(v, v); 9 | 10 | for i = 2:v 11 | 12 | edge = randi(i-1, 1, 1); 13 | 14 | %Update adjancency relations 15 | L(i, edge) = -1; 16 | L(edge, i) = -1; 17 | 18 | %Update node degrees 19 | L(i, i) = L(i, i) + 1; 20 | L(edge, edge) = L(edge, edge) + 1; 21 | end 22 | 23 | %This works becuase all nodes have at least 1 degree. Choose from only 24 | %upper diagonal portion 25 | potEdges = find(triu(bsxfun(@xor, L, 1)) == 1); 26 | sz = size(L); 27 | 28 | numEdges = min(e, length(potEdges)); 29 | 30 | if (numEdges <= 0) 31 | return 32 | end 33 | 34 | %Indices of randomly chosen extra edges 35 | edgeIndices = randperm(length(potEdges), numEdges); 36 | 37 | for index = edgeIndices 38 | 39 | [i, j] = ind2sub(sz, potEdges(index)); 40 | 41 | %Update adjacency relation 42 | L(i, j) = -1; 43 | L(j, i) = -1; 44 | 45 | %Update degree relation 46 | L(i, i) = L(i, i) + 1; 47 | L(j, j) = L(j, j) + 1; 48 | end 49 | end 50 | 51 | -------------------------------------------------------------------------------- /utilities/controllers/create_automatic_parking_controller.m: -------------------------------------------------------------------------------- 1 | function [ automatic_parking_controller ] = create_automatic_parking_controller(varargin) 2 | % CREATE_AUTOMATIC_PARKING_CONTROLLER Creates a controller that drive a 3 | % unicycle-modeled sytsem to a particular point and stops it (within 4 | % tolerances) 5 | % 6 | % 7 | % See also CREATE_PARKING_CONTROLLER, CREATE_IS_INITIALIZED 8 | 9 | p = inputParser; 10 | addOptional(p, 'ApproachAngleGain', 1); 11 | addOptional(p, 'DesiredAngleGain', 2.7); 12 | addOptional(p, 'RotationErrorGain', 1); 13 | addOptional(p, 'PositionError', 0.01); 14 | addOptional(p, 'RotationError', 0.25); 15 | parse(p, varargin{:}); 16 | 17 | gamma = p.Results.ApproachAngleGain; 18 | k = p.Results.DesiredAngleGain; 19 | h = p.Results.RotationErrorGain; 20 | 21 | init_checker = create_is_initialized('PositionError', p.Results.PositionError, ... 22 | 'RotationError', p.Results.RotationError); 23 | parking_controller = create_parking_controller('ApproachAngleGain', gamma, 'DesiredAngleGain', k, ... 24 | 'RotationErrorGain', h); 25 | 26 | automatic_parking_controller = @automatic_parking_controller_; 27 | 28 | function dxu = automatic_parking_controller_(states, poses) 29 | dxu = parking_controller(states, poses); 30 | [~, idxs] = init_checker(states, poses); 31 | 32 | dxu(:, idxs) = zeros(2, size(idxs, 2)); 33 | end 34 | end 35 | 36 | -------------------------------------------------------------------------------- /examples/go_to_goal/go_to_pose.m: -------------------------------------------------------------------------------- 1 | % Initializing the agents to random positions with barrier certificates. 2 | % This script shows how to initialize robots to a particular pose. 3 | % Paul Glotfelter edited by Sean Wilson 4 | % 07/2019 5 | 6 | N = 6; 7 | initial_positions = generate_initial_conditions(N, 'Spacing', 0.5); 8 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true, 'InitialConditions', initial_positions); 9 | 10 | % Set some parameters for use with the barrier certificates. We don't want 11 | % our agents to collide 12 | 13 | % Create a barrier certificate for use with the above parameters 14 | unicycle_barrier_certificate = create_uni_barrier_certificate_with_boundary(); 15 | 16 | %Get randomized initial conditions in the robotarium arena 17 | final_goal_points = generate_initial_conditions(N, ... 18 | 'Width', r.boundaries(2)-r.boundaries(1)-r.robot_diameter, ... 19 | 'Height', r.boundaries(4)-r.boundaries(3)-r.robot_diameter, ... 20 | 'Spacing', 0.5); 21 | 22 | args = {'PositionError', 0.025, 'RotationError', 0.05}; 23 | init_checker = create_is_initialized(args{:}); 24 | controller = create_waypoint_controller(args{:}); 25 | 26 | % Get initial location data for while loop condition. 27 | x=r.get_poses(); 28 | r.step(); 29 | 30 | while(~init_checker(x, final_goal_points)) 31 | x = r.get_poses(); 32 | dxu = controller(x, final_goal_points); 33 | dxu = unicycle_barrier_certificate(dxu, x); 34 | 35 | r.set_velocities(1:N, dxu); 36 | r.step(); 37 | end 38 | 39 | % We can call this function to debug our experiment! Fix all the errors 40 | % before submitting to maximize the chance that your experiment runs 41 | % successfully. 42 | r.debug(); 43 | -------------------------------------------------------------------------------- /examples/go_to_goal/go_to_point.m: -------------------------------------------------------------------------------- 1 | % Initializing the agents to random positions with barrier certificates. 2 | % This script shows how to initialize robots to a particular point. 3 | % Sean Wilson 4 | % 07/2019 5 | 6 | N = 6; 7 | initial_positions = generate_initial_conditions(N, 'Spacing', 0.5); 8 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true, 'InitialConditions', initial_positions); 9 | 10 | % Create a barrier certificate so that the robots don't collide 11 | si_barrier_certificate = create_si_barrier_certificate(); 12 | si_to_uni_dynamics = create_si_to_uni_dynamics(); 13 | 14 | %Get randomized initial conditions in the robotarium arena 15 | final_goal_points = generate_initial_conditions(N, ... 16 | 'Width', r.boundaries(2)-r.boundaries(1)-r.robot_diameter, ... 17 | 'Height', r.boundaries(4)-r.boundaries(3)-r.robot_diameter, ... 18 | 'Spacing', 0.5); 19 | % We'll make the rotation error huge so that the initialization checker 20 | % doesn't care about it 21 | args = {'PositionError', 0.02, 'RotationError', 50}; 22 | init_checker = create_is_initialized(args{:}); 23 | controller = create_si_position_controller(); 24 | 25 | % Get initial location data for while loop condition. 26 | x=r.get_poses(); 27 | r.step(); 28 | 29 | while(~init_checker(x, final_goal_points)) 30 | 31 | x = r.get_poses(); 32 | dxi = controller(x(1:2, :), final_goal_points(1:2, :)); 33 | 34 | dxi = si_barrier_certificate(dxi, x(1:2, :)); 35 | dxu = si_to_uni_dynamics(dxi, x); 36 | 37 | r.set_velocities(1:N, dxu); 38 | r.step(); 39 | end 40 | 41 | % We can call this function to debug our experiment! Fix all the errors 42 | % before submitting to maximize the chance that your experiment runs 43 | % successfully. 44 | r.debug(); 45 | 46 | -------------------------------------------------------------------------------- /utilities/misc/create_is_initialized.m: -------------------------------------------------------------------------------- 1 | %% create_is_initialized 2 | % Creates a function to check for initialization. The function returns 3 | % whether all the agents have been initialized and those that have already 4 | % finished. 5 | %% Detailed Description 6 | % * PositionError - affects how close the agents are required to get to the 7 | % desired position 8 | % * RotationError - affects how close the agents are required to get to the 9 | % desired rotation 10 | %% Example Usage 11 | % initialization_checker = create_is_initialized('PositionError', 0.l, 12 | % 'RotationError', 0.01) 13 | % [all_initialized, done_idxs] = initialization_checker(robot_poses, 14 | % desired_poses) 15 | %% Implementation 16 | function [ created_is_initialized ] = create_is_initialized(varargin) 17 | 18 | parser = inputParser; 19 | parser.addParameter('PositionError', 0.01); 20 | parser.addParameter('RotationError', 0.5); 21 | parse(parser, varargin{:}); 22 | 23 | position_error = parser.Results.PositionError; 24 | rotation_error = parser.Results.RotationError; 25 | 26 | created_is_initialized = @(states, initial_conditions) is_initialized(states, initial_conditions); 27 | 28 | function [done, idxs] = is_initialized(states, initial_conditions) 29 | 30 | [M, N] = size(states); 31 | [M_ic, N_ic] = size(initial_conditions); 32 | 33 | assert(M==3, 'Dimension of states (%i) must be 3', M); 34 | assert(M_ic==3, 'Dimension of conditions (%i) must be 3', M_ic); 35 | assert(N_ic==N, 'Column dimension of states (%i) and conditions (%i) must be the same', N, N_ic); 36 | 37 | wrap = @(x) atan2(sin(x), cos(x)); 38 | f = @(x, ic) (norm(x(1:2) - ic(1:2)) <= position_error) && (abs(wrap(x(3) - ic(3))) <= rotation_error); 39 | result = arrayfun(@(x) f(states(:, x), initial_conditions(:, x)), 1:N); 40 | 41 | [done, idxs] = find(result == 1); 42 | done = (length(done) == N); 43 | end 44 | end 45 | 46 | -------------------------------------------------------------------------------- /utilities/transformations/create_si_to_uni_mapping.m: -------------------------------------------------------------------------------- 1 | %% create_si_to_uni_Mapping 2 | % Returns a mapping from single-integrator to 3 | % unicycle dynamics $\left( f: \mathbf{R}^{2 \times N} \times 4 | % \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ 5 | % and a mapping between their states $\left(f: \mathbf{R}^{3 \times N} \to 6 | % \mathbf{R}^{2 \times N} \right)$ 7 | % Using this 8 | % particular method, the single-integrator dynamics must be computed in the 9 | % single-integrator domain. 10 | %% Detailed Description 11 | % * ProjectionDistance - affects how far the single-integrator is projected 12 | % in front of the unicycle system. 13 | %% Example Usage 14 | % % si === single integrator 15 | % [si_to_uni_dynamics, uni_to_si_states] = create_si_to_uni_mapping('ProjectionDistance', 16 | % 0.05) 17 | % si_states = uni_to_si_states(robot_poses) 18 | % dx_si = si_algorithm(si_states) 19 | % dx_uni = si_to_uni_dynamics(dx_si, states) 20 | %% Implementation 21 | function [si_to_uni_dyn, uni_to_si_states] = create_si_to_uni_mapping(varargin) 22 | 23 | parser = inputParser; 24 | addOptional(parser, 'ProjectionDistance', 0.05); 25 | parse(parser, varargin{:}); 26 | 27 | projection_distance = parser.Results.ProjectionDistance; 28 | 29 | si_to_uni_dyn = @si_to_uni; 30 | uni_to_si_states = @uni_to_si_states_; 31 | 32 | T = [1 0; 0 1/projection_distance]; 33 | % First mapping from SI -> unicycle. Keeps the projected SI system at 34 | % a fixed distance from the unicycle model 35 | function dxu = si_to_uni(dxi, states) 36 | N = size(dxi, 2); 37 | dxu = zeros(2, N); 38 | for i = 1:N 39 | dxu(:, i) = T * [cos(states(3, i)) sin(states(3, i)); -sin(states(3, i)) cos(states(3,i))] * dxi(:, i); 40 | end 41 | end 42 | 43 | % Projects the single-integrator system a distance in front of the 44 | % unicycle system 45 | function xi = uni_to_si_states_(states) 46 | xi = states(1:2, :) + projection_distance*[cos(states(3, :)) ; sin(states(3, :))]; 47 | end 48 | end 49 | 50 | -------------------------------------------------------------------------------- /utilities/transformations/create_si_to_uni_dynamics.m: -------------------------------------------------------------------------------- 1 | %% create_si_to_uni_dynamics 2 | % Returns a mapping $\left( f: \mathbf{R}^{2 \times N} \times \mathbf{R}^{3 3 | % \times N} \to \mathbf{R}^{2 \times N} \right)$ 4 | % from single-integrator to unicycle dynamics 5 | %% Detailed Description 6 | % * LinearVelocityGain - affects the linear velocity for the unicycle 7 | % * AngularVelocityLimit - affects the upper (lower) bound for the 8 | % unicycle's angular velocity 9 | %% Example Usage 10 | % % si === single integrator 11 | % si_to_uni_dynamics = create_si_to_uni_dynamics() 12 | % dx_si = si_algorithm(si_states) 13 | % dx_uni = si_to_uni_dynamics(dx_si, states) 14 | %% Implementation 15 | function [si_to_uni_dyn] = create_si_to_uni_dynamics(varargin) 16 | 17 | parser = inputParser; 18 | addOptional(parser, 'LinearVelocityGain', 1); 19 | addOptional(parser, 'AngularVelocityLimit', pi); 20 | parse(parser, varargin{:}); 21 | 22 | lvg = parser.Results.LinearVelocityGain; 23 | avl = parser.Results.AngularVelocityLimit; 24 | 25 | si_to_uni_dyn = @si_to_uni; 26 | % A mapping from si -> uni dynamics. THis is more of a 27 | % projection-based method. Though, it's definitely similar to the 28 | % NIDs. 29 | function dxu = si_to_uni(dxi, states) 30 | 31 | [M, N] = size(dxi); 32 | [M_states, N_states] = size(states); 33 | 34 | assert(M==2, 'Column size of given SI velocities (%i) must be 2', M); 35 | assert(M_states==3, 'Column size of given poses (%i) must be 3', M_states); 36 | assert(N==N_states, 'Row sizes of SI velocities (%i) and poses (%i) must be the same', N, N_states); 37 | 38 | dxu = zeros(2, N); 39 | for i = 1:N 40 | dxu(1, i) = lvg * [cos(states(3, i)) sin(states(3, i))] * dxi(:, i); 41 | %Normalizing the output of atan2 to between -kw and kw 42 | dxu(2, i) = avl * atan2([-sin(states(3, i)) cos(states(3, i))]*dxi(:, i), ... 43 | [cos(states(3, i)) sin(states(3, i))]*dxi(:, i))/(pi/2); 44 | end 45 | end 46 | end 47 | 48 | -------------------------------------------------------------------------------- /patch_generation/gritsbot_patch.m: -------------------------------------------------------------------------------- 1 | function [ patch_data ] = gritsbot_patch() 2 | %GRITSBOT_PATCH This is a helper function to generate patches for the 3 | %simulated GRITSbots. YOU SHOULD NEVER HAVE TO USE THIS FUNCTION. 4 | % 5 | % PATCH_DATA = GRITSBOT_PATCH() generates a struct containing patch data 6 | % for a robot patch. 7 | 8 | % Make it facing 0 rads 9 | robot_width = 0.095; 10 | robot_height = 0.09; 11 | wheel_width = 0.02; 12 | wheel_height = 0.04; 13 | led_size = 0.01; 14 | 15 | % Helper functions to generate vertex coordinates for a centered 16 | % rectangle and a helper function to shift a rectangle. 17 | rectangle = @(w, h) [w/2 h/2 1; -w/2 h/2 1; -w/2 -h/2 1; w/2 -h/2 1]; 18 | shift = @(r, x, y) r + repmat([x, y, 0], size(r, 1), 1); 19 | 20 | % Create vertices for body, wheel, and led. 21 | body = rectangle(robot_width, robot_height); 22 | wheel = rectangle(wheel_width, wheel_height); 23 | led = rectangle(led_size, led_size); 24 | 25 | % Use pre-generated vertices and shift them around to create a robot 26 | left_wheel = shift(wheel, -(robot_width + wheel_width)/2, robot_height/6); 27 | right_wheel = shift(wheel, (robot_width + wheel_width)/2, robot_height/6); 28 | body = shift(body, 0, robot_height/2); 29 | left_led = shift(led, robot_width/8, robot_height - 2*led_size); 30 | right_led = shift(led, robot_width/4, robot_height - 2*led_size); 31 | 32 | % Putting all the robot vertices together 33 | vertices = [ 34 | body ; 35 | left_wheel; 36 | right_wheel; 37 | left_led; 38 | right_led 39 | ]; 40 | 41 | % Only color the body of the robot. Everything else is black. 42 | colors = [ 43 | [238, 138, 17]/255; 44 | 0 0 0; 45 | 0 0 0; 46 | 0 0 0; 47 | 0 0 0 48 | ]; 49 | 50 | % This seems weird, but it basically tells the patch function which 51 | % vertices to connect. 52 | faces = repmat([1 2 3 4 1], 5, 1); 53 | 54 | for i = 2:5 55 | faces(i, :) = faces(i, :) + (i-1)*4; 56 | end 57 | 58 | patch_data = []; 59 | patch_data.vertices = vertices; 60 | patch_data.colors = colors; 61 | patch_data.faces = faces; 62 | end 63 | 64 | -------------------------------------------------------------------------------- /utilities/controllers/create_si_position_controller.m: -------------------------------------------------------------------------------- 1 | %% create_si_position_controller 2 | % Returns a controller ($u: \mathbf{R}^{2 \times N} \times \mathbf{R}^{2 \times N} \to \mathbf{R}^{2 \times N}$) 3 | % for a single-integrator system. 4 | %% Detailed Description 5 | % * XVelocityGain - affects the horizontal velocity of the 6 | % single integrator 7 | % * YVelocityGain - affects the vertical velocity of the single integrator 8 | %% Example Usage 9 | % si_position_controller = create_si_position_controller('XVelocityGain', 10 | % 1, 'YVelocityGain', 1); 11 | %% Implementation 12 | function [si_position_controller] = create_si_position_controller(varargin) 13 | 14 | parser = inputParser; 15 | addOptional(parser, 'XVelocityGain', 0.8); 16 | addOptional(parser, 'YVelocityGain', 0.8); 17 | addOptional(parser, 'VelocityMagnitudeLimit', 0.15); 18 | 19 | parse(parser, varargin{:}); 20 | 21 | x_vel_gain = parser.Results.XVelocityGain; 22 | y_vel_gain = parser.Results.YVelocityGain; 23 | velocity_magnitude_limit = parser.Results.VelocityMagnitudeLimit; 24 | gains = diag([x_vel_gain ; y_vel_gain]); 25 | 26 | si_position_controller = @position_si; 27 | 28 | 29 | function [ dx ] = position_si(states, poses) 30 | %POSITIONINT Position controller via single integrator dynamics 31 | 32 | % Error checking 33 | 34 | [M, N] = size(states); 35 | [M_poses, N_poses] = size(poses); 36 | 37 | assert(M == 2, 'Row size of states (%i) must be 2', M); 38 | assert(M_poses==2, 'Row size of SI poses (%i) must be 2', M_poses); 39 | assert(N==N_poses, 'Column size of states (%i) must be the same as poses (%i)', N, N_poses); 40 | 41 | dx = zeros(2, N); 42 | for i = 1:N 43 | dx(:, i) = gains*(poses(:, i) - states(:, i)); 44 | end 45 | 46 | % Normalize velocities to magnitude 47 | norms = arrayfun(@(idx) norm(dx(:, idx)), 1:N); 48 | to_normalize = norms > velocity_magnitude_limit; 49 | if(~isempty(norms(to_normalize))) 50 | dx(:, to_normalize) = velocity_magnitude_limit*dx(:, to_normalize)./norms(to_normalize); 51 | end 52 | end 53 | end 54 | 55 | -------------------------------------------------------------------------------- /utilities/controllers/create_unicycle_position_controller.m: -------------------------------------------------------------------------------- 1 | %% create_unicycle_position_controller 2 | % Returns a unicycle model position controller ($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{2 \times N} \to \mathbf{R}^{2 \times N}$) 3 | % given parameters 4 | %% Detailed Description 5 | %% 6 | % * LinearVelocityGain - a gain for the produced unicycle linear velocity 7 | % * AngularVelocityGain - a gain for the produced unicycle angular velocity 8 | %% Example Usage 9 | %% 10 | % controller = create_unicycle_position_controller('PositionErrorGain', 2) 11 | % controller = create_unicycle_position_controller('RotationErrorGain', 1, 12 | % 'PositionErrorGain, 2') 13 | %% Implementation 14 | function [ created_position_controller ] = create_unicycle_position_controller(varargin) 15 | p = inputParser; 16 | addOptional(p, 'LinearVelocityGain', 1); 17 | addOptional(p, 'AngularVelocityGain', 1); 18 | parse(p, varargin{:}) 19 | linear_velocity_gain = p.Results.LinearVelocityGain; 20 | angular_velocity_gain = p.Results.AngularVelocityGain; 21 | 22 | created_position_controller = @position_uni_clf; 23 | 24 | function [ dxu ] = position_uni_clf(states, poses) 25 | %POSITIONCLF Utilizes a Controlled Lyapunov Function (CLF) to drive a 26 | %unicycle system to a desired position 27 | % This function operates on unicycle states and desired poses and returns 28 | % a unicycle-velocity-valued vector. 29 | 30 | [M_states, N_states] = size(states); 31 | [M_poses, N_poses] = size(poses); 32 | 33 | assert(M_states == 3, 'Row size of states vector must be 3! Given size is %i', M_states); 34 | assert(M_poses == 2, 'Row size of desired poses (%i) must be 2!', M_poses); 35 | assert(N_states == N_poses, 'Row size of states vector (%i) must be row size of desired poses (%i)', N_states, N_poses); 36 | 37 | dxu = zeros(2, N_poses); 38 | 39 | for i = 1:N_poses 40 | pos_error = poses(:, i) - states(1:2, i); 41 | rot_error = atan2(pos_error(2), pos_error(1)); 42 | dist = norm(pos_error); 43 | 44 | dxu(1, i) = linear_velocity_gain*dist*cos(rot_error - states(3, i)); 45 | dxu(2, i) = angular_velocity_gain*dist*sin(rot_error - states(3, i)); 46 | end 47 | end 48 | end 49 | 50 | -------------------------------------------------------------------------------- /utilities/misc/generate_initial_conditions.m: -------------------------------------------------------------------------------- 1 | function [ poses ] = generate_initial_conditions(N, varargin) 2 | % GENERATE_INITIAL_CONDITIONS generate random poses in a rectangle 3 | % while ensuring a minimum spacing between each pose. 4 | % 5 | % generate_initial_conditions(5) generates 3x5 matrix of random poses 6 | % with default spacing and rectangular dimensions. 7 | % 8 | % generate_initial_conditions(5, 'Spacing', 0.2, 'Width', 3.2, 'Height', 2) 9 | % allows customizing the spacing and the rectangle dimensions. 10 | % 11 | % Output: 12 | % poses - 3 x N matrix: [x; y; theta] with theta in [-pi, pi] 13 | 14 | poses = zeros(3, N); 15 | 16 | parser = inputParser; 17 | parser.addParameter('Spacing', 0.3); 18 | parser.addParameter('Width', 3.0); 19 | parser.addParameter('Height', 1.8); 20 | parse(parser, varargin{:}); 21 | 22 | spacing = parser.Results.Spacing; 23 | width = parser.Results.Width; 24 | height = parser.Results.Height; 25 | 26 | % === Feasibility Check === 27 | approx_max_points = floor((width * height) / (spacing^2)); 28 | if N > approx_max_points 29 | error(['Cannot fit %d points with spacing %.2f into %.2fm x %.2fm area. ' ... 30 | 'Maximum possible (approx.): %d.'], N, spacing, width, height, approx_max_points); 31 | end 32 | 33 | % === Rejection Sampling === 34 | points = []; 35 | max_attempts = 200; 36 | attempts = 0; 37 | 38 | while size(points, 1) < N && attempts < max_attempts 39 | candidate = [(rand - 0.5) * width, (rand - 0.5) * height]; 40 | 41 | if isempty(points) 42 | points = candidate; 43 | else 44 | dists = sqrt(sum((points - candidate).^2, 2)); 45 | if all(dists >= spacing) 46 | points = [points; candidate]; 47 | end 48 | end 49 | attempts = attempts + 1; 50 | end 51 | 52 | if size(points, 1) < N 53 | error('Could not generate enough poses with the given spacing. Try lowering N or Spacing.'); 54 | end 55 | 56 | % Assign x, y 57 | poses(1:2, :) = points'; 58 | 59 | % Assign random theta between -pi and pi 60 | poses(3, :) = (rand(1, N) * 2 * pi) - pi; 61 | end -------------------------------------------------------------------------------- /utilities/transformations/create_si_to_uni_dynamics_with_backwards_motion.m: -------------------------------------------------------------------------------- 1 | %% create_si_to_uni_dynamics_with_backwards_motion 2 | % Returns a mapping $\left( f: \mathbf{R}^{2 \times N} \times \mathbf{R}^{3 3 | % \times N} \to \mathbf{R}^{2 \times N} \right)$ 4 | % from single-integrator to unicycle dynamics. 5 | % 6 | % This implementation of the mapping allows for robots to drive backwards 7 | % if that direction of linear velocity requires less rotation. 8 | %% Parameter Description 9 | % * LinearVelocityGain - affects the linear velocity for the unicycle 10 | % * AngularVelocityLimit - affects the upper (lower) bound for the 11 | % unicycle's angular velocity 12 | %% Example Usage 13 | % % si === single integrator 14 | % si_to_uni_dynamics = create_si_to_uni_dynamics_with_backwards_motion() 15 | % dx_si = si_algorithm(si_states) 16 | % dx_uni = si_to_uni_dynamics(dx_si, states) 17 | %% Implementation 18 | function [si_to_uni_dyn] = create_si_to_uni_dynamics_with_backwards_motion(varargin) 19 | 20 | parser = inputParser; 21 | addOptional(parser, 'LinearVelocityGain', 1); 22 | addOptional(parser, 'AngularVelocityLimit', pi); 23 | parse(parser, varargin{:}); 24 | 25 | lvg = parser.Results.LinearVelocityGain; 26 | avl = parser.Results.AngularVelocityLimit; 27 | wrap = @(x) atan2(sin(x), cos(x)); 28 | 29 | si_to_uni_dyn = @si_to_uni; 30 | 31 | 32 | function dxu = si_to_uni(dxi, states) 33 | 34 | [M, N] = size(dxi); 35 | [M_states, N_states] = size(states); 36 | 37 | assert(M==2, 'Column size of given SI velocities (%i) must be 2', M); 38 | assert(M_states==3, 'Column size of given poses (%i) must be 3', M_states); 39 | assert(N==N_states, 'Row sizes of SI velocities (%i) and poses (%i) must be the same', N, N_states); 40 | 41 | dxu = zeros(2, N); 42 | for i = 1:N 43 | angle = wrap(atan2(dxi(2, i), dxi(1, i)) - states(3, i)); 44 | if(angle > -pi/2 && angle < pi/2) 45 | s = 1; 46 | else 47 | s = -1; 48 | end 49 | if(s < 0) 50 | states(3, i) = wrap(states(3, i) + pi); 51 | end 52 | dxu(1, i) = lvg*[cos(states(3, i)) sin(states(3, i))] * dxi(:, i); 53 | %Normalizing the output of atan2 to between -kw and kw 54 | dxu(2, i) = avl*atan2([-sin(states(3, i)) cos(states(3, i))]*dxi(:, i), ... 55 | dxu(1, i))/(pi/2); 56 | dxu(1, i) = s*dxu(1, i); 57 | end 58 | end 59 | end 60 | 61 | -------------------------------------------------------------------------------- /utilities/controllers/create_automatic_parking_controller2.m: -------------------------------------------------------------------------------- 1 | function [ automatic_parking_controller ] = create_automatic_parking_controller2(varargin) 2 | % CREATE_AUTOMATIC_PARKING_CONTROLLER Creates a controller that drive a 3 | % unicycle-modeled sytsem to a particular point and stops it (within 4 | % tolerances) 5 | % Works by driving the unicycle to within PositionError of the point then 6 | % rotating it to within RotationError of the desired rotation 7 | % 8 | % Args: 9 | % LinearVelocityGain, optional: see also 10 | % AngularVelocityLimit, optional: see also 11 | % PositionError, optional: Error tolerance for position 12 | % RotationError, optional: Error tolerance for rotation 13 | % VelocityMagnitudeLimit, optional: Limit for velocity while driving 14 | % to position 15 | % 16 | % See also CREATE_SI_TO_UNI_MAPPING3 17 | 18 | p = inputParser; 19 | addOptional(p, 'LinearVelocityGain', 0.5); 20 | addOptional(p, 'AngularVelocityLimit', pi/2); 21 | addOptional(p, 'PositionError', 0.03); 22 | addOptional(p, 'RotationError', 0.25); 23 | addOptional(p, 'VelocityMagnitudeLimit', 0.2) 24 | parse(p, varargin{:}); 25 | 26 | lin_vel_gain = p.Results.LinearVelocityGain; 27 | ang_vel_gain = p.Results.AngularVelocityLimit; 28 | vel_mag_limit = p.Results.VelocityMagnitudeLimit; 29 | pos_err = p.Results.PositionError; 30 | rot_err = p.Results.RotationError; 31 | 32 | position_controller = create_si_to_uni_dynamics('LinearVelocityGain', lin_vel_gain, ... 33 | 'AngularVelocityLimit', ang_vel_gain); 34 | 35 | automatic_parking_controller = @automatic_parking_controller_; 36 | 37 | function dxu = automatic_parking_controller_(states, poses) 38 | 39 | N = size(states, 2); 40 | dxu = zeros(2, N); 41 | 42 | for i = 1:N 43 | 44 | wrapped = poses(3, i) - states(3, i); 45 | wrapped = atan2(sin(wrapped), cos(wrapped)); 46 | 47 | dxi = poses(1:2, i) - states(1:2, i); 48 | 49 | % Normalize 50 | norm_ = norm(dxi); 51 | if(norm_ > vel_mag_limit) 52 | dxi = vel_mag_limit*dxi/norm_; 53 | end 54 | 55 | if(norm(dxi) > pos_err) 56 | dxu(:, i) = position_controller(dxi, states(:, i)); 57 | elseif(abs(wrapped) > rot_err) 58 | dxu(1, i) = 0; 59 | dxu(2, i) = wrapped; 60 | else 61 | dxu(:, i) = zeros(2, 1); 62 | end 63 | end 64 | end 65 | end 66 | 67 | 68 | -------------------------------------------------------------------------------- /utilities/controllers/create_parking_controller.m: -------------------------------------------------------------------------------- 1 | %% create_parking_controller 2 | % Returns a controller ($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N}$) that will drive a unicycle-modeled agent to a pose 3 | % (i.e., position & orientation). 4 | %% Detailed Description 5 | %% 6 | % * ApproachAngleGain - affects how the unicycle approaches the desired 7 | % position 8 | % * DesiredAngleGain - affects how the unicycle approaches th desired angle 9 | % * RotataionErrorGain - affects how quickly the unicycle corrects rotation 10 | % errors 11 | %% Example Usage 12 | % parking_controller = create_parking_controller('ApproachAngleGain', 1, 13 | % DesiredAngleGain', 1, 'RotationErrorGain', 1); 14 | %% Implementation 15 | function [ parking_controller ] = create_parking_controller(varargin) 16 | 17 | p = inputParser; 18 | addOptional(p, 'ApproachAngleGain', 1); 19 | addOptional(p, 'DesiredAngleGain', 2.7); 20 | addOptional(p, 'RotationErrorGain', 1); 21 | parse(p, varargin{:}); 22 | 23 | gamma = p.Results.ApproachAngleGain; 24 | k = p.Results.DesiredAngleGain; 25 | h = p.Results.RotationErrorGain; 26 | 27 | parking_controller = @park; 28 | 29 | function [ dxu ] = park(states, poses) 30 | %PARK Drives a unicycle-based system to a desired pose 31 | % This controller employs a CLF to drive a unicycle-modeled system to 32 | % a desired pose (i.e., position AND orientation) 33 | 34 | [M_states, N_states] = size(states); 35 | [M_poses, N_poses] = size(poses); 36 | 37 | assert(M_states == 3, 'Row size of states vector must be 3! Given size is %i', M_states); 38 | assert(M_poses == 3, 'Row size of desired poses (%i) must be 2!', M_poses); 39 | assert(N_states == N_poses, 'Row size of states vector (%i) must be row size of desired poses (%i)', N_states, N_poses); 40 | 41 | R = @(x) [cos(x) -sin(x) ; sin(x) cos(x)]; 42 | 43 | N_states = size(states, 2); 44 | dxu = zeros(2, N_states); 45 | 46 | for i = 1:N_states 47 | 48 | translate = R(-poses(3, i))*(poses(1:2, i) - states(1:2, i)); 49 | e = norm(translate); 50 | theta = atan2(translate(2), translate(1)); 51 | alpha = theta - (states(3, i) - poses(3, i)); 52 | alpha = atan2(sin(alpha), cos(alpha)); 53 | 54 | ca = cos(alpha); 55 | sa = sin(alpha); 56 | 57 | dxu(1, i) = gamma * e * ca; 58 | dxu(2, i) = k*alpha + gamma*((ca*sa)/alpha)*(alpha + h*theta); 59 | end 60 | end 61 | end 62 | 63 | -------------------------------------------------------------------------------- /utilities/transformations/create_uni_to_si_mapping.m: -------------------------------------------------------------------------------- 1 | %% create_uni_to_si_mapping 2 | % Returns a mapping from unicycle to 3 | % single-integrator dynamics $\left( f: \mathbf{R}^{2 \times N} \times 4 | % \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ 5 | % and a mapping between their states $\left(f: \mathbf{R}^{3 \times N} \to 6 | % \mathbf{R}^{2 \times N} \right)$ 7 | % Using this 8 | % particular method, the single-integrator dynamics must be computed in the 9 | % single-integrator domain. 10 | %% Example Usage 11 | % % si === single integrator 12 | % [uni_to_si_dyn, si_to_uni_states] = create_uni_to_si_mapping('ProjectionDistance', 13 | % 0.05) 14 | % uni_states = si_to_uni_states(robot_poses) 15 | % dx_uni = uni_algorithm(uni_states) 16 | % dx_si = uni_to_si_dyn(dx_uni, states) 17 | %% Implementation 18 | function [uni_to_si_dyn, si_to_uni_states] = create_uni_to_si_mapping(varargin) 19 | 20 | parser = inputParser; 21 | addOptional(parser, 'ProjectionDistance', 0.05); 22 | parse(parser, varargin{:}); 23 | 24 | projection_distance = parser.Results.ProjectionDistance; 25 | 26 | uni_to_si_dyn = @uni_to_si; 27 | si_to_uni_states = @si_to_uni_states_; 28 | 29 | T = [1 0; 0 projection_distance]; 30 | % First mapping from SI -> unicycle. Keeps the projected SI system at 31 | % a fixed distance from the unicycle model 32 | function dxi = uni_to_si(dxu, states) 33 | [M, N] = size(dxu); 34 | [M_states, N_states] = size(states); 35 | 36 | % Dimensionality checks 37 | assert(M==2, 'Column size of unicycle velocity vector (%i) must be 2', M); 38 | assert(M_states==3, 'Column size of poses (%i) must be 3', M_states); 39 | assert(N==N_states', 'Row size of unicycle velocities (%i) must be the same as poses (%i)', N, N_states); 40 | 41 | dxi = zeros(2, N); 42 | for i = 1:N 43 | dxi(:, i) = [cos(states(3, i)) -sin(states(3, i)); sin(states(3, i)) cos(states(3,i))] * T * dxu(:, i); 44 | end 45 | end 46 | 47 | % Projects the single-integrator system a distance in front of the 48 | % unicycle system 49 | function xi = si_to_uni_states_(uni_states, si_states) 50 | 51 | % Dimensionality checks 52 | [M, N] = size(uni_states); 53 | [M_si, N_si] = size(si_states); 54 | 55 | assert(M==3, 'Column size of poses (%i) must be 3', M); 56 | assert(M_si==2, 'Column size of SI poses (%i) must be 2', M_si); 57 | assert(N==N_si, 'Row size of poses (%i) must be the same as SI poses (%i)', N, N_si); 58 | 59 | 60 | xi = si_states(1:2, :) - projection_distance*[cos(uni_states(3, :)) ; sin(uni_states(3, :))]; 61 | end 62 | end 63 | 64 | -------------------------------------------------------------------------------- /examples/consensus_static/consensus.m: -------------------------------------------------------------------------------- 1 | %% Consensus with a static, undirected topology 2 | % Sean Wilson 3 | % 07/2019 4 | % Demonstrates a theoretical example of the consensus algorithm. Lacks 5 | % implementation considerations such as obstacle avoidance, keeping robots 6 | % inside the workspace, and robot velocity thresholding, which you must 7 | % include. If you submit this example to be run on the Robotarium it will 8 | % be rejected. 9 | 10 | N = 12; 11 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true); 12 | 13 | %% Experiment constants 14 | 15 | % Generate a cyclic graph Laplacian from our handy utilities. For this 16 | % algorithm, any connected graph will yield consensus 17 | L = cycleGL(N); 18 | 19 | %% Grab tools we need to convert from single-integrator to unicycle dynamics 20 | 21 | % Gain for the diffeomorphism transformation between single-integrator and 22 | % unicycle dynamics 23 | [si_to_uni_dyn, uni_to_si_states] = create_si_to_uni_mapping(); 24 | 25 | % Select the number of iterations for the experiment. This value is 26 | % arbitrary 27 | iterations = 1000; 28 | 29 | % Initialize velocity vector for agents. Each agent expects a 2 x 1 30 | % velocity vector containing the linear and angular velocity, respectively. 31 | dxi = zeros(2, N); 32 | 33 | %Iterate for the previously specified number of iterations 34 | for t = 1:iterations 35 | 36 | % Retrieve the most recent poses from the Robotarium. The time delay is 37 | % approximately 0.033 seconds 38 | x = r.get_poses(); 39 | 40 | % Convert to SI states 41 | xi = uni_to_si_states(x); 42 | 43 | %% Algorithm 44 | 45 | for i = 1:N 46 | 47 | % Initialize velocity to zero for each agent. This allows us to sum 48 | %over agent i's neighbors 49 | dxi(:, i) = [0 ; 0]; 50 | 51 | % Get the topological neighbors of agent i based on the graph 52 | %Laplacian L 53 | neighbors = topological_neighbors(L, i); 54 | 55 | % Iterate through agent i's neighbors 56 | for j = neighbors 57 | 58 | % For each neighbor, calculate appropriate consensus term and 59 | %add it to the total velocity 60 | dxi(:, i) = dxi(:, i) + (xi(:, j) - xi(:, i)); 61 | end 62 | end 63 | 64 | %% Map to unicycle dynamics 65 | % Transform the single-integrator to unicycle dynamics using the the 66 | % transformation we created earlier 67 | dxu = si_to_uni_dyn(dxi, x); 68 | 69 | %% Send velocities to agents 70 | 71 | % Set velocities of agents 1,...,N 72 | r.set_velocities(1:N, dxu); 73 | 74 | % Send the previously set velocities to the agents. This function must be called! 75 | r.step(); 76 | end 77 | 78 | % We can call this function to debug our experiment! Fix all the errors 79 | % before submitting to maximize the chance that your experiment runs 80 | % successfully. 81 | r.debug(); 82 | -------------------------------------------------------------------------------- /examples/barrier_certificates/uni_barriers.m: -------------------------------------------------------------------------------- 1 | %% Single-integrator Barrier Certificate Algorithm 2 | % by Sean Wilson 3 | % 7/2019 4 | 5 | %% Set up Robotarium object 6 | % Before starting the algorithm, we need to initialize the Robotarium 7 | % object so that we can communicate with the agents 8 | 9 | N = 10; 10 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true); 11 | 12 | %This iis how many times the main loop will execute. 13 | iterations = 3000; 14 | 15 | %% Experiment constants 16 | % Next, we set up some experiment constants 17 | 18 | % Initialize velocity vector for agents. Each agent expects a 2 x 1 19 | % velocity vector containing the linear and angular velocity, respectively. 20 | dx = zeros(2, N); 21 | 22 | % This code ensures that the agents are initially distributed around an 23 | % ellipse. 24 | xybound = [-1, 1, -0.8, 0.8]; 25 | p_theta = (1:2:2*N)/(2*N)*2*pi; 26 | p_circ = [xybound(2)*cos(p_theta) xybound(2)*cos(p_theta+pi); xybound(4)*sin(p_theta) xybound(4)*sin(p_theta+pi)]; 27 | 28 | x_goal = p_circ(:,1:N); 29 | 30 | flag = 0; %flag of task completion 31 | 32 | %% Retrieve tools for single-integrator -> unicycle mapping 33 | 34 | % Let's retrieve some of the tools we'll need. We would like a 35 | % single-integrator position controller, a single-integrator barrier 36 | % function, and a mapping from single-integrator to unicycle dynamics 37 | position_control = create_si_position_controller(); 38 | si_to_uni_dyn = create_si_to_uni_dynamics(); 39 | uni_barrier_certificate = create_uni_barrier_certificate2(); 40 | 41 | %% Begin the experiment 42 | % This section contains the actual implementation of the barrier 43 | % certificate experiment. 44 | 45 | %Iterate for the previously specified number of iterations 46 | for t = 1:iterations 47 | 48 | % Retrieve the most recent poses from the Robotarium. The time delay is 49 | % approximately 0.033 seconds 50 | x = r.get_poses(); 51 | 52 | x_temp = x(1:2,:); 53 | 54 | %% Algorithm 55 | 56 | % Let's make sure we're close enough the the goals 57 | if norm(x_goal-x_temp,1)<0.03 58 | flag = 1-flag; 59 | end 60 | 61 | % This code makes the robots switch positions on the ellipse 62 | if flag == 0 63 | x_goal = p_circ(:,1:N); 64 | else 65 | x_goal = p_circ(:,N+1:2*N); 66 | end 67 | 68 | % Use a single-integrator position controller to drive the agents to 69 | % the circular formation 70 | dx = position_control(x(1:2, :), x_goal); 71 | 72 | %% Apply barrier certs. and map to unicycle dynamics 73 | % Transform the single-integrator dynamics to unicycle dynamics using a 74 | % diffeomorphism, which can be found in the utilities 75 | dx = si_to_uni_dyn(dx, x); 76 | 77 | %Ensure the robots don't collide 78 | dx = uni_barrier_certificate(dx, x); 79 | 80 | %% Set the velocities of the agents 81 | % Set velocities of agents 1,...,N 82 | r.set_velocities(1:N, dx); 83 | 84 | % Send the previously set velocities to the agents. This function must be called! 85 | r.step(); 86 | end 87 | 88 | % Print out any simulation problems that will produce implementation 89 | %differences and potential submission rejection. 90 | r.debug() -------------------------------------------------------------------------------- /examples/barrier_certificates/si_barriers.m: -------------------------------------------------------------------------------- 1 | %% Single-integrator Barrier Certificate Algorithm 2 | % by Sean Wilson 3 | % 07/2019 4 | 5 | %% Set up Robotarium object 6 | % Before starting the algorithm, we need to initialize the Robotarium 7 | % object so that we can communicate with the agents 8 | N = 10; 9 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true); 10 | 11 | %This iis how many times the main loop will execute. 12 | iterations = 3000; 13 | 14 | %% Experiment constants 15 | % Next, we set up some experiment constants 16 | 17 | % Initialize velocity vector for agents. Each agent expects a 2 x 1 18 | % velocity vector containing the linear and angular velocity, respectively. 19 | dx = zeros(2, N); 20 | 21 | % This code ensures that the agents are initially distributed around an 22 | % ellipse. 23 | xybound = [-1, 1, -0.8, 0.8]; 24 | p_theta = (1:2:2*N)/(2*N)*2*pi; 25 | p_circ = [xybound(2)*cos(p_theta) xybound(2)*cos(p_theta+pi); xybound(4)*sin(p_theta) xybound(4)*sin(p_theta+pi)]; 26 | 27 | x_goal = p_circ(:,1:N); 28 | 29 | flag = 0; %flag of task completion 30 | 31 | %% Retrieve tools for single-integrator -> unicycle mapping 32 | 33 | % Let's retrieve some of the tools we'll need. We would like a 34 | % single-integrator position controller, a single-integrator barrier 35 | % function, and a mapping from single-integrator to unicycle dynamics 36 | position_control = create_si_position_controller(); 37 | si_barrier_certificate = create_si_barrier_certificate_with_boundary(); 38 | si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion(); 39 | 40 | %% Begin the experiment 41 | % This section contains the actual implementation of the barrier 42 | % certificate experiment. 43 | 44 | %Iterate for the previously specified number of iterations 45 | for t = 1:iterations 46 | 47 | % Retrieve the most recent poses from the Robotarium. The time delay is 48 | % approximately 0.033 seconds 49 | x = r.get_poses(); 50 | 51 | x_position = x(1:2,:); 52 | 53 | %% Algorithm 54 | 55 | % Let's make sure we're close enough the the goals 56 | if norm(x_goal-x_position,1)<0.03 57 | flag = 1-flag; 58 | end 59 | 60 | % This code makes the robots switch positions on the ellipse 61 | if flag == 0 62 | x_goal = p_circ(:,1:N); 63 | else 64 | x_goal = p_circ(:,N+1:2*N); 65 | end 66 | 67 | % Use a single-integrator position controller to drive the agents to 68 | % the circular formation 69 | dx = position_control(x(1:2, :), x_goal); 70 | 71 | %% Apply barrier certs. and map to unicycle dynamics 72 | %Ensure the robots don't collide 73 | dx = si_barrier_certificate(dx, x); 74 | 75 | % Transform the single-integrator dynamics to unicycle dynamics using a 76 | % diffeomorphism, which can be found in the utilities 77 | dx = si_to_uni_dyn(dx, x); 78 | 79 | %% Set the velocities of the agents 80 | % Set velocities of agents 1,...,N 81 | r.set_velocities(1:N, dx); 82 | 83 | % Send the previously set velocities to the agents. This function must be called! 84 | r.step(); 85 | end 86 | 87 | % Print out any simulation problems that will produce implementation 88 | %differences and potential submission rejection. 89 | r.debug() 90 | -------------------------------------------------------------------------------- /utilities/controllers/create_waypoint_controller.m: -------------------------------------------------------------------------------- 1 | function [ automatic_parking_controller ] = create_waypoint_controller(varargin) 2 | % CREATE_AUTOMATIC_PARKING_CONTROLLER Creates a controller that drive a 3 | % unicycle-modeled system to a particular point and stops it (within 4 | % tolerances) 5 | % Works by driving the unicycle to within PositionError of the point then 6 | % rotating it to within RotationError of the desired rotation 7 | % 8 | % Args: 9 | % LinearVelocityGain, optional: see also 10 | % AngularVelocityLimit, optional: see also 11 | % PositionError, optional: Error tolerance for position 12 | % PositionEpsilon, optional: Epsilon 13 | % RotationError, optional: Error tolerance for rotation 14 | % VelocityMagnitudeLimit, optional: Limit for velocity while driving 15 | % to position 16 | 17 | p = inputParser; 18 | addOptional(p, 'LinearVelocityGain', 0.8); 19 | addOptional(p, 'AngularVelocityLimit', pi); 20 | addOptional(p, 'PositionError', 0.03); 21 | addOptional(p, 'PositionEpsilon', 0.01) 22 | addOptional(p, 'RotationError', 0.05); 23 | addOptional(p, 'VelocityMagnitudeLimit', 0.15) 24 | parse(p, varargin{:}); 25 | 26 | lin_vel_gain = p.Results.LinearVelocityGain; 27 | ang_vel_limit = p.Results.AngularVelocityLimit; 28 | vel_mag_limit = p.Results.VelocityMagnitudeLimit; 29 | pos_err = p.Results.PositionError; 30 | pos_eps = p.Results.PositionEpsilon; 31 | rot_err = p.Results.RotationError; 32 | approach_state = []; 33 | 34 | position_controller = create_si_to_uni_dynamics('LinearVelocityGain', lin_vel_gain, ... 35 | 'AngularVelocityLimit', ang_vel_limit); 36 | 37 | automatic_parking_controller = @automatic_parking_controller_; 38 | 39 | function [dxu, output_approach_state] = automatic_parking_controller_(states, poses, input_approach_state) 40 | 41 | N = size(states, 2); 42 | dxu = zeros(2, N); 43 | if nargin > 2 44 | approach_state = input_approach_state; 45 | elseif isempty(approach_state) 46 | approach_state = ones(1,N); 47 | end 48 | 49 | for i = 1:N 50 | 51 | wrapped = poses(3, i) - states(3, i); 52 | wrapped = atan2(sin(wrapped), cos(wrapped)); 53 | 54 | dxi = poses(1:2, i) - states(1:2, i); 55 | 56 | % Normalize 57 | norm_ = norm(dxi); 58 | 59 | if(norm_ > pos_err - pos_eps && approach_state(i)) 60 | if(norm_ > vel_mag_limit) 61 | dxi = vel_mag_limit*dxi/norm_; 62 | end 63 | dxu(:, i) = position_controller(dxi, states(:, i)); 64 | 65 | elseif(abs(wrapped) > rot_err) 66 | approach_state(i) = 0; 67 | if(norm(dxi) > pos_err) 68 | approach_state(i) = 1; 69 | end 70 | dxu(1, i) = 0; 71 | dxu(2, i) = 2*wrapped; 72 | else 73 | dxu(:, i) = zeros(2, 1); 74 | end 75 | end 76 | 77 | if nargout > 1 78 | output_approach_state = approach_state; 79 | end 80 | end 81 | end 82 | 83 | 84 | -------------------------------------------------------------------------------- /examples/consensus_static/consensus_fewer_errors.m: -------------------------------------------------------------------------------- 1 | %% Consensus with a static, undirected topology 2 | % Sean Wilson 3 | % 07/2019 4 | 5 | % Demontrates a bare-bones example of the consensus algorithm. Same as 6 | % consensus.m. Except, this script contains modifications that 7 | % consider implementation needs. 8 | 9 | N = 12; 10 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true); 11 | 12 | %% Experiment constants 13 | 14 | % Generate a cyclic graph Laplacian from our handy utilities. For this 15 | % algorithm, any connected graph will yield consensus 16 | L = cycleGL(N); 17 | 18 | %% Grab tools we need to convert from single-integrator to unicycle dynamics 19 | 20 | % Gain for the diffeomorphism transformation between single-integrator and 21 | % unicycle dynamics 22 | [~, uni_to_si_states] = create_si_to_uni_mapping(); 23 | si_to_uni_dyn = create_si_to_uni_dynamics_with_backwards_motion(); 24 | 25 | uni_barrier_cert_boundary = create_uni_barrier_certificate_with_boundary(); 26 | 27 | % Select the number of iterations for the experiment. This value is 28 | % arbitrary 29 | iterations = 2000; 30 | 31 | % Initialize velocity vector for agents. Each agent expects a 2 x 1 32 | % velocity vector containing the linear and angular velocity, respectively. 33 | dxi = zeros(2, N); 34 | 35 | %Iterate for the previously specified number of iterations 36 | for t = 1:iterations 37 | 38 | % Retrieve the most recent poses from the Robotarium. The time delay is 39 | % approximately 0.033 seconds 40 | x = r.get_poses(); 41 | 42 | % Convert to SI states 43 | xi = uni_to_si_states(x); 44 | 45 | %% Algorithm 46 | 47 | for i = 1:N 48 | 49 | % Initialize velocity to zero for each agent. This allows us to sum 50 | %over agent i's neighbors 51 | dxi(:, i) = [0 ; 0]; 52 | 53 | % Get the topological neighbors of agent i based on the graph 54 | %Laplacian L 55 | neighbors = topological_neighbors(L, i); 56 | 57 | % Iterate through agent i's neighbors 58 | for j = neighbors 59 | 60 | % For each neighbor, calculate appropriate consensus term and 61 | %add it to the total velocity 62 | dxi(:, i) = dxi(:, i) + (xi(:, j) - xi(:, i)); 63 | end 64 | end 65 | 66 | %% Avoid actuator errors 67 | 68 | % To avoid errors, we need to threshold dxi 69 | norms = arrayfun(@(x) norm(dxi(:, x)), 1:N); 70 | threshold = 3/4*r.max_linear_velocity; 71 | to_thresh = norms > threshold; 72 | dxi(:, to_thresh) = threshold*dxi(:, to_thresh)./norms(to_thresh); 73 | 74 | %% Map SI to Uni dynamics and utilize barrier certificates 75 | 76 | % Transform the single-integrator to unicycle dynamics using the the 77 | % transformation we created earlier 78 | dxu = si_to_uni_dyn(dxi, x); 79 | 80 | dxu = uni_barrier_cert_boundary(dxu, x); 81 | 82 | %% Send velocities to agents 83 | 84 | % Set velocities of agents 1,...,N 85 | r.set_velocities(1:N, dxu); 86 | 87 | % Send the previously set velocities to the agents. This function must be called! 88 | r.step(); 89 | end 90 | 91 | % We can call this function to debug our experiment! Fix all the errors 92 | % before submitting to maximize the chance that your experiment runs 93 | % successfully. 94 | r.debug(); 95 | -------------------------------------------------------------------------------- /utilities/barrier_certificates/create_si_barrier_certificate2.m: -------------------------------------------------------------------------------- 1 | %% create_si_barrier_certificate 2 | % Returns a single-integrator barrier certificate function ($f : 3 | % \mathbf{R}^{2 \times N} \times \mathbf{R}^{2 \times N} \to \mathbf{R}^{2 4 | % \times N}$). This function takes a 2 x N, 2 x N single-integrator 5 | % velocity and state vector, respectively, and returns a single-integrator 6 | % velocity vector that does not induce collisions in the agents. 7 | %% Detailed Description 8 | %% 9 | % * BarrierGain - affects how quickly the agents can approach each other 10 | % * SafetyRadius - affects the distance the agents maintain 11 | %% 12 | % A good rule of thumb is to make SafetyRadius a bit larger than the agent 13 | % itself (0.08 m for the GRITSbot). 14 | 15 | %% Implementation 16 | function [ si_barrier_certificate ] = create_si_barrier_certificate2(varargin) 17 | 18 | parser = inputParser; 19 | parser.addParameter('SafeBarrierGain', 100) 20 | parser.addParameter('UnsafeBarrierGain', 1e6) 21 | parser.addParameter('SafetyRadius', 0.15); 22 | parse(parser, varargin{:}) 23 | opts = optimoptions(@quadprog,'Display','off'); 24 | 25 | gamma_safe = parser.Results.SafeBarrierGain; 26 | gamma_unsafe = parser.Results.UnsafeBarrierGain; 27 | safety_radius = parser.Results.SafetyRadius; 28 | 29 | si_barrier_certificate = @barrier_certificate; 30 | 31 | function [ dx ] = barrier_certificate(dxi, x) 32 | %BARRIERCERTIFICATE Wraps single-integrator dynamics in safety barrier 33 | %certificates 34 | % This function accepts single-integrator dynamics and wraps them in 35 | % barrier certificates to ensure that collisions do not occur. Note that 36 | % this algorithm bounds the magnitude of the generated output to 0.1. 37 | % 38 | % dx = BARRIERCERTIFICATE(dxi, x, safetyRadius) 39 | % dx: generated safe, single-integrator inputs 40 | % dxi: single-integrator synamics 41 | % x: States of the agents 42 | % safetyRadius: Size of the agents (or desired separation distance) 43 | 44 | N = size(dxi, 2); 45 | 46 | if(N < 2) 47 | dx = dxi; 48 | return 49 | end 50 | 51 | x = x(1:2, :); 52 | 53 | %Generate constraints for barrier certificates based on the size of 54 | %the safety radius 55 | num_constraints = nchoosek(N, 2); 56 | A = zeros(num_constraints, 2*N); 57 | b = zeros(num_constraints, 1); 58 | count = 1; 59 | for i = 1:(N-1) 60 | for j = (i+1):N 61 | h = norm(x(1:2,i)-x(1:2,j))^2-safety_radius^2; 62 | A(count, (2*i-1):(2*i)) = -2*(x(:,i)-x(:,j)); 63 | A(count, (2*j-1):(2*j)) = 2*(x(:,i)-x(:,j))'; 64 | if(h >= 0) 65 | b(count) = gamma_safe*h^3; 66 | else 67 | b(count) = gamma_unsafe*h^3; 68 | end 69 | count = count + 1; 70 | end 71 | end 72 | 73 | %Solve QP program generated earlier 74 | vhat = reshape(dxi,2*N,1); 75 | H = 2*eye(2*N); 76 | f = -2*vhat; 77 | 78 | vnew = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opts); 79 | 80 | %Set robot velocities to new velocities 81 | dx = reshape(vnew, 2, N); 82 | end 83 | end 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /utilities/barrier_certificates/create_si_barrier_certificate.m: -------------------------------------------------------------------------------- 1 | function [ si_barrier_certificate ] = create_si_barrier_certificate(varargin) 2 | % CREATE_SI_BARRIER_CERTIFICATE Creates a single-integrator barrier 3 | % certificate function to avoid collisions. 4 | % 5 | % Args: 6 | % BarrierGain, optional: Positive double 7 | % SafetyRadius, optional: Positive double 8 | % 9 | % Returns: 10 | % A barrier certificate function (2xN, 2xN) -> 2xN representing the 11 | % barrier certificate 12 | % 13 | % CREATE_SI_BARRIER_CERTIFICATE('SafetyRadius', 0.2) creates a function 14 | % from (2xN, 2xN) -> 2xN to keep robots at least 0.2 meters apart 15 | % (measured from their centers). 16 | % 17 | % CREATE_SI_BARRIER_CERTIFICATE('BarrierGain', 10e4) creates a 18 | % barrier certificate with a particular gain. The higher the gain, 19 | % the more quickly the robots can approach each other. 20 | % 21 | % Example: 22 | % bc = create_si_barrier_certificate('SafetyRadius', 0.2) 23 | % 24 | % Notes: 25 | % SafetyRadius should be a positive double 26 | % BarrierGain should be a positive double 27 | % In practice, the value for SafetyRadius should be a little more than double the 28 | % size of the robots. 29 | 30 | parser = inputParser; 31 | parser.addParameter('BarrierGain', 100); 32 | parser.addParameter('SafetyRadius', 0.15); 33 | parse(parser, varargin{:}) 34 | opts = optimoptions(@quadprog,'Display','off'); 35 | 36 | gamma = parser.Results.BarrierGain; 37 | safety_radius = parser.Results.SafetyRadius; 38 | 39 | si_barrier_certificate = @barrier_certificate; 40 | 41 | function [ dx ] = barrier_certificate(dxi, x) 42 | % BARRIERCERTIFICATE Wraps single-integrator dynamics in safety barrier 43 | % certificates 44 | % This function accepts single-integrator dynamics and wraps them in 45 | % barrier certificates to ensure that collisions do not occur. Note that 46 | % this algorithm bounds the magnitude of the generated output to 0.1. 47 | % 48 | % BARRIER_CERTIFICATE(dxi, x) modifies dxi to become collision 49 | % free. 50 | % 51 | % Example: 52 | % dxi is size 2xN 53 | % x is size 2xN 54 | % BARRIER_CERTIFICATE(dxi, x) 55 | % 56 | % Notes: 57 | % Try not to threshold outputs of this function. Rather, 58 | % threshold them before calling the barrier certificate. 59 | 60 | N = size(dxi, 2); 61 | 62 | if(N < 2) 63 | dx = dxi; 64 | return 65 | end 66 | 67 | x = x(1:2, :); 68 | 69 | %Generate constraints for barrier certificates based on the size of 70 | %the safety radius 71 | num_constraints = nchoosek(N, 2); 72 | A = zeros(num_constraints, 2*N); 73 | b = zeros(num_constraints, 1); 74 | count = 1; 75 | for i = 1:(N-1) 76 | for j = (i+1):N 77 | h = norm(x(1:2,i)-x(1:2,j))^2-safety_radius^2; 78 | A(count, (2*i-1):(2*i)) = -2*(x(:,i)-x(:,j)); 79 | A(count, (2*j-1):(2*j)) = 2*(x(:,i)-x(:,j))'; 80 | b(count) = gamma*h^3; 81 | count = count + 1; 82 | end 83 | end 84 | 85 | %Solve QP program generated earlier 86 | vhat = reshape(dxi,2*N,1); 87 | H = 2*eye(2*N); 88 | f = -2*vhat; 89 | 90 | vnew = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opts); 91 | 92 | %Set robot velocities to new velocities 93 | dx = reshape(vnew, 2, N); 94 | end 95 | end 96 | 97 | -------------------------------------------------------------------------------- /examples/leader_follower_static/leader_follower.m: -------------------------------------------------------------------------------- 1 | %% Leader-follower with static topology 2 | % Paul Glotfelter edited by Sean Wilson 3 | % 07/2019 4 | 5 | %% Experiment Constants 6 | 7 | %Run the simulation for a specific number of iterations 8 | iterations = 5000; 9 | 10 | %% Set up the Robotarium object 11 | 12 | N = 4; 13 | initial_positions = generate_initial_conditions(N, 'Width', 1, 'Height', 1, 'Spacing', 0.5); 14 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true, 'InitialConditions', initial_positions); 15 | 16 | %% Create the desired Laplacian 17 | 18 | %Graph laplacian 19 | followers = -completeGL(N-1); 20 | L = zeros(N, N); 21 | L(2:N, 2:N) = followers; 22 | L(2, 2) = L(2, 2) + 1; 23 | L(2, 1) = -1; 24 | 25 | %Initialize velocity vector 26 | dxi = zeros(2, N); 27 | 28 | %State for leader 29 | state = 1; 30 | 31 | % These are gains for our formation control algorithm 32 | formation_control_gain = 10; 33 | desired_distance = 0.2; 34 | 35 | %% Grab tools we need to convert from single-integrator to unicycle dynamics 36 | 37 | % Single-integrator -> unicycle dynamics mapping 38 | si_to_uni_dyn = create_si_to_uni_dynamics('LinearVelocityGain', 0.8); 39 | % Single-integrator barrier certificates 40 | uni_barrier_cert = create_uni_barrier_certificate_with_boundary(); 41 | % Single-integrator position controller 42 | leader_controller = create_si_position_controller('XVelocityGain', 0.8, 'YVelocityGain', 0.8, 'VelocityMagnitudeLimit', 0.1); 43 | 44 | waypoints = [-1 0.8; -1 -0.8; 1 -0.8; 1 0.8]'; 45 | close_enough = 0.05; 46 | 47 | for t = 1:iterations 48 | 49 | % Retrieve the most recent poses from the Robotarium. The time delay is 50 | % approximately 0.033 seconds 51 | x = r.get_poses(); 52 | 53 | %% Algorithm 54 | 55 | for i = 2:N 56 | 57 | %Zero velocity and get the topological neighbors of agent i 58 | dxi(:, i) = [0 ; 0]; 59 | 60 | neighbors = topological_neighbors(L, i); 61 | 62 | for j = neighbors 63 | dxi(:, i) = dxi(:, i) + ... 64 | formation_control_gain*(norm(x(1:2, j) - x(1:2, i))^2 - desired_distance^2)*(x(1:2, j) - x(1:2, i)); 65 | end 66 | end 67 | 68 | %% Make the leader travel between waypoints 69 | 70 | waypoint = waypoints(:, state); 71 | 72 | switch state 73 | case 1 74 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 75 | if(norm(x(1:2, 1) - waypoint) < close_enough) 76 | state = 2; 77 | end 78 | case 2 79 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 80 | if(norm(x(1:2, 1) - waypoint) < close_enough) 81 | state = 3; 82 | end 83 | case 3 84 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 85 | if(norm(x(1:2, 1) - waypoint) < close_enough) 86 | state = 4; 87 | end 88 | case 4 89 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 90 | if(norm(x(1:2, 1) - waypoint) < close_enough) 91 | state = 1; 92 | end 93 | end 94 | 95 | 96 | %% Avoid actuator errors 97 | 98 | % To avoid errors, we need to threshold dxi 99 | norms = arrayfun(@(x) norm(dxi(:, x)), 1:N); 100 | threshold = 3/4*r.max_linear_velocity; 101 | to_thresh = norms > threshold; 102 | dxi(:, to_thresh) = threshold*dxi(:, to_thresh)./norms(to_thresh); 103 | 104 | %% Use barrier certificate and convert to unicycle dynamics 105 | dxu = si_to_uni_dyn(dxi, x); 106 | dxu = uni_barrier_cert(dxu, x); 107 | 108 | %% Send velocities to agents 109 | 110 | %Set velocities 111 | r.set_velocities(1:N, dxu); 112 | 113 | %Iterate experiment 114 | r.step(); 115 | end 116 | 117 | % We can call this function to debug our experiment! Fix all the errors 118 | % before submitting to maximize the chance that your experiment runs 119 | % successfully. 120 | r.debug(); -------------------------------------------------------------------------------- /examples/formation_control/formation_control.m: -------------------------------------------------------------------------------- 1 | %% Formation control utilizing edge tension energy with a static, undirected 2 | % communication topology 3 | % Paul Glotfelter updated by Sean Wilson 4 | % 07/2019 5 | 6 | %% Set up Robotarium object 7 | 8 | N = 6; 9 | initial_conditions = generate_initial_conditions(N, 'Width', 2, 'Height', 1, 'Spacing', 0.5); 10 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true, 'InitialConditions', initial_conditions); 11 | 12 | %% Set up constants for experiment 13 | 14 | %Gains for the transformation from single-integrator to unicycle dynamics 15 | formation_control_gain = 10; 16 | 17 | % Select the number of iterations for the experiment. This value is 18 | % arbitrary 19 | iterations = 2000; 20 | 21 | % Communication topology for the desired formation. We need 2 * N - 3 = 9 22 | % edges to ensure that the formation is rigid. 23 | L = [3 -1 0 -1 0 -1 ; ... 24 | -1 3 -1 0 -1 0 ; ... 25 | 0 -1 3 -1 0 -1 ; ... 26 | -1 0 -1 3 -1 0 ; ... 27 | 0 -1 0 -1 3 -1 ; ... 28 | -1 0 -1 0 -1 3]; 29 | 30 | % The desired inter-agent distance for the formation 31 | d = 0.4; 32 | 33 | % Pre-compute diagonal values for the rectangular formation 34 | ddiag = sqrt((2*d)^2 + d^2); 35 | 36 | % Weight matrix containing the desired inter-agent distances to achieve a 37 | % rectuangular formation 38 | weights = [ 0 d 0 d 0 ddiag; ... 39 | d 0 d 0 d 0; ... 40 | 0 d 0 ddiag 0 d; ... 41 | d 0 ddiag 0 d 0; ... 42 | 0 d 0 d 0 d; ... 43 | ddiag 0 d 0 d 0]; 44 | 45 | % Initialize velocity vector for agents. Each agent expects a 2 x 1 46 | % velocity vector containing the linear and angular velocity, respectively. 47 | dx = zeros(2, N); 48 | 49 | %% Grab tools for converting to single-integrator dynamics and ensuring safety 50 | 51 | uni_barrier_cert = create_uni_barrier_certificate_with_boundary(); 52 | si_to_uni_dyn = create_si_to_uni_dynamics('LinearVelocityGain', 0.5, 'AngularVelocityLimit', pi/2); 53 | 54 | % Iterate for the previously specified number of iterations 55 | for t = 0:iterations 56 | 57 | 58 | % Retrieve the most recent poses from the Robotarium. The time delay is 59 | % approximately 0.033 seconds 60 | x = r.get_poses(); 61 | 62 | %% Algorithm 63 | 64 | %This section contains the actual algorithm for formation control! 65 | 66 | %Calculate single integrator control inputs using edge-energy consensus 67 | for i = 1:N 68 | 69 | % Initialize velocity to zero for each agent. This allows us to sum 70 | % over agent i's neighbors 71 | dx(:, i) = [0 ; 0]; 72 | 73 | % Get the topological neighbors of agent i from the communication 74 | % topology 75 | for j = topological_neighbors(L, i) 76 | 77 | % For each neighbor, calculate appropriate formation control term and 78 | % add it to the total velocity 79 | 80 | dx(:, i) = dx(:, i) + ... 81 | formation_control_gain*(norm(x(1:2, i) - x(1:2, j))^2 - weights(i, j)^2) ... 82 | *(x(1:2, j) - x(1:2, i)); 83 | end 84 | end 85 | 86 | %% Avoid actuator errors 87 | 88 | % To avoid errors, we need to threshold dx 89 | norms = arrayfun(@(x) norm(dx(:, x)), 1:N); 90 | threshold = 3/4*r.max_linear_velocity; 91 | to_thresh = norms > threshold; 92 | dx(:, to_thresh) = threshold*dx(:, to_thresh)./norms(to_thresh); 93 | 94 | % Transform the single-integrator dynamics to unicycle dynamics using a provided utility function 95 | dx = si_to_uni_dyn(dx, x); 96 | dx = uni_barrier_cert(dx, x); 97 | 98 | % Set velocities of agents 1:N 99 | r.set_velocities(1:N, dx); 100 | 101 | % Send the previously set velocities to the agents. This function must be called! 102 | r.step(); 103 | end 104 | 105 | % We can call this function to debug our experiment! Fix all the errors 106 | % before submitting to maximize the chance that your experiment runs 107 | % successfully. 108 | r.debug(); 109 | -------------------------------------------------------------------------------- /utilities/barrier_certificates/create_uni_barrier_certificate.m: -------------------------------------------------------------------------------- 1 | function [ uni_barrier_certificate ] = create_uni_barrier_certificate(varargin) 2 | % CREATE_SI_BARRIER_CERTIFICATE Creates a unicycle barrier 3 | % certificate function to avoid collisions. 4 | % 5 | % Args: 6 | % BarrierGain, optional: How quickly robots can approach eachother 7 | % SafetyRadius, optional: How far apart centers of robots should 8 | % remain 9 | % ProjectionDistance, optional: How far ahead to project a virtual 10 | % single integrator 11 | % VelocityMagnitudeLimit, optional: The maximum velocity for the 12 | % virtual single integrator 13 | % 14 | % Returns: 15 | % A barrier certificate function (2xN, 3xN) -> 2xN representing the 16 | % barrier certificate 17 | % 18 | % CREATE_UNI_BARRIER_CERTIFICATE('BarrierGain', bg) 19 | % 20 | % CREATE_UNI_BARRIER_CERTIFICATE('SafetyRadius', sr) 21 | % 22 | % CREATE_UNI_BARRIER_CERTIFICATE('SafetyRadius', sr, 'BarrierGain', bg) 23 | % 24 | % Example: 25 | % bc = create_si_barrier_certificate('SafetyRadius', 0.2) 26 | % 27 | % Notes: 28 | % SafetyRadius should be a positive double 29 | % BarrierGain should be a positive double 30 | % In practice, the value for SafetyRadius should be a little more than double the 31 | % size of the robots. 32 | 33 | parser = inputParser; 34 | addOptional(parser, 'BarrierGain', 100); 35 | addOptional(parser, 'SafetyRadius', 0.15); 36 | addOptional(parser, 'ProjectionDistance', 0.05); 37 | addOptional(parser, 'VelocityMagnitudeLimit', 0.2); 38 | parse(parser, varargin{:}) 39 | 40 | opts = optimoptions(@quadprog,'Display','off'); 41 | gamma = parser.Results.BarrierGain; 42 | safety_radius = parser.Results.SafetyRadius; 43 | projection_distance = parser.Results.ProjectionDistance; 44 | velocity_magnitude_limit = parser.Results.VelocityMagnitudeLimit; 45 | 46 | [si_uni_dyn, uni_si_states] = create_si_to_uni_mapping('ProjectionDistance', projection_distance); 47 | uni_si_dyn = create_uni_to_si_mapping('ProjectionDistance', projection_distance); 48 | 49 | uni_barrier_certificate = @barrier_unicycle; 50 | 51 | function [ dxu ] = barrier_unicycle(dxu, x) 52 | % BARRIER_UNICYCLE The parameterized barrier function 53 | % 54 | % Args: 55 | % dxu: 2xN vector of unicycle control inputs 56 | % x: 3xN vector of unicycle states 57 | % 58 | % Returns: 59 | % A 2xN matrix of safe unicycle control inputs 60 | % 61 | % BARRIER_UNICYCLE(dxu, x) 62 | 63 | N = size(dxu, 2); 64 | 65 | if(N < 2) 66 | return 67 | end 68 | 69 | %Shift to single integrator domain 70 | xi = uni_si_states(x); 71 | dxi = uni_si_dyn(dxu, x); 72 | 73 | % Normalize velocities 74 | norms = arrayfun(@(idx) norm(dxi(:, idx)), 1:N); 75 | to_normalize = norms > velocity_magnitude_limit; 76 | if(size(to_normalize, 2) > 0) 77 | dxi(:, to_normalize) = velocity_magnitude_limit*dxi(:, to_normalize)./norms(to_normalize); 78 | end 79 | 80 | %Generate constraints for barrier certificates based on the size of 81 | %the safety radius 82 | num_constraints = nchoosek(N, 2); 83 | A = zeros(num_constraints, 2*N); 84 | b = zeros(num_constraints, 1); 85 | count = 1; 86 | for i = 1:(N-1) 87 | for j = (i+1):N 88 | h = norm(xi(:,i)-xi(:,j))^2-(safety_radius + 2*projection_distance)^2; 89 | A(count, (2*i-1):(2*i)) = 2*(xi(:,i)-xi(:,j))'; 90 | A(count, (2*j-1):(2*j)) = -2*(xi(:,i)-xi(:,j))'; 91 | b(count) = -gamma*h^3; 92 | count = count + 1; 93 | end 94 | end 95 | 96 | A = -A; 97 | b = -b; 98 | 99 | %Solve QP program generated earlier 100 | vhat = reshape(dxi,2*N,1); 101 | H = 2*eye(2*N); 102 | f = -2*vhat; 103 | 104 | vnew = quadprog(sparse(H), double(f), A, b, [], [], [], [], [], opts); 105 | 106 | %Set robot velocities to new velocities 107 | dxu = si_uni_dyn(reshape(vnew, 2, N), x); 108 | 109 | end 110 | end 111 | 112 | -------------------------------------------------------------------------------- /utilities/barrier_certificates/create_uni_barrier_certificate_with_obstacles.m: -------------------------------------------------------------------------------- 1 | function [ uni_barrier_certificate ] = create_uni_barrier_certificate_with_obstacles(varargin) 2 | % CREATE_UNI_BARRIER_CERTIFICATE_WITH_OBSTACLES Creates a barrier certificate for a 3 | % unicycle-modeled systems 4 | % Works by projecting a virtual single-integrator system ahead of the 5 | % unicycle and applying a suitably large barrier certificate to the virtual 6 | % system. 7 | % 8 | % Args: 9 | % BarrierGain, optional: A gain for how quickly the system can 10 | % approach obstacles 11 | % SafetyRadius, optional: How far points should remain apart. 12 | % Nominal value is 1.5*robot_diameter 13 | % ProjectionDistance: How far ahead to place the virtual 14 | % single-integrator system 15 | % VelocityMagnitudeLimit: Limit for the magnitude of the virtual 16 | % single integrator 17 | % 18 | % Returns: 19 | % A barrier function (2xN, 3xN, 2xN) -> 2xN that generates safe 20 | % control inputs for unicycle-modeled systems. 21 | % 22 | % CREATE_UNI_BARRIER_CERTIFICATE_WITH_OBSTACLES('BarrierGain', 8e3) 23 | % 24 | % CREATE_UNI_BARRIER_CERTIFICATE_WITH_OBSTACLES('SafetyRadius', 0.15) 25 | % 26 | % CREATE_UNI_BARRIER_CERTIFICATE_WITH_OBSTACLES('ProjectionDistance', 27 | % 0.05) 28 | % 29 | % CREATE_UNI_BARRIER_CERTIFICATE_WITH_OBSTACLES('VelocityMagnitudeLimit', 30 | % 0.4) 31 | 32 | parser = inputParser; 33 | addOptional(parser, 'BarrierGain', 100); 34 | addOptional(parser, 'SafetyRadius', 0.15); 35 | addOptional(parser, 'ProjectionDistance', 0.05) 36 | addOptional(parser, 'VelocityMagnitudeLimit', 0.2); 37 | parse(parser, varargin{:}) 38 | 39 | opts = optimoptions(@quadprog,'Display','off'); 40 | gamma = parser.Results.BarrierGain; 41 | safety_radius = parser.Results.SafetyRadius; 42 | projection_distance = parser.Results.ProjectionDistance; 43 | velocity_magnitude_limit = parser.Results.VelocityMagnitudeLimit; 44 | 45 | [si_uni_dyn, uni_si_states] = create_si_to_uni_mapping('ProjectionDistance', projection_distance); 46 | uni_si_dyn = create_uni_to_si_mapping('ProjectionDistance', projection_distance); 47 | 48 | uni_barrier_certificate = @barrier_unicycle; 49 | 50 | function [ dxu ] = barrier_unicycle(dxu, x, obstacles) 51 | N = size(dxu, 2); 52 | 53 | %Shift to single integrator domain 54 | xi = uni_si_states(x); 55 | dxi = uni_si_dyn(dxu, x); 56 | 57 | % Normalize velocities 58 | norms = arrayfun(@(idx) norm(dxi(:, idx)), 1:N); 59 | to_normalize = norms > velocity_magnitude_limit; 60 | 61 | if(any(to_normalize == 1)) 62 | dxi(:, to_normalize) = velocity_magnitude_limit*dxi(:, to_normalize)./norms(to_normalize); 63 | end 64 | 65 | %Generate constraints for barrier certificates based on the size of 66 | %the safety radius 67 | if(N > 2) 68 | temp = nchoosek(N, 2); 69 | else 70 | temp = N - 1; 71 | end 72 | num_constraints = temp + size(obstacles, 2); 73 | A = zeros(num_constraints, 2*N); 74 | b = zeros(num_constraints, 1); 75 | count = 1; 76 | for i = 1:(N-1) 77 | for j = (i+1):N 78 | h = norm(xi(:,i)-xi(:,j))^2-(safety_radius + 2*projection_distance)^2; 79 | A(count, (2*i-1):(2*i)) = 2*(xi(:,i)-xi(:,j))'; 80 | A(count, (2*j-1):(2*j)) = -2*(xi(:,i)-xi(:,j))'; 81 | b(count) = -gamma*h^3; 82 | count = count + 1; 83 | end 84 | end 85 | 86 | % Do obstacles 87 | for i = 1:N 88 | for j = 1:size(obstacles, 2) 89 | h = norm(xi(:,i)-obstacles(:,j))^2-(safety_radius + projection_distance)^2; 90 | A(count, (2*i-1):(2*i)) = 2*(xi(:,i)-obstacles(:,j))'; 91 | b(count) = -gamma*h^3; 92 | count = count + 1; 93 | end 94 | end 95 | 96 | A = -A; 97 | b = -b; 98 | 99 | %Solve QP program generated earlier 100 | vhat = reshape(dxi,2*N,1); 101 | H = 2*eye(2*N); 102 | f = -2*vhat; 103 | 104 | vnew = quadprog(sparse(H), double(f), A, b, [], [], [], [], [], opts); 105 | 106 | %Set robot velocities to new velocities 107 | dxu = si_uni_dyn(reshape(vnew, 2, N), x); 108 | 109 | end 110 | end 111 | 112 | -------------------------------------------------------------------------------- /utilities/graph/html/completeGL.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | completeGL:

completeGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$

Returns a completely connected graph Laplacian

Contents

Example Usage

L = completeGL(5);
70 |

Implementation

function [ L ] = completeGL( n )
71 |     L = n * eye(n) - ones(n,n);
72 | end
73 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/graph/html/cycleGL.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | cycleGL:

cycleGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$

Returns a cycle graph Laplacian

Contents

Example Usage

L = cycleGL(4)
70 |

Implementation

function [ L ] = cycleGL( n )
71 |     L = 2*eye(n) - diag(ones(1,(n-1)), 1) - diag(ones(1,(n-1)), -1);
72 |     L(n, 1) = -1;
73 |     L(1, n) = -1;
74 | end
75 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/graph/html/lineGL.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | lineGL:

lineGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$

Returns a line graph Laplacian of size n x n

Contents

Example Usage

L = lineGL(5)
70 |

Implementation

function [ L ] = lineGL(n)
71 |     L = 2*eye(n) - diag(ones(1,(n-1)), 1) - diag(ones(1, n-1), -1);
72 |     L(1, 1) = 1;
73 |     L(n, n) = 1;
74 | end
75 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/barrier_certificates/create_si_barrier_certificate_with_boundary.m: -------------------------------------------------------------------------------- 1 | function [ si_barrier_certificate ] = create_si_barrier_certificate_with_boundary(varargin) 2 | % CREATE_SI_BARRIER_CERTIFICATE Creates a single-integrator barrier 3 | % certificate function to avoid collisions and remain inside a bounded rectangle. 4 | % 5 | % Args: 6 | % BarrierGain, optional: Positive double 7 | % SafetyRadius, optional: Positive double 8 | % MagnitudeLimit, optional: Positive double 9 | % BoundaryPoints, optional: Array of rectangle limits [xmin, xmax, ymin, ymax] 10 | % 11 | % Returns: 12 | % A barrier certificate function (2xN, 2xN) -> 2xN representing the 13 | % barrier certificate 14 | % 15 | % CREATE_SI_BARRIER_CERTIFICATE('SafetyRadius', 0.2) creates a function 16 | % from (2xN, 2xN) -> 2xN to keep robots at least 0.2 meters apart 17 | % (measured from their centers). 18 | % 19 | % CREATE_SI_BARRIER_CERTIFICATE('BarrierGain', 10e4) creates a 20 | % barrier certificate with a particular gain. The higher the gain, 21 | % the more quickly the robots can approach each other. 22 | % 23 | % Example: 24 | % bc = create_si_barrier_certificate('SafetyRadius', 0.2) 25 | % 26 | % Notes: 27 | % SafetyRadius should be a positive double 28 | % BarrierGain should be a positive double 29 | % In practice, the value for SafetyRadius should be a little more than double the 30 | % size of the robots. 31 | 32 | parser = inputParser; 33 | parser.addParameter('BarrierGain', 100); 34 | parser.addParameter('SafetyRadius', 0.15); 35 | parser.addParameter('MagnitudeLimit', 0.2); 36 | parser.addParameter('BoundaryPoints', [-1.6, 1.6, -1.0, 1.0]) 37 | parse(parser, varargin{:}) 38 | opts = optimoptions(@quadprog,'Display','off'); 39 | 40 | gamma = parser.Results.BarrierGain; 41 | safety_radius = parser.Results.SafetyRadius; 42 | magnitude_limit = parser.Results.MagnitudeLimit; 43 | boundary_points = parser.Results.BoundaryPoints; 44 | 45 | si_barrier_certificate = @barrier_certificate; 46 | 47 | function [ dx ] = barrier_certificate(dxi, x) 48 | % BARRIERCERTIFICATE Wraps single-integrator dynamics in safety barrier 49 | % certificates 50 | % This function accepts single-integrator dynamics and wraps them in 51 | % barrier certificates to ensure that collisions do not occur. Note that 52 | % this algorithm bounds the magnitude of the generated output to 0.1. 53 | % 54 | % BARRIER_CERTIFICATE(dxi, x) modifies dxi to become collision 55 | % free. 56 | % 57 | % Example: 58 | % dxi is size 2xN 59 | % x is size 2xN 60 | % BARRIER_CERTIFICATE(dxi, x) 61 | % 62 | % Notes: 63 | % Try not to threshold outputs of this function. Rather, 64 | % threshold them before calling the barrier certificate. 65 | 66 | N = size(dxi, 2); 67 | 68 | if(N < 2) 69 | dx = dxi; 70 | return 71 | end 72 | 73 | x = x(1:2, :); 74 | 75 | %Generate constraints for barrier certificates based on the size of 76 | %the safety radius 77 | num_constraints = nchoosek(N, 2); 78 | A = zeros(num_constraints, 2*N); 79 | b = zeros(num_constraints, 1); 80 | count = 1; 81 | for i = 1:(N-1) 82 | for j = (i+1):N 83 | h = norm(x(1:2,i)-x(1:2,j))^2-safety_radius^2; 84 | A(count, (2*i-1):(2*i)) = -2*(x(:,i)-x(:,j)); 85 | A(count, (2*j-1):(2*j)) = 2*(x(:,i)-x(:,j))'; 86 | b(count) = gamma*h^3; 87 | count = count + 1; 88 | end 89 | end 90 | 91 | for k = 1:N 92 | %Pos Y 93 | A(count, (2*k-1):(2*k)) = [0,1]; 94 | b(count) = 0.4*gamma*(boundary_points(4)-safety_radius/2 - x(2,k))^3; 95 | count = count + 1; 96 | 97 | %Neg Y 98 | A(count, (2*k-1):(2*k)) = [0,-1]; 99 | b(count) = 0.4*gamma*(-boundary_points(3)-safety_radius/2 + x(2,k))^3; 100 | count = count + 1; 101 | 102 | %Pos X 103 | A(count, (2*k-1):(2*k)) = [1,0]; 104 | b(count) = 0.4*gamma*(boundary_points(2)-safety_radius/2 - x(1,k))^3; 105 | count = count + 1; 106 | 107 | %Neg X 108 | A(count, (2*k-1):(2*k)) = [-1,0]; 109 | b(count) = 0.4*gamma*(-boundary_points(1)-safety_radius/2 + x(1,k))^3; 110 | count = count + 1; 111 | end 112 | 113 | % Threshold control inputs before QP 114 | norms = arrayfun(@(x) norm(dxi(:, x)), 1:N); 115 | threshold = magnitude_limit; 116 | to_thresh = norms > threshold; 117 | dxi(:, to_thresh) = threshold*dxi(:, to_thresh)./norms(to_thresh); 118 | 119 | %Solve QP program generated earlier 120 | vhat = reshape(dxi,2*N,1); 121 | H = 2*eye(2*N); 122 | f = -2*vhat; 123 | 124 | vnew = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opts); 125 | 126 | %Set robot velocities to new velocities 127 | dx = reshape(vnew, 2, N); 128 | end 129 | end 130 | 131 | -------------------------------------------------------------------------------- /utilities/graph/html/topological_neighbors.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | topological_neighbors

topological_neighbors $\mathbf{R}^{N \times N} \times \mathbf{Z}^{+} \to \mathbf{Z}^{+}$

Returns the topological neighbors of a given agent

function [neighbors] = topological_neighbors(L, agent)
70 | 
71 |     N = size(L, 2);
72 | 
73 |     assert(agent<=N && agent>=1, 'Supplied agent (%i) must be between 1 and %i', agent, N);
74 | 
75 |     L_agent = L(agent, :);
76 |     L_agent(agent) = 0;
77 | 
78 |     neighbors = find(L_agent ~= 0);
79 | end
80 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/misc/html/unique_filename.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | unique_filename

unique_filename

Returns a timestamped filename with *.mat appended

Contents

Example Usage

filename = unique_filename('filename');
70 | 
function [ filePath ] = unique_filename(file_name)
71 |     date = datetime('now');
72 |     filePath = strcat(file_name, '_', num2str(date.Month), '_', num2str(date.Day), '_', num2str(date.Year), '_', num2str(date.Hour), '_', num2str(date.Minute), '_', num2str(date.Second), '.mat');
73 | end
74 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/graph/html/delta_disk_neighbors.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | delta_disk_neighbors

delta_disk_neighbors $\mathbf{R}^{2 \times N} \times \mathbf{Z}^{+} \times \mathbf{R} \to \mathbf{Z}^{+}$

Returns the agents within the 2-norm of the supplied agent

Contents

Example Usage

neighbors = delta_disk_neighbors(robot_poses, 1, 0.5);
 70 | 
function [ neighbors ] = delta_disk_neighbors(poses, agent, delta)
 71 | 
 72 |     N = size(poses, 2);
 73 |     agents = 1:N;
 74 |     agents(agent) = [];
 75 | 
 76 |     assert(agent<=N && agent>=1, 'Supplied agent (%i) must be between 1 and %i', agent, N);
 77 | 
 78 |     within_distance = arrayfun(@(x) norm(poses(1:2, x) - poses(1:2, agent)) <= delta, agents);
 79 |     neighbors = agents(find(within_distance));
 80 | end
 81 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/misc/html/generate_initial_conditions.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | generate_initial_conditions:

generate_initial_conditions: $\mathbf{Z}^{+} \to \mathbf{R}^{3 \times N}$

Returns a set of random poses distributed in the Robotarium workspace

Contents

Example Usage

initial_conditions = generate_initial_conditions(4);
 70 |

Implementation

function [ poses ] = generate_initial_conditions(N)
 71 | 
 72 |     poses = zeros(3, N);
 73 | 
 74 |     safetyRadius = 0.2;
 75 |     width = 1.15;
 76 |     height = 0.65;
 77 | 
 78 |     numX = floor(width / safetyRadius);
 79 |     numY = floor(height / safetyRadius);
 80 |     values = randperm(numX * numY, N);
 81 | 
 82 |     for i = 1:N
 83 |        [x, y] = ind2sub([numX numY], values(i));
 84 |        x = x*safetyRadius - (width/2);
 85 |        y = y*safetyRadius - (height/2);
 86 |        poses(1:2, i) = [x ; y];
 87 |     end
 88 | 
 89 |     poses(3, :) = (rand(1, N)*2*pi - pi);
 90 | end
 91 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/graph/html/randomGL.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | randomGL:

randomGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$

Returns a random grab laplacian with a specified number of verticies and edges

Contents

Example Usage

L = randomGL(5, 3);
 70 |

Implementation

function [ L ] = randomGL(v, e)
 71 | 
 72 |     L = tril(ones(v, v));
 73 | 
 74 |     %This works becuase I can't select diagonals
 75 |     potEdges = find(triu(L) == 0);
 76 |     sz = size(L);
 77 | 
 78 |     %Rest to zeros
 79 |     L = L - L;
 80 | 
 81 |     numEdges = min(e, length(potEdges));
 82 |     edgeIndices = randperm(length(potEdges), numEdges);
 83 | 
 84 |     for index = edgeIndices
 85 | 
 86 |        [i, j] = ind2sub(sz, potEdges(index));
 87 | 
 88 |         %Update adjacency relation
 89 |         L(i, j) = -1;
 90 |         L(j, i) = -1;
 91 | 
 92 |         %Update degree relation
 93 |         L(i, i) = L(i, i) + 1;
 94 |         L(j, j) = L(j, j) + 1;
 95 |     end
 96 | end
 97 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/barrier_certificates/create_uni_barrier_certificate2.m: -------------------------------------------------------------------------------- 1 | function [ uni_barrier_certificate ] = create_uni_barrier_certificate2(varargin) 2 | % CREATE_SI_BARRIER_CERTIFICATE Creates a unicycle barrier 3 | % certificate function to avoid collisions. 4 | % 5 | % Args: 6 | % BarrierGain, optional: How quickly robots can approach eachother 7 | % SafetyRadius, optional: How far apart centers of robots should 8 | % remain 9 | % ProjectionDistance, optional: How far ahead to project a virtual 10 | % single integrator 11 | % VelocityMagnitudeLimit, optional: The maximum velocity for the 12 | % virtual single integrator 13 | % 14 | % Returns: 15 | % A barrier certificate function (2xN, 3xN) -> 2xN representing the 16 | % barrier certificate 17 | % 18 | % CREATE_UNI_BARRIER_CERTIFICATE('BarrierGain', bg) 19 | % 20 | % CREATE_UNI_BARRIER_CERTIFICATE('SafetyRadius', sr) 21 | % 22 | % CREATE_UNI_BARRIER_CERTIFICATE('SafetyRadius', sr, 'BarrierGain', bg) 23 | % 24 | % Example: 25 | % bc = create_si_barrier_certificate('SafetyRadius', 0.2) 26 | % 27 | % Notes: 28 | % SafetyRadius should be a positive double 29 | % BarrierGain should be a positive double 30 | % In practice, the value for SafetyRadius should be a little more than double the 31 | % size of the robots. 32 | parser = inputParser; 33 | addOptional(parser, 'BarrierGain', 150); 34 | addOptional(parser, 'SafetyRadius', 0.12); 35 | addOptional(parser, 'ProjectionDistance', 0.05); 36 | addOptional(parser, 'BaseLength', 0.105); 37 | addOptional(parser, 'WheelRadius', 0.016); 38 | addOptional(parser, 'WheelVelocityLimit', 12.5); 39 | addOptional(parser, 'Disturbance', 5); 40 | addOptional(parser, 'MaxNumRobots', 30); 41 | addOptional(parser, 'MaxNumObstacles', 100); 42 | parse(parser, varargin{:}) 43 | 44 | opts = optimoptions(@quadprog,'Display', 'off', 'TolFun', 1e-5, 'TolCon', 1e-4); 45 | gamma = parser.Results.BarrierGain; 46 | safety_radius = parser.Results.SafetyRadius; 47 | projection_distance = parser.Results.ProjectionDistance; 48 | wheel_radius = parser.Results.WheelRadius; 49 | base_length = parser.Results.BaseLength; 50 | wheel_vel_limit = parser.Results.WheelVelocityLimit; 51 | d = parser.Results.Disturbance; 52 | max_num_robots = parser.Results.MaxNumRobots; 53 | max_num_obstacles = parser.Results.MaxNumObstacles; 54 | 55 | D = [wheel_radius/2, wheel_radius/2; -wheel_radius/base_length, wheel_radius/base_length]; 56 | L = [1,0;0,projection_distance] * D; 57 | disturb = [-d,-d,d,d;-d,d,d,-d]; 58 | num_disturbs = size(disturb, 2); 59 | % 60 | % max_num_constraints = (num_disturbs^2)*nchoosek(max_num_robots, 2) + max_num_robots*max_num_obstacles*num_disturbs + max_num_robots; 61 | 62 | max_num_constraints = nchoosek(max_num_robots, 2) + max_num_robots*max_num_obstacles; %+ max_num_robots; 63 | A = zeros(max_num_constraints, 2*max_num_robots); 64 | b = zeros(max_num_constraints, 1); 65 | 66 | Os = zeros(2,max_num_robots); 67 | ps = zeros(2,max_num_robots); 68 | Ms = zeros(2,2*max_num_robots); 69 | 70 | 71 | 72 | uni_barrier_certificate = @barrier_unicycle; 73 | 74 | 75 | function [ dxu, ret ] = barrier_unicycle(dxu, x, obstacles) 76 | % BARRIER_UNICYCLE The parameterized barrier function 77 | % 78 | % Args: 79 | % dxu: 2xN vector of unicycle control inputs 80 | % x: 3xN vector of unicycle states 81 | % obstacles: Optional 2xN vector of obtacle points. 82 | % 83 | % Returns: 84 | % A 2xN matrix of safe unicycle control inputs 85 | % 86 | % BARRIER_UNICYCLE(dxu, x) 87 | 88 | if nargin < 3 89 | obstacles = []; 90 | end 91 | 92 | num_robots = size(dxu, 2); 93 | num_obstacles = size(obstacles, 2); 94 | 95 | if(num_robots < 2) 96 | temp = 0; 97 | else 98 | temp = nchoosek(num_robots, 2); 99 | end 100 | 101 | %Generate constraints for barrier certificates based on the size of 102 | %the safety radius 103 | % num_constraints = (num_disturbs^2)*temp + num_robots*num_obstacles*num_disturbs + num_robots; 104 | num_constraints = temp + num_robots*num_obstacles; 105 | A(1:num_constraints, 1:2*num_robots) = 0; 106 | Os(1,1:num_robots) = cos(x(3, :)); 107 | Os(2,1:num_robots) = sin(x(3, :)); 108 | ps(:,1:num_robots) = x(1:2, :) + projection_distance*Os(:,1:num_robots); 109 | Ms(1,1:2:2*num_robots) = Os(1,1:num_robots); 110 | Ms(1,2:2:2*num_robots) = -projection_distance*Os(2,1:num_robots); 111 | Ms(2,2:2:2*num_robots) = projection_distance*Os(1,1:num_robots); 112 | Ms(2,1:2:2*num_robots) = Os(2,1:num_robots); 113 | ret = zeros(1, temp); 114 | 115 | count = 1; 116 | for i = 1:(num_robots-1) 117 | for j = (i+1):num_robots 118 | diff = ps(:, i) - ps(:, j); 119 | hs = sum(diff.^2,1) - safety_radius^2; 120 | 121 | h_dot_i = 2*(diff)'*Ms(:,2*i-1:2*i)*D; 122 | h_dot_j = -2*(diff)'*Ms(:,2*j-1:2*j)*D; 123 | A(count, (2*i-1):(2*i)) = h_dot_i; 124 | A(count, (2*j-1):(2*j)) = h_dot_j; 125 | b(count) = -gamma*hs.^3 - min(h_dot_i*disturb) - min(h_dot_j*disturb); %repmat(h_i_disturbs, num_disturbs, 1) + repelem(h_j_disturbs, num_disturbs, 1); 126 | ret(count) = hs; 127 | 128 | count = count + 1; 129 | end 130 | end 131 | 132 | if ~isempty(obstacles) 133 | % Do obstacles 134 | for i = 1:num_robots 135 | diffs = (ps(:, i) - obstacles)'; 136 | h = sum(diffs.^2, 2) - safety_radius^2; 137 | h_dot_i = 2*diffs*Ms(:,2*i-1:2*i)*D; 138 | A(count:count+num_obstacles-1,(2*i-1):(2*i)) = h_dot_i; 139 | b(count:count+num_obstacles-1) = -gamma*h.^3 - min(h_dot_i*disturb, [], 2); 140 | count = count + num_obstacles; 141 | end 142 | end 143 | 144 | 145 | %Solve QP program generated earlier 146 | L_all = kron(eye(num_robots), L); 147 | dxu = D \ dxu; % Convert user input to differential drive 148 | %dxu(1:2,1:4) 149 | vhat = reshape(dxu,2*num_robots,1); 150 | %disp('vhat') 151 | %vhat(1:4) 152 | H = 2*(L_all')*L_all; 153 | f = -2*vhat'*(L_all')*L_all; 154 | vnew = quadprog(H, double(f), -A(1:num_constraints,1:2*num_robots), -b(1:num_constraints), [], [], -wheel_vel_limit*ones(2*num_robots,1), wheel_vel_limit*ones(2*num_robots,1), [], opts); 155 | %disp('f') 156 | %f(1:4) 157 | %Set robot velocities to new velocities 158 | dxu = reshape(vnew, 2, num_robots); 159 | dxu = D*dxu; 160 | 161 | end 162 | end 163 | 164 | -------------------------------------------------------------------------------- /Robotarium.m: -------------------------------------------------------------------------------- 1 | classdef Robotarium < ARobotarium 2 | % Robotarium This object represents your communications with the 3 | % GRITSbots. 4 | % 5 | % THIS CLASS SHOULD NEVER BE MODIFIED 6 | 7 | properties (GetAccess = private, SetAccess = private) 8 | checked_poses_already = false % Whether GET_POSES has been checked this iteration 9 | called_step_already = true % Whether STEP has been called this iteration 10 | 11 | iteration = 0; % How many times STEP has been called 12 | errors = {}; % Accumulated errors for the simulation 13 | end 14 | 15 | methods 16 | function this = Robotarium(varargin) 17 | % ROBOTARIUM Initializes the object. 18 | % 19 | % ROBOTARIUM('NumberOfRobots', 4) creates a ROBOTARIUM 20 | % object with 4 robots. 21 | % 22 | % ROBOTARIUM('NumberOfRobots', 1, 'ShowFigure', false) 23 | % creates a ROBOTARIUM object with 1 robot and shows no 24 | % figure. 25 | % 26 | % Example: 27 | % r = Robotarium('NumberOfRobots', 10, 'ShowFigure', 28 | % true) 29 | % 30 | % Notes: 31 | % The option NumberOfRobots should be a positive integer. 32 | % The option ShowFigure should be a boolean value. 33 | % The option InitialConditions should be a 3 x 34 | % NumberOfRobots matrix of initial poses. 35 | 36 | parser = inputParser; 37 | 38 | parser.addParameter('NumberOfRobots', -1); 39 | parser.addParameter('ShowFigure', true); 40 | parser.addParameter('FigureHandle', []); 41 | parser.addParameter('InitialConditions', []); 42 | 43 | parse(parser, varargin{:}) 44 | 45 | % The input will be validated by ARobotarium 46 | this = this@ARobotarium(parser.Results.NumberOfRobots, ... 47 | parser.Results.ShowFigure, parser.Results.FigureHandle); 48 | 49 | initial_conditions = parser.Results.InitialConditions; 50 | 51 | if(isempty(initial_conditions)) 52 | initial_conditions = generate_initial_conditions(this.number_of_robots, ... 53 | 'Spacing', 1.5*this.robot_diameter, ... 54 | 'Width', this.boundaries(2)-this.boundaries(1)-this.robot_diameter, ... 55 | 'Height', this.boundaries(4)-this.boundaries(3))-this.robot_diameter; 56 | end 57 | 58 | assert(all(size(initial_conditions) == [3, this.number_of_robots]), 'Initial conditions must be 3 x %i', this.number_of_robots); 59 | 60 | % Call initialize during initialization 61 | this.initialize(initial_conditions); 62 | end 63 | 64 | function poses = get_poses(this) 65 | % GET_POSES Returns the current poses of the robots 66 | % 67 | % GET_POSES() returns a 3 x NUMBER_OF_ROBOTS matrix of poses 68 | % 69 | % Example: 70 | % x = this.get_poses() 71 | % 72 | % Notes: 73 | % This function should only be called once per call of 74 | % STEP 75 | 76 | assert(~this.checked_poses_already, 'Can only call get_poses() once per call of step()!'); 77 | 78 | poses = this.poses; 79 | 80 | %Make sure it's only called once per iteration 81 | this.checked_poses_already = true; 82 | this.called_step_already = false; 83 | end 84 | 85 | function initialize(this, initial_conditions) 86 | this.poses = initial_conditions; 87 | end 88 | 89 | function step(this) 90 | % STEP Steps the simulation, updating poses of robots and 91 | % checking for errors 92 | % 93 | % STEP() 94 | % 95 | % Example: 96 | % object.step() 97 | % 98 | % Notes: 99 | % Should be called everytime GET_POSES is called 100 | 101 | assert(~this.called_step_already, 'Make sure you call get_poses before calling step!'); 102 | 103 | %Vectorize update to states 104 | i = 1:this.number_of_robots; 105 | 106 | % Validate before thresholding velocities 107 | es = this.validate(); 108 | this.errors = [this.errors, es]; 109 | this.iteration = this.iteration + 1; 110 | 111 | this.velocities = this.threshold(this.velocities); 112 | 113 | %Update velocities using unicycle dynamics 114 | temp = this.time_step.*this.velocities(1, i); 115 | this.poses(1, i) = this.poses(1, i) + temp.*cos(this.poses(3, i)); 116 | this.poses(2, i) = this.poses(2, i) + temp.*sin(this.poses(3, i)); 117 | this.poses(3, i) = this.poses(3, i) + this.time_step.*this.velocities(2, i); 118 | 119 | %Ensure that the orientations are in the right range 120 | this.poses(3, i) = atan2(sin(this.poses(3, i)), cos(this.poses(3, i))); 121 | 122 | %Allow getting of poses again 123 | this.checked_poses_already = false; 124 | this.called_step_already = true; 125 | 126 | if(this.show_figure) 127 | this.draw_robots(); 128 | uistack([this.robot_handle{:}],'top'); 129 | end 130 | end 131 | 132 | function debug(this) 133 | num_errors = 3; 134 | count = zeros(1, num_errors); 135 | for i = 1:numel(this.errors) 136 | count(this.errors{i}) = count(this.errors{i}) + 1; 137 | end 138 | 139 | fprintf('Your simulation took approximately %.2f real seconds.\n', this.iteration*this.time_step); 140 | 141 | error_strings = cell(1, num_errors); 142 | error_strings{RobotariumError.RobotsTooClose} = 'time steps where robots were too close (potential collision)'; 143 | error_strings{RobotariumError.RobotsOutsideBoundaries} = 'time steps where robots were outside boundaries'; 144 | error_strings{RobotariumError.ExceededActuatorLimits} = 'staged velocity commands exceeded actuator limits'; 145 | 146 | fprintf('Error count for current simulation:\n'); 147 | print_error = @(x) fprintf('\t Simulation had %i %s errors.\n', count(x), error_strings{x}); 148 | print_error(RobotariumError.RobotsTooClose) 149 | print_error(RobotariumError.RobotsOutsideBoundaries); 150 | print_error(RobotariumError.ExceededActuatorLimits); 151 | 152 | if(isempty(this.errors)) 153 | fprintf('No errors in your simulation! Acceptance of experiment likely.\n') 154 | else 155 | fprintf('Please fix the noted errors in your simulation; otherwise, your experiment may be rejected.\n'); 156 | end 157 | end 158 | end 159 | end 160 | -------------------------------------------------------------------------------- /utilities/graph/html/random_connectedGL.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | random_connectedGL:

random_connectedGL: $\mathbf{Z}^{+} \to \mathbf{Z}^{N \times N}$

Returns a random, connected GL with v verticies and (v-1) + e edges

Contents

Example Usage

L = random_connectedGL(4, 3);
 70 |

Implementation

function [ L ] = random_connectedGL(v, e)
 71 | 
 72 |     L = zeros(v, v);
 73 | 
 74 |     for i = 2:v
 75 | 
 76 |         edge = randi(i-1, 1, 1);
 77 | 
 78 |         %Update adjancency relations
 79 |         L(i, edge) = -1;
 80 |         L(edge, i) = -1;
 81 | 
 82 |         %Update node degrees
 83 |         L(i, i) = L(i, i) + 1;
 84 |         L(edge, edge) = L(edge, edge) + 1;
 85 |     end
 86 | 
 87 |     %This works becuase all nodes have at least 1 degree.  Choose from only
 88 |     %upper diagonal portion
 89 |     potEdges = find(triu(bsxfun(@xor, L, 1)) == 1);
 90 |     sz = size(L);
 91 | 
 92 |     numEdges = min(e, length(potEdges));
 93 | 
 94 |     if (numEdges <= 0)
 95 |        return
 96 |     end
 97 | 
 98 |     %Indices of randomly chosen extra edges
 99 |     edgeIndices = randperm(length(potEdges), numEdges);
100 | 
101 |     for index = edgeIndices
102 | 
103 |        [i, j] = ind2sub(sz, potEdges(index));
104 | 
105 |         %Update adjacency relation
106 |         L(i, j) = -1;
107 |         L(j, i) = -1;
108 | 
109 |         %Update degree relation
110 |         L(i, i) = L(i, i) + 1;
111 |         L(j, j) = L(j, j) + 1;
112 |     end
113 | end
114 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping2.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | create_si_to_uni_mapping2

create_si_to_uni_mapping2

Returns a mapping $\left( f: \mathbf{R}^{2 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ from single-integrator to unicycle dynamics

Contents

Detailed Description

Example Usage

% si === single integrator
 70 | si_to_uni_dynamics = create_si_to_uni_mapping2('LinearVelocityGain',
 71 | 1, 'AngularVelocityLimit', pi)
 72 | dx_si = si_algorithm(si_states)
 73 | dx_uni = si_to_uni_dynamics(dx_si, states)
 74 |

Implementation

function [si_to_uni_dyn] = create_si_to_uni_mapping2(varargin)
 75 | 
 76 |     parser = inputParser;
 77 |     addOptional(parser, 'LinearVelocityGain', 1);
 78 |     addOptional(parser, 'AngularVelocityLimit', pi);
 79 |     parse(parser, varargin{:});
 80 | 
 81 |     lvg = parser.Results.LinearVelocityGain;
 82 |     avl = parser.Results.AngularVelocityLimit;
 83 | 
 84 |     si_to_uni_dyn = @si_to_uni;
 85 |     % A mapping from si -> uni dynamics.  THis is more of a
 86 |     % projection-based method.  Though, it's definitely similar to the
 87 |     % NIDs.
 88 |     function dxu = si_to_uni(dxi, states)
 89 |         N = size(dxi, 2);
 90 |         dxu = zeros(2, N);
 91 |         for i = 1:N
 92 |             dxu(1, i) = lvg * [cos(states(3, i)) sin(states(3, i))] * dxi(:, i);
 93 |             %Normalizing the output of atan2 to between -kw and kw
 94 |             dxu(2, i) = avl * atan2([-sin(states(3, i)) cos(states(3, i))]*dxi(:, i), ...
 95 |                                   [cos(states(3, i)) sin(states(3, i))]*dxi(:, i))/(pi/2);
 96 |         end
 97 |     end
 98 | end
 99 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/barrier_certificates/create_uni_barrier_certificate_with_boundary.m: -------------------------------------------------------------------------------- 1 | function [ uni_barrier_certificate ] = create_uni_barrier_certificate_with_boundary(varargin) 2 | % CREATE_SI_BARRIER_CERTIFICATE Creates a unicycle barrier 3 | % certificate function to avoid collisions. 4 | % 5 | % Args: 6 | % BarrierGain, optional: How quickly robots can approach eachother 7 | % SafetyRadius, optional: How far apart centers of robots should 8 | % remain 9 | % ProjectionDistance, optional: How far ahead to project a virtual 10 | % single integrator 11 | % VelocityMagnitudeLimit, optional: The maximum velocity for the 12 | % virtual single integrator 13 | % 14 | % Returns: 15 | % A barrier certificate function (2xN, 3xN) -> 2xN representing the 16 | % barrier certificate 17 | % 18 | % CREATE_UNI_BARRIER_CERTIFICATE('BarrierGain', bg) 19 | % 20 | % CREATE_UNI_BARRIER_CERTIFICATE('SafetyRadius', sr) 21 | % 22 | % CREATE_UNI_BARRIER_CERTIFICATE('SafetyRadius', sr, 'BarrierGain', bg) 23 | % 24 | % Example: 25 | % bc = create_si_barrier_certificate('SafetyRadius', 0.2) 26 | % 27 | % Notes: 28 | % SafetyRadius should be a positive double 29 | % BarrierGain should be a positive double 30 | % In practice, the value for SafetyRadius should be a little more than double the 31 | % size of the robots. 32 | 33 | parser = inputParser; 34 | addOptional(parser, 'BarrierGain', 150); 35 | addOptional(parser, 'SafetyRadius', 0.12); 36 | addOptional(parser, 'ProjectionDistance', 0.03); 37 | addOptional(parser, 'BaseLength', 0.105); 38 | addOptional(parser, 'WheelRadius', 0.016); 39 | addOptional(parser, 'WheelVelocityLimit', 12.5); 40 | addOptional(parser, 'Disturbance', 2); 41 | addOptional(parser, 'MaxNumRobots', 30); 42 | addOptional(parser, 'MaxObstacles', 50); 43 | addOptional(parser, 'MaxNumBoundaryPoints', 4); 44 | addOptional(parser, 'BoundaryPoints', [-1.6 1.6 -1.0 1.0]); 45 | parse(parser, varargin{:}) 46 | 47 | opts = optimoptions(@quadprog,'Display', 'off', 'TolFun', 1e-5, 'TolCon', 1e-4); 48 | gamma = parser.Results.BarrierGain; 49 | safety_radius = parser.Results.SafetyRadius; 50 | projection_distance = parser.Results.ProjectionDistance; 51 | wheel_radius = parser.Results.WheelRadius; 52 | base_length = parser.Results.BaseLength; 53 | wheel_vel_limit = parser.Results.WheelVelocityLimit; 54 | d = parser.Results.Disturbance; 55 | max_num_robots = parser.Results.MaxNumRobots; 56 | max_num_boundaries = parser.Results.MaxNumBoundaryPoints; 57 | max_num_obstacles = parser.Results.MaxObstacles; 58 | boundary_points = parser.Results.BoundaryPoints; 59 | 60 | %Check given boundary points 61 | assert(length(boundary_points)==4, "Boundary points must represent a rectangle.") 62 | assert(boundary_points(2) > boundary_points(1), "Difference between x coordinates of defined rectangular boundary points must be positive.") 63 | assert(boundary_points(4) > boundary_points(3), "Difference between y coordinates of defined rectangular boundary points must be positive.") 64 | 65 | D = [wheel_radius/2, wheel_radius/2; -wheel_radius/base_length, wheel_radius/base_length]; 66 | L = [1,0;0,projection_distance] * D; 67 | disturb = [-d,-d,d,d;-d,d,d,-d]; 68 | num_disturbs = size(disturb, 2); 69 | % 70 | % max_num_constraints = (num_disturbs^2)*nchoosek(max_num_robots, 2) + max_num_robots*max_num_obstacles*num_disturbs + max_num_robots; 71 | 72 | max_num_constraints = nchoosek(max_num_robots, 2) + max_num_robots*max_num_boundaries + max_num_robots*max_num_obstacles; %+ max_num_robots; 73 | A = zeros(max_num_constraints, 2*max_num_robots); 74 | b = zeros(max_num_constraints, 1); 75 | 76 | Os = zeros(2,max_num_robots); 77 | ps = zeros(2,max_num_robots); 78 | Ms = zeros(2,2*max_num_robots); 79 | 80 | 81 | 82 | uni_barrier_certificate = @barrier_unicycle; 83 | 84 | 85 | function [ dxu, ret ] = barrier_unicycle(dxu, x, obstacles) 86 | % BARRIER_UNICYCLE The parameterized barrier function 87 | % 88 | % Args: 89 | % dxu: 2xN vector of unicycle control inputs 90 | % x: 3xN vector of unicycle states 91 | % obstacles: Optional 2xN vector of obtacle points. 92 | % 93 | % Returns: 94 | % A 2xN matrix of safe unicycle control inputs 95 | % 96 | % BARRIER_UNICYCLE(dxu, x) 97 | 98 | if nargin < 3 99 | obstacles = []; 100 | end 101 | 102 | num_robots = size(dxu, 2); 103 | num_obstacles = size(obstacles, 2); 104 | 105 | if(num_robots < 2) 106 | temp = 0; 107 | else 108 | temp = nchoosek(num_robots, 2); 109 | end 110 | 111 | %Generate constraints for barrier certificates based on the size of 112 | %the safety radius 113 | % num_constraints = (num_disturbs^2)*temp + num_robots*num_obstacles*num_disturbs + num_robots; 114 | num_constraints = temp + num_robots*num_obstacles + 4*num_robots; 115 | A(1:num_constraints, 1:2*num_robots) = 0; 116 | Os(1,1:num_robots) = cos(x(3, :)); 117 | Os(2,1:num_robots) = sin(x(3, :)); 118 | ps(:,1:num_robots) = x(1:2, :) + projection_distance*Os(:,1:num_robots); 119 | Ms(1,1:2:2*num_robots) = Os(1,1:num_robots); 120 | Ms(1,2:2:2*num_robots) = -projection_distance*Os(2,1:num_robots); 121 | Ms(2,2:2:2*num_robots) = projection_distance*Os(1,1:num_robots); 122 | Ms(2,1:2:2*num_robots) = Os(2,1:num_robots); 123 | ret = zeros(1, temp); 124 | 125 | count = 1; 126 | for i = 1:(num_robots-1) 127 | for j = (i+1):num_robots 128 | diff = ps(:, i) - ps(:, j); 129 | hs = sum(diff.^2,1) - safety_radius^2; 130 | 131 | h_dot_i = 2*(diff)'*Ms(:,2*i-1:2*i)*D; 132 | h_dot_j = -2*(diff)'*Ms(:,2*j-1:2*j)*D; 133 | A(count, (2*i-1):(2*i)) = h_dot_i; 134 | A(count, (2*j-1):(2*j)) = h_dot_j; 135 | b(count) = -gamma*hs.^3 - min(h_dot_i*disturb) - min(h_dot_j*disturb); %repmat(h_i_disturbs, num_disturbs, 1) + repelem(h_j_disturbs, num_disturbs, 1); 136 | ret(count) = hs; 137 | 138 | count = count + 1; 139 | end 140 | end 141 | 142 | if ~isempty(obstacles) 143 | % Do obstacles 144 | for i = 1:num_robots 145 | diffs = (ps(:, i) - obstacles)'; 146 | h = sum(diffs.^2, 2) - safety_radius^2; 147 | h_dot_i = 2*diffs*Ms(:,2*i-1:2*i)*D; 148 | A(count:count+num_obstacles-1,(2*i-1):(2*i)) = h_dot_i; 149 | b(count:count+num_obstacles-1) = -gamma*h.^3 - min(h_dot_i*disturb, [], 2); 150 | count = count + num_obstacles; 151 | end 152 | end 153 | 154 | for i = 1:num_robots 155 | %Pos Y 156 | A(count,(2*i-1):(2*i)) = -Ms(2,(2*i-1):(2*i))*D; 157 | b(count) = -0.4*gamma*(boundary_points(4) - safety_radius/2 - ps(2,i))^3; 158 | count = count + 1; 159 | 160 | %Neg Y 161 | A(count,(2*i-1):(2*i)) = Ms(2,(2*i-1):(2*i))*D; 162 | b(count) = -0.4*gamma*(-boundary_points(3) - safety_radius/2 + ps(2,i))^3; 163 | count = count + 1; 164 | 165 | %Pos X 166 | A(count,(2*i-1):(2*i)) = -Ms(1,(2*i-1):(2*i))*D; 167 | b(count) = -0.4*gamma*(boundary_points(2) - safety_radius/2 - ps(1,i))^3; 168 | count = count + 1; 169 | 170 | %Neg X 171 | A(count,(2*i-1):(2*i)) = Ms(1,(2*i-1):(2*i))*D; 172 | b(count) = -0.4*gamma*(-boundary_points(1) - safety_radius/2 + ps(1,i))^3; 173 | count = count + 1; 174 | end 175 | 176 | %Solve QP program generated earlier 177 | L_all = kron(eye(num_robots), L); 178 | dxu = D \ dxu; % Convert user input to differential drive 179 | %dxu(1:2,1:4) 180 | vhat = reshape(dxu,2*num_robots,1); 181 | %disp('vhat') 182 | %vhat(1:4) 183 | H = 2*(L_all')*L_all; 184 | f = -2*vhat'*(L_all')*L_all; 185 | vnew = quadprog(H, double(f), -A(1:num_constraints,1:2*num_robots), -b(1:num_constraints), [], [], -wheel_vel_limit*ones(2*num_robots,1), wheel_vel_limit*ones(2*num_robots,1), [], opts); 186 | %disp('f') 187 | %f(1:4) 188 | %Set robot velocities to new velocities 189 | dxu = reshape(vnew, 2, num_robots); 190 | dxu = D*dxu; 191 | 192 | end 193 | end 194 | 195 | -------------------------------------------------------------------------------- /utilities/misc/html/create_is_initialized.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | create_is_initialized

create_is_initialized

Creates a function to check for initialization. The function returns whether all the agents have been initialized and those that have already finished.

Contents

Detailed Description

Example Usage

initialization_checker = create_is_initialized('PositionError', 0.l,
 70 | 'RotationError', 0.01)
 71 | [all_initialized, done_idxs] = initialization_checker(robot_poses,
 72 | desired_poses)
 73 |

Implementation

function [ created_is_initialized ] = create_is_initialized(varargin)
 74 | 
 75 |     parser = inputParser;
 76 |     parser.addParameter('PositionError', 0.01);
 77 |     parser.addParameter('RotationError', 0.5);
 78 |     parse(parser, varargin{:});
 79 | 
 80 |     position_error = parser.Results.PositionError;
 81 |     rotation_error = parser.Results.RotationError;
 82 | 
 83 |     created_is_initialized = @(states, initial_conditions) is_initialized(states, initial_conditions);
 84 | 
 85 |     function [done, idxs] = is_initialized(states, initial_conditions)
 86 | 
 87 |         [M, N] = size(states);
 88 |         [M_ic, N_ic] = size(initial_conditions);
 89 | 
 90 |         assert(M==3, 'Dimension of states (%i) must be 3', M);
 91 |         assert(M_ic==3, 'Dimension of conditions (%i) must be 3', M_ic);
 92 |         assert(N_ic==N, 'Column dimension of states (%i) and conditions (%i) must be the same', N, N_ic);
 93 | 
 94 |         wrap = @(x) atan2(sin(x), cos(x));
 95 |         f = @(x, ic) (norm(x(1:2) - ic(1:2)) <= position_error) && (abs(wrap(x(3) - ic(3))) <= rotation_error);
 96 |         result = arrayfun(@(x) f(states(:, x), initial_conditions(:, x)), 1:N);
 97 | 
 98 |         [done, idxs] = find(result == 1);
 99 |         done = (length(done) == N);
100 |     end
101 | end
102 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /utilities/transformations/html/create_uni_to_si_mapping.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | create_uni_to_si_mapping

create_uni_to_si_mapping

Returns a mapping from unicycle to single-integrator dynamics $\left( f: \mathbf{R}^{2 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ and a mapping between their states $\left(f: \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.

Contents

Example Usage

% si === single integrator
 70 | [uni_to_si_dyn, si_to_uni_states] = create_uni_to_si_mapping('ProjectionDistance',
 71 | 0.05)
 72 | uni_states = si_to_uni_states(robot_poses)
 73 | dx_uni = uni_algorithm(uni_states)
 74 | dx_si = uni_to_si_dyn(dx_uni, states)
 75 |

Implementation

function [uni_to_si_dyn, si_to_uni_states] = create_uni_to_si_mapping(varargin)
 76 | 
 77 |     parser = inputParser;
 78 |     addOptional(parser, 'ProjectionDistance', 0.05);
 79 |     parse(parser, varargin{:});
 80 | 
 81 |     projection_distance = parser.Results.ProjectionDistance;
 82 | 
 83 |     uni_to_si_dyn = @uni_to_si;
 84 |     si_to_uni_states = @si_to_uni_states_;
 85 | 
 86 |     T = [1 0; 0 projection_distance];
 87 |     % First mapping from SI -> unicycle.  Keeps the projected SI system at
 88 |     % a fixed distance from the unicycle model
 89 |     function dxi = uni_to_si(dxu, states)
 90 |         N = size(dxu, 2);
 91 |         dxi = zeros(2, N);
 92 |         for i = 1:N
 93 |             dxi(:, i) = [cos(states(3, i)) -sin(states(3, i)); sin(states(3, i)) cos(states(3,i))] * T * dxu(:, i);
 94 |         end
 95 |     end
 96 | 
 97 |     % Projects the single-integrator system a distance in front of the
 98 |     % unicycle system
 99 |     function xi = si_to_uni_states_(uni_states, si_states)
100 |        xi = si_states(1:2, :) - projection_distance*[cos(uni_states(3, :)) ; sin(uni_states(3, :))];
101 |     end
102 | end
103 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /examples/go_to_goal/html/go_to_pose.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | go_to_pose
%Initializing the agents to random positions with barrier certificates
 70 | %and data plotting.  This script shows how to initialize robots to a
 71 | %particular pose
 72 | %Paul Glotfelter
 73 | %3/24/2016
 74 | 
 75 | % Get Robotarium object used to communicate with the robots/simulator
 76 | rb = RobotariumBuilder();
 77 | 
 78 | % Get the number of available agents from the Robotarium.  We don't need a
 79 | % specific value for this algorithm
 80 | N = rb.get_available_agents();
 81 | 
 82 | % Set the number of agents and whether we would like to save data.  Then,
 83 | % build the Robotarium simulator object!
 84 | r = rb.set_number_of_agents(N).set_save_data(false).build();
 85 | 
 86 | % Initialize x so that we don't run into problems later.  This isn't always
 87 | % necessary
 88 | x = r.get_poses();
 89 | r.step();
 90 | 
 91 | % Set some parameters for use with the barrier certificates.  We don't want
 92 | % our agents to collide
 93 | 
 94 | % Create a barrier certificate for use with the above parameters
 95 | unicycle_barrier_certificate = create_uni_barrier_certificate('SafetyRadius', 0.06, ...
 96 |     'ProjectionDistance', 0.03);
 97 | 
 98 | %Get randomized initial conditions in the robotarium arena
 99 | initial_conditions = generate_initial_conditions(N, 'Width', r.boundaries(2), 'Height', r.boundaries(4), 'Spacing', 0.2);
100 | 
101 | args = {'PositionError', 0.01, 'RotationError', 0.1};
102 | init_checker = create_is_initialized(args{:});
103 | automatic_parker = create_automatic_parking_controller(args{:});
104 | 
105 | while(~init_checker(x, initial_conditions))
106 | 
107 |     x = r.get_poses();
108 |     dxu = automatic_parker(x, initial_conditions);
109 |     dxu = unicycle_barrier_certificate(dxu, x);
110 | 
111 |     r.set_velocities(1:N, dxu);
112 |     r.step();
113 | end
114 | 
115 | % Though we didn't save any data, we still should call r.call_at_scripts_end() after our
116 | % experiment is over!
117 | r.call_at_scripts_end();
118 |


Published with MATLAB® R2016b

-------------------------------------------------------------------------------- /utilities/transformations/html/create_si_to_uni_mapping.html: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | create_si_to_uni_Mapping

create_si_to_uni_Mapping

Returns a mapping from single-integrator to unicycle dynamics $\left( f: \mathbf{R}^{2 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ and a mapping between their states $\left(f: \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N} \right)$ Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.

Contents

Detailed Description

Example Usage

% si === single integrator
 70 | [si_to_uni_dynamics, uni_to_si_states] = create_si_to_uni_mapping('ProjectionDistance',
 71 | 0.05)
 72 | si_states = uni_to_si_states(robot_poses)
 73 | dx_si = si_algorithm(si_states)
 74 | dx_uni = si_to_uni_dynamics(dx_si, states)
 75 |

Implementation

function [si_to_uni_dyn, uni_to_si_states] = create_si_to_uni_mapping(varargin)
 76 | 
 77 |     parser = inputParser;
 78 |     addOptional(parser, 'ProjectionDistance', 0.05);
 79 |     parse(parser, varargin{:});
 80 | 
 81 |     projection_distance = parser.Results.ProjectionDistance;
 82 | 
 83 |     si_to_uni_dyn = @si_to_uni;
 84 |     uni_to_si_states = @uni_to_si_states_;
 85 | 
 86 |     T = [1 0; 0 1/projection_distance];
 87 |     % First mapping from SI -> unicycle.  Keeps the projected SI system at
 88 |     % a fixed distance from the unicycle model
 89 |     function dxu = si_to_uni(dxi, states)
 90 |         N = size(dxi, 2);
 91 |         dxu = zeros(2, N);
 92 |         for i = 1:N
 93 |             dxu(:, i) = T * [cos(states(3, i)) sin(states(3, i)); -sin(states(3, i)) cos(states(3,i))] * dxi(:, i);
 94 |         end
 95 |     end
 96 | 
 97 |     % Projects the single-integrator system a distance in front of the
 98 |     % unicycle system
 99 |     function xi = uni_to_si_states_(states)
100 |        xi = states(1:2, :) + projection_distance*[cos(states(3, :)) ; sin(states(3, :))];
101 |     end
102 | end
103 |


Published with MATLAB® R2015a

-------------------------------------------------------------------------------- /examples/plotting/leader_follower_with_plotting.m: -------------------------------------------------------------------------------- 1 | % Same script as leader_follower.m but with additional plotting routines. 2 | % In this experiment edges between robots will be plotted as well as robot 3 | % labels and leader goal locations. 4 | 5 | % Sean Wilson 6 | % 07/2019 7 | 8 | %% Experiment Constants 9 | 10 | %Run the simulation for a specific number of iterations 11 | iterations = 5000; 12 | 13 | %% Set up the Robotarium object 14 | 15 | N = 4; 16 | initial_positions = generate_initial_conditions(N, 'Width', 1, 'Height', 1, 'Spacing', 0.5); 17 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true, 'InitialConditions', initial_positions); 18 | 19 | %% Create the desired Laplacian 20 | 21 | %Graph laplacian 22 | followers = -completeGL(N-1); 23 | L = zeros(N, N); 24 | L(2:N, 2:N) = followers; 25 | L(2, 2) = L(2, 2) + 1; 26 | L(2, 1) = -1; 27 | 28 | %Initialize velocity vector 29 | dxi = zeros(2, N); 30 | 31 | %State for leader 32 | state = 1; 33 | 34 | % These are gains for our formation control algorithm 35 | formation_control_gain = 5; 36 | desired_distance = 0.3; 37 | 38 | %% Grab tools we need to convert from single-integrator to unicycle dynamics 39 | 40 | % Single-integrator -> unicycle dynamics mapping 41 | si_to_uni_dyn = create_si_to_uni_dynamics('LinearVelocityGain', 0.8); 42 | % Single-integrator barrier certificates 43 | uni_barrier_cert = create_uni_barrier_certificate_with_boundary(); 44 | % Single-integrator position controller 45 | leader_controller = create_si_position_controller('XVelocityGain', 0.8, 'YVelocityGain', 0.8, 'VelocityMagnitudeLimit', 0.08); 46 | 47 | waypoints = [-1 0.8; -1 -0.8; 1 -0.8; 1 0.8]'; 48 | close_enough = 0.03; 49 | 50 | %% Plotting Setup 51 | 52 | % Color Vector for Plotting 53 | % Note the Robotarium MATLAB instance runs in a docker container which will 54 | % produce the same rng value every time unless seeded by the user. 55 | CM = ['k','b','r','g']; 56 | 57 | %Marker, font, and line sizes 58 | marker_size_goal = determine_marker_size(r, 0.20); 59 | font_size = determine_font_size(r, 0.05); 60 | line_width = 5; 61 | 62 | % Create goal text and markers. 63 | for i = 1:length(waypoints) 64 | % Text with goal identification 65 | goal_caption = sprintf('G%d', i); 66 | % Plot colored square for goal location. 67 | g(i) = plot(waypoints(1,i), waypoints(2,i),'s','MarkerSize',marker_size_goal,'LineWidth',line_width,'Color',CM(i)); 68 | % Plot the goal identification text inside the goal location 69 | goal_labels{i} = text(waypoints(1,i)-0.05, waypoints(2,i), goal_caption, 'FontSize', font_size, 'FontWeight', 'bold'); 70 | end 71 | 72 | % Plot graph connections 73 | %Need location of robots 74 | x=r.get_poses(); 75 | 76 | % Follower connections to each other 77 | [rows, cols] = find(L == 1); 78 | 79 | %Only considering half due to symmetric nature 80 | for k = 1:length(rows)/2+1 81 | lf(k) = line([x(1,rows(k)), x(1,cols(k))],[x(2,rows(k)), x(2,cols(k))], 'LineWidth', line_width, 'Color', 'b'); 82 | end 83 | 84 | % Leader connection assuming only connection between first and second 85 | % robot. 86 | ll = line([x(1,1), x(1,2)],[x(2,1), x(2,2)], 'LineWidth', line_width, 'Color', 'r'); 87 | 88 | % Follower plot setup 89 | for j = 1:N-1 90 | % Text for robot identification 91 | follower_caption{j} = sprintf('Follower Robot %d', j); 92 | % Plot the robot label text 93 | follower_labels{j} = text(500, 500, follower_caption{j}, 'FontSize', font_size, 'FontWeight', 'bold'); 94 | end 95 | 96 | %Leader plot setup 97 | leader_label = text(500, 500, 'Leader Robot', 'FontSize', font_size, 'FontWeight', 'bold', 'Color', 'r'); 98 | 99 | r.step(); 100 | 101 | for t = 1:iterations 102 | 103 | % Retrieve the most recent poses from the Robotarium. The time delay is 104 | % approximately 0.033 seconds 105 | x = r.get_poses(); 106 | 107 | %% Algorithm 108 | 109 | for i = 2:N 110 | 111 | %Zero velocity and get the topological neighbors of agent i 112 | dxi(:, i) = [0 ; 0]; 113 | 114 | neighbors = topological_neighbors(L, i); 115 | 116 | for j = neighbors 117 | dxi(:, i) = dxi(:, i) + ... 118 | formation_control_gain*(norm(x(1:2, j) - x(1:2, i))^2 - desired_distance^2)*(x(1:2, j) - x(1:2, i)); 119 | end 120 | end 121 | 122 | %% Make the leader travel between waypoints 123 | 124 | waypoint = waypoints(:, state); 125 | 126 | switch state 127 | case 1 128 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 129 | if(norm(x(1:2, 1) - waypoint) < close_enough) 130 | state = 2; 131 | end 132 | case 2 133 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 134 | if(norm(x(1:2, 1) - waypoint) < close_enough) 135 | state = 3; 136 | end 137 | case 3 138 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 139 | if(norm(x(1:2, 1) - waypoint) < close_enough) 140 | state = 4; 141 | end 142 | case 4 143 | dxi(:, 1) = leader_controller(x(1:2, 1), waypoint); 144 | if(norm(x(1:2, 1) - waypoint) < close_enough) 145 | state = 1; 146 | end 147 | end 148 | 149 | 150 | %% Avoid actuator errors 151 | 152 | % To avoid errors, we need to threshold dxi 153 | norms = arrayfun(@(x) norm(dxi(:, x)), 1:N); 154 | threshold = 3/4*r.max_linear_velocity; 155 | to_thresh = norms > threshold; 156 | to_thresh(1) = 0; 157 | dxi(:, to_thresh) = threshold*dxi(:, to_thresh)./norms(to_thresh); 158 | 159 | %% Use barrier certificate and convert to unicycle dynamics 160 | dxu = si_to_uni_dyn(dxi, x); 161 | dxu = uni_barrier_cert(dxu, x); 162 | 163 | %% Send velocities to agents 164 | 165 | %Set velocities 166 | r.set_velocities(1:N, dxu); 167 | 168 | %% Update Plot Handles 169 | 170 | %Update position of labels for followers 171 | for q = 1:N-1 172 | follower_labels{q}.Position = x(1:2, q+1) + [-0.15;0.15]; 173 | end 174 | 175 | %Update position of graph connection lines 176 | for m = 1:length(rows)/2+1 177 | lf(m).XData = [x(1,rows(m)), x(1,cols(m))]; 178 | lf(m).YData = [x(2,rows(m)), x(2,cols(m))]; 179 | end 180 | 181 | %Update position of label and graph connection for leader 182 | leader_label.Position = x(1:2, 1) + [-0.15;0.15]; 183 | ll.XData = [x(1,1), x(1,2)]; 184 | ll.YData = [x(2,1), x(2,2)]; 185 | 186 | % Resize Marker Sizes (In case user changes simulated figure window 187 | % size, this is unnecessary in submission as the figure window 188 | % does not change size). 189 | 190 | marker_size_goal = num2cell(ones(1,length(waypoints))*determine_marker_size(r, 0.20)); 191 | [g.MarkerSize] = marker_size_goal{:}; 192 | font_size = determine_font_size(r, 0.05); 193 | leader_label.FontSize = font_size; 194 | 195 | for n = 1:N 196 | % Have to update font in loop for some conversion reasons. 197 | % Again this is unnecessary when submitting as the figure 198 | % window does not change size when deployed on the Robotarium. 199 | follower_labels{n}.FontSize = font_size; 200 | goal_labels{n}.FontSize = font_size; 201 | end 202 | 203 | %Iterate experiment 204 | r.step(); 205 | end 206 | 207 | % We can call this function to debug our experiment! Fix all the errors 208 | % before submitting to maximize the chance that your experiment runs 209 | % successfully. 210 | r.debug(); 211 | 212 | %% Helper Functions 213 | 214 | % Marker Size Helper Function to scale size with figure window 215 | % Input: robotarium instance, desired size of the marker in meters 216 | function marker_size = determine_marker_size(robotarium_instance, marker_size_meters) 217 | 218 | % Get the size of the robotarium figure window in pixels 219 | curunits = get(robotarium_instance.figure_handle, 'Units'); 220 | set(robotarium_instance.figure_handle, 'Units', 'Points'); 221 | cursize = get(robotarium_instance.figure_handle, 'Position'); 222 | set(robotarium_instance.figure_handle, 'Units', curunits); 223 | 224 | % Determine the ratio of the robot size to the x-axis (the axis are 225 | % normalized so you could do this with y and figure height as well). 226 | marker_ratio = (marker_size_meters)/(robotarium_instance.boundaries(2) -... 227 | robotarium_instance.boundaries(1)); 228 | 229 | % Determine the marker size in points so it fits the window. cursize(3) is 230 | % the width of the figure window in pixels. (the axis are 231 | % normalized so you could do this with y and figure height as well). 232 | marker_size = cursize(3) * marker_ratio; 233 | 234 | end 235 | 236 | % Font Size Helper Function to scale size with figure window 237 | % Input: robotarium instance, desired height of the font in meters 238 | function font_size = determine_font_size(robotarium_instance, font_height_meters) 239 | 240 | % Get the size of the robotarium figure window in point units 241 | curunits = get(robotarium_instance.figure_handle, 'Units'); 242 | set(robotarium_instance.figure_handle, 'Units', 'Pixels'); 243 | cursize = get(robotarium_instance.figure_handle, 'Position'); 244 | set(robotarium_instance.figure_handle, 'Units', curunits); 245 | 246 | % Determine the ratio of the font height to the y-axis 247 | font_ratio = (font_height_meters)/(robotarium_instance.boundaries(4) -... 248 | robotarium_instance.boundaries(3)); 249 | 250 | % Determine the font size in points so it fits the window. cursize(4) is 251 | % the hight of the figure window in points. 252 | font_size = cursize(4) * font_ratio; 253 | 254 | end 255 | --------------------------------------------------------------------------------