Origami_Workspace_MATLAB/rrtVisualization.m at main · comet-lab/Origami_Workspace_MATLAB · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
function rrtVisualization(simulationID)

    fileName = [simulationID '.mat'];
    load(fileName);
    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;
        b = 4 * idxList(ii);
        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);
        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,2:end), pplist(8,2:end), pplist(9,2:end),'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');

    reachableSpace(pplist);
end