clarence.borie
/
motion_planning
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

second

This commit is contained in:
Clarence BORIE 2023-01-20 21:33:12 +01:00
parent 65be987938
commit 3a328bca6d
14 changed files with 1063 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
PRM/buildPRM.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} buildPRM (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

2
PRM/desktop.ini Normal file
View File

@ -0,0 +1,2 @@
[LocalizedFileNames]
dh2ForwardKinematics.m=@dh2ForwardKinematics,0

46
PRM/dh2ForwardKinematics.m Normal file
View File

@ -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

37
PRM/indexGplanPRM.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} indexGplanPRM (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

38
PRM/indexSplanPRM.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} indexSplanPRM (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

122
PRM/interCartesian.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} interCartesian (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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)<gap && x_stored>-L2 && x_stored<L2)
bool = 1;
connectionMatrixC (j,n) = 1;
connectionMatrixC (n,j) = 1;
break;
elseif (x_stored > x)
for(g = x:gap:x_stored)
if(-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixC (j,n) = 1;
connectionMatrixC (n,j) = 1;
break;
else
bool = 0;
connectionMatrixC (j,n) = 0;
connectionMatrixC (n,j) = 0;
endif
endfor
elseif (x_stored < x)
for (g = x_stored:gap:x)
if (-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixC (j,n) = 1;
connectionMatrixC (n,j) = 1;
break;
else
bool = 0;
connectionMatrixC (j,n) = 0;
connectionMatrixC (n,j) = 0;
endif
endfor
endif
if bool == 0 && connectionMatrixC(j, n) == 0 && n!=j
xplot = [x, x_stored];
yplot = [y, y_stored];
xyplot = [xplot; yplot];
plot(xplot, yplot, 'o-r', 'Color', 'b');
drawnow
endif
endif
endfor
endfunction

113
PRM/interGoal.m Normal file
View File

@ -0,0 +1,113 @@
## 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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} interGoal (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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))<gap && x_stored>-L2 && x_stored<L2)
bool = 1;
connectionMatrixG (j,n) = 1;
connectionMatrixG (n,j) = 1;
break;
elseif (x_stored > G(1,1))
for(g = G(1,1):gap:x_stored)
if(-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixG (j,n) = 1;
connectionMatrixG (n,j) = 1;
break;
else
bool = 0;
connectionMatrixG (j,n) = 0;
connectionMatrixG (n,j) = 0;
endif
endfor
elseif (x_stored < G(1,1))
for (g = x_stored:gap:x)
if (-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixG (j,n) = 1;
connectionMatrixG (n,j) = 1;
break;
else
bool = 0;
connectionMatrixG (j,n) = 0;
connectionMatrixG (n,j) = 0;
endif
endfor
endif
if bool == 0 && connectionMatrixG(j, n) == 0 && n!=j
xplot = [G(1,1), x_stored];
yplot = [G(2,1), y_stored];
xyplot = [xplot; yplot];
plot(xplot, yplot, 'o-r', 'Color', 'r');
drawnow
endif
endif
endfor
endfunction

113
PRM/interJoint.m Normal file
View File

@ -0,0 +1,113 @@
## 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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} interJoint (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

96
PRM/interStart.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} interStart (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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))<gap && x_stored>-L2 && x_stored<L2)
bool = 1;
connectionMatrixS (j,n) = 1;
connectionMatrixS (n,j) = 1;
break;
elseif (x_stored > S(1,1))
for(g = S(1,1):gap:x_stored)
if(-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixS (j,n) = 1;
connectionMatrixS (n,j) = 1;
break;
else
bool = 0;
connectionMatrixS (j,n) = 0;
connectionMatrixS (n,j) = 0;
endif
endfor
elseif (x_stored < S(1,1))
for (g = x_stored:gap:x)
if (-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixS (j,n) = 1;
connectionMatrixS (n,j) = 1;
break;
else
bool = 0;
connectionMatrixS (j,n) = 0;
connectionMatrixS (n,j) = 0;
endif
endfor
endif
if bool == 0 && connectionMatrixS(j, n) == 0 && n!=j
xplot = [S(1,1), x_stored];
yplot = [S(2,1), y_stored];
xyplot = [xplot; yplot];
plot(xplot, yplot, 'o-r', 'Color', 'r');
drawnow
endif
endif
endfor
endfunction

107
PRM/planPathPRM.m Normal file
View File

@ -0,0 +1,107 @@
## 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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} planPathPRM (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

56
PRM/solveIK2LinkPlanarRobot.m Normal file
View File

@ -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

93
RRT/buildRRT.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} buildRRT (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

38
RRT/indexSplanPRM.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} indexSplanPRM (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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

114
RRT/interCartesian.m Normal file
View File

@ -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
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} interCartesian (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## 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))<gap && x_stored>-L2 && x_stored<L2)
bool = 1;
connectionMatrixS (j,n) = 1;
connectionMatrixS (n,j) = 1;
break;
elseif (x_stored > S(1,1))
for(g = S(1,1):gap:x_stored)
if(-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixS (j,n) = 1;
connectionMatrixS (n,j) = 1;
break;
else
bool = 0;
connectionMatrixS (j,n) = 0;
connectionMatrixS (n,j) = 0;
endif
endfor
elseif (x_stored < S(1,1))
for (g = x_stored:gap:x)
if (-L2<g && g< L2 && -L2<Y(g) && Y(g)<L2)
bool = 1;
connectionMatrixS (j,n) = 1;
connectionMatrixS (n,j) = 1;
break;
else
bool = 0;
connectionMatrixS (j,n) = 0;
connectionMatrixS (n,j) = 0;
endif
endfor
endif
if bool == 0 && connectionMatrixC(j, n) == 0 && n!=j
xplot = [x, x_stored];
yplot = [y, y_stored];
xyplot = [xplot; yplot];
plot(xplot, yplot, 'o-r', 'Color', 'b');
drawnow
endif
endif
endfor
endfunction