133 lines
4.4 KiB
Matlab
133 lines
4.4 KiB
Matlab
function [nbNodes, obstacle, points] = 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)
|
|
%
|
|
% Inputs: q1 random variable
|
|
% q2 random variable
|
|
%
|
|
% 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
|
|
addpath("C:/Users/Admin/Documents/ProjectMotionPlanning/motion_planning");
|
|
|
|
[S, G, randVmin, randVmax, L1, L2, threshold] = setParams();
|
|
|
|
|
|
threshold = 0.000001;
|
|
|
|
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, 2, 0);
|
|
Sq1 = qi(1,1)
|
|
Sq2 = qi(1,2)
|
|
|
|
[nbSol, qi] = solveIK2LinkPlanarRobot(2, 1, -2, 0);
|
|
Gq1 = qi(1,1)
|
|
Gq2 = qi(1,2)
|
|
|
|
points = [Sq1 Sq2 S(1) S(2)]';
|
|
obstacle = [];
|
|
GapValue = 10;
|
|
X_space = [S(1) G(1)];
|
|
Y_space = [S(2) G(2)];
|
|
|
|
n=columns(points); % represents the number of validated points
|
|
WhileCond = 12; % number of desired points in the tree
|
|
while n<WhileCond
|
|
|
|
q1 = randVmin + (randVmax - randVmin)* rand();% setting a new random point r
|
|
q2 = randVmin + (randVmax - randVmin)* rand();
|
|
|
|
%[x, y]=MyFK(L1,L2,q1,q2); remove the '%' on that line and add it on the next ones for faster computation
|
|
jTee=dh2ForwardKinematics([q1; q2], [0; 0], [L1; L2], [0; 0], 1);
|
|
x= jTee(1,4);
|
|
y= jTee(2,4);
|
|
%Checking if the new random point (end effector) is part of a known obstacle in X Y cartesian space
|
|
if not((y >= L1 - threshold || y <= -L1 + threshold || (x>=-L2 - threshold && x<=L2 - threshold && y>=-L2 - threshold && y<=L2 + threshold))
|
|
|
|
n=columns(points);
|
|
|
|
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 function is used to plot points with their description
|
|
|
|
|
|
figure 2
|
|
hold on
|
|
text(G(1), G(2), 'G', 'FontSize', 20);
|
|
|
|
q1 = Gq1; % the values are not random but known
|
|
q2 = Gq2;
|
|
x = G(1);
|
|
y = G(2);
|
|
|
|
points = [points [q1;q2;x;y]]; %Store q1 q2 x y values
|
|
|
|
obstacle(n+1,n+1) = NaN;
|
|
elseif n==1 % if it is the first iteration (concerns the start point)
|
|
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; % 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]];
|
|
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 % 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); % and each one that is stored (q1_stored q2_stored)
|
|
q2_stored = points(2,j);
|
|
|
|
|
|
obstacle(j,n) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % we store the distance (NaN if invalid) between the points in a matrix
|
|
obstacle(n,j) = checkingLine(GapValue, L1, L2, n, j, points, threshold); % 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') % we plot the link between them
|
|
drawnow
|
|
endif
|
|
endfor
|
|
|
|
endif
|
|
endwhile
|
|
nbNodes = WhileCond-2;
|
|
endfunction
|