test_wc_rrt2.m

clear; clc; close all;
c = distinguishable_colors(20);
tic;

addpath("stlTools/");
addpath('path-planning/');

%% load data
path = fullfile("stl/","PP_room3.stl");
[vertices, faces, ~, ~] = stlRead(path);
room.vertices = vertices * 0.0254;
room.faces = faces;

wc = WheelChair(3, path);
% wc.wc_stl = fullfile("stl/wheelchair.stl");
%%
% wc.setT_wc_robotBase([0, 0, 1, 0.1; 0, 1, 0, -330/1000; -1, 0, 0, 1000/1000; 0, 0, 0, 1]);
wc.setT_wc_robotBase([[0, 0, 1; 1, 0, 0; 0, 1, 0], [0; 0; 1]; 0, 0, 0, 1]);
%%
wc.T_world_wc = [[0, 0, 1; 1, 0, 0; 0, 1, 0], [2,6,0.05]'; 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 = 2;
% N = 200;        % number of moves
XY = zeros(nSteps,2);% store wheelchair positions

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;
    
    % Store (x,y)
    XY(i,:) = wc.T_world_wc(1:2,4)';

    %% Part 1. Step-by-step testing of RRT
    fprintf('Testing RRT...\n')
    % define the robot's range of motion
    maxS = 0.2; % [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, ...
        wc, ...
        room, ...
        20);
    
    disp(qList)
    % qqlist = [qqlist, qList];
    if size(qList, 2) == 1
        % If qlist has one column, add the value directly
        qqlist = [qqlist, qList];
    else
        % If qlist has two columns, add the second column value
        qqlist = [qqlist, qList(:, 2:end)];
    end
    
    % pplist = [pplist, pList];
    if size(pList, 2) == 1
        % If qlist has one column, add the value directly
        pplist = [pplist, pList];
    else
        % If qlist has two columns, add the second column value
        pplist = [pplist, pList(:, 2:end)];
    end
    TwcList = [TwcList; wc.T_world_wc];
    idxList = [idxList, ones(1, size(qList, 2) - 1)*idx];
    idx = idx + 1;

    waitbar(i/nSteps, hw, 'Overall Status');

    toc;
end
close(hw);

fprintf(['RRT execution complete. Total sampled points: ' num2str(size(qList,2)) ' \n\n']);
% save('test2000.mat')

figure

% Visualize the robot inside the cavity
ii = 1;
h1 = stlPlot(Vnew, Fnew, 'Collision detection test.');
hold on
qq = qqlist(:,1);
bb = wc.robots(1).backbone(qq(1), qq(2), qq(3), wc.T_world_wc*wc.T_wc_robotBase, 100);
robotPhysicalModel = wc.robots(1).robot_body(bb, 2, 7);
% robotPhysicalModel = robot.robot_body(qList(:,1), T);
h2 = surf(robotPhysicalModel.X, ...
    robotPhysicalModel.Y, ...
    robotPhysicalModel.Z, ...
    'FaceColor','blue');

axis equal

%for ii = 1 : size(pList, 2)
while true
    % robotPhysicalModel = robot.makePhysicalModel(qList(:,ii), T);

    qq = qqlist(:,ii);
    a = 4 * idxList(ii) - 3;
    % a = 4 * ii - 3;
    b = 4 * idxList(ii);
    % b = 4 * ii;
    disp([a, b])
    bb = wc.robots(1).backbone(qq(1), qq(2), qq(3), TwcList(a:b, :)*wc.T_wc_robotBase, 100);
    TT1 = wc.robots(1).fkin(qq(1), qq(2), qq(3), TwcList(a:b, :)*wc.T_wc_robotBase);
    robotPhysicalModel = wc.robots(1).robot_body(bb, 2, 7);

    plot3(XY(:,1), XY(:,2), 0.05*ones(size(XY,1),1), ...
      '-o', 'Color', c(13,:), 'LineWidth', 2, ...
      'MarkerFaceColor', c(13,:), 'MarkerSize', 2, ...
      'DisplayName','Wheelchair Path');

    if length(robot)==2
        bb2 = wc.robots(2).backbone(qq(4), qq(5), qq(6), TT1, 100);
        model2 = wc.robots(2).robot_body(bb2, 2, 7);
        h2.XData = [robotPhysicalModel.X; model2.X];
        h2.YData = [robotPhysicalModel.Y; model2.Y];
        h2.ZData = [robotPhysicalModel.Z; model2.Z];
        title(['Pose ' num2str(ii) ' of ' num2str(size(pplist, 2))]);
    elseif length(robot)==3
        bb2 = wc.robots(2).backbone(qq(4), qq(5), qq(6), TT1, 100);
        TT2 = wc.robots(2).fkin(qq(4), qq(5), qq(6), TT1);
        model2 = wc.robots(2).robot_body(bb2, 2, 7);
        bb3 = wc.robots(3).backbone(qq(7), qq(8), qq(9), TT2, 100);
        model3 = wc.robots(3).robot_body(bb3, 2, 7);
        h2.XData = [robotPhysicalModel.X; model2.X; model3.X];
        h2.YData = [robotPhysicalModel.Y; model2.Y; model3.Y];
        h2.ZData = [robotPhysicalModel.Z; model2.Z; model3.Z];
        title(['Pose ' num2str(ii) ' of ' num2str(size(pplist, 2))]);
    else    

        h2.XData = robotPhysicalModel.X;
        h2.YData = robotPhysicalModel.Y;
        h2.ZData = robotPhysicalModel.Z;
        title(['Pose ' num2str(ii) ' of ' num2str(size(pplist, 2))]);
    end

    fprintf('Press "n" to move forward or "p" to move back.\n')
    fprintf('Press any other key to stop testing and generate the reachable workspace.\n\n')

    while ~waitforbuttonpress, end
    k = get(gcf, 'CurrentCharacter');

    switch k
        case 'p'
            ii = ii - 1;
            if ii < 1, ii = 1; end
        case 'n'
            ii = ii + 1;
            if ii > size(pplist, 2), ii = size(pplist, 2); end
        otherwise
            break
    end
end

%%
% load("test100.mat")
%%
close all

fprintf('\n Generating reachable workspace...\n')
shrinkFactor = 1;
[k,v] = boundary(pplist(7,:)', pplist(8,:)', pplist(9,:)', shrinkFactor);

% figure
% scatter3(qList(1,:), qList(2,:), qList(3,:));
% axis equal, grid on
% xlabel('Robot Arc Length s [mm]');
% ylabel('Bending Angle \theta [rad]');
% zlabel('Bending Direction \phi [rad]');
% title('Configurations generated by RRT');
% 
% figure
% scatter3(qListNormalized(1,:), qListNormalized(2,:), qListNormalized(3,:));
% axis equal, grid on
% xlabel('Robot Arc Length s [mm]');
% ylabel('Bending Angle \theta [rad]');
% zlabel('Bending Direction \phi [rad]');
% title('Configurations generated by RRT (normalized)');
% 
% Visualize ear model
figure, hold on
stlPlot(wc.Vertices, wc.Faces, 'Synthetic Model');
view([17.8 30.2]);

scatter3(pplist(7,:), pplist(8,:), pplist(9,:),'red','filled');
axis equal, grid on
xlabel('X [m]'), ylabel('Y [m]'), zlabel('Z [m]');
title('Reachable points in the task space');

figure, hold on
stlPlot(wc.Vertices, wc.Faces, 'Synthetic Model');
view([17.8 30.2]);
trisurf(k, pplist(7,:)', pplist(8,:)', pplist(9,:)','FaceColor','red','FaceAlpha',0.1)
title('Reachable workspace');

fprintf('Testing complete.\n')

reachableSpace(pplist)