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

buildRRT unfinished

This commit is contained in:
Gwenn DURPOIX-ESPINASSON 2023-01-12 14:02:45 +01:00
parent 019b6f3b62
commit e14ea178c3
1 changed files with 45 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
buildRRT.m
View File

@ -16,7 +16,7 @@
% 08/01/2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength, start, goal)
hold off;
i = 1; %while
@ -46,8 +46,12 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
poly_a=circleToPolygon([0 0 L2-L1], 32);%create a polygon for matgeom with the circle info (smaller one)
poly_b=circleToPolygon([0 0 L1+L2], 32);%bigger one radius=3
Points(1, 1:2)=[2,0];%starting point
drawPoint(Points(1, 1), Points(1,2));
%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));
while i <= nbPoints
@ -60,28 +64,10 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
bTee=dh2ForwardKinematics(theta, d, a, alpha, jointNumber); %FW kinematics
jTee=bTee(1:2, 4); %only retrieve the x and y (2D) values
jTee=jTee';
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]);
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
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)
hold on;%plotting the line
drawEdge(L);
MatrixOfLinks(i, i)=1;
@ -91,7 +77,41 @@ function buildRRT(rangeQ1Q2, nbPoints, L1, L2, fixedLength)
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]);
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
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;
endif
endif
endif
endwhile