Skip to content

Commit 5bf320b

Browse files
ncovic1ncovic1
authored
Add files via upload
0 parents  commit 5bf320b

37 files changed

+4166
-0
lines changed

‎Algorithms/DRGBT.m

Lines changed: 463 additions & 0 deletions
Large diffs are not rendered by default.

‎Algorithms/RBT.m

Lines changed: 281 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,281 @@
1+
classdef RBT
2+
properties
3+
eps = 0.1; % Step in C-space used by RRT-based algorithms
4+
N_max = 10000; % Max. number of considered nodes
5+
N_spines = 7; % Number of bur spines
6+
d_crit = 0.03; % Critical distance in W-space when RBT becomes RRT
7+
delta = pi; % Radius of hypersphere from q to q_e
8+
path = []; % Traversed path (sequence of nodes from q_init to q_goal)
9+
N_iter = 0; % Iteration counter
10+
T_alg = 0; % Total algorithm runtime
11+
T_max = 1; % Maximal algorithm runtime in [s]
12+
end
13+
14+
methods
15+
function this = RBT(eps, N_max, N_spines, d_crit, delta)
16+
if nargin > 0
17+
this.eps = eps;
18+
this.N_max = N_max;
19+
this.N_spines = N_spines;
20+
this.d_crit = d_crit;
21+
this.delta = delta;
22+
end
23+
end
24+
25+
26+
function this = Run(this)
27+
global robot tree;
28+
TN = 2; % Determines which trees is chosen, 1: from q_init; 2: from q_goal
29+
tree.nodes = {robot.q_init, robot.q_goal}; % Consisting of two parts, one from q_init and another from q_goal
30+
tree.pointers = {{0}, {0}}; % Pointing to location of parent/children in trees
31+
tree.distances = {NaN, NaN}; % Distance to each obstacle for each node
32+
this.T_alg = tic;
33+
34+
if CheckCollision(robot.q_init)
35+
disp('Initial robot configuration is in the collision!');
36+
return;
37+
end
38+
if CheckCollision(robot.q_goal)
39+
disp('Goal robot configuration is in the collision!');
40+
return;
41+
end
42+
43+
while true
44+
%% Generating bur
45+
TN = 3 - TN;
46+
q_e = this.GetRandomNode();
47+
[q_near, q_near_p] = GetNearestNode(tree.nodes{TN}, q_e);
48+
d_c = this.Get_dc(TN, q_near_p);
49+
if d_c > this.d_crit
50+
for i = 1:this.N_spines
51+
q_e = q_near + this.GetRandomNode();
52+
q_e = this.SaturateSpine(q_near, q_e, this.delta);
53+
q_e = this.PruneSpine(q_near, q_e);
54+
[q_new, ~] = this.GenerateSpine(q_near, q_e, d_c);
55+
q_new_p = UpgradeTree(TN, q_near_p, q_new, NaN);
56+
end
57+
else % Distance to obstacle is less than d_crit
58+
[q_new, ~, collision] = this.GenerateEdge(q_near, q_e); % Spine is generated using RRT
59+
if ~collision
60+
q_new_p = UpgradeTree(TN, q_near_p, q_new, NaN);
61+
else
62+
q_new_p = q_near_p;
63+
end
64+
end
65+
66+
%% Bur-Connect
67+
TN = 3 - TN;
68+
[q_near, q_near_p] = GetNearestNode(tree.nodes{TN}, q_new);
69+
collision = false; % Is collision occured
70+
reached = false; % If trees are connected
71+
while ~collision && ~reached
72+
d_c = this.Get_dc(TN, q_near_p);
73+
if d_c > this.d_crit
74+
[q_near, reached] = this.GenerateSpine(q_near, q_new, d_c); % Spine is generated using RBT
75+
else
76+
[q_near, reached, collision] = this.GenerateEdge(q_near, q_new); % Spine is generated using RRT
77+
end
78+
if ~collision
79+
q_near_p = UpgradeTree(TN, q_near_p, q_near, NaN);
80+
end
81+
end
82+
83+
this.N_iter = this.N_iter + 1;
84+
if reached
85+
this.path = GetPath(q_near_p, q_new_p, TN);
86+
this.T_alg = toc(this.T_alg);
87+
disp(['The path is found in ', num2str(this.T_alg*1000), ' [ms].']);
88+
return;
89+
elseif size(tree.nodes{1},2)+size(tree.nodes{2},2) >= this.N_max
90+
this.T_alg = toc(this.T_alg);
91+
disp('The path is not found.');
92+
return;
93+
end
94+
TN = 3 - TN;
95+
end
96+
end
97+
98+
99+
function q_e = GetRandomNode(~)
100+
% Adding a random node with uniform distribution in C-space
101+
global robot;
102+
103+
q_e = (robot.range(:,2)-robot.range(:,1)).*rand(robot.N_DOF,1) + robot.range(:,1);
104+
end
105+
106+
107+
function [q_new, reached, collision] = GenerateEdge(this, q, q_e)
108+
% Edge is generated from q towards q_e for eps
109+
% q_new is new reached node
110+
% collision means whether collision occured when moving from q towards q_e
111+
% reached means whether q_e is reached
112+
113+
D = norm(q_e-q);
114+
if D < this.eps
115+
reached = true;
116+
if D == 0
117+
collision = false;
118+
q_new = q;
119+
return;
120+
end
121+
step = D;
122+
else
123+
reached = false;
124+
step = this.eps;
125+
end
126+
127+
eps0 = this.eps/10;
128+
K = ceil(step/eps0);
129+
Step = step/(K*D);
130+
for k = K:-1:1
131+
q_new = q + k*Step*(q_e-q);
132+
collision = CheckCollision(q_new);
133+
if collision
134+
q_new = q;
135+
reached = false;
136+
return;
137+
end
138+
end
139+
q_new = q + K*Step*(q_e-q);
140+
141+
end
142+
143+
144+
function [q_new, reached] = GenerateSpine(~, q, q_e, d_c)
145+
global robot;
146+
% Spine is generated from q towards q_e
147+
% q_new represents new reached node
148+
% reached means whether q_e is reached
149+
150+
q_new = q;
151+
if q_e == q
152+
reached = true;
153+
return;
154+
end
155+
156+
reached = false;
157+
[xyz_q, ~] = DirectKinematics(robot, q);
158+
xyz_q_new = xyz_q;
159+
rho = 0; % Path length in W-space
160+
K_max = 5; % Number of iterations for computing q*
161+
k = 1;
162+
while true
163+
step = ComputeStep(q_new, q_e, d_c-rho, xyz_q_new); % 'd_c-rho' is the remaining path length in W-space
164+
if step > 1
165+
q_new = q_e;
166+
reached = true;
167+
else
168+
q_new = q_new + step*(q_e-q_new);
169+
end
170+
if k == K_max || reached
171+
break;
172+
end
173+
[xyz_q_new, ~] = DirectKinematics(robot, q_new);
174+
Rho = zeros(robot.N_links,1);
175+
for i = 2:robot.N_links+1
176+
Rho(i-1) = norm(xyz_q_new(:,i)-xyz_q(:,i));
177+
end
178+
rho = max(Rho);
179+
k = k + 1;
180+
end
181+
182+
function step = ComputeStep(q, q_e, fi, xyz)
183+
if robot.dim == 2 % assumes that N_links = N_DOF
184+
d = 0;
185+
for ii = 1:robot.N_links
186+
r = [robot.DH_table(ii,2), zeros(1,robot.N_links-ii)];
187+
for kk = ii+1:robot.N_links
188+
r(kk) = norm(xyz(:,kk+1)-xyz(:,ii));
189+
end
190+
d = d + max(r)*abs(q_e(ii)-q(ii));
191+
end
192+
else
193+
if robot.model == 1
194+
L = [robot.DH_table(1,1), robot.DH_table(2,2), robot.DH_table(4,1), robot.DH_table(6,1)];
195+
r(1) = max([norm(xyz(1:2,3)), norm(xyz(1:2,4)), norm(xyz(1:2,5))]);
196+
r(2) = max([L(2), norm(xyz(:,4)-xyz(:,2)), norm(xyz(:,5)-xyz(:,2))]);
197+
r(3) = max([L(3), norm(xyz(:,5)-xyz(:,3))]);
198+
C_proj = Get_C_proj(xyz(:,3), xyz(:,4), xyz(:,5));
199+
r(4) = norm(xyz(:,5)-C_proj);
200+
r(5) = L(4);
201+
elseif robot.model == 2
202+
L = [robot.DH_table(1,1), robot.DH_table(2,2), robot.DH_table(3,2), ...
203+
robot.DH_table(4,1), robot.DH_table(5,2), robot.DH_table(6,1)];
204+
r(1) = max([norm(xyz(1:2,3)), norm(xyz(1:2,4)), norm(xyz(1:2,5)), norm(xyz(1:2,6)), norm(xyz(1:2,7))]'+robot.radii(2:end));
205+
r(2) = max([L(2), norm(xyz(:,4)-xyz(:,2)), norm(xyz(:,5)-xyz(:,2)), norm(xyz(:,6)-xyz(:,2)), norm(xyz(:,7)-xyz(:,2))]'+robot.radii(2:end));
206+
r(3) = max([L(3), norm(xyz(:,5)-xyz(:,3)), norm(xyz(:,6)-xyz(:,3)), norm(xyz(:,7)-xyz(:,3))]'+robot.radii(3:end));
207+
C_proj1 = Get_C_proj(xyz(:,4), xyz(:,5), xyz(:,6));
208+
C_proj2 = Get_C_proj(xyz(:,4), xyz(:,5), xyz(:,7));
209+
r(4) = max([norm(xyz(:,6)-C_proj1), norm(xyz(:,7)-C_proj2)]'+robot.radii(5:end));
210+
r(5) = max([L(5), norm(xyz(:,7)-xyz(:,5))]'+robot.radii(5:end));
211+
end
212+
d = r*abs(q(1:end-1)-q_e(1:end-1));
213+
end
214+
step = fi/d;
215+
216+
function C_proj = Get_C_proj(A, B, C)
217+
% C_proj is projection of C on line determined with A and B
218+
AB = B - A;
219+
t_opt = (C-A)'*AB/(AB'*AB);
220+
C_proj = A + t_opt*AB;
221+
end
222+
end
223+
end
224+
225+
226+
function q_e = SaturateSpine(~, q, q_e, length)
227+
Norm = norm(q_e-q);
228+
if Norm > 0
229+
q_e = q + length*(q_e-q)/Norm;
230+
end
231+
end
232+
233+
234+
function q_e = PruneSpine(~, q, q_e)
235+
% Prune spine from q to q_e, if it comes out C-space domain
236+
global robot;
237+
238+
bounds = zeros(robot.N_DOF,1);
239+
indices = [];
240+
for k = 1:robot.N_DOF
241+
if q_e(k) > pi
242+
bounds(k) = pi;
243+
indices = [indices, k];
244+
elseif q_e(k) < -pi
245+
bounds(k) = -pi;
246+
indices = [indices, k];
247+
end
248+
end
249+
if length(indices) == 1
250+
t = (bounds(indices)-q(indices))/(q_e(indices)-q(indices));
251+
q_e = q + t*(q_e-q);
252+
elseif length(indices) > 1
253+
for k = indices
254+
t = (bounds(k)-q(k))/(q_e(k)-q(k));
255+
q_temp = q + t*(q_e-q);
256+
if q_temp >= -pi*ones(robot.N_DOF,1) & q_temp <= pi*ones(robot.N_DOF,1)
257+
q_e = q_temp;
258+
break;
259+
end
260+
end
261+
end
262+
end
263+
264+
265+
function d_c = Get_dc(~, TN, q_p)
266+
% Get minimal distance from q (determined with q_p) to obstacles
267+
% TN - tree number
268+
% q_p - pointer at node q
269+
global tree;
270+
271+
if isnan(tree.distances{TN}(q_p))
272+
q = tree.nodes{TN}(:,q_p);
273+
[d_c, ~] = GetDistance(q);
274+
tree.distances{TN}(q_p) = d_c;
275+
else
276+
d_c = tree.distances{TN}(q_p);
277+
end
278+
end
279+
280+
end
281+
end

0 commit comments

Comments
 (0)