adrien.lasserre
/
motion_planning_td5
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

corrected the error for RRT - finished it

This commit is contained in:
Adrien LASSERRE 2023-01-12 15:24:46 +01:00
parent e14ea178c3
commit 3ed014ca0a
1 changed files with 44 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
buildRRT.m
View File

@ -4,15 +4,21 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function buildRRT (rangeQ1Q2, nbPoints, L1, L2, MapFilename)
%
% Task:
% Task: Use the RRT method to reach a desired goal on the cartesian space
%
% Inputs:
% -
% - rangeQ1Q2 : range of values (in degrees) acceptable for joints Q1 and Q2
% - nbPoints : number of points required
% - L1, L2 : lengths of the links (in m)
% - fixedLength : length of the short link
% - start : starting point
% - goal : ending point
%
% Outputs:
% -
% - None
%
% Adrien Lasserre (adrien.lasserre@ecam.fr) & Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
% Adrien Lasserre (adrien.lasserre@ecam.fr) &
% Gwenn Durpoix-Espinasson (g.durpoix-espinasson@ecam.fr)
% 08/01/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -26,7 +32,7 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
alpha=[0;0];
d=[0;0];
a=[L1;L2];
jointNumber=[1;2];
jointNumber=1;
figure 1; hold on;
b=drawCircle(0, 0, L1+L2); %teacher's functions for drawing circles
@ -48,7 +54,7 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
%set the starting point
Points(1,1:2) = start;
%draw the start and goal points
drawPoint(start(1, 1), start(1,2));
drawPoint(goal(1, 1), goal(1,2));
@ -64,10 +70,29 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
jTee=jTee';
L = createEdge(goal, jTee);
if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
index=findClosestPoint(jTee, Points);
dx=jTee(1,1)-Points(index, 1);
dy=jTee(1,2)-Points(index, 2);
E=sqrt(dx^2+dy^2);
dx=(dx)*fixedLength/E;
dy=(dy)*fixedLength/E;
L = createEdge(Points(index, :), [Points(index, 1)+dx, Points(index,2)+dy]);
if ((Points(index,2)+dy)>=L1)
OutOfRange=1; %is not valid if in that area
elseif ((Points(index,2)+dy)<=-L1)
OutOfRange=1;
elseif (abs((Points(index,1)+dx)) <= L2 && abs((Points(index,2)+dy)) <=L2)
OutOfRange=1;
endif
if (OutOfRange==0)
if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
intersect=1; % intersection happenned
disp('Intersect')
else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
hold on;%plotting the line
drawEdge(L);
MatrixOfLinks(i, i)=1;
@ -76,50 +101,18 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
Points(i, 1:2)=[Points(index, 1)+dx, Points(index,2)+dy];
hold on;
drawPoint(Points(i, 1), Points(i,2)); %draw the point
i=i+1;
break;
else
index=findClosestPoint(jTee, Points)
dx=jTee(1,1)-Points(index, 1);
dy=jTee(1,2)-Points(index, 2);
## D = distancePoints(jTee, Points(index, :));
E=sqrt(dx^2+dy^2);
dx=(dx)*fixedLength/E;
dy=(dy)*fixedLength/E;
L = createEdge(Points(index, :), [Points(index, 1)+dx, Points(index,2)+dy]);
intersect=0;
L = createEdge(goal, Points(i, 1:2));
if ((Points(index,2)+dy)>=L1)
OutOfRange=1; %is not valid if in that area
elseif ((Points(index,2)+dy)<=-L1)
OutOfRange=1;
elseif (abs((Points(index,1)+dx)) <= L2 && abs((Points(index,2)+dy)) <=L2)
OutOfRange=1;
endif
if (OutOfRange==0)
if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1 | isempty(intersectEdgePolygon(L, top_line))!=1 | isempty(intersectEdgePolygon(L, bottom_line))!=1)
intersect=1; % intersection happenned
else %if there is no intersection, plot the line-segment and the point and adds it to the list of valid points
hold on;%plotting the line
if (isempty(intersectEdgePolygon(L, poly_a))!=1 | isempty(intersectEdgePolygon(L, poly_b))!=1 | isempty(intersectEdgePolygon(L, center_box))!=1)
intersect=1;
else
drawEdge(L);
MatrixOfLinks(i, i)=1;
MatrixOfLinks(i, index)=1;
MatrixOfLinks(index, i)=1;
Points(i, 1:2)=[Points(index, 1)+dx, Points(index,2)+dy];
hold on;
drawPoint(Points(i, 1), Points(i,2)); %draw the point
i=i+1;
break;
endif
endif
i=i+1;
endif
endif
endwhile
## findClosestPoint(Point, POINTARRAY) Nx2
## compute dx and dy
## create edge fixed length, with closestpoint value and dx dy
## draw edge
## draw point
## if line intersects do not draw neither points nor edge
qGraph (Q_storage, nbPoints, MatrixOfLinks)
endfunction