rrt_motion_planning.m

function percorso=rrt_motion_planning(q0,qf,max_iterations,...
                                      env_dimension,map,figure_num)
start_point = q0(1:3);
end_point   = qf(1:3);

xmax=env_dimension(1);
ymax=env_dimension(2);
zmax=env_dimension(3);

EPS = 5;
i=0;
find=false;

% Lista dei nodi
nodes(1,:) = start_point;

% Lista degli archi dove i primi tre elementi sono il nodo predecessore
% gli ultimi tre sono il nodo successore.
%                       [xp yp zp xs ys zs]
archi(1,:) = [ 0  0  0  0  0  0];

figure(figure_num)
subplot(1,2,1)
sgtitle('\textbf{Motion Planning}','fontsize',18, 'interpreter', 'latex') 

while (i<max_iterations)
    % Incremento il numero di iterazioni/nodi
    i=i+1; 
    caption = sprintf('RTT - Iteration %d / %d', i , max_iterations);
    title(caption, 'FontSize', 14);

    % Calcolo qrand
    qrand = rand_conf(xmax,ymax,zmax);
    % Calcolo qnear
    [qnear,val] = nearest_vertex(nodes,qrand);
    % Calcolo q_new
    qnew=new_conf(qrand, qnear, val, EPS);
    % Collision Check
    collision=checkcollision(map,qnear,qnew);

    if collision == false
       % In assenza di collisione lungo l'arco qnear-qnew 
       % aggiungo qnew all'insieme dei nodi 
       % e l'arco alla lista degli archi. Infine li mostro a video 
       nodes = [nodes; qnew];
       archi(end+1,:) = [qnear qnew];
       
       plot3(qnew(1), qnew(2), qnew(3), 'go')
       line([qnear(1), qnew(1)],...
            [qnear(2), qnew(2)],...
            [qnear(3), qnew(3)], 'Color', 'k', 'LineWidth', 1);
       
       % Verifico di essere abbastanza vicino al goal e in caso positivo
       % avvio una collision check. Se quest'ultimo controllo va a buon
       % fine allora sono arrivato al goal.
       if norm(end_point-qnew)<=10
           finalcollision = checkcollision(map,qnew,end_point);
           if finalcollision == false
             archi(end+1,:) = [qnew end_point];
             plot3(qnew(1),qnew(2),qnew(3), 'ro')
             line([qnew(1), end_point(1)],...
                  [qnew(2), end_point(2)],...
                  [qnew(3), end_point(3)], 'Color', 'k', 'LineWidth', 1); 
             find = true;
           end
       end
    end
    % Animazione
    drawnow;
end   

% Visualizzazione percorso
if find == true
    disp("Find Path")
    nodes(end,:)=end_point;
   
    percorso=[];
    path_cost=0;
    [percorso,path_cost]=search_path(start_point,end_point,archi);

    t=linspace(1,10);
    for j=1:size(percorso,1)-1
       x=linspace(percorso(j,1),percorso(j+1,1),length(t));
       y=linspace(percorso(j,2),percorso(j+1,2),length(t));
       z=linspace(percorso(j,3),percorso(j+1,3),length(t));
    
       for i=1:length(t)
           plot3(x(1:i),y(1:i),z(1:i),"ro","LineWidth",1.5)
       end
    end
    disp("RRT - Path Cost")
    disp(path_cost)

    figure(figure_num)
    subplot(1,2,2)
    title('\textbf{Planned Path}','fontsize',14,'interpreter','latex')
    
   t=linspace(1,10);
   for j=1:size(percorso,1)-1
    
        x=linspace(percorso(j,1),percorso(j+1,1),length(t));
        y=linspace(percorso(j,2),percorso(j+1,2),length(t));
        z=linspace(percorso(j,3),percorso(j+1,3),length(t));
    
        for i=1:length(t)
            plot3(x(1:i),y(1:i),z(1:i),"ro","LineWidth",1.5)
        end
    end
else
   disp("Path Not Found")
end

end


%% Funzioni

function q_rand = rand_conf(xmax,ymax,zmax)
    q_rand = [ceil(rand(1)*xmax) ceil(rand(1)*ymax) ceil(rand(1)*zmax)];
    plot3(q_rand(1), q_rand(2), q_rand(3), 'x', 'Color',  [0 0.4470 0.7410])
end

function [q_near,val] = nearest_vertex(nodes,q_rand)
    dist_nodes=[];
    indice=0;
    val=0;
    for i=1:size(nodes,1)
        dist_nodes(i)=norm(q_rand-nodes(i,:));
    end
    [val,indice]=min(dist_nodes);
    q_near=round(nodes(indice,:));
   
 end

function q_new = new_conf(q_rand, q_near, val, eps)
   q_new = [0 0 0];
   if val >= eps
       q_new(1) = q_near(1) + ((q_rand(1)-q_near(1))*eps)/norm(q_rand-q_near);
       q_new(2) = q_near(2) + ((q_rand(2)-q_near(2))*eps)/norm(q_rand-q_near);
       q_new(3) = q_near(3) + ((q_rand(3)-q_near(3))*eps)/norm(q_rand-q_near);
       q_new=[round(q_new(1)) round(q_new(2))  round(q_new(3))] ;
   else
       q_new(1) = q_rand(1);
       q_new(2) = q_rand(2);
       q_new(3) = q_rand(3);
   end
end

function risultato = checkcollision(scenario,q1,q2)
   
    found = false;
    if checkOccupancy(scenario,q2)==1
        found = true;
    end
    if found == false
       
        dt = 0:0.1:1;
        segmento_x = q1(1) + dt*(q2(1)-q1(1));
        segmento_y = q1(2) + dt*(q2(2)-q1(2));
        segmento_z = q1(3) + dt*(q2(3)-q1(3));
        
        i=1;
        while (i<size(segmento_x,2) && found==false)
             if checkOccupancy(scenario, [ floor(segmento_x(i)) floor(segmento_y(i)) floor(segmento_z(i))])==1
                found = true;
             end
             i=i+1;
        end
    end
   
    risultato = found;
end

% Funzione che ricerca il percorso nell'albero a partire dalla destinazione
function [percorso,path_cost]= search_path(start_point,end_point,archi)

    % Creo un buffer in cui mettere la destinazione
    buffer=end_point;
    percorso=[buffer];
    path_cost=0;
    
    % Il ciclo while continua fin quando non raggiungo la partenza. 
    while isequal(buffer,start_point)==false
       
       index=0;
       % Metto un valore arbitrariamente alto per la distanza minima
       min_dist=1000;
       % Per ogni nodo destinazione cerco il precedessore più vicino.
       for i=1:size(archi,1)
           if isequal(buffer,archi(i,4:6))==true &&...
                      norm(buffer-archi(i,1:3))<=min_dist
               min_dist=norm(buffer-archi(i,1:3));
               path_cost=path_cost+min_dist;
               index=i;
           end
       end
       % Aggiorno il buffer   
       buffer=archi(index,1:3);
       % Lo aggiungo al percorso
       percorso=[percorso; buffer];
    end
    % Inverto il percorso per avere l'ordine corretto
    percorso=flip(percorso);
end