169 lines
4.1 KiB
Matlab
169 lines
4.1 KiB
Matlab
function [nbNode, obstacle, points] = buildPRM()
|
|
|
|
addpath("C:/motion_planning");
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function [x, y] = buildPRM()
|
|
%
|
|
% 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
|
|
%
|
|
%
|
|
%
|
|
%
|
|
%
|
|
% Yanis DIALLO (yanis.diallo@ecam.fr)
|
|
% 30/11/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
clc
|
|
close all
|
|
S = [2, 0];
|
|
G = [-2, 0];
|
|
[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;
|
|
while n<WhileCond
|
|
|
|
q1 = randVmin + (randVmax - randVmin)* rand();
|
|
q2 = randVmin + (randVmax - randVmin)* rand();
|
|
|
|
[x, y]=MyFK(L1,L2,q1,q2);
|
|
|
|
%Is the end effector colliding with obstacle
|
|
if not(y >= L1 || y <= -L1 || (x>=-L2 && x<=L2 && y>=-L2 && y<=L2))
|
|
|
|
n=columns(points);
|
|
|
|
%Store q1 q2 x y values
|
|
|
|
|
|
|
|
if n==WhileCond-1
|
|
figure 1
|
|
hold on
|
|
text(Gq1, Gq2, 'G', 'FontSize', 20);
|
|
|
|
|
|
figure 2
|
|
hold on
|
|
text(G(1), G(2), 'G', 'FontSize', 20);
|
|
|
|
q1 = Gq1;
|
|
q2 = Gq2;
|
|
x = G(1);
|
|
y = G(2);
|
|
|
|
points = [points [q1;q2;x;y]];
|
|
|
|
obstacle(n+1,n+1) = NaN;
|
|
elseif n==1
|
|
figure 1
|
|
hold on
|
|
text(Sq1, Sq2, 'S', 'FontSize', 20);
|
|
text(q1, q2, int2str(n), 'FontSize', 20);
|
|
|
|
|
|
figure 2
|
|
hold on
|
|
text(S(1), S(2), 'S', 'FontSize', 20);
|
|
|
|
obstacle(n,n) = NaN;
|
|
obstacle(n+1,n+1) = NaN;
|
|
|
|
points = [points [q1;q2;x;y]];
|
|
elseif n<WhileCond
|
|
figure 1
|
|
hold on
|
|
text(q1, q2, int2str(n), 'FontSize', 20);
|
|
|
|
points = [points [q1;q2;x;y]];
|
|
obstacle(n+1,n+1) = NaN;
|
|
endif
|
|
drawnow
|
|
|
|
|
|
for j=1:n
|
|
q1 = points(1,n);
|
|
q2 = points(2,n);
|
|
q1_stored = points(1,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
|
|
|
|
%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);
|
|
Xplot = [q1, q1_stored];
|
|
Yplot = [q2, q2_stored];
|
|
figure 1
|
|
plot(Xplot, Yplot, 'Color', 'b')
|
|
drawnow
|
|
endif
|
|
|
|
endif
|
|
|
|
endfor
|
|
|
|
endif
|
|
endwhile
|
|
nbNode = WhileCond-2;
|
|
endfunction
|