From 0b627e95f40d4e7b18196c887df1f8f60f2103b3 Mon Sep 17 00:00:00 2001 From: Thomas Olive Date: Sat, 18 Dec 2021 23:45:52 +0100 Subject: [PATCH] Finished : everything working and commented --- DrawObstacles.m | 18 ++++++- MyFK.m | 23 +++++++++ MyIK.m | 25 ++++++++++ buildPRM.m | 126 +++++++++++++++--------------------------------- buildRRT.m | 99 ++++++++++++++++++++++--------------- checkingLine.m | 45 +++++++++++++---- planPathPRM.m | 36 ++++++++++---- planPathRRT.m | 105 ++++++++++++++++++++++++++++++++++------ setParams.m | 31 ++++++++++++ 9 files changed, 347 insertions(+), 161 deletions(-) create mode 100644 setParams.m diff --git a/DrawObstacles.m b/DrawObstacles.m index 351ce40..b839a56 100644 --- a/DrawObstacles.m +++ b/DrawObstacles.m @@ -1,5 +1,21 @@ 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; L2 = 1; qi = []; diff --git a/MyFK.m b/MyFK.m index 9f171a8..8a7baf2 100644 --- a/MyFK.m +++ b/MyFK.m @@ -1,4 +1,27 @@ 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); y = L1*sind(Q1) + L2*sind(Q1+Q2); endfunction \ No newline at end of file diff --git a/MyIK.m b/MyIK.m index 4f03836..224a2ea 100644 --- a/MyIK.m +++ b/MyIK.m @@ -1,4 +1,29 @@ 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))); q2 = 180 - acosd( (L1^2+L2^2-(x^2+y^2)) / (2*L1*L2)); diff --git a/buildPRM.m b/buildPRM.m index 18fb54b..f5baff4 100644 --- a/buildPRM.m +++ b/buildPRM.m @@ -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 % 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 % % % % -% Yanis DIALLO (yanis.diallo@ecam.fr) -% 30/11/2021 +% Thomas OLIVE (thomas.olive@ecam.fr) +% 18/12/2021 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc 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 [Gq1, Gq2] = MyIK(2,1,G(1),G(2)); %151.04 104.48 points = [Sq1 Sq2 S(1) S(2)]'; obstacle = []; - randVmin = -180; - randVmax = 180; - L1 = 2; - L2 = 1; GapValue = 10; X_space = [S(1) G(1)]; Y_space = [S(2) G(2)]; - - %DH Parameters -## d = [0, 0]; -## 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; + n=columns(points); % represents the number of validated points + WhileCond = 12; % number of desired points in the tree while n= L1 || y <= -L1 || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2)) n=columns(points); - %Store q1 q2 x y values - - - - if n==WhileCond-1 + if n==WhileCond-1 % if it is the last iteration (concerns the goal point) figure 1 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 hold on text(G(1), G(2), 'G', 'FontSize', 20); - q1 = Gq1; + q1 = Gq1; % the values are not random but known q2 = Gq2; x = G(1); 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; - elseif n==1 + elseif n==1 % if it is the first iteration (concerns the start point) figure 1 hold on text(Sq1, Sq2, 'S', 'FontSize', 20); @@ -91,7 +80,7 @@ function [nbNode, obstacle, points] = buildPRM() hold on 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; points = [points [q1;q2;x;y]]; @@ -106,63 +95,26 @@ function [nbNode, obstacle, points] = buildPRM() drawnow - for j=1:n - q1 = points(1,n); + for j=1:n % for each point of the map + q1 = points(1,n); % we are going to compute links between the new point (q1 q2) 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); - %{ - 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 - - %defining Q2 as a function of Q1 - 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); + 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 + + if ~isnan(obstacle(j, n)) && n!=j % if the points of index n and j can be linked Xplot = [q1, q1_stored]; Yplot = [q2, q2_stored]; figure 1 - plot(Xplot, Yplot, 'Color', 'b') + plot(Xplot, Yplot, 'Color', 'b') % we plot the link between them drawnow - endif - - endif - + endif endfor endif endwhile - nbNode = WhileCond-2; + nbNodes = WhileCond-2; endfunction diff --git a/buildRRT.m b/buildRRT.m index 298573a..19c5549 100644 --- a/buildRRT.m +++ b/buildRRT.m @@ -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 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 [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(); - [x, y]=MyFK(L1,L2,q1_1st,q2_1st); - points = [Sq1 Sq2 S(1) S(2)]'; + 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); + points = [Sq1 Sq2 S(1) S(2)]'; %flling the first column of the point array with the start point data GapValue = 5; distArr = []; @@ -26,28 +46,28 @@ function [nbNode, obstacle, points] = buildRRT() axis([-180 180 -180 180]) hold on 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 = []; - n=0; - WhileCond = 15; + n=0; % represents the number of validated points + WhileCond = 15; % number of desired points in the tree while n 1 - for j=1:n + elseif n > 1 % if is not the first iteration + for j=1:n % for each point in the tree q1_p = points(1,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 - 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 - else + else % if it is the first iteration q1_p = points(1,1); q2_p = points(2,1); 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 - [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); q2_p = points(2, minIndex); - q1_pdefL = q1_p + (pdefL * (q1_r - q1_p) / minDist); - q2_pdefL = q2_p + (pdefL * (q2_r - q2_p) / minDist); % from thales theorem + 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); % 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; if n+1!=minIndex - obstacle(n+1, minIndex) = pdefL; - obstacle(minIndex, n+1) = pdefL; + obstacle(n+1, minIndex) = pdefL; % we note the distance between p and r in the obstacle matrix + obstacle(minIndex, n+1) = pdefL; % which is built as a double entry table (going from S to G) endif figure 1 hold on - plot(points(1, n), points(2, n)) - if pdefL == min(distArr) %which occurs only at the last iteration + plot(points(1, n), points(2, n)) % we plot the new values and add a description to it + 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); 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); endif Xplot = [q1_p, q1_pdefL]; @@ -109,4 +129,5 @@ function [nbNode, obstacle, points] = buildRRT() drawnow endif endwhile + nbNodes = WhileCond-1; endfunction \ No newline at end of file diff --git a/checkingLine.m b/checkingLine.m index ff98f9f..76f595f 100644 --- a/checkingLine.m +++ b/checkingLine.m @@ -1,11 +1,43 @@ 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); q2 = points(2,j); q1_ = points(1,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 dist = NaN; endif @@ -29,21 +61,16 @@ function dist = checkingLine(GapValue, L1, L2, n, j, points) Q1test = 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); - %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 dist = NaN; - - %{ - X_space = [X_space Xtest]; - Y_space = [Y_space Ytest]; - %} endif 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); endif endif diff --git a/planPathPRM.m b/planPathPRM.m index e5cd6d1..d501ef0 100644 --- a/planPathPRM.m +++ b/planPathPRM.m @@ -1,19 +1,34 @@ 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(); [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph); - visGraph addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); Q1plot = []; Q2plot = []; X_plot = []; Y_plot = []; 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) - Q1plot = [Q1plot points(1, nodeTrajectory(i))]; + for i=1:columns(nodeTrajectory) %runs through the 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))]; + % we define the lines composing the shortest path using the usual Ax+B method if i>1 @@ -22,6 +37,7 @@ function planPathPRM() Q2 = @(Q1) A*Q1+B; + % we sample the line if Q1plot(i) > Q1plot(i-1) gap = -GapValue; else @@ -31,10 +47,10 @@ function planPathPRM() Q1test = 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); - 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]; endfor endif @@ -45,17 +61,19 @@ function planPathPRM() axis([-180 180 -180 180]); title('q1 q2 Joint Space'); 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'); + 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); + text(points(3, nodeTrajectory(i)), points(4, nodeTrajectory(i)), int2str(nodeTrajectory(i)-1), 'FontSize', 20); % we add the description of each node endfor + + % 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'); diff --git a/planPathRRT.m b/planPathRRT.m index 85bcdec..8e57375 100644 --- a/planPathRRT.m +++ b/planPathRRT.m @@ -1,40 +1,113 @@ function planPathRRT - addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning"); - GapValue=5; - L1=2; - L2=1; +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%function planPathPRM() +% +% 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 = []; [nbNode, visGraph, points] = buildRRT(); + for i=1:columns(visGraph) for j=1:columns(visGraph) - if visGraph(i,j)==0 - visGraph(i,j) = NaN; + if visGraph(i,j)==0 % if a link has not been noted with the distance + visGraph(i,j) = NaN; % we set it as invalid endif endfor endfor + [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNode, visGraph); Q1plot = []; 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 - i - if ~isnan(checkingLine(GapValue, L1, L2, 1, nodeTrajectory(i), points)) - nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; + for i=columns(nodeTrajectory):-1:1 %runs through the nodeTrajectory array from the end to the beginning + 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 + nodeTrajCut = [nodeTrajCut nodeTrajectory(i)]; % we note the distance between those points + maxIndex = i; endif endfor - nodeTrajCut - nodeTrajectory = [16 max(nodeTrajCut) 1] - for i=1:columns(nodeTrajectory) - Q1plot = [Q1plot points(1, nodeTrajectory(i))]; + + 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) % 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))]; + + 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 figure 1 axis([-180 180 -180 180]); title('q1 q2 Joint Space'); 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 \ No newline at end of file diff --git a/setParams.m b/setParams.m new file mode 100644 index 0000000..34c734e --- /dev/null +++ b/setParams.m @@ -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 \ No newline at end of file