Codes

% CS548 - Robot Motion Control and Planning Assignment IV
% PRM - Rapidly Growing Random Tree(RRT)
%
% Author: A. Deniz Gven

%Simulate.m-------------------------------------------------------------
%Initialization code for GUI----------------------------------------------
function varargout = Simulate(varargin)
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @Simulate_OpeningFcn, ...
'gui_OutputFcn', @Simulate_OutputFcn, ...
'gui_LayoutFcn', [], ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end


%
% MAIN FRAME FUNCTIONS-----------------------------------------------------
% --- Executes just before Simulate is made visible.
function Simulate_OpeningFcn(hObject, eventdata, handles, varargin) %#ok<INUSL>
% Choose default command line output for Visibility
handles.output = hObject;

set(handles.status,'String','Status: Idle...');

%Data Structures-----------------------------------------------------------
% linkLength defines link length for the planar robot arm
% start - 1x4 holds, base frame x, base frame y, Q1 and Q2
% obstacles - struct osbtacles (MAX 10 obstacles allowed)
% - vertexCount, denotes number of vertices
% - vertices, hold x,y coordinate of each vertex
% (MAX 14 vertices allowed)
%--------------------------------------------------------------------------

%Define Global Variables
global linkLength
global start goal
global obstacleCount
global obstacles
global loading

%Initialize
linkLength = 10;
start = zeros(1,4);
goal = zeros(1,4);
obstacleCount = 0;
loading = 0;

---

for i=1:10
obstacles(i).vertexCount = 0;
for j=1:14
obstacles(i).vertices(j).x = 0;
obstacles(i).vertices(j).y = 0;
end
end

%Draw workspace boundaries
DrawBoundaries;

% Update handles structure
guidata(hObject, handles);

% --- Outputs from this function are returned to the command line.
function varargout = Simulate_OutputFcn(hObject, eventdata, handles) %#ok<INUSL>
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;
%End of main frame fncs----------------------------------------------------
%

%
%--------------------------------------------------------------------------
function goal_Q1_CreateFcn(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function goal_Q2_CreateFcn(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function start_Q1_CreateFcn(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function start_Q2_CreateFcn(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function saveText_CreateFcn(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
function loadText_CreateFcn(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
if ispc && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
%

---

function goal_Q1_Callback(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
function goal_Q2_Callback(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
function start_Q1_Callback(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
function start_Q2_Callback(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
function saveText_Callback(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
function loadText_Callback(hObject, eventdata, handles) %#ok<INUSD,DEFNU>
function saveWithConfigurations_Callback(hObject, eventdata, handles)
%#ok<INUSD,DEFNU>
function loadWithConfigurations_Callback(hObject, eventdata, handles)
%#ok<INUSD,DEFNU>

%--------------------------------------------------------------------------


%
%CALLBACK FUNCTIONS--------------------------------------------------------
function drawStart_pushButton_Callback(hObject, eventdata, handles)
%#ok<INUSL,DEFNU>
global start
set(handles.status,'String','Edit Mode: Mark start configuration base frame...
Default: (0,0)');

q1 = get(handles.start_Q1,'String');
q2 = get(handles.start_Q2,'String');


q1 = str2num(q1); %#ok<ST2NM>
q2 = str2num(q2); %#ok<ST2NM>

%Convert to radians
q1 = (2*pi*q1)/360;
q2 = (2*pi*q2)/360;

start(1,3) = q1;
start(1,4) = q2;

hold on;
DrawStart;
hold off;

guidata(hObject, handles); %updates the handles

function drawGoal_pushButton_Callback(hObject, eventdata, handles)
%#ok<DEFNU,INUSL>
global goal
set(handles.status,'String','Edit Mode: Mark goal configuration base frame...
Default: (0,0)');

q1 = get(handles.goal_Q1,'String');
q2 = get(handles.goal_Q2,'String');


q1 = str2num(q1); %#ok<ST2NM>
q2 = str2num(q2); %#ok<ST2NM>

%Convert to radians
q1 = (2*pi*q1)/360;
q2 = (2*pi*q2)/360;

---

goal(1,3) = q1;
goal(1,4) = q2;

hold on;
DrawGoal;
hold off;

guidata(hObject, handles); %updates the handles

function drawObstacle_pushButton_Callback(hObject, eventdata, handles)
%#ok<INUSL,DEFNU>
set(handles.status,'String','Edit Mode: Draw obstacle...');

hold on;
DrawObstacle;
hold off;
guidata(hObject, handles); %updates the handles

function clearWorkspace_pushButton_Callback(hObject, eventdata, handles)
%#ok<INUSL,DEFNU>

cla(handles.axes); %clear axes
set(handles.status,'String','Status: Workspace cleared...');
guidata(hObject, handles); %updates the handles

global obstacleCount
obstacleCount = 0;


function animate_toggleButton_Callback(hObject, eventdata, handles)
%#ok<INUSL,DEFNU>
buttonStatus = get(handles.animate_toggleButton,'Value');
if(buttonStatus)
set(handles.status,'String','Status: Animation started...');
hold on;
Animate;
hold off;
else
set(handles.status,'String','Status: Animation stopped...');
end
guidata(hObject, handles); %updates the handles

function save_pushButton_Callback(hObject, eventdata, handles) %#ok<INUSL,DEFNU>
global obstacles obstacleCount start goal%#ok<NUSED>
checkBoxValue = get(handles.saveWithConfigurations,'Value');

str = get(handles.saveText,'String');
if(checkBoxValue == 1)
save (str,'obstacles','obstacleCount','start','goal');
else
save (str,'obstacles','obstacleCount');
end
set(handles.status,'String','Status: Workspace saved successfuly...')
guidata(hObject, handles);

function load_pushButton_Callback(hObject, eventdata, handles) %#ok<INUSL,DEFNU>
cla(handles.axes);

global obstacles obstacleCount start goal loading%#ok<NUSED>

---

checkBoxValue = get(handles.loadWithConfigurations,'Value');

str = get(handles.loadText,'String');
load (str);
if checkBoxValue == 1
loading = 1; %#ok<NASGU>
hold on;
DrawObstacles;
DrawStart;
DrawGoal;
hold off;

set(handles.start_Q1,'String',num2str(start(1,3)*180/pi));
set(handles.start_Q2,'String',num2str(start(1,4)*180/pi));

set(handles.goal_Q1,'String',num2str(goal(1,3)*180/pi));
set(handles.goal_Q2,'String',num2str(goal(1,4)*180/pi));
else
DrawObstacles;
end
loading = 0;
set(handles.status,'String','Status: Workspace loaded successfuly...')
guidata(hObject, handles);


%End of callback functions-------------------------------------------------
%

%End of Simulate.m-------------------------------------------------------
%





%Animate.m-----------------------------------------------------------------
%DrawRobot;

global success

BuildTrees;
if success
set(handles.status,'String','Status: Path found from Qstart to Qgoal');
pause(0.9);
TraversePath;
else
set(handles.status,'String','Status: No path found from Qstart to Qgoal');
end


%End of Animate.m----------------------------------------------------------




%BuildTrees.m---------------------------------------------------------------
%Build two RRT trees, one rooted at startNode and one rooted at goalNode

---

%Then it tries to merge the trees into one
%If the trees are merged,
%then it set success to 1 for Animate.m to call on FindPath.m
%otherwise it is set to 0 to report on failure

global success
global start goal
global color1
global isMerged
global aPath

isMerged = 0;

startNode = Node(start,1);
goalNode = Node(goal,1);

T1 = Tree(startNode);
T2 = Tree(goalNode);

N = 180;%number of trials to expand the trees

%Expand T1 first-----------------------------------------------------------
set(handles.status,'String','Status: Building the first tree...');
color1 = [ .4 .9 .4 ]; %#ok<NASGU>
T1 = expand(T1,N);
%Tree.display(T1)

%Now expand T2-------------------------------------------------------------
set(handles.status,'String','Status: Building the second tree...');
color1 = [ .9 .4 .4 ]; %#ok<NASGU>
T2 = expand(T2,N);

%Try to merge T1 and T2----------------------------------------------------
set(handles.status,'String','Status: Trying to merge the trees...');

aPath = merge(T1,T2);
if isMerged
success = 1;
else
success = 0;
end
%End of BuildTrees.m-------------------------------------------------------
%







function outputTree = expand(aTree,trialCount)

% 3: for i = 1 to n do
% 4: qrand ? a randomly chosen free configuration
% 5: extend RRT (T, qrand)
% 6: end for

% Input:

---

% T = (V, E): an RRT
% q:a configuration toward which the tree T is grown
% Output:
% A new configuration qnew toward q,or NIL in case of failure
% 1: qnear ? closest neighbor of q in T
% 2: qnew ? progress qnear by step_size along the straight line in between qnear
and qrand
% 3: if qnew is collision-free then
% 4: V ? V ? {qnew}
% 5: E ? E ? {(qnear, qnew)}
% 6: return qnew
% 7: end if
% 8: return NIL

for j=1:trialCount

first = 1;
%Find a FREE configuration
while first || ~free
if first
first = 0;
end
randomConf = randomConfiguration();
free = isFree(randomConf);
end

%drawRandom(randomConf);
closestNode = Tree.findClosest(aTree,randomConf);
pause(0.000000000000000000000000000000000000001);
newConf = step(closestNode,randomConf);
if isFree(newConf) == 1

free = isInside(newConf);
if free
aLine = line([ closestNode.configuration(1) newConf(1) ], ...
[ closestNode.configuration(2) newConf(2) ]);
set(aLine,'Color',[ .4 .4 .4],'LineWidth',1);

drawRobot(newConf);
aTree = Tree.addToTree(aTree,closestNode,newConf);
end
end
%j
end


outputTree = aTree;

function newConf = step(closestNode, randomConf)
%
x_step = 3;
y_step = 1.5;
angle_step = 9; %in degrees
%angle_step = (2*pi*angle_step)/360; %in radians

myConfiguration = closestNode.configuration;

xc = myConfiguration(1);

---

yc = myConfiguration(2);
q1c = myConfiguration(3);
q2c = myConfiguration(4);

%Convert to degrees
q1c = (360*q1c)/(2*pi);
q2c = (360*q2c)/(2*pi);

xr = randomConf(1);
yr = randomConf(2);
q1r = randomConf(3);
q2r = randomConf(4);

%Convert to degrees
q1r = (360*q1r)/(2*pi);
q2r = (360*q2r)/(2*pi);

if xc < xr
xn = xc + x_step;
else
xn = xc - x_step;
end

if yc < yr
yn = yc + y_step;
else
yn = yc - y_step;
end


%Angles....

if q1c < q1r

d1 = q1r - q1c;
d2 = 360 - d1;

if d2 < d1
q1n = q1c - angle_step;
else
q1n = q1c + angle_step;
end
else
d1 = q1c - q1r;
d2 = 360 - d1;

if d2 < d1
q1n = q1c + angle_step;
else
q1n = q1c - angle_step;
end

end


if q1n < 0
q1n = 360 - q1n;
end

---

%Second angle....
if q2c < q2r

d1 = q2r - q2c;
d2 = 360 - d1;

if d2 < d1
q2n = q2c - angle_step;
else
q2n = q2c + angle_step;
end
else
d1 = q2c - q2r;
d2 = 360 - d1;

if d2 < d1
q2n = q2c + angle_step;
else
q2n = q2c - angle_step;
end

end


if q2n < 0
q2n = 360 - q2n;
end

%Convert angles to radians
q1n = (2*pi*q1n)/360;
q2n = (2*pi*q2n)/360;

newConf = [ xn yn q1n q2n];






function free = isFree(aConfiguration)
global obstacleCount obstacles
global linkLength

% obstacles - struct osbtacles (MAX 10 obstacles allowed)
% - vertexCount, denotes number of vertices
% - vertices, hold x,y coordinate of each vertex
% (MAX 14 vertices allowed)

q1 = aConfiguration(3);
q2 = aConfiguration(4);

x1 = aConfiguration(1);
y1 = aConfiguration(2);

x2 = x1 + linkLength*cos(q1);
y2 = y1 + linkLength*sin(q1);

x3 = x2 + linkLength*cos(q2);

---

y3 = y2 + linkLength*sin(q2);

p12 = [ x1 y1 x2 y2 ];
p34 = [ x2 y2 x3 y3 ];

free = 1;
for i=1:obstacleCount

up = obstacles(i).vertexCount;
for j=1:up

%Construct the edges
x5 = obstacles(i).vertices(j).x;
y5 = obstacles(i).vertices(j).y;

if j == up %last edge
x6 = obstacles(i).vertices(1).x;
y6 = obstacles(i).vertices(1).y;
else
x6 = obstacles(i).vertices(j+1).x;
y6 = obstacles(i).vertices(j+1).y;
end

p56 = [ x5 y5 x6 y6 ];

%Check first link with the current edge
%segmentsIntersect(p12,p34,type)
intersect1 = segmentsIntersect(p12,p56);
%Check second link with the current edge
intersect2 = segmentsIntersect(p34,p56);

if intersect1 || intersect2
free = 0;
break;
end

end % check all edges of an obstacle

if free == 0
break;
end
end %check all obstacles

%Check boundary
if free == 1
for i=1:4
switch i
case 1
x5 = 0;
y5 = 0;
x6 = 120;
y6 = 0;
case 2
x5 = 120;
y5 = 0;
x6 = 120;
y6 = 100;
case 3
x5 = 120;

---