Origami_Workspace_MATLAB/test_origami.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
clear; clc; close all;
c = distinguishable_colors(20);

% Collision Detection Package filepath
addpath('/Users/wenpengwang/Desktop/Comet Lab Projects/NSF/matlab/Shoemaker-Levy/src/collision/MatlabCode')

robot = OrigamiRobot(200, 300, 8);

%% Testing
% testing the forward_kinematics
% T1 = origami_fkin(5, pi/2, pi/2, eye(4))

%% Main
% Define total parameters for the robot
L = 800.0;                % Total arc length (mm)
theta_total = pi/2;       % Total bending angle for the full length (radians), e.g., 60 degrees
phi = 0;             % Bending plane orientation (radians), e.g., 45 degrees
nPoints = 100;

T1 = robot.fkin(L, pi/2, pi/4, eye(4));
T2 = robot.fkin(L, -pi/2, 0, T1);

% OD = 200;
backbone1 = robot.backbone(L, pi/2, pi/4, eye(4), nPoints);
backbone2 = robot.backbone(L, -theta_total, phi, T1, nPoints);
body1 = robot.robot_body(backbone1, 2, 20);
body2 = robot.robot_body(backbone2, 2, 20);


%% Test OctTree and Collision Detection
T3 = robot.fkin(L+50, pi/5, -pi/3, [1,0,0,0;0,1,0,220;0,0,1,0;0,0,0,1]);
backbone3 = robot.backbone(L+50, pi/5, -pi/3, [1,0,0,0;0,1,0,220;0,0,1,0;0,0,0,1], nPoints);
body3 = robot.robot_body(backbone3, 2, 20);


[w, l] = size(body1.X');
mesh1 = robot.mesh(body1);
mesh2 = robot.mesh(body2);
mesh3 = robot.mesh(body3);


% octree_body1 = OcTree(mesh1, 'maxSize', 20);
%
% octree_body1.Plot()
%
% octree_body2 = OcTree(mesh2, 'maxSize', 20);
% octree_body3 = OcTree(mesh3, 'maxSize', 20);
%
%
% collision_flag = CollisionDetection(octree_body1, octree_body3, 'Plotting', true);
% if collision_flag == 1
%     fprintf("Collision Detected!")
% else
%     fprintf("No Collision")
% end


%% Robot Visualization
% Plot the computed backbone in 3D
figure;

plot3(backbone1(1,:), backbone1(2,:), backbone1(3,:), 'LineWidth', 2, 'Color', c(11,:));
hold on;
plot3(backbone2(1,:), backbone2(2,:), backbone2(3,:), 'LineWidth', 2, 'Color', c(12,:));
% plot3(backbone3(1,:), backbone3(2,:), backbone3(3,:), 'LineWidth', 2, 'Color', c(13,:));
surf(body1.X, body1.Y, body1.Z, 'FaceAlpha', 0.5, 'EdgeColor', 'none', 'FaceColor', c(11,:), 'HandleVisibility', 'off');
surf(body2.X, body2.Y, body2.Z, 'FaceAlpha', 0.5, 'EdgeColor', 'none', 'FaceColor', c(12,:), 'HandleVisibility', 'off');
% surf(body3.X, body3.Y, body3.Z, 'FaceAlpha', 0.5, 'EdgeColor', 'none', 'FaceColor', c(13,:), 'HandleVisibility', 'off');
set(gca, 'FontName', 'CMU Serif', 'FontSize', 16)
legend({"Robot 1", "Robot 2"}, location="southeast")
robot.frame(eye(4), 50); % base frame
robot.frame(T1, 50); % frame at end tip - Robot 1
robot.frame(T2, 50); % frame at end tip - Robot 2
% robot.frame(T3, 50); % frame at end tip - Robot 3
% xlim([-10, 10]);
% ylim([-10, 10]);
% zlim([-10, 10]);
axis equal;
grid on;
xlabel('X [mm]');
ylabel('Y [mm]');
zlabel('Z [mm]');


% if collision_flag == 1
%     title('Collision Detected!')
% else
%     title('No Collision')
% end