## nealliu / rrt_dynamic .gitee-modal { width: 500px !important; }

Explore and code with more than 6 million developers，Free private repositories ！：）
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
RunMe.m 7.74 KB
Chahat Deep Singh authored 2017-04-27 05:04 . First Commit
%% 661: H5: RRT Implementation (Chahat Deep Singh)
% Date: April 21 2017

%{
Problem 1:

start (x,y,theta):  [80,60,pi]
goal region (x,y,radius) = [0, 0, 20]
epsilon = 5

---------------------

Problem 2:

start (x,y,theta):  [5,60,0]
goal region (x,y,radius) = [100, 60, 20]
epsilon = 10

----------------------
Problem 3:

start (x,y,theta):  [10,90,3*pi/2]
goal region (x,y,radius) = [100, 90, 10]
epsilon = 5

----------------------
Problem 4:

start (x,y,theta):  [63,80,pi/2]
goal region (x,y,radius) = [60, 10, 10]
epsilon = 10

----------------------
Problem 5:

start (x,y,theta):  [40,40,pi/4]
goal region (x,y,radius) = [0, 0, 20]
epsilon = 20
%}

clc; clear; close all
warning off;

xdim=100;
ydim=100;

problem = 5;

if problem == 1

start_node = [80 60];
Target = [1 1];
eta = 5; %EPSILON

elseif problem == 2
start_node = [5 60];
Target = [100 60];
eta = 10; %EPSILON

elseif problem == 3

start_node = [10 90];
Target = [100 90];
eta = 5; %EPSILON

elseif problem == 4

start_node = [63 80];
Target = [60 10];
eta = 10; %EPSILON

elseif problem == 5

start_node = [40 40];
Target = [1 1];
eta = 20; %EPSILON

else

disp('Input problem statement not defined!!!');

end

ax=gca; backColor=[1 1 1];
ax.Color = backColor;
axis([0 xdim 0 ydim])
daspect([1 1 .1])
view(2);
grid on;
grid minor;

global eta d gama obd
d=2;
gama=200;
obd=5;
extrude=.5;

%% Define Obstacles:
r = 4;
r2 = 8;
c1 = [80 40]; ob1 = [c1(1)-r c1(1)+r c1(2)-r c1(2)+r]; circleExtrude(c1(1), c1(1), r2);
c2 = [73 30]; ob2 = [c2(1)-r c2(1)+r c2(2)-r c2(2)+r]; circleExtrude(c2(1), c2(2), r2);
c3 = [63 25]; ob3 = [c3(1)-r c3(1)+r c3(2)-r c3(2)+r]; circleExtrude(c3(1), c3(2), r2);
c4 = [53 25]; ob4 = [c4(1)-r c4(1)+r c4(2)-r c4(2)+r]; circleExtrude(c4(1), c4(2), r2);
c5 = [43 26]; ob5 = [c5(1)-r c5(1)+r c5(2)-r c5(2)+r]; circleExtrude(c5(1), c5(2), r2);
c6 = [33 25]; ob6 = [c6(1)-r c6(1)+r c6(2)-r c6(2)+r]; circleExtrude(c6(1), c6(2), r2);
c7 = [28 35]; ob7 = [c7(1)-r c7(1)+r c7(2)-r c7(2)+r]; circleExtrude(c7(1), c7(2), r2);
c8 = [25 47]; ob8 = [c8(1)-r c8(1)+r c8(2)-r c8(2)+r]; circleExtrude(c8(1), c8(2), r2);
c9 = [25 57]; ob9 = [c9(1)-r c9(1)+r c9(2)-r c9(2)+r]; circleExtrude(c9(1), c9(2), r2);
c10 = [25 67]; ob10 = [c10(1)-r c10(1)+r c10(2)-r c10(2)+r]; circleExtrude(c10(1), c10(2), r2);
c11 = [25 77]; ob11 = [c11(1)-r c11(1)+r c11(2)-r c11(2)+r]; circleExtrude(c11(1), c11(2), r2);
c12 = [35 80]; ob12 = [c12(1)-r c12(1)+r c12(2)-r c12(2)+r]; circleExtrude(c12(1), c12(2), r2);
c13 = [40 80]; ob13 = [c13(1)-r c13(1)+r c13(2)-r c13(2)+r]; circleExtrude(c13(1), c13(2), r2);
c14 = [45 80]; ob14 = [c14(1)-r c14(1)+r c14(2)-r c14(2)+r]; circleExtrude(c14(1), c14(2), r2);
c15 = [80 80]; ob15 = [c15(1)-r c15(1)+r c15(2)-r c15(2)+r]; circleExtrude(c15(1), c15(2), r2);
c16 = [85 80]; ob16 = [c16(1)-r c16(1)+r c16(2)-r c16(2)+r]; circleExtrude(c16(1), c16(2), r2);
c17 = [90 80]; ob17 = [c17(1)-r c17(1)+r c17(2)-r c17(2)+r]; circleExtrude(c17(1), c17(2), r2);
c18 = [95 80]; ob18 = [c18(1)-r c18(1)+r c18(2)-r c18(2)+r]; circleExtrude(c18(1), c18(2), r2);
c19 = [100 80]; ob19 =[c19(1)-r c19(1)+r c19(2)-r c19(2)+r]; circleExtrude(c19(1), c19(2), r2);
c20 = [100 0]; ob20 = [c20(1)-r c20(1)+r c20(2)-r c20(2)+r]; circleExtrude(c20(1), c20(2), r2);
c21 = [100 5]; ob21 = [c21(1)-r c21(1)+r c21(2)-r c21(2)+r]; circleExtrude(c21(1), c21(2), r2);
c22 = [100 10]; ob22 =[c22(1)-r c22(1)+r c22(2)-r c22(2)+r]; circleExtrude(c22(1), c22(2), r2);
c23 = [60 100]; ob23 =[c23(1)-r c23(1)+r c23(2)-r c23(2)+r]; circleExtrude(c23(1), c23(2), r2);

obstacles = [ob1; ob2; ob3; ob4; ob5; ob6; ob7; ob8; ob9; ob10; ob11; ob12; ob13; ...
ob14; ob15; ob16; ob17; ob18; ob19; ob20; ob21; ob22; ob23];

circleExtrude(30,80,5);
%% obtaining the random inital point
z=0;
while(z==0)

V(1,:)=[xdim*rand ydim*rand];
z=ObtacleFree(V(1,:),obstacles);
end
V(1,:)= start_node;   %uncomment to force inital position
% plot(V(1,1), V(1,2), '.k', 'MarkerSize',30)
%%

Cost(1,:)=[V 0];
vv=[V V];
iter=0;
stp=0;

while (stp==0)

iter=iter+1;

xrand=[xdim*rand ydim*rand];
xnearest=Nearest(xrand,V);
xnew=Steer(xnearest,xrand);

if CollisionFree(xnearest,xnew,obstacles)
V=[V;xnew];
temp=(gama*log(length(V))/length(V))^(1/d);
r=min(eta,temp);
Xnear=Near(V,xnew,r);
xmin=xnearest;

costrow=find(any(Cost(:,1)==xnearest(1),2));
cost=Cost(costrow,3)+norm(xnearest-xnew);
cmin=cost;
Cost=[Cost;[xnew cost]];
oldc=cost;

for indx=1:size(Xnear,1)
xnear=Xnear(indx,:);
costrow=find(any(Cost(:,1)==xnear(1),2));
cxnear=Cost(costrow,3)+norm(xnear-xnew);
if CollisionFree(xnear,xnew,obstacles) &&  (cxnear<cmin)
xmin=xnear; cmin=cxnear;
end
end
costrow=find(any(Cost(:,1)==xnew(1),2));
Cost(costrow,:)=[];

costrow=find(any(Cost(:,1)==xmin(1),2));
cost=Cost(costrow,3)+norm(xmin-xnew);
Cost=[Cost;[xnew cost]];
vv=[vv;[xmin xnew]];
plot([xmin(1) xnew(1)],[xmin(2) xnew(2)],'green') %% plot tree branch

%% Rewire the tree
for indx=1:size(Xnear,1)
xnear=Xnear(indx,:);
cost=cost+norm(xnew-xnear);
costrow=find(any(Cost(:,1)==xnear(1),2));

if CollisionFree(xnew,xnear,obstacles) &&  (cost<Cost(costrow,3))
row=find(any(vv(:,3)==xnear(1),2));

plot([vv(row,1) xnear(1)],[vv(row,2) xnear(2)],'Color',backColor)%delete old branch on the plot
vv(row,:)=[];%delete old branch from the edges matrix
vv=[vv;[xnew xnear]];
plot([xnew(1) xnear(1)],[xnew(2) xnear(2)],'r')%draw the new branch
end
end
%%
stp=1;
row=find(any(vv(:,3)==xnew(1),2));
pathpoints=[];
while row>1
parentx=vv(row,1);
plot([vv(row,1) vv(row,3)],[vv(row,2) vv(row,4)],':r','LineWidth',3);
pathpoints=[pathpoints;[vv(row,1) vv(row,2)]];
row=find(any(vv(:,3)==parentx,2));
end
end
drawnow update
end
end

pat=[];
for i=1:size(pathpoints,1)-1
n=eta*5;
r=norm(pathpoints(i,:)-pathpoints(i+1,:))/n;

for j=1:n
pat=[pat;Steer2(pathpoints(i,:),pathpoints(i+1,:),j*r)];

end
end

pause(2);

iter=1;

%% SMOOTHING:
ax=pat(1,1);
ay=pat(1,2);
pats=[];

for t=1:size(pat,1)
f=0.98;
ax=f*ax+(1-f)*pat(t,1);
ay=f*ay+(1-f)*pat(t,2);
pats=[pats;[ax ay]];
end

%% Import Robot:
C = robot;
C = [C{1} C{2}];

plot_freq = 40;

plot(pats(:,1),pats(:,2),'cyan','LineWidth',2)

for k = size(pat,1):-1:2

xdiff=pats(k-1,2)-pats(k,2);
ydiff=pats(k-1,1)-pats(k,1);

th(k)=atan2(ydiff,xdiff);
camx=40*sin(th(k));
camy=40*cos(th(k));

% Rotational Matrix:
R = [sin(th(k)) -cos(th(k));
cos(th(k)) sin(th(k))]';
C_rotated =  C * R;

if mod(iter, plot_freq) == 0
plot( C_rotated(:,1) + pats(k,1), C_rotated(:,2) + pats(k,2), 'blue.');
end

pause(.01)
iter=iter+1;

end
pause(1)

mat_file_calc;

csvwrite(['Problem',num2str(problem),'.csv'], flipud([flipud(time) x y th2 v_now omegaf acc gammaf]));