From 3a328bca6d211888844b55f1c36593acbf705e78 Mon Sep 17 00:00:00 2001 From: Clarence Borie Date: Fri, 20 Jan 2023 21:33:12 +0100 Subject: [PATCH] second --- PRM/buildPRM.m | 88 ++++++++++++++++++++++++ PRM/desktop.ini | 2 + PRM/dh2ForwardKinematics.m | 46 +++++++++++++ PRM/indexGplanPRM.m | 37 +++++++++++ PRM/indexSplanPRM.m | 38 +++++++++++ PRM/interCartesian.m | 122 ++++++++++++++++++++++++++++++++++ PRM/interGoal.m | 113 +++++++++++++++++++++++++++++++ PRM/interJoint.m | 113 +++++++++++++++++++++++++++++++ PRM/interStart.m | 96 ++++++++++++++++++++++++++ PRM/planPathPRM.m | 107 +++++++++++++++++++++++++++++ PRM/solveIK2LinkPlanarRobot.m | 56 ++++++++++++++++ RRT/buildRRT.m | 93 ++++++++++++++++++++++++++ RRT/indexSplanPRM.m | 38 +++++++++++ RRT/interCartesian.m | 114 +++++++++++++++++++++++++++++++ 14 files changed, 1063 insertions(+) create mode 100644 PRM/buildPRM.m create mode 100644 PRM/desktop.ini create mode 100644 PRM/dh2ForwardKinematics.m create mode 100644 PRM/indexGplanPRM.m create mode 100644 PRM/indexSplanPRM.m create mode 100644 PRM/interCartesian.m create mode 100644 PRM/interGoal.m create mode 100644 PRM/interJoint.m create mode 100644 PRM/interStart.m create mode 100644 PRM/planPathPRM.m create mode 100644 PRM/solveIK2LinkPlanarRobot.m create mode 100644 RRT/buildRRT.m create mode 100644 RRT/indexSplanPRM.m create mode 100644 RRT/interCartesian.m diff --git a/PRM/buildPRM.m b/PRM/buildPRM.m new file mode 100644 index 0000000..79c328b --- /dev/null +++ b/PRM/buildPRM.m @@ -0,0 +1,88 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} buildPRM (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +function [points, connectionMatrix]= buildPRM () + + +addpath("C:/motion_planning"); + +clc + close all + + randV = 360; + L1 = 2; + L2 = 1; + points = []; #Creation of empty matrices for storing + cMatrix = []; + connectionMatrix = []; + #points' coordinate and the one of onstacle + gap = 0.2; #Interval between sampled pointns on the line + + + %DH Parameters + d = [0; 0]; + a = [L1; L2]; + alpha = [0; 0]; + jointNumber = 1; + %End effector position + Bmatrix = [0; 0; 0; 1]; + + + + n=1; + nPts = 10; #Number of points we want + while n<=nPts + + q1 = randi(randV); + q2 = randi(randV); + + theta = [q1; q2]; + jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber); + b_P_ee = jTee*Bmatrix; + %Is the end effector colliding with connectionMatrix + x=b_P_ee(1); + y=b_P_ee(2); + + if(y >= L1 || y <= -L1 || (-L2<=x && x<=L2 && -L2<=y && y<=L2)) + disp('You are in a prohibited area'); + else + %Store q1 q2 values + n++; + points = [points [q1;q2;x;y]]; + + [connectionMatrixC] = interCartesian (n, L2, gap, points, x, y) + [connectionMatrixJ] = interJoint (n, L1, L2, gap, points, q1, q2, d, a, alpha, jointNumber, Bmatrix) + + hold on + + + endif + endwhile + + [q_S, q_G] = planPathPRM (n, L2, gap, points, x, y); + # connectionMatrixJ + # connectionMatrixC + +endfunction diff --git a/PRM/desktop.ini b/PRM/desktop.ini new file mode 100644 index 0000000..cdc9c67 --- /dev/null +++ b/PRM/desktop.ini @@ -0,0 +1,2 @@ +[LocalizedFileNames] +dh2ForwardKinematics.m=@dh2ForwardKinematics,0 diff --git a/PRM/dh2ForwardKinematics.m b/PRM/dh2ForwardKinematics.m new file mode 100644 index 0000000..195a16d --- /dev/null +++ b/PRM/dh2ForwardKinematics.m @@ -0,0 +1,46 @@ +function jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% function wTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber) +% Task: Determine the 3D transformation matrix corresponding to a set of Denavit-Hartenberg parameters +% +% Inputs: +% - theta: an array of theta parameters (rotation around z in degrees) +% - d: an array of d parameters (translation along z in mm) +% - a: an array of a parameters (translation along x in mm) +% - alpha: an array of alpha parameters (rotation around x in degrees) +% - jointNumber: joint number you want to start with (>=1 && <=size(theta,1)) +% +% Output: +% -jTee: the transformation matrix from the {World} reference frame to the {end-effector} reference frame +% +% +% author: Guillaume Gibert, guillaume.gibert@ecam.fr +% date: 29/01/2021 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% checks if the arrays have the same size +if (size(theta, 1) != size(d,1) || size(theta,1) != size(a, 1) || size(theta,1) != size(alpha, 1)) + disp('[ERROR](dh2ForwardKinematics)-> sizes of input arrays do not match!') + return; +end + +% creates the output matrix as an identity one +jTee = eye(4); + +% checks if jointNumber is in the good range [1 size(theta,1)] +if (jointNumber < 1 || jointNumber > size(theta, 1)) + disp('[ERROR](dh2ForwardKinematics)-> jointNumber is out of range!') + return; +end + +% loops over all the joints and create the transformation matrix as follow: +% for joint i: Trot(theta(i), z) Ttrans(d(i), z) Ttrans (a(i), x) Trot(alpha(i), x) +for l_joint=jointNumber:size(theta, 1) + % determine the transformation matrices for theta, d, a and alpha values of each joint + thetaTransformMatrix = create3DTransformationMatrix(0, 0, theta(l_joint), 1, 0, 0, 0); % Rz + dTransformMatrix = create3DTransformationMatrix(0, 0, 0, 1, 0, 0, d(l_joint)); % Tz + aTransformMatrix = create3DTransformationMatrix(0, 0, 0, 1, a(l_joint), 0, 0); % Tx + alphaTransformMatrix = create3DTransformationMatrix(alpha(l_joint), 0, 0, 1, 0, 0, 0); % Rx + + jTee = jTee * thetaTransformMatrix * dTransformMatrix * aTransformMatrix *alphaTransformMatrix; +end diff --git a/PRM/indexGplanPRM.m b/PRM/indexGplanPRM.m new file mode 100644 index 0000000..62f9a7b --- /dev/null +++ b/PRM/indexGplanPRM.m @@ -0,0 +1,37 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} indexGplanPRM (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +function [index2] = indexGplanPRM (G, dx_G, points) + + for i=1:columns(points) + + u = norm(G-points(1,i)); + dx_G(i) = [u] + i++ + [minval, index2] = min(dx_G) + + endfor + +endfunction diff --git a/PRM/indexSplanPRM.m b/PRM/indexSplanPRM.m new file mode 100644 index 0000000..8b1932b --- /dev/null +++ b/PRM/indexSplanPRM.m @@ -0,0 +1,38 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} indexSplanPRM (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +function [index2] = indexGplanPRM (S, dx_S, points) + + for i=1:columns(points) + + u = norm(S-points(1,i)); + dx_S(i) = [u] + i++ + [minval, index1] = min(dx_S) + + endfor + + +endfunction diff --git a/PRM/interCartesian.m b/PRM/interCartesian.m new file mode 100644 index 0000000..caeebf4 --- /dev/null +++ b/PRM/interCartesian.m @@ -0,0 +1,122 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} interCartesian (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +##function [connectionMatrixC] = interCartesian (n, L2, gap, points, x, y) +## +## Task: Implement a code that check intersections with obstacle using the original position of +## the end effector in the C-space +## +## Inputs: n +## L2, joint value +## gap, interval of sampling +## points, matrix containing joint and cartesian values +## x position of end effector in x-axis +## y position of end effector in y-axis +## +## Outputs: connectionMatrixC +## + +function [connectionMatrixC] = interCartesian (n, L2, gap, points, x, y) + +connectionMatrixC = zeros(10,10); + xplot = []; + yplot = []; + gap = 0.001; + hold on + figure(1) + axis ([-4 4 -4 4]); + + title ("Points and Obstacles in C-Space"); + h = rectangle('Position', [-L2, -L2, 2*L2, 2*L2]); #Square + b = rectangle('Position', [-500*L2, -4*L2, 1000*L2, 2*L2]); ;#Lower boundary + c = rectangle('Position', [500*L2, 4*L2, -1000*L2, -2*L2]); #Upper boundary + + set (h, "FaceColor", [1, 1, 1]); + set (b, "FaceColor", [1, 1, 0]); + set (c, "FaceColor", [1, 1, 0]); + + for j=1:columns(points)-1 + + x_stored = points(3,j) + y_stored = points(4,j) +## texte = int2str(columns(points)); #Transform integer to string +## text(x, y, texte, 'FontSize', 23); #Display the points by apperance + + if x_stored != x + A = (y_stored - y)/(x_stored - x); + B = y - A *x; + Y = @(X) A*X+B; + if( abs(x_stored-x)-L2 && x_stored x) + + for(g = x:gap:x_stored) + if(-L2. + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} interGoal (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +##function [connectionMatrixSG] = interCartesian (n, L2, gap, points, x, y, G, index2, connectionMatrixC, connectionMatrixS) +## +## Task: Implement a code that check intersections with obstacle for the ending point of +## the end effector in the C-space and fill a big matrix with +## the result obtain for point x, y and S +## +## Inputs: n +## L2, joint value +## gap, interval of sampling +## points, matrix containing joint and cartesian values +## x position of end effector in x-axis +## y position of end effector in y-axis +## G ending point (-2,0) +## index 2 of minimal value in the array computing the distance between S and other points +## +## Outputs: connectionMatrixS +## + +function [connectionMatrixSG] = interGoal (n, L2, gap, points, x, y, G,index2, connectionMatrixC, connectionMatrixS) + + hold on + connectionMatrixG = zeros(11,1); + connectionMatrixSG = [zeros(12,12)]; + xplot_G = []; + yplot_G = []; + gap = 0.0001; + + + for j = index2 + x_stored = points(3,j); + y_stored = points(4,j); +## texte = int2str(columns(points)); #Transform integer to string +## text(x, y, texte, 'FontSize', 23); #Display the points by apperance + + if (x_stored != G(1,1)) + + A = (y_stored - G(2,1))/(x_stored - G(1,1)); + B = G(2,1) - A *G(1,1); + Y = @(X) A*X+B; + if( abs(x_stored-G(1,1))-L2 && x_stored G(1,1)) + + for(g = G(1,1):gap:x_stored) + if(-L2. + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} interJoint (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +##function [connectionMatrixJ] = interCartesian (n, L2, gap, points, q1, q2, d, a, alpha, jointNumber, Bmatrix) +## +## Task: Implement a code that check intersections with obstacle using the original position of +## the end effector in the joint-space +## +## Inputs: n +## L2, joint value +## gap, interval of sampling +## points, matrix containing joint and cartesian values +## q1 joint value +## q2 joint value +## d prismatic matrix in z +## a prismatic matrix in x +## alpha joint matrix in x +## jointNumber = number of joints +## Bmatrix +## +## Outputs: connectionMatrixC +## + +function [connectionMatrixJ] = interJoint (n, L1, L2, gap, points, q1, q2, d, a, alpha, jointNumber, Bmatrix) + +gap = 0.2; + connectionMatrixJ = []; + hold on + figure(2) + axis ([0 400 0 400]); + %getting y=ax+b values + for j=1:columns(points)-1 + q1_stored = points(1,j); + q2_stored = points(2,j); + + if q1_stored != 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 = gap; + else + gap = -gap; + endif + + connectionMatrixJ(j,n) = 0; + connectionMatrixJ(n,j) = 0; + + %getting the sample points + for g=q1_stored:gap:q1 + Q1test = g; + Q2test = Q2(g); + + + theta = [Q1test; Q2test]; + jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber); + b_P_ee = jTee*Bmatrix; + %Is the end effector colliding with obstacle + Xtest = b_P_ee(1); + Ytest = b_P_ee(2); + + %filling the obstacle matrix + if(Ytest >= L1 || Ytest <= -L1 || (-L2<=Xtest && Xtest<=L2 && -L2<=Ytest && Ytest<=L2)) %verifie les obstacles + connectionMatrixJ(j,n) = 1; + connectionMatrixJ(n,j) = 1; + endif + + + endfor + if connectionMatrixJ(j, n) == 0 && n != j + + + + + Xplot = [q1, q1_stored]; + Yplot = [q2, q2_stored]; + title ("Points in Joint-Space"); + plot(Xplot, Yplot, 'o-r', 'Color', 'b'); + drawnow + + endif + + endif + + endfor + +endfunction diff --git a/PRM/interStart.m b/PRM/interStart.m new file mode 100644 index 0000000..e847882 --- /dev/null +++ b/PRM/interStart.m @@ -0,0 +1,96 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} interStart (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +function [connectionMatrixS] = interStart (n, L2, gap, points, x, y, S,index1, connectionMatrixC) + +hold on + connectionMatrixS = zeros(11,1); + xplot_S = []; + yplot_S = []; + gap = 0.0001; + + + for j = index1 + x_stored = points(3,j) + y_stored = points(4,j) +## texte = int2str(columns(points)); #Transform integer to string +## text(x, y, texte, 'FontSize', 23); #Display the points by apperance + + if (x_stored != S(1,1)) + + A = (y_stored - S(2,1))/(x_stored - S(1,1)); + B = S(2,1) - A *S(1,1); + Y = @(X) A*X+B; + if( abs(x_stored-S(1,1))-L2 && x_stored S(1,1)) + + for(g = S(1,1):gap:x_stored) + if(-L2. + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} planPathPRM (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +##function [q_S, q_G] = planPathPRM (n, L2, gap, points, x, y) +## +## 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: n +## L2, joint value +## gap, interval of sampling +## points, matrix containing joint and cartesian values +## x position of end effector in x-axis +## y position of end effector in y-axis +## +## Outputs: JTee +## +## +## + + +function [q_S, q_G] = planPathPRM (n, L2, gap, points, x, y) + + qi_S = []; + q1_S = []; + q2_S = []; + + qi_G = []; + q1_G = []; + q2_G = []; + + + L1 =2; + L2 = 1; + S = [2;0]; #Starting point + G = [-2;0]; #Goal location + +## #Filling x and y component of S and G in a XY Matrix + newPoints_xy = []; + newPoints_xy = [points(3,:); points(4,:)]; + newPoints_xy = [S newPoints_xy G]; + + #Start Pt - Inverse Kinematic for havig join values + [nbSol, qi_S] = solveIK2LinkPlanarRobot(L1, L2, S(1,1), S(2,1)); + q1_S = [q1_S qi_S(1,2) ]; #qi_S(1,2)]; #We only decided to deal with one of the solution + q2_S = [q2_S qi_S(2,2)];# qi_S(2,2)]; + q_S = [q1_S;q2_S]; + + #Goal Pt - Inverse Kinematic for havig join values + [nbSol, qi_G] = solveIK2LinkPlanarRobot(L1, L2, G(1,1), G(2,1)); + q1_G = [q1_G qi_G(1,1)];# qi_G(1,2)]; + q2_G = [q2_G qi_G(2,1)];# qi_G(2,2)]; + q_G = [q1_G;q2_G]; + +## Plotting point S and G +figure(1) + text(S(1,1),S(2,1), " S",'Fontsize',15); + plot(S(1,1),S(2,1),'o-r'); + + text(G(1,1),G(2,1), " G",'Fontsize',15); + plot(G(1,1),G(2,1),'o-r'); + + figure(2) + text(q_S(1,1),q_S(2,1), " S",'Fontsize',15); + plot(q_S(1,1),q_S(2,1),'o-r'); + + text(q_G(1,1),q_G(2,1), " G",'Fontsize',15); + plot(q_G(1,1),q_G(2,1),'o-r'); + +## Obtaining connection matrices +[connectionMatrixC] = interCartesian (n, L2, gap, points, x, y); + dx_S = []; + dx_G = []; + dx_qS = []; + dx_qG = []; + ##S and G in C plot +[index1] = indexSplanPRM (S, dx_S, points); +[connectionMatrixS] = interStart (n, L2, gap, points, x, y, S,index1, connectionMatrixC) + + +[index2] = indexGplanPRM (G, dx_G, points); +[connectionMatrixSG] = interGoal (n, L2, gap, points, x, y, G,index2, connectionMatrixC, connectionMatrixS) + + +endfunction diff --git a/PRM/solveIK2LinkPlanarRobot.m b/PRM/solveIK2LinkPlanarRobot.m new file mode 100644 index 0000000..3624df4 --- /dev/null +++ b/PRM/solveIK2LinkPlanarRobot.m @@ -0,0 +1,56 @@ +function [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% function [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y) +% Task: solve Inverse Kinematics (if it exists) for a 2 link planar robot +% +% Inputs: +% - L1: length of link 1 (in m) +% - L2: length of link 1 (in m) +% - x: target x coordinate (in m) +% - y: target y coordinate (in m) +% +% Outputs: +% - nbSol: number of solutions for this IK problem +% - qi: array of joint angle values (in degrees) +% +% +% author: Guillaume Gibert, guillaume.gibert@ecam.fr +% date: 09/02/2021 +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%config +threshold = 1e-10; + +cos_q2 =(x*x + y*y -(L1*L1 + L2*L2)) / (2*L1*L2); + +% no solution cos_q2 >1 or <-1 +if (cos_q2 > 1.0 || cos_q2 < -1.0) + nbSol = 0; + qi = []; + +% one solution cos_q2=1 or -1 +elseif (abs(cos_q2-1.0) < threshold) + nbSol = 1; + q2 = 0; + q1 = atan2(y,x); + qi = [rad2deg(q1) rad2deg(q2)]; +elseif (abs(cos_q2+1.0) < threshold) + nbSol = 1; + q2 = pi; + q1 = atan2(y,x) + qi = [rad2deg(q1) rad2deg(q2)] + +% two solutions -1< cos_q2 < 1 +elseif (cos_q2 > -1.0 && cos_q2 < 1.0) + nbSol = 2; + q2_1 = acos(cos_q2); + q2_2 = 2*pi-acos(cos_q2); + + q1_1 = atan2(y,x)-atan2(L2*sin(q2_1),(L1+L2*cos_q2)); + q1_2 = atan2(y,x)+atan2(L2*sin(q2_1),(L1+L2*cos_q2)); + + qi = [rad2deg(q1_1) rad2deg(q2_1); rad2deg(q1_2) rad2deg(q2_2)]; +end + + + diff --git a/RRT/buildRRT.m b/RRT/buildRRT.m new file mode 100644 index 0000000..63d58c3 --- /dev/null +++ b/RRT/buildRRT.m @@ -0,0 +1,93 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} buildRRT (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +function retval = buildRRT (input1, input2) + +addpath("C:/motion_planning"); + clc + close all + + randV = 360; + L1 = 2; + L2 = 1; + points = []; #Creation of empty matrices for storing + #points' coordinate and the one of onstacle + S = [2;0]; + G = [-2;0]; + gap = 0.2; #Interval between sampled pointns on the line + + axis ([-4 4 -4 4]); + h = rectangle('Position', [-L2, -L2, 2*L2, 2*L2]); #Square + b = rectangle('Position', [-500*L2, -4*L2, 1000*L2, 2*L2]); ;#Lower boundary + c = rectangle('Position', [500*L2, 4*L2, -1000*L2, -2*L2]); #Upper boundary + + set (h, "FaceColor", [1, 1, 1]); + set (b, "FaceColor", [0, 1, 1]); + set (c, "FaceColor", [0, 1, 1]); + hold on + + text(S(1,1),S(2,1), " S",'Fontsize',15); + plot(S(1,1),S(2,1),'o-r'); + + text(G(1,1),G(2,1), " G",'Fontsize',15); + plot(G(1,1),G(2,1),'o-r'); + + + + + %DH Parameters + d = [0; 0]; + a = [L1; L2]; + alpha = [0; 0]; + jointNumber = 1; + %End effector position + Bmatrix = [0; 0; 0; 1]; + + n=1; + nPts = 10; #Number of points we want + while n<=nPts + + q1 = randi(randV); + q2 = randi(randV); + + theta = [q1; q2]; + jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber); + b_P_ee = jTee*Bmatrix; + %Is the end effector colliding with connectionMatrix + x=b_P_ee(1); + y=b_P_ee(2); + + if(y >= L1 || y <= -L1 || (-L2<=x && x<=L2 && -L2<=y && y<=L2)) + disp('You are in a prohibited area'); + else + n++; + points = [points [q1;q2;x;y]]; + [connectionMatrixC] = interCartesian (n, L2, gap, points, S) + + endif + +endwhile + +endfunction diff --git a/RRT/indexSplanPRM.m b/RRT/indexSplanPRM.m new file mode 100644 index 0000000..8b1932b --- /dev/null +++ b/RRT/indexSplanPRM.m @@ -0,0 +1,38 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} indexSplanPRM (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +function [index2] = indexGplanPRM (S, dx_S, points) + + for i=1:columns(points) + + u = norm(S-points(1,i)); + dx_S(i) = [u] + i++ + [minval, index1] = min(dx_S) + + endfor + + +endfunction diff --git a/RRT/interCartesian.m b/RRT/interCartesian.m new file mode 100644 index 0000000..f428032 --- /dev/null +++ b/RRT/interCartesian.m @@ -0,0 +1,114 @@ +## Copyright (C) 2023 borie +## +## This program is free software: you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation, either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program. If not, see +## . + +## -*- texinfo -*- +## @deftypefn {} {@var{retval} =} interCartesian (@var{input1}, @var{input2}) +## +## @seealso{} +## @end deftypefn + +## Author: borie +## Created: 2023-01-20 + +##function [connectionMatrixC] = interCartesian (n, L2, gap, points, x, y) +## +## Task: Implement a code that check intersections with obstacle using the original position of +## the end effector in the C-space +## +## Inputs: n +## L2, joint value +## gap, interval of sampling +## points, matrix containing joint and cartesian values +## x position of end effector in x-axis +## y position of end effector in y-axis +## +## Outputs: connectionMatrixC +## +## + +function [connectionMatrixC] = interCartesian (n, L2, gap, points, x, y) + + hold on + connectionMatrixS = zeros(11,1); + xplot_S = []; + yplot_S = []; + gap = 0.0001; + + + for j = index1 + x_stored = points(3,j) + y_stored = points(4,j) +## texte = int2str(columns(points)); #Transform integer to string +## text(x, y, texte, 'FontSize', 23); #Display the points by apperance + + if (x_stored != S(1,1)) + + A = (y_stored - S(2,1))/(x_stored - S(1,1)); + B = S(2,1) - A *S(1,1); + Y = @(X) A*X+B; + if( abs(x_stored-S(1,1))-L2 && x_stored S(1,1)) + + for(g = S(1,1):gap:x_stored) + if(-L2