rrtWheelChair.m

function rrtWheelChair(path, steps, simulationID)
    fprintf('* Estimation of the reachable workspace *\n')
    addpath("stlTools/");
    addpath('path-planning/');

    % path = fullfile("stl/","room_space.stl");
    [vertices, faces, ~, ~] = stlRead(path);
    room.vertices = vertices / 1000;
    room.faces = faces;
    
    wc = WheelChair(3, path);
    wc.T_world_wc = [eye(3), [1,0,0]'; 0, 0, 0, 1]; % move the wheelchair to free space

    room.baseTransform = wc.T_world_wc * wc.T_wc_robotBase;
    
    qqlist = [];
    pplist = [];
    TwcList = [];
    idx = 1;
    idxList = [];
    
    hw = waitbar(0, 'Sampling the configuration space. Please wait...');
    
    nSteps = steps;

    for i = 1:nSteps
        % add wheelchair into workspace
        % wc.spawnWheelChair();
        wc.driveWheelChair();
        [V, F] = wc.boundingBox();
        [Vnew, Fnew] = stlAddVerts(wc.Vertices, wc.Faces, V, F);
        room.vertices = Vnew;
        room.faces = Fnew;
        % Vnew = wc.Vertices;
        % Fnew = wc.Faces;
    
        %% Part 1. Step-by-step testing of RRT
        fprintf('Testing RRT...\n')
        % define the robot's range of motion
        maxS = 0.5; % [m]
        maxTheta     = pi/2; % [rad]
        maxPhi  = 2 * pi; % [rad]
    
        robot = [wc.robots(1), wc.robots(2), wc.robots(3)];
        room.baseTransform = wc.T_world_wc * wc.T_wc_robotBase;
        [qListNormalized,qList,pList,aList] = rrt_wc(robot, ...
            [maxS maxTheta maxPhi maxS maxTheta maxPhi maxS maxTheta maxPhi], ...
            room, ...
            50);
        qqlist = [qqlist, qList];
        
        pplist = [pplist, pList];
        TwcList = [TwcList; wc.T_world_wc];
        idxList = [idxList, ones(1, length(qList))*idx];
        idx = idx + 1;
    
        waitbar(i/nSteps, hw, 'Overall Status');
    end
    close(hw);
    
    fprintf(['RRT execution complete. Total sampled points: ' num2str(size(qList,2)) ' \n\n']);
    save([simulationID '.mat'])
end