thomas.olive
/
MotionPlanning
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Finished : everything working and commented

This commit is contained in:
Thomas OLIVE 2021-12-18 23:45:52 +01:00
parent 519897d82d
commit 0b627e95f4
9 changed files with 347 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
DrawObstacles.m
View File

@ -1,4 +1,20 @@
function DrawObstacles() function DrawObstacles()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function DrawObstacles
%
% Task: Creates 10 000 random points in the x y cartesian space that are part of the known obstacles
% Computes their corresponding q1 q2 values thanks to Inverse Kinematics
% adds the obstacles in a red color to the figure that should have been previously drawned using PRM or RRT algorithms
%
%
% Inputs:
%
% Outputs:
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L1 = 2; L1 = 2;
L2 = 1; L2 = 1;

23
MyFK.m
View File

@ -1,4 +1,27 @@
function [x, y] = MyFK(L1, L2, Q1, Q2) function [x, y] = MyFK(L1, L2, Q1, Q2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [q1 q2 q1_ q2_] = MyFK(L1, L2, Q1, Q2)
%
% Task: returns the x y coordinates in the cartesian space
% from a given point in the q1 q2 joint space
% using geometry rules since we know that we have two links only of length L1 and L2
%
%
% Inputs:
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-q1: q1 coordinate of the given point
%-q2: q2 coordinate of the given point
%
% Outputs:
%-x: x coordinate of the given point
%-y: x coordinate of the given point
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = L1*cosd(Q1) + L2*cosd(Q1+Q2); x = L1*cosd(Q1) + L2*cosd(Q1+Q2);
y = L1*sind(Q1) + L2*sind(Q1+Q2); y = L1*sind(Q1) + L2*sind(Q1+Q2);
endfunction endfunction

25
MyIK.m
View File

@ -1,4 +1,29 @@
function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y) function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
%
% Task: returns the two possible solutions of q1 q2 coordinate in the joint space
% from a given point in the x y cartesian space
% using geometry rules since we know that we have two links only of length L1 and L2
%
%
% Inputs:
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-x: x coordinate of the given point
%-y: x coordinate of the given point
%
% Outputs:
%-q1: q1 coordinate of the given point (first solution)
%-q2: q2 coordinate of the given point (first solution)
%-q1_: q1 coordinate of the given point (second solution)
%-q2_: q2 coordinate of the given point (second solution)
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q1 = acosd(x/sqrt(x^2+y^2)) - acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2))); q1 = acosd(x/sqrt(x^2+y^2)) - acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
q2 = 180 - acosd( (L1^2+L2^2-(x^2+y^2)) / (2*L1*L2)); q2 = 180 - acosd( (L1^2+L2^2-(x^2+y^2)) / (2*L1*L2));

120
buildPRM.m
View File

@ -1,86 +1,75 @@
function [nbNode, obstacle, points] = buildPRM() function [nbNodes, obstacle, points] = buildPRM()
addpath("C:/motion_planning");
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [x, y] = buildPRM() %function [nbNodes, obstacle, points] = buildPRM()
%
% Task: Implement a code that creates a map of random points in the q1 q2 joint space
% and checks the presence of an obstacle between each of them in the x y cartesian space
% That algorithm is called Probabilistic RoadMaps (PRM)
% %
% Task: Implement a code that creates a map and the finds a path between
% a given start point and a goal point (in C-space) while avoiding collisions
% Inputs: q1 random variable % Inputs: q1 random variable
% q2 random variable % q2 random variable
% %
% Outputs: JTee % Outputs:
% -nbNodes: number of nodes of the graph excluding the starting and goal points
% -obstacle: a matrix containing the distance between connected nodes
% (NaN refers to not connected nodes)
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
% the 1st column describing the 1st (start) point
% %
% %
% %
% %
% % Thomas OLIVE (thomas.olive@ecam.fr)
% Yanis DIALLO (yanis.diallo@ecam.fr) % 18/12/2021
% 30/11/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc clc
close all close all
S = [2, 0];
G = [-2, 0]; [S, G, randVmin, randVmax, L1, L2] = setParams();
[Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995 104.48 [Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995 104.48
[Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04 104.48 [Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04 104.48
points = [Sq1 Sq2 S(1) S(2)]'; points = [Sq1 Sq2 S(1) S(2)]';
obstacle = []; obstacle = [];
randVmin = -180;
randVmax = 180;
L1 = 2;
L2 = 1;
GapValue = 10; GapValue = 10;
X_space = [S(1) G(1)]; X_space = [S(1) G(1)];
Y_space = [S(2) G(2)]; Y_space = [S(2) G(2)];
%DH Parameters n=columns(points); % represents the number of validated points
## d = [0, 0]; WhileCond = 12; % number of desired points in the tree
## a = [L1, L2];
## alpha = [0, 0];
## jointNumber = 1;
## %End effector position
## Bmatrix = [0; 0; 0; 1];
%____________Generation des 10 points_________
n=columns(points);
WhileCond = 12;
while n<WhileCond while n<WhileCond
q1 = randVmin + (randVmax - randVmin)* rand(); q1 = randVmin + (randVmax - randVmin)* rand();% setting a new random point r
q2 = randVmin + (randVmax - randVmin)* rand(); q2 = randVmin + (randVmax - randVmin)* rand();
[x, y]=MyFK(L1,L2,q1,q2); [x, y]=MyFK(L1,L2,q1,q2);
%Is the end effector colliding with obstacle %Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
if not(y >= L1 || y <= -L1 || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) if not(y >= L1 || y <= -L1 || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
n=columns(points); n=columns(points);
%Store q1 q2 x y values if n==WhileCond-1 % if it is the last iteration (concerns the goal point)
if n==WhileCond-1
figure 1 figure 1
hold on hold on
text(Gq1, Gq2, 'G', 'FontSize', 20); text(Gq1, Gq2, 'G', 'FontSize', 20); %text function is used to plot points with their description
figure 2 figure 2
hold on hold on
text(G(1), G(2), 'G', 'FontSize', 20); text(G(1), G(2), 'G', 'FontSize', 20);
q1 = Gq1; q1 = Gq1; % the values are not random but known
q2 = Gq2; q2 = Gq2;
x = G(1); x = G(1);
y = G(2); y = G(2);
points = [points [q1;q2;x;y]]; points = [points [q1;q2;x;y]]; %Store q1 q2 x y values
obstacle(n+1,n+1) = NaN; obstacle(n+1,n+1) = NaN;
elseif n==1 elseif n==1 % if it is the first iteration (concerns the start point)
figure 1 figure 1
hold on hold on
text(Sq1, Sq2, 'S', 'FontSize', 20); text(Sq1, Sq2, 'S', 'FontSize', 20);
@ -91,7 +80,7 @@ function [nbNode, obstacle, points] = buildPRM()
hold on hold on
text(S(1), S(2), 'S', 'FontSize', 20); text(S(1), S(2), 'S', 'FontSize', 20);
obstacle(n,n) = NaN; obstacle(n,n) = NaN; % enlarges the obstacle matrix so we can write later the valid links computed around the new point
obstacle(n+1,n+1) = NaN; obstacle(n+1,n+1) = NaN;
points = [points [q1;q2;x;y]]; points = [points [q1;q2;x;y]];
@ -106,63 +95,26 @@ function [nbNode, obstacle, points] = buildPRM()
drawnow drawnow
for j=1:n for j=1:n % for each point of the map
q1 = points(1,n); q1 = points(1,n); % we are going to compute links between the new point (q1 q2)
q2 = points(2,n); q2 = points(2,n);
q1_stored = points(1,j); q1_stored = points(1,j); % and each one that is stored (q1_stored q2_stored)
q2_stored = points(2,j); q2_stored = points(2,j);
%{
obstacle(j,n) = checkingLine(q1, q2, q1_stored, q2_stored, GapValue, L1, L2, n, j, points);
obstacle(n,j) = checkingLine(q1, q2, q1_stored, q2_stored, GapValue, L1, L2, n, j, points);
%}
if q1_stored != q1 obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points); % we store the distance (NaN if invalid) between the points in a matrix
obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points); % that is designed as a double entry table
%defining Q2 as a function of Q1 if ~isnan(obstacle(j, n)) && n!=j % if the points of index n and j can be linked
A = (q2_stored- q2)/(q1_stored - q1);
B = q2 - A * q1;
Q2 = @(Q1) A*Q1+B;
%getting the sampling direction
if q1 > q1_stored
gap = -GapValue;
else
gap = GapValue;
endif
%getting the sample points
for g=q1:gap:q1_stored
Q1test = g;
Q2test = Q2(g);
%Is the end effector colliding with obstacle
[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test);
%filling the obstacle matrix
if obstacle(j, n) !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
obstacle(j,n) = NaN;
obstacle(n,j) = NaN;
endif
endfor
if obstacle(j, n)==0 && n!=j
obstacle(j,n) = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2);
obstacle(n,j) = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2);
Xplot = [q1, q1_stored]; Xplot = [q1, q1_stored];
Yplot = [q2, q2_stored]; Yplot = [q2, q2_stored];
figure 1 figure 1
plot(Xplot, Yplot, 'Color', 'b') plot(Xplot, Yplot, 'Color', 'b') % we plot the link between them
drawnow drawnow
endif endif
endif
endfor endfor
endif endif
endwhile endwhile
nbNode = WhileCond-2; nbNodes = WhileCond-2;
endfunction endfunction

95
buildRRT.m
View File

@ -1,22 +1,42 @@
function [nbNode, obstacle, points] = buildRRT() function [nbNodes, obstacle, points] = buildRRT()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [nbNodes, obstacle, points] = buildPRM()
%
% Task: Implement a code that creates a tree of points that are drawn in a random way in the q1 q2 joint space
% The tree progresses by a predefined length towards random direction, starting from the S point,
% until it finds a way to connect the start and goal points
% that algorithm is called Rapidly-exploring Random Trees algorithm
%
%
% Inputs:
%
% Outputs:
% -nbNodes: number of nodes of the graph excluding the starting and goal points
% -obstacle: a matrix containing the distance between connected nodes
% (NaN refers to not connected nodes)
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
% the 1st column describing the 1st (start) point
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc clc
close all close all
S = [2, 0]; [S, G, randVmin, randVmax, L1, L2] = setParams();
G = [-2, 0];
[Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995 104.48 [Sq1, Sq2] = MyIK(2,1,S(1),S(2)); %-28.995 104.48
[Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04 104.48 [Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04 104.48
randVmin = -180;
randVmax = 180;
L1 = 2;
L2 = 1;
q1_1st = randVmin + (randVmax - randVmin)* rand();
q2_1st = randVmin + (randVmax - randVmin)* rand(); q1_1st = randVmin + (randVmax - randVmin)* rand(); % first q1 random value
q2_1st = randVmin + (randVmax - randVmin)* rand(); % first q2 random value
[x, y]=MyFK(L1,L2,q1_1st,q2_1st); [x, y]=MyFK(L1,L2,q1_1st,q2_1st);
points = [Sq1 Sq2 S(1) S(2)]'; points = [Sq1 Sq2 S(1) S(2)]'; %flling the first column of the point array with the start point data
GapValue = 5; GapValue = 5;
distArr = []; distArr = [];
@ -26,28 +46,28 @@ function [nbNode, obstacle, points] = buildRRT()
axis([-180 180 -180 180]) axis([-180 180 -180 180])
hold on hold on
plot(points(1, 1), points(2, 1)) plot(points(1, 1), points(2, 1))
text(points(1, 1), points(2, 1), 'S', 'FontSize', 20); text(points(1, 1), points(2, 1), 'S', 'FontSize', 20); %drawing a 'S' letter at q1 q2
minTable = []; minTable = [];
n=0; n=0; % represents the number of validated points
WhileCond = 15; WhileCond = 15; % number of desired points in the tree
while n<WhileCond while n<WhileCond
distArr = []; distArr = []; %resets the distArr array for each point (each iteration)
q1_r = randVmin + (randVmax - randVmin)* rand(); q1_r = randVmin + (randVmax - randVmin)* rand(); % setting a new random point r
q2_r = randVmin + (randVmax - randVmin)* rand(); q2_r = randVmin + (randVmax - randVmin)* rand();
[x, y]=MyFK(L1,L2,q1_r,q2_r); [x, y]=MyFK(L1,L2,q1_r,q2_r); % getting the X Y coordinates of r point
n = columns(points); n = columns(points);
minTable(1, n) = inf; minTable(1, n) = inf;
obstacle(n,n) = NaN; obstacle(n,n) = NaN; % increases the size of obstacle matrix
if n==WhileCond if n==WhileCond % if we have enough points
q1_r = Gq1; q1_r = Gq1; % we do not link a p point with a random one but with the Goal one
q2_r = Gq2; q2_r = Gq2;
for j=1:n for j=1:n
q1_p = points(1,j); q1_p = points(1,j);
@ -56,51 +76,51 @@ function [nbNode, obstacle, points] = buildRRT()
pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp
distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)]; distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)];
endfor endfor
if isnan(min(distArr)) if isnan(min(distArr)) % if the line can not be drawn between the closest and goal point
WhileCond++; WhileCond++; % we keep adding more points
else else
pdefL = min(distArr); pdefL = min(distArr); % we do not use the predefined length but the one separing the closest and goal points
endif endif
elseif n > 1 elseif n > 1 % if is not the first iteration
for j=1:n for j=1:n % for each point in the tree
q1_p = points(1,j); q1_p = points(1,j);
q2_p = points(2,j); q2_p = points(2,j);
pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp pointsTemp = [points [q1_r q2_r 0 0]']; %new point r values are stored in n+1 column of pointsTemp
distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)]; distArr = [distArr checkingLine(GapValue, L1, L2, n+1, j, pointsTemp)]; % we compute the distance between each point in the tree (index j) and the new r one (index n+1)
endfor endfor
else else % if it is the first iteration
q1_p = points(1,1); q1_p = points(1,1);
q2_p = points(2,1); q2_p = points(2,1);
pointsTemp = [points [q1_r q2_r 0 0]']; pointsTemp = [points [q1_r q2_r 0 0]'];
distArr = [distArr checkingLine(GapValue, L1, L2, 1, 2, pointsTemp)]; distArr = [distArr checkingLine(GapValue, L1, L2, 1, 2, pointsTemp)]; % we compute the distance between the first point in the tree (index 1) and the new r one (index n+1)
endif endif
[minDist, minIndex] = min(distArr); %validé [minDist, minIndex] = min(distArr); % we save which point is the closest one from the new r point and we note the distance between them
if ~isnan(minDist) if ~isnan(minDist) % if the line between p and r can be drawn (does not intersect an obstacle in X Y cartesian space)
q1_p = points(1, minIndex); q1_p = points(1, minIndex);
q2_p = points(2, minIndex); q2_p = points(2, minIndex);
q1_pdefL = q1_p + (pdefL * (q1_r - q1_p) / minDist); q1_pdefL = q1_p + (pdefL * (q1_r - q1_p) / minDist); % thanks to some theory coming from the Thales theorem
q2_pdefL = q2_p + (pdefL * (q2_r - q2_p) / minDist); % from thales theorem q2_pdefL = q2_p + (pdefL * (q2_r - q2_p) / minDist); % we compute the q1 and q2 values of the point that is on the line between p and r, away of pdefL from p
points(1, n+1) = q1_pdefL; points(1, n+1) = q1_pdefL; %we note those two values in the points (= tree) matrix since we consider them as 'validated'
points(2, n+1) = q2_pdefL; points(2, n+1) = q2_pdefL;
if n+1!=minIndex if n+1!=minIndex
obstacle(n+1, minIndex) = pdefL; obstacle(n+1, minIndex) = pdefL; % we note the distance between p and r in the obstacle matrix
obstacle(minIndex, n+1) = pdefL; obstacle(minIndex, n+1) = pdefL; % which is built as a double entry table (going from S to G)
endif endif
figure 1 figure 1
hold on hold on
plot(points(1, n), points(2, n)) plot(points(1, n), points(2, n)) % we plot the new values and add a description to it
if pdefL == min(distArr) %which occurs only at the last iteration if pdefL == min(distArr) % this condition is valid only for the last iteration
text(points(1, n+1), points(2, n+1), 'G', 'FontSize', 20); text(points(1, n+1), points(2, n+1), 'G', 'FontSize', 20);
endif endif
if n>1 if n>1 % valid for every iteration except the first and last ones
text(points(1, n), points(2, n), int2str(n-1), 'FontSize', 20); text(points(1, n), points(2, n), int2str(n-1), 'FontSize', 20);
endif endif
Xplot = [q1_p, q1_pdefL]; Xplot = [q1_p, q1_pdefL];
@ -109,4 +129,5 @@ function [nbNode, obstacle, points] = buildRRT()
drawnow drawnow
endif endif
endwhile endwhile
nbNodes = WhileCond-1;
endfunction endfunction

45
checkingLine.m
View File

@ -1,11 +1,43 @@
function dist = checkingLine(GapValue, L1, L2, n, j, points) function dist = checkingLine(GapValue, L1, L2, n, j, points)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function dist = checkingLine(GapValue, L1, L2, n, j, points)
%
% Task: Computes the distance between two points in the q1 q2 joint space
% excepts when they can not be linked because it would collide a known obstacle in the X Y cartesian space
% The 'colliding' (or not) is verified by sampling the line in q1 q2 joint space and computing the X Y corresponding parameters
% for each of the sampled points on the line, as we know the obstacles position in the X Y cartesian space only
%
% Inputs:
%-GapValue: the distance between the q1 values of each sampled points along the line
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-n: index of the first given point in the 'points' matrix
%-j: index of the second given point in the 'points' matrix
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
% the 1st column describing the 1st (start) point
%
% Outputs:
% -nbNodes: number of nodes of the graph excluding the starting and goal points
% -obstacle: a matrix containing the distance between connected nodes
% (NaN refers to not connected nodes)
% The matrix has a size of (nbNodes+2)x(nbNodes+2)
%-points: a matrix saving the q1 q2 and x y coordinates of each point,
% the 1st column describing the 1st (start) point
%
%
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
q1 = points(1,j); q1 = points(1,j);
q2 = points(2,j); q2 = points(2,j);
q1_ = points(1,n); q1_ = points(1,n);
q2_ = points(2,n); q2_ = points(2,n);
dist = 0; dist = 0; % 0 value, if not modified, will represent that we didn't note about an invalid link between the n and j points
if n==j if n==j
dist = NaN; dist = NaN;
endif endif
@ -29,21 +61,16 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points)
Q1test = g; Q1test = g;
Q2test = Q2(g); Q2test = Q2(g);
%Is the end effector colliding with obstacle %Is each sampled point from the line colliding with an obstacle in X Y cartesian space
[Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test); [Xtest, Ytest]=MyFK(L1,L2,Q1test,Q2test);
%filling the obstacle matrix % if a sampled point is colliding and we haven't already noted it, we do
if dist !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles if dist !=NaN && (Ytest >= L1 || Ytest <= -L1 || (Xtest>=-L2 && Xtest<=L2 && Ytest>=-L2 && Ytest<=L2)) %verifie les obstacles
dist = NaN; dist = NaN;
%{
X_space = [X_space Xtest];
Y_space = [Y_space Ytest];
%}
endif endif
endfor endfor
if dist ==0 && n!=j if dist ==0 && n!=j % if we didn't note nothing about the link between n and j, we compute the distance
dist = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2); dist = sqrt( (points(1,j)-points(1,n))^2 + (points(2,j)-points(2,n))^2);
endif endif
endif endif

36
planPathPRM.m
View File

@ -1,19 +1,34 @@
function planPathPRM() function planPathPRM()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function planPathPRM()
%
% Task: Finding the shortest path between the start and goal points (using dijkstra algorithm)
% in a map of linked random points that explores the q1 q2 space (using Probabilistic RoadMaps algorithm)
%
%
% Inputs:
%
% Outputs:
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nbNode, visGraph, points] = buildPRM(); [nbNode, visGraph, points] = buildPRM();
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph); [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
visGraph
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
Q1plot = []; Q1plot = [];
Q2plot = []; Q2plot = [];
X_plot = []; X_plot = [];
Y_plot = []; Y_plot = [];
GapValue = 1; GapValue = 1;
nodeTrajectory = [columns(points) nodeTrajectory]; nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
for i=1:columns(nodeTrajectory) for i=1:columns(nodeTrajectory) %runs through the nodeTrajectory array
Q1plot = [Q1plot points(1, nodeTrajectory(i))]; Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
Q2plot = [Q2plot points(2, nodeTrajectory(i))]; Q2plot = [Q2plot points(2, nodeTrajectory(i))];
% we define the lines composing the shortest path using the usual Ax+B method
if i>1 if i>1
@ -22,6 +37,7 @@ function planPathPRM()
Q2 = @(Q1) A*Q1+B; Q2 = @(Q1) A*Q1+B;
% we sample the line
if Q1plot(i) > Q1plot(i-1) if Q1plot(i) > Q1plot(i-1)
gap = -GapValue; gap = -GapValue;
else else
@ -31,10 +47,10 @@ function planPathPRM()
Q1test = g; Q1test = g;
Q2test = Q2(g); Q2test = Q2(g);
%Is the end effector colliding with obstacle %we compute the x y coordinates that are matching the sample points
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test); [Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
X_plot = [X_plot Xtest]; X_plot = [X_plot Xtest];% we note the x and y values of the sample points
Y_plot = [Y_plot Ytest]; Y_plot = [Y_plot Ytest];
endfor endfor
endif endif
@ -45,17 +61,19 @@ function planPathPRM()
axis([-180 180 -180 180]); axis([-180 180 -180 180]);
title('q1 q2 Joint Space'); title('q1 q2 Joint Space');
hold on hold on
plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5) plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5)% we plot the shortest path in the q1 q2 joint space
figure 2 figure 2
title('X-Y Cartesian Space'); title('X-Y Cartesian Space');
axis([-3 3 -3 3]); axis([-3 3 -3 3]);
hold all hold all
text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g'); text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');% we plot the shortest path in the x y cartesian space
for i=2:columns(nodeTrajectory)-1 for i=2:columns(nodeTrajectory)-1
text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
endfor endfor
% we draw the two circles defining the workspace
x = 3*cos(0:0.01*pi:2*pi); x = 3*cos(0:0.01*pi:2*pi);
y = 3*sin(0:0.01*pi:2*pi); y = 3*sin(0:0.01*pi:2*pi);
plot(x,y, 'Color', 'k'); plot(x,y, 'Color', 'k');

105
planPathRRT.m
View File

@ -1,40 +1,113 @@
function planPathRRT function planPathRRT
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
GapValue=5; %function planPathPRM()
L1=2; %
L2=1; % Task: Finding the shortest path between the start and goal points (using dijkstra algorithm)
% in a tree of linked random points that explores the q1 q2 space (using Rapidly-exploring Random Trees algorithm)
%
%
% Inputs:
%
% Outputs:
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); % gets access to the scripts that are stored in that folder
[S, G, randVmin, randVmax, L1, L2] = setParams();
GapValue=1;
nodeTrajCut = []; nodeTrajCut = [];
[nbNode, visGraph, points] = buildRRT(); [nbNode, visGraph, points] = buildRRT();
for i=1:columns(visGraph) for i=1:columns(visGraph)
for j=1:columns(visGraph) for j=1:columns(visGraph)
if visGraph(i,j)==0 if visGraph(i,j)==0 % if a link has not been noted with the distance
visGraph(i,j) = NaN; visGraph(i,j) = NaN; % we set it as invalid
endif endif
endfor endfor
endfor endfor
[distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph); [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph);
Q1plot = []; Q1plot = [];
Q2plot = []; Q2plot = [];
X_plot = [];
Y_plot = [];
nodeTrajectory = [columns(points) nodeTrajectory] nodeTrajectory = [columns(points) nodeTrajectory]; % we add the index of the last (goal) point to that array
for i=columns(nodeTrajectory):-1:1 for i=columns(nodeTrajectory):-1:1 %runs through the nodeTrajectory array from the end to the beginning
i if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points)) % if a line can be drawn between the start point and the one of index i
if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points)) nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; % we note the distance between those points
nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; maxIndex = i;
endif endif
endfor endfor
nodeTrajCut
nodeTrajectory = [16 max(nodeTrajCut) 1] nodeTrajectory = [nodeTrajectory(1:maxIndex) max(nodeTrajCut) 1]; % as we want to travel the maximal distance from the start point, withouth going through the first branches, we take the maximum value of the nodeTrajCut array
for i=1:columns(nodeTrajectory)
Q1plot = [Q1plot points(1, nodeTrajectory(i))]; for i=1:columns(nodeTrajectory) % running through the new nodeTrajectory array
Q1plot = [Q1plot points(1, nodeTrajectory(i))]; %we note the q1 and q2 values that are to be plot as the shortest path
Q2plot = [Q2plot points(2, nodeTrajectory(i))]; Q2plot = [Q2plot points(2, nodeTrajectory(i))];
if i>1 % for the points that are not the start one
% we define the lines composing the shortest path using the usual Ax+B method
A = (Q2plot(i-1)- Q2plot(i))/(Q1plot(i-1) - Q1plot(i));
B = Q2plot(i) - A * Q1plot(i);
Q2 = @(Q1) A*Q1+B;
% we sample the line
if Q1plot(i) > Q1plot(i-1)
gap = -GapValue;
else
gap = GapValue;
endif
for g=Q1plot(i):gap:Q1plot(i-1)
Q1test = g;
Q2test = Q2(g);
%we compute the x y coordinates that are matching the sample points
[Xtest, Ytest]=MyFK(2,1,Q1test,Q2test);
if g==Q1plot(i) % as the sampling starts from a node that condition means that we are computing about a node
points(3, nodeTrajectory(i)) = Xtest; % we note the x and y values of the nodes
points(4, nodeTrajectory(i)) = Ytest;
endif
X_plot = [X_plot Xtest]; % we note the x and y values of the sample points
Y_plot = [Y_plot Ytest];
endfor
endif
endfor endfor
figure 1 figure 1
axis([-180 180 -180 180]); axis([-180 180 -180 180]);
title('q1 q2 Joint Space'); title('q1 q2 Joint Space');
hold on hold on
plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5) plot(Q1plot, Q2plot, 'Color', 'g', 'LineWidth', 1.5) % we plot the shortest path in the q1 q2 joint space
figure 2
title('X-Y Cartesian Space');
axis([-3 3 -3 3]);
hold all
text(X_plot, Y_plot, '*', 'FontSize', 10, 'Color', 'g');% we plot the shortest path in the x y cartesian space
for i=2:columns(nodeTrajectory)-1
text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node
endfor
text(S(1), S(2), 'S', 'FontSize', 20);
text(G(1), G(2), 'G', 'FontSize', 20);
% we draw the two circles defining the workspace
x = 3*cos(0:0.01*pi:2*pi);
y = 3*sin(0:0.01*pi:2*pi);
plot(x,y, 'Color', 'k');
x = cos(0:0.01*pi:2*pi);
y = sin(0:0.01*pi:2*pi);
plot(x,y, 'Color', 'k');
endfunction endfunction

31
setParams.m Normal file
View File

@ -0,0 +1,31 @@
function [S, G, randVmin, randVmax, L1, L2] = setParams()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [S, G, randVmin, randVmax, L1, L2] = setParams()
%
% Task: defines the parameters that are necessary to the running scrpits
% so we don't have to define them at the beginning of each scrpit
% and we can change them here once, the changes will be taken in account everywhere
%
%
% Inputs:
%
% Outputs:
%-S: x y coordinates of the start point
%-G: x y coordinates of the goal point
%-randVmin: minimum limit for the random values
%-randVmax: maximum limit for the random values
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
S = [2, 0];
G = [-2, 0];
randVmin = -180;
randVmax = 180;
L1 = 2;
L2 = 1;
endfunction