test_rrt.m

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

addpath('../origamimaker/path-planning/');
% addpath('../utils/');
addpath('../Shoemaker-Levy/src/utils/stlTools/')
addpath('../Shoemaker-Levy/anatomical-models/')

path = fullfile( '..', 'Shoemaker-Levy', 'anatomical-models', 'synthetic-model.stl');
[vertices, faces, ~, ~] = stlRead(path);
earModel.vertices = vertices;
earModel.faces = faces;
% earModel.vertices = [];
% earModel.faces = [];

% Calculate the base transform for the robot
t = [30 10 10];
R = [0 0 -1; 0 1 0; 1 0 0];
T = [R t'; 0 0 0 1];
earModel.baseTransform = T;


robot1 = OrigamiRobot(0.7, 3);
robot2 = OrigamiRobot(0.7, 3);
robot3 = OrigamiRobot(0.7, 3);

%% Part 1. Step-by-step testing of RRT
fprintf('Testing RRT...\n')
% define the robot's range of motion
maxS = 5; % [mm]
maxTheta     = pi/2; % [rad]
maxPhi  = 2 * pi; % [rad]

robot = [robot1, robot2, robot3];
[qListNormalized,qList,pList,aList] = rrt(robot, ...
    [maxS maxTheta maxPhi maxS maxTheta maxPhi maxS maxTheta maxPhi], ...
    earModel, ...
    10);

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

figure

% Visualize the robot inside the cavity
ii = 1;
h1 = stlPlot(earModel.vertices, earModel.faces, 'Collision detection test.');
hold on
qq = qList(:,1);
bb = robot1.backbone(qq(1), qq(2), qq(3), T, 100);
robotPhysicalModel = robot1.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 = qList(:,ii);
    bb = robot1.backbone(qq(1), qq(2), qq(3), T, 100);
    TT1 = robot1.fkin(qq(1), qq(2), qq(3), T);
    robotPhysicalModel = robot1.robot_body(bb, 2, 7);
    if length(robot)==2
        bb2 = robot2.backbone(qq(4), qq(5), qq(6), TT1, 100);
        model2 = robot2.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(pList, 2))]);
    elseif length(robot)==3
        bb2 = robot2.backbone(qq(4), qq(5), qq(6), TT1, 100);
        TT2 = robot2.fkin(qq(4), qq(5), qq(6), TT1);
        model2 = robot2.robot_body(bb2, 2, 7);
        bb3 = robot3.backbone(qq(7), qq(8), qq(9), TT2, 100);
        model3 = robot3.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(pList, 2))]);
    else    

        h2.XData = robotPhysicalModel.X;
        h2.YData = robotPhysicalModel.Y;
        h2.ZData = robotPhysicalModel.Z;
        title(['Pose ' num2str(ii) ' of ' num2str(size(pList, 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(pList, 2), ii = size(pList, 2); end
        otherwise
            break
    end
end

close all

fprintf('\n Generating reachable workspace...\n')
shrinkFactor = 1;
[k,v] = boundary(pList(1,:)', pList(2,:)', pList(3,:)', 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(vertices, faces, 'Synthetic Model');
view([17.8 30.2]);

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

figure, hold on
stlPlot(vertices, faces, 'Synthetic Model');
view([17.8 30.2]);
trisurf(k, pList(7,:)', pList(8,:)', pList(9,:)','FaceColor','red','FaceAlpha',0.1)
title('Reachable workspace');

fprintf('Testing complete.\n')