├── .gitignore
├── ARobotarium.m
├── LICENSE.txt
├── README.md
├── Robotarium.m
├── RobotariumError.m
├── examples
├── barrier_certificates
│ ├── html
│ │ ├── main.html
│ │ ├── mainUni.html
│ │ ├── main_eq09235582099063973062.png
│ │ ├── main_eq17134034933542560612.png
│ │ └── main_uni.html
│ ├── si_barriers.m
│ └── uni_barriers.m
├── consensus_static
│ ├── consensus.m
│ ├── consensus_fewer_errors.m
│ └── html
│ │ └── main.html
├── data_saving
│ └── leader_follower_save_data.m
├── formation_control
│ ├── formation_control.m
│ └── html
│ │ └── main.html
├── go_to_goal
│ ├── go_to_point.m
│ ├── go_to_pose.m
│ └── html
│ │ ├── go_to_pose.html
│ │ └── main.html
├── leader_follower_static
│ ├── html
│ │ └── main.html
│ └── leader_follower.m
└── plotting
│ ├── GTLogo.png
│ ├── go_to_pose_with_plotting.m
│ └── leader_follower_with_plotting.m
├── init.m
├── patch_generation
└── gritsbot_patch.m
├── simulation_skeleton.m
└── utilities
├── barrier_certificates
├── create_si_barrier_certificate.m
├── create_si_barrier_certificate2.m
├── create_si_barrier_certificate_with_boundary.m
├── create_uni_barrier_certificate.m
├── create_uni_barrier_certificate2.m
├── create_uni_barrier_certificate_with_boundary.m
└── create_uni_barrier_certificate_with_obstacles.m
├── controllers
├── create_automatic_parking_controller.m
├── create_automatic_parking_controller2.m
├── create_parking_controller.m
├── create_si_position_controller.m
├── create_unicycle_position_controller.m
└── create_waypoint_controller.m
├── graph
├── completeGL.m
├── cycleGL.m
├── delta_disk_neighbors.m
├── html
│ ├── completeGL.html
│ ├── completeGL_eq08486233743349304198.png
│ ├── completeGL_eq12942671300991736101.png
│ ├── cycleGL.html
│ ├── cycleGL_eq12942671300991736101.png
│ ├── delta_disk_neighbors.html
│ ├── delta_disk_neighbors_eq14324477822824332334.png
│ ├── lineGL.html
│ ├── lineGL_eq12942671300991736101.png
│ ├── randomGL.html
│ ├── randomGL_eq12942671300991736101.png
│ ├── random_connectedGL.html
│ ├── random_connectedGL_eq12942671300991736101.png
│ ├── topological_neighbors.html
│ └── topological_neighbors_eq12941163982435923632.png
├── lineGL.m
├── randomGL.m
├── random_connectedGL.m
└── topological_neighbors.m
├── misc
├── create_is_initialized.m
├── generate_initial_conditions.m
├── html
│ ├── create_is_initialized.html
│ ├── generate_initial_conditions.html
│ ├── generate_initial_conditions_eq07459738348397623860.png
│ ├── generate_initial_conditions_eq16033661748480904678.png
│ └── unique_filename.html
└── unique_filename.m
└── transformations
├── create_si_to_uni_dynamics.m
├── create_si_to_uni_dynamics_with_backwards_motion.m
├── create_si_to_uni_mapping.m
├── create_uni_to_si_mapping.m
└── html
├── create_si_to_uni_mapping.html
├── create_si_to_uni_mapping2.html
├── create_si_to_uni_mapping2_eq12450574036302976132.png
├── 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.html
├── create_uni_to_si_mapping_eq12450574036302976132.png
└── create_uni_to_si_mapping_eq13491430810795123718.png
/.gitignore:
--------------------------------------------------------------------------------
1 | *.mat
2 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/examples/barrier_certificates/html/main_eq09235582099063973062.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/741b721c4dd5f96a3f3910a983155a62655b2a4c/examples/barrier_certificates/html/main_eq09235582099063973062.png
--------------------------------------------------------------------------------
/examples/barrier_certificates/html/main_eq17134034933542560612.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/741b721c4dd5f96a3f3910a983155a62655b2a4c/examples/barrier_certificates/html/main_eq17134034933542560612.png
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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/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/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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/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 |
--------------------------------------------------------------------------------
/utilities/graph/html/topological_neighbors_eq12941163982435923632.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/robotarium/robotarium-matlab-simulator/741b721c4dd5f96a3f3910a983155a62655b2a4c/utilities/graph/html/topological_neighbors_eq12941163982435923632.png
--------------------------------------------------------------------------------
/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/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 |
--------------------------------------------------------------------------------
/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/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/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/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/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.
--------------------------------------------------------------------------------
/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/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 |
--------------------------------------------------------------------------------
/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/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_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 |
--------------------------------------------------------------------------------
/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 and a mapping between their states Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.
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 |
Returns a mapping from unicycle to single-integrator dynamics and a mapping between their states Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.
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 |
and a mapping between their states 
Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.
from single-integrator to unicycle dynamics
and a mapping between their states 
Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.