--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/Main.m:
--------------------------------------------------------------------------------
1 | %% Ce code permet de parametrer et de lancer la simulation %%
2 | % %
3 | % --------------- NE PAS MODIFIER --------------- %
4 | % %
5 | % Pour programmer les robots, utilisez Robot.m %
6 | % %
7 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
8 |
9 |
10 | %% Add path to subfolder
11 | addpath(genpath('utilities'));
12 | %addpath('./');
13 |
14 |
15 | %% Set up Robotarium object
16 |
17 | N = 12;
18 | xi = [-0.45 -0.15 0.15 0.45 -0.45 -0.15 0.15 0.45 -0.45 -0.15 0.15 0.45];
19 | yi = [0.3 0.3 0.3 0.3 0 0 0 0 -0.3 -0.3 -0.3 -0.3] ;
20 | ai = rand(1,N).*2* pi - pi ;
21 |
22 | initial_conditions = [xi ; yi ; ai ] ;
23 |
24 | r = Robotarium('NumberOfRobots', N, 'ShowFigure', true, 'InitialConditions', initial_conditions);
25 |
26 |
27 | %% PARAMETERS
28 | SPEED = r.max_linear_velocity ; % Moving speed of the robots
29 | DETECTION_RANGE = 0.05 ; % Distance to the target for detection
30 | ATTACK_RANGE = 0.5 ; % Distance to the target for attacking
31 | ATTACK_STRENGTH = 0.01 ; % Reduction of target energy for robot attacking the target
32 |
33 | PERCEPTION_RANGE = 0.35 ; % Perception range of the robots (for walls or neighbors)
34 |
35 | [x_target, y_target] = getTargetPosition();
36 |
37 | %% VARIABLES
38 | target_energy = 100; % Experiment ends when target energy is down to 0
39 |
40 | target_detected = zeros(1,N); % Which robots have detected the target
41 | target_attacked = zeros(1,N); % Which robots are attacking the taget
42 |
43 | %% AFFICHAGE
44 | d = plot(x_target,y_target,'ro');
45 | target_caption = text(-1.5, -1.1, sprintf('Energie de la cible : %0.1f%%', target_energy), 'FontSize', 15, 'FontWeight', 'bold', 'Color','r');
46 | time_caption = text(-1.5, -1.2, sprintf('Temps écoulé : 0 s'), 'FontSize', 14, 'FontWeight', 'bold', 'Color','r');
47 | uistack(target_caption, 'top');
48 | uistack(time_caption, 'top');
49 | uistack(d, 'bottom');
50 |
51 | % Initialize velocity vector for agents.
52 | v = zeros(2, N);
53 |
54 | %% BARRIER CERTIFICATES
55 | uni_barrier_cert = create_uni_barrier_certificate_with_boundary();
56 | %si_to_uni_dyn = create_si_to_uni_dynamics('LinearVelocityGain', 0.5, 'AngularVelocityLimit', pi/2);
57 | si_to_uni_dyn = create_si_to_uni_dynamics();
58 |
59 | %% CREATE THE ROBOTS
60 | clear allRobots
61 | for i=1:N
62 | allRobots{i} = Robot(i , initial_conditions(1,i) , initial_conditions(2,i) , initial_conditions(3,i) , SPEED);
63 | end
64 |
65 | %% DATA COLLECTION
66 | expectedDuration = 5000 ;
67 | total_time = 0 ;
68 |
69 | data_target = nan(expectedDuration,2);
70 | data_X = nan(expectedDuration,N);
71 | data_Y = nan(expectedDuration,N);
72 | data_attack = nan(expectedDuration,N);
73 | data_detect = nan(expectedDuration,N);
74 |
75 |
76 |
77 | %% START OF SIMULATION
78 | iteration = 0 ;
79 |
80 | while target_energy>0 && total_time<900
81 |
82 | % Update iteration
83 | iteration = iteration + 1 ;
84 |
85 | % UPdate total time
86 | total_time = total_time + r.time_step ;
87 |
88 | % Retrieve velocity and position of the robots
89 | x = r.get_poses();
90 | for i=1:N
91 | allRobots{i}.x = x(1,i);
92 | allRobots{i}.y = x(2,i);
93 | allRobots{i}.orientation = x(3,i);
94 | end
95 |
96 | % Distance to the target
97 | d_target = sqrt((x_target - x(1,:)).^2 + (y_target - x(2,:)).^2);
98 |
99 | % Walls detection
100 | dWalls.up = abs(1-x(2,:));
101 | dWalls.up(dWalls.up>PERCEPTION_RANGE) = NaN ;
102 | dWalls.down = abs(-1-x(2,:));
103 | dWalls.down(dWalls.down>PERCEPTION_RANGE) = NaN;
104 | dWalls.left = abs(-1.6-x(1,:));
105 | dWalls.left(dWalls.left>PERCEPTION_RANGE) = NaN;
106 | dWalls.right = abs(1.6-x(1,:));
107 | dWalls.right(dWalls.right>PERCEPTION_RANGE) = NaN;
108 |
109 | %% Behavioural algorithm for the robots
110 |
111 | % Compute distance between robots
112 | dm = squareform(pdist([x(1,:)' x(2,:)']));
113 |
114 | for i = 1:N
115 |
116 | % Detect the neighbors within the perception range
117 | %neighbors = delta_disk_neighbors(x, i, PERCEPTION_RANGE);
118 | neighbors = find(dm(i,:)<=PERCEPTION_RANGE & dm(i,:)>0);
119 |
120 | % Collect local information for robot i
121 | clear INFO
122 |
123 | % Walls information
124 | INFO.murs.dist_haut = dWalls.up(i);
125 | INFO.murs.dist_bas = dWalls.down(i);
126 | INFO.murs.dist_gauche = dWalls.left(i);
127 | INFO.murs.dist_droite = dWalls.right(i);
128 |
129 | % Neighbors information (when applicable)
130 | INFO.nbVoisins = length(neighbors);
131 | for v = 1:length(neighbors)
132 | INFO.voisins{v} = allRobots{neighbors(v)};
133 | end
134 |
135 | % Target information (when applicable)
136 | if (d_target(i) threshold);
187 | if ~isempty(to_thresh)
188 | dx(:, to_thresh) = threshold*dx(:, to_thresh)./norms(to_thresh);
189 | end
190 |
191 | % Transform the single-integrator dynamics to unicycle dynamics using a provided utility function
192 | dx = si_to_uni_dyn(dx, x);
193 | dx = uni_barrier_cert(dx, x);
194 |
195 | % Set velocities of agents 1:N
196 | r.set_velocities(1:N, dx);
197 |
198 | % Save data
199 | tmpAtt = zeros(1,N);
200 | tmpDet = zeros(1,N);
201 | for i=1:N
202 | if (allRobots{i}.cible_attacked==1)
203 | tmpAtt(i) = 1 ;
204 | end
205 | if (allRobots{i}.cible_detected==1)
206 | tmpDet(i) = 1 ;
207 | end
208 | end
209 | data_attack(iteration,:) = tmpAtt ;
210 | data_detect(iteration,:) = tmpDet ;
211 | data_X(iteration,:) = x(1,:);
212 | data_Y(iteration,:) = x(2,:);
213 | data_target(iteration,:) = [target_energy total_time] ;
214 |
215 |
216 | % Send the previously set velocities to the agents. This function must be called!
217 | r.step();
218 |
219 | % Display
220 | target_caption.String = sprintf('Energie de la cible %0.1f%%', round(10.*target_energy)/10);
221 | time_caption.String = sprintf('Temps écoulé : %d s', round(iteration*r.time_step));
222 |
223 | end
224 |
225 | % Resultat
226 | disp(['Temps total pour détruire la cible : ' num2str(round(iteration*r.time_step)) ' secondes'])
227 |
228 |
229 | % Données finales
230 | data_X = data_X(1:iteration,:);
231 | data_Y = data_Y(1:iteration,:);
232 | data_attack = data_attack(1:iteration,:);
233 | data_detect = data_detect(1:iteration,:);
234 | data_target = data_target(1:iteration,:);
235 |
236 |
237 |
238 | %r.debug();
--------------------------------------------------------------------------------
/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.
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 |
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 |
--------------------------------------------------------------------------------
/Robot.m:
--------------------------------------------------------------------------------
1 | classdef Robot < handle
2 |
3 | %% Voici un robot.
4 | % Cette classe determine le comportement du robot.
5 | % À partir d'ici, vous pourrez programmer son déplacement et
6 | % la manière dont il communique avec les autres robots.
7 | %
8 | % Lors de la simulation, il y aura 12 robots en même temps, tous identiques
9 | % (techniquement : 12 instances de cette classe).
10 | % Leur mission : trouver une cible cachée dans l'environement et la
11 | % détruire le plus vite possible.
12 | %
13 | % Vous pouvez ajouter votre code dans la fonction *update*
14 | % Vous êtes aussi libre d'ajouter des attributs, des méthodes ou des
15 | % scripts annexes si nécessaire.
16 |
17 |
18 | %% Propriétés du robot
19 | properties
20 | id % L'identifiant du robot, compris entre 1 et 12.
21 | x % La position x du robot dans l'arène
22 | y % La position y du robot dans l'arène
23 | orientation % L'orientation du robot. C'est un angle exprimé en radian où 0 indique que le robot est orienté vers l'est.
24 |
25 | % Les 4 premières propriétés se mettent à jour automatiquement.
26 | % Vous pouvez les lire si vous en avez besoin mais vous ne
27 | % pouvez pas les modifier manuellement.
28 | % Pour changer la position du robot, vous devez lui appliquer un
29 | % mouvement (vx, vy)
30 |
31 | vx % Le mouvement que le robot essaye de produire, le long de l'axe x.
32 | vy % Le mouvement que le robot essaye de produire, le long de l'axe y.
33 |
34 | % Les valeurs (vx,vy) constitue un vecteur de déplacement. Elles
35 | % sont initialisées à (0,0) ce qui signifie que le robot sera
36 | % immobile au début. Pour faire bouger votre robot, changez ces
37 | % deux valeurs à votre convenance. Par exemple pour faire bouger votre
38 | % robot vers la droite, choisissez vx = 1 et vy = 0. Pour le faire
39 | % bouger vers le bas, choisissez vx = 0 et vy = -1.
40 | %
41 | % ATTENTION : Vous ne pouvez pas déplacer le robot à une
42 | % coordonnée absolue (x,y). Le robot n'a pas de GPS, ni de
43 | % perception globale de l'environement. Vous pouvez juste lui
44 | % ordonner de se déplacer dans une certaine direction par rapport à
45 | % son emplacement actuel.
46 | %
47 | % Il est recommandé d'utiliser la fonction 'move' (voir plus bas) pour
48 | % mettre à jour vx et vy.
49 | %
50 | % Attention: le mouvement du robot est contraint par la physique du système.
51 | % Si le robot essaye d'aller contre un mur ou risque une collision avec un
52 | % autre robot, des procédures de sécurité l'en empecheront.
53 |
54 |
55 | cible_detected % 1 si le robot connait l'emplacement de la cible, 0 sinon.
56 | cible_attacked % 1 si le robot est en train d'attaquer la cible, 0 sinon.
57 | cible_x % Position x de la cible dans l'environement (initialement inconnue)
58 | cible_y % Position y de la cible dans l'environement (initialement inconnue)
59 |
60 | % Ces 4 propriétés se mettent aussi à jour automatiquement.
61 | % Vous pouvez les lire si vous en avez besoin mais vous ne
62 | % devez pas les modifier manuellement.
63 | % Dès que le robot est en contact avec la cible, la
64 | % propriété *cible_detected* passera automatiquement à 1
65 | % et les coordonnées (cible_x, cible_y) seront renseignées. Vous
66 | % n'avez pas besoin de vous en occuper.
67 | %
68 | % Une fois la cible detectée, le robot attaquera automatiquement la
69 | % cible s'il est suffisamment proche (*cible_attacked* passera à 1)
70 | % et cessera de l'attaquer s'il s'éloigne (*cible_attacked* passera
71 | % à 0).
72 | % Un robot qui a détécté la cible peut passer l'information à ses
73 | % voisins, comme nous le verront pas tard.
74 |
75 |
76 | MAX_SPEED % La vitesse de déplacement du robot. Vous ne pouvez pas modifier cette propriété.
77 |
78 | end
79 |
80 |
81 |
82 | methods
83 |
84 | function robot = Robot(id, x,y,orientation , SPEED)
85 | % Cette fonction est utilisée par le simulateur pour initialiser
86 | % le robot. Vous n'avez pas besoin de l'utiliser.
87 |
88 | robot.id = id ;
89 | robot.x = x;
90 | robot.y = y;
91 | robot.orientation = orientation ;
92 | robot.vx = 0;
93 | robot.vy = 0;
94 | robot.MAX_SPEED = SPEED ;
95 | robot.cible_detected = 0 ;
96 | robot.cible_attacked = 0 ;
97 | robot.cible_x = NaN ;
98 | robot.cible_y = NaN ;
99 |
100 | end
101 |
102 |
103 | function robot = move(robot, vx, vy)
104 | % Utilisez cette fonction pour donner une direction de
105 | % déplacement (vx, vy) à ce robot ou à un robot voisin.
106 | % Par exemple move(1,0) produira un déplacement vers la
107 | % droite.
108 |
109 | v = [vx ; vy];
110 | if (norm(v)>0)
111 | v = robot.MAX_SPEED .* v ./ norm(v);
112 | end
113 |
114 | robot.vx = v(1) ;
115 | robot.vy = v(2) ;
116 | end
117 |
118 |
119 | function robot = set_info_cible(robot, cible_x, cible_y)
120 | % Cette fonction permet de renseigner les informations sur la
121 | % cible. Vous pouvez vous en servir pour passer l'information
122 | % à un autre robot voisin.
123 |
124 | robot.cible_detected = 1 ;
125 |
126 | robot.cible_x = cible_x ;
127 | robot.cible_y = cible_y ;
128 | end
129 |
130 |
131 | function robot = start_attack(robot)
132 | % Cette fonction est appelée automatiquement lorsque le robot
133 | % à détécté la cible et est à distance d'attaque.
134 | % Vous ne devez pas l'utiliser manuellement.
135 | robot.cible_attacked = 1 ;
136 | end
137 |
138 |
139 | function robot = stop_attack(robot)
140 | % Cett fonction est appelée automatiquement lorsque le robot
141 | % n'est plus à distance d'attaque de la cible.
142 | % Vous ne devez pas l'utiliser manuellement.
143 | robot.cible_attacked = 0 ;
144 | end
145 |
146 |
147 | %% À vous de jouer !!
148 |
149 | % Le robot va executer la fonction *update* ci-dessous 30 fois
150 | % par seconde. Vous pouvez y mettre le code que vous souhaitez.
151 |
152 | % À chaque fois que la fonction *update* est appelée, le robot
153 | % reçoit des informations sur son environement proche.
154 | % Ces informations sont contenues dans la variable INFO.
155 |
156 | % La variable INFO contient les informations suivantes :
157 | %
158 | % INFO.murs.dist_haut : La distance entre le robot et le mur
159 | % du haut de l'arène.
160 | %
161 | % INFO.murs.dist_bas : La distance entre le robot et le mur
162 | % du bas de l'arène.
163 | %
164 | % INFO.murs.dist_gauche : La distance entre le robot et le mur
165 | % de gauche de l'arène.
166 | %
167 | % INFO.murs.dist_droite : La distance entre le robot et le mur
168 | % de droite de l'arène.
169 | %
170 | % Si un mur est hors du rayon de perception du robot, la distance
171 | % indiquée sera NaN (inconnue).
172 | %
173 | %
174 | % INFO.nbVoisins : Le nombre d'autres robots présents dans le rayon
175 | % de perception du robot.
176 | %
177 | % INFO.voisins : La liste des autres robots présents dans le rayon
178 | % de perception du robot. Le robot peut lire les propriétés de ces
179 | % autres robots et interagir avec eux.
180 | %
181 | % Par exemple,
182 | %
183 | % INFO.voisins{i}.id vous donne l'identifiant du voisin i.
184 | %
185 | % INFO.voisins{i}.cible_detected permet de savoir si le voisin i
186 | % a détécté la cible.
187 | %
188 | % INFO.voisins{i}.move(vx,vy) permet d'ordonner au voisin i de se
189 | % déplacer le long de vx,vy.
190 | %
191 | % INFO.voisins{i}.set_info_cible(x,y) permet de donner les coordonnées
192 | % de la cible au voisin i.
193 | %
194 | % etc...
195 | %
196 | % Les informations contenues dans INFO sont essentielles pour adapter le
197 | % comportement de votre robot.
198 |
199 | function robot = update(robot, INFO)
200 |
201 |
202 | %%%%COLLISIONS MURS
203 | border_v = [0 0]; % direction opposée aux murs
204 | SAFE_BORDER = 0.2; % distance d'évitement des murs (20 cm)
205 | if INFO.murs.dist_droite < SAFE_BORDER
206 | border_v = border_v + [-0.1 0];
207 | end
208 | if INFO.murs.dist_gauche < SAFE_BORDER
209 | border_v = border_v + [0.3 0.2];
210 | end
211 | if INFO.murs.dist_haut < SAFE_BORDER
212 | border_v = border_v + [0 -0.1];
213 | end
214 | if INFO.murs.dist_bas < SAFE_BORDER
215 | border_v = border_v + [0.2 0.3];
216 | end
217 |
218 | %%%%RECHERCHES CIBLE
219 | start(robot);
220 | v = proximite(robot, INFO);
221 | v = v + border_v ;
222 | robot.move(v(1),v(2));
223 |
224 | %%%%ATTAQUE CIBLE
225 | if (robot.cible_detected==1)
226 | for i=1:INFO.nbVoisins
227 | voisin = INFO.voisins{i};
228 | voisin.set_info_cible(robot.cible_x, robot.cible_y);
229 | end
230 | if (robot.cible_attacked==0)
231 | vx = robot.cible_x-robot.x ;
232 | vy = robot.cible_y-robot.y ;
233 | robot.move(vx,vy);
234 | else
235 | robot.move(0,0);
236 | end
237 | end
238 | end
239 |
240 |
241 | end
242 | end
243 |
244 |
--------------------------------------------------------------------------------
/utilities/ARobotarium.m:
--------------------------------------------------------------------------------
1 | classdef ARobotarium < handle
2 | % AROBOTARIUM This is an interface for the Robotarium class that
3 | % ensures the simulator and the robots match up properly. You should
4 | % definitely NOT MODIFY this file. Also, don't submit this file.
5 |
6 | properties (GetAccess = protected, SetAccess = protected)
7 | robot_handle % Handle for the robots patch objects
8 | robot_body % Base robot body position used for rendering
9 |
10 | boundary_patch % Path to denote the Robotarium's boundary
11 | end
12 |
13 | properties (Constant)
14 | time_step = 0.033
15 | max_linear_velocity = 0.2
16 | robot_diameter = 0.11
17 | wheel_radius = 0.016;
18 | base_length = 0.105;
19 | boundaries = [-1.6, 1.6, -1, 1];
20 | end
21 |
22 | properties (GetAccess = public, SetAccess = protected)
23 | % Maximum wheel velocitry of the robots
24 | max_wheel_velocity = ARobotarium.max_linear_velocity/ARobotarium.wheel_radius;
25 |
26 | max_angular_velocity = ...
27 | 2*(ARobotarium.wheel_radius/ARobotarium.robot_diameter) ...
28 | *(ARobotarium.max_linear_velocity/ARobotarium.wheel_radius);
29 |
30 | number_of_robots
31 | figure_handle
32 | end
33 |
34 | properties (GetAccess = protected, SetAccess = protected)
35 |
36 |
37 | velocities
38 | poses
39 | left_leds
40 | right_leds
41 | % Figure handle for simulator
42 |
43 | show_figure
44 | end
45 |
46 | methods (Abstract)
47 | % Getters
48 | get_poses(this)
49 |
50 | % Initialization
51 | initialize(this, initial_conditions)
52 |
53 | %Update functions
54 | step(this);
55 | debug(this);
56 | end
57 |
58 | methods
59 | function this = ARobotarium(number_of_robots, show_figure, figure_handle)
60 |
61 | assert(number_of_robots >= 0 && number_of_robots <= 50, ...
62 | 'Number of robots (%i) must be >= 0 and <= 50', number_of_robots);
63 | this.number_of_robots = number_of_robots;
64 | N = number_of_robots;
65 |
66 | this.poses = zeros(3, N);
67 | this.show_figure = show_figure;
68 | this.velocities = zeros(2, N);
69 | this.left_leds = zeros(3, N);
70 | this.right_leds = zeros(3, N);
71 |
72 | if(show_figure)
73 | if(isempty(figure_handle))
74 | this.figure_handle = figure();
75 | else
76 | this.figure_handle = figure_handle;
77 | end
78 |
79 | this.initialize_visualization();
80 | end
81 | end
82 |
83 | function agents = get_number_of_robots(this)
84 | agents = this.number_of_robots;
85 | end
86 |
87 | function this = set_velocities(this, ids, vs)
88 | N = size(vs, 2);
89 |
90 | assert(N<=this.number_of_robots, 'Row size of velocities (%i) must be <= to number of agents (%i)', ...
91 | N, this.number_of_robots);
92 |
93 | this.velocities(:, ids) = vs;
94 | end
95 |
96 | function this = set_left_leds(this, ids, rgbs)
97 | N = size(rgbs, 2);
98 |
99 | assert(N<=this.number_of_robots, 'Row size of rgb values (%i) must be <= to number of agents (%i)', ...
100 | N, this.number_of_robots);
101 |
102 | assert(all(all(rgbs(1:3, :) <= 255)) && all(all(rgbs(1:3, :) >= 0)), 'RGB commands must be between 0 and 255');
103 |
104 | % Only set LED commands for the selected robots
105 | this.left_leds(:, ids) = rgbs;
106 | end
107 |
108 | function this = set_right_leds(this, ids, rgbs)
109 | N = size(rgbs, 2);
110 |
111 | assert(N<=this.number_of_robots, 'Row size of rgb values (%i) must be <= to number of agents (%i)', ...
112 | N, this.number_of_robots);
113 |
114 | assert(all(all(rgbs(1:3, :) <= 255)) && all(all(rgbs(1:3, :) >= 0)), 'RGB commands must be between 0 and 255');
115 |
116 | % Only set LED commands for the selected robots
117 | this.right_leds(:, ids) = rgbs;
118 | end
119 |
120 | function iters = time2iters(this, time)
121 | iters = time / this.time_step;
122 | end
123 | end
124 |
125 | methods (Access = protected)
126 |
127 | function dxu = threshold(this, dxu)
128 | dxdd = this.uni_to_diff(dxu);
129 |
130 | to_thresh = abs(dxdd) > this.max_wheel_velocity;
131 | dxdd(to_thresh) = this.max_wheel_velocity*sign(dxdd(to_thresh));
132 |
133 | dxu = this.diff_to_uni(dxdd);
134 | end
135 |
136 | function dxdd = uni_to_diff(this, dxu)
137 | r = this.wheel_radius;
138 | l = this.base_length;
139 | dxdd = [
140 | (1/(2*r))*(2*dxu(1, :) - l*dxu(2, :)) ; ...
141 | (1/(2*r))*(2*dxu(1, :) + l*dxu(2, :))
142 | ];
143 | end
144 |
145 | function dxu = diff_to_uni(this, dxdd)
146 | r = this.wheel_radius;
147 | l = this.base_length;
148 | dxu = [
149 | r/2*(dxdd(1, :) + dxdd(2, :));
150 | r/l*(dxdd(2, :) - dxdd(1, :))
151 | ];
152 | end
153 |
154 | function errors = validate(this)
155 | % VALIDATE meant to be called on each iteration of STEP.
156 | % Checks that robots are operating normally.
157 |
158 | p = this.poses;
159 | b = this.boundaries;
160 | N = this.number_of_robots;
161 | errors = {};
162 |
163 | for i = 1:N
164 | x = p(1, i);
165 | y = p(2, i);
166 |
167 | if(x < b(1) || x > b(2) || y < b(3) || y > b(4))
168 | errors{end+1} = RobotariumError.RobotsOutsideBoundaries;
169 | end
170 | end
171 |
172 | for i = 1:(N-1)
173 | for j = i+1:N
174 | if(norm(p(1:2, i) - p(1:2, j)) <= ARobotarium.robot_diameter)
175 | errors{end+1} = RobotariumError.RobotsTooClose;
176 | end
177 | end
178 | end
179 |
180 | dxdd = this.uni_to_diff(this.velocities);
181 | exceeding = abs(dxdd) > this.max_wheel_velocity;
182 | if(any(any(exceeding)))
183 | errors{end+1} = RobotariumError.ExceededActuatorLimits;
184 | end
185 | end
186 | end
187 |
188 | % Visualization methods
189 | methods (Access = protected)
190 |
191 | % Initializes visualization of GRITSbots
192 | function initialize_visualization(this)
193 | % Initialize variables
194 | N = this.number_of_robots;
195 | offset = 0.05;
196 |
197 | % fig = figure;
198 | % this.figure_handle = fig;
199 | fig = this.figure_handle;
200 |
201 | % Plot Robotarium boundaries
202 | b = this.boundaries;
203 | boundary_points = {[b(1) b(2) b(2) b(1)], [b(3) b(3) b(4) b(4)]};
204 | this.boundary_patch = patch('XData', boundary_points{1}, ...
205 | 'YData', boundary_points{2}, ...
206 | 'FaceColor', 'none', ...
207 | 'LineWidth', 3, ...,
208 | 'EdgeColor', [0, 0, 0]);
209 |
210 | set(fig, 'color', 'white');
211 |
212 | % Set axis
213 | ax = fig.CurrentAxes;
214 |
215 | % Limit view to xMin/xMax/yMin/yMax
216 | axis(ax, [this.boundaries(1)-offset, this.boundaries(2)+offset, this.boundaries(3)-offset, this.boundaries(4)+offset])
217 | set(ax, 'PlotBoxAspectRatio', [1 1 1], 'DataAspectRatio', [1 1 1])
218 |
219 | % Store axes
220 | axis(ax, 'off')
221 |
222 | % Static legend
223 | setappdata(ax, 'LegendColorbarManualSpace', 1);
224 | setappdata(ax, 'LegendColorbarReclaimSpace', 1);
225 |
226 | % Apparently, this statement is necessary to avoid issues with
227 | % axes reappearing.
228 | hold on
229 | set(gcf , 'Position' , [139 288 1188 623]);
230 |
231 | this.robot_handle = cell(1, N);
232 | for i = 1:N
233 | data = gritsbot_patch;
234 | this.robot_body = data.vertices;
235 | x = this.poses(1, i);
236 | y = this.poses(2, i);
237 | th = this.poses(3, i) - pi/2;
238 | rotation_matrix = [
239 | cos(th) -sin(th) x;
240 | sin(th) cos(th) y;
241 | 0 0 1];
242 | transformed = this.robot_body*rotation_matrix';
243 | this.robot_handle{i} = patch(...
244 | 'Vertices', transformed(:, 1:2), ...
245 | 'Faces', data.faces, ...
246 | 'FaceColor', 'flat', ...
247 | 'FaceVertexCData', data.colors, ...
248 | 'EdgeColor','none');
249 | end
250 | end
251 |
252 | function draw_robots(this)
253 | for i = 1:this.number_of_robots
254 | x = this.poses(1, i);
255 | y = this.poses(2, i);
256 | th = this.poses(3, i) - pi/2;
257 | rotation_matrix = [...
258 | cos(th) -sin(th) x;
259 | sin(th) cos(th) y;
260 | 0 0 1
261 | ];
262 | transformed = this.robot_body*rotation_matrix';
263 | set(this.robot_handle{i}, 'Vertices', transformed(:, 1:2));
264 |
265 | % Set LEDs
266 | left = this.left_leds/255;
267 | right = this.right_leds/255;
268 |
269 | this.robot_handle{i}.FaceVertexCData(4, :) = left(:, i);
270 | this.robot_handle{i}.FaceVertexCData(5, :) = right(:, i);
271 | end
272 |
273 | drawnow limitrate
274 | end
275 | end
276 | end
277 |
--------------------------------------------------------------------------------
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.
and a mapping between their states 
Using this particular method, the single-integrator dynamics must be computed in the single-integrator domain.