1 Star 0 Fork 1

nealliu / rrt_dynamic

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
RunMe.m 7.74 KB
Copy Edit Web IDE Raw Blame History
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];
radius = 20;
eta = 5; %EPSILON
elseif problem == 2
start_node = [5 60];
Target = [100 60];
radius = 20;
eta = 10; %EPSILON
elseif problem == 3
start_node = [10 90];
Target = [100 90];
radius = 10;
eta = 5; %EPSILON
elseif problem == 4
start_node = [63 80];
Target = [60 10];
radius = 10;
eta = 10; %EPSILON
elseif problem == 5
start_node = [40 40];
Target = [1 1];
radius = 20;
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];
circle(Target(1),Target(2),radius);
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
%%
if norm(xnew-Target)<radius
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;
%% Follow the Smoothed path
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]));

Comment ( 0 )

Sign in for post a comment

Matlab
1
https://gitee.com/nealliu/rrt_dynamic.git
git@gitee.com:nealliu/rrt_dynamic.git
nealliu
rrt_dynamic
rrt_dynamic
master

Search