237 lines
8.4 KiB
Matlab
237 lines
8.4 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for data
|
|
addpath('./src/'); % Path for functions
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
% Simulink Model name
|
|
mdl = 'nano_hexapod_model';
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Obtained Nano Hexapod Design
|
|
nano_hexapod = initializeStewartPlatform();
|
|
nano_hexapod = initializeFramesPositions(nano_hexapod, ...
|
|
'H', 95e-3, ...
|
|
'MO_B', 150e-3);
|
|
|
|
nano_hexapod = generateGeneralConfiguration(nano_hexapod, ...
|
|
'FH', 15e-3, ...
|
|
'FR', 120e-3, ...
|
|
'FTh', [220, 320, 340, 80, 100, 200]*(pi/180), ...
|
|
'MH', 15e-3, ...
|
|
'MR', 110e-3, ...
|
|
'MTh', [255, 285, 15, 45, 135, 165]*(pi/180));
|
|
nano_hexapod = computeJointsPose(nano_hexapod);
|
|
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'k', 1);
|
|
nano_hexapod = computeJacobian(nano_hexapod);
|
|
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 125e-3, 'Mpr', 115e-3);
|
|
|
|
%% Obtained architecture for the Nano Hexapod
|
|
displayArchitecture(nano_hexapod, 'labels', true, 'frames', true);
|
|
% Bottom circle
|
|
h = 15e-3;
|
|
r = 120e-3;
|
|
theta = linspace(0, 2*pi, 100);
|
|
x = r * cos(theta);
|
|
y = r * sin(theta);
|
|
z = h * ones(size(theta)); % All points at same height
|
|
plot3(x, y, z, '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
|
|
for i = 1:6
|
|
plot3([0, nano_hexapod.platform_F.Fa(1,i)], [0, nano_hexapod.platform_F.Fa(2,i)], [h,h], '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
|
|
end
|
|
% Top circle
|
|
h = 95e-3 - 15e-3;
|
|
r = 110e-3;
|
|
theta = linspace(0, 2*pi, 100);
|
|
x = r * cos(theta);
|
|
y = r * sin(theta);
|
|
z = h * ones(size(theta)); % All points at same height
|
|
plot3(x, y, z, '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
|
|
for i = 1:6
|
|
plot3([0, nano_hexapod.platform_M.Mb(1,i)], [0, nano_hexapod.platform_M.Mb(2,i)], [h,h], '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
|
|
end
|
|
|
|
%% Estimate required actuator stroke for the wanted mobility
|
|
max_translation = 50e-6; % Wanted translation mobility [m]
|
|
max_rotation = 50e-6; % Wanted rotation mobility [rad]
|
|
|
|
Dxs = linspace(-max_translation, max_translation, 3);
|
|
Dys = linspace(-max_translation, max_translation, 3);
|
|
Dzs = linspace(-max_translation, max_translation, 3);
|
|
Rxs = linspace(-max_rotation, max_rotation, 3);
|
|
Rys = linspace(-max_rotation, max_rotation, 3);
|
|
|
|
L_min = 0; % Required actuator negative stroke [m]
|
|
L_max = 0; % Required actuator negative stroke [m]
|
|
|
|
for Dx = Dxs
|
|
for Dy = Dys
|
|
for Dz = Dzs
|
|
for Rx = Rxs
|
|
for Ry = Rys
|
|
ARB = [ cos(Ry) 0 sin(Ry);
|
|
0 1 0;
|
|
-sin(Ry) 0 cos(Ry)] * ...
|
|
[ 1 0 0;
|
|
0 cos(Rx) -sin(Rx);
|
|
0 sin(Rx) cos(Rx)];
|
|
|
|
[~, Ls] = inverseKinematics(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);
|
|
|
|
if min(Ls) < L_min
|
|
L_min = min(Ls);
|
|
end
|
|
if max(Ls) > L_max
|
|
L_max = max(Ls);
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
|
|
sprintf('Actuator stroke should be from %.0f um to %.0f um', 1e6*L_min, 1e6*L_max)
|
|
|
|
%% Compute mobility in translation with combined angular motion
|
|
% Direction of motion (spherical coordinates)
|
|
thetas = linspace(0, pi, 100);
|
|
phis = linspace(0, 2*pi, 100);
|
|
|
|
% Considered Rotations
|
|
Rxs = linspace(-max_rotation, max_rotation, 3);
|
|
Rys = linspace(-max_rotation, max_rotation, 3);
|
|
|
|
% Maximum distance that can be reached in the direction of motion
|
|
% Considering combined angular motion and limited actuator stroke
|
|
rs = zeros(length(thetas), length(phis));
|
|
worst_rx_ry = zeros(length(thetas), length(phis), 2);
|
|
|
|
for i = 1:length(thetas)
|
|
for j = 1:length(phis)
|
|
% Compute unitary motion in the considered direction
|
|
Tx = sin(thetas(i))*cos(phis(j));
|
|
Ty = sin(thetas(i))*sin(phis(j));
|
|
Tz = cos(thetas(i));
|
|
% Start without considering rotations
|
|
dL_lin = nano_hexapod.kinematics.J*[Tx; Ty; Tz; 0; 0; 0];
|
|
% Strut motion for maximum displacement in the considered direction
|
|
dL_lin_max = L_max*dL_lin/max(abs(dL_lin));
|
|
|
|
% Find rotation that gives worst case stroke
|
|
dL_worst = max(abs(dL_lin_max)); % This should be equal to L_max
|
|
dL_rot_max = zeros(6,1);
|
|
% Perform (small) rotations, and find the (worst) case requiring maximum strut motion
|
|
for Rx = Rxs
|
|
for Ry = Rys
|
|
dL_rot = nano_hexapod.kinematics.J*[0; 0; 0; Rx; Ry; 0];
|
|
if max(abs(dL_lin_max + dL_rot)) > dL_worst
|
|
dL_worst = max(abs(dL_lin_max + dL_rot));
|
|
dL_rot_max = dL_rot;
|
|
worst_rx_ry(i,j,:) = [Rx, Ry];
|
|
end
|
|
end
|
|
end
|
|
|
|
stroke_ratio = min(abs([( L_max - dL_rot_max) ./ dL_lin_max; (-L_max - dL_rot_max) ./ dL_lin_max]));
|
|
dL_real = dL_lin_max*stroke_ratio + dL_rot_max;
|
|
|
|
% % Obtained maximum displacement in the considered direction with angular motion
|
|
rs(i, j) = stroke_ratio*L_max/max(abs(dL_lin));
|
|
end
|
|
end
|
|
|
|
%% Wanted translation mobility of the Nano-Hexapod and computed Mobility
|
|
[phi_grid, theta_grid] = meshgrid(phis, thetas);
|
|
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
|
|
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
|
|
Z = 1e6 * rs .* cos(theta_grid);
|
|
|
|
vertices = 1e6*max_translation*[
|
|
-1 -1 -1; % vertex 1
|
|
1 -1 -1; % vertex 2
|
|
1 1 -1; % vertex 3
|
|
-1 1 -1; % vertex 4
|
|
-1 -1 1; % vertex 5
|
|
1 -1 1; % vertex 6
|
|
1 1 1; % vertex 7
|
|
-1 1 1 % vertex 8
|
|
];
|
|
|
|
% Define the faces using the vertex indices
|
|
faces = [
|
|
1 2 3 4; % bottom face
|
|
5 6 7 8; % top face
|
|
1 2 6 5; % front face
|
|
2 3 7 6; % right face
|
|
3 4 8 7; % back face
|
|
4 1 5 8 % left face
|
|
];
|
|
|
|
figure;
|
|
hold on;
|
|
s = surf(X, Y, Z, 'FaceColor', 'none');
|
|
s.EdgeColor = colors(2, :);
|
|
patch('Vertices', vertices, 'Faces', faces, ...
|
|
'FaceColor', [0.7 0.7 0.7], ...
|
|
'EdgeColor', 'black', ...
|
|
'FaceAlpha', 1);
|
|
hold off;
|
|
axis equal;
|
|
grid on;
|
|
xlabel('X Translation [$\mu$m]'); ylabel('Y Translation [$\mu$m]'); zlabel('Z Translation [$\mu$m]');
|
|
xlim([-150 150]); ylim([-150, 150]); zlim([-120, 120]);
|
|
view(-160, 20)
|
|
|
|
%% Estimate required actuator stroke for the wanted mobility
|
|
max_translation = 50e-6; % Wanted translation mobility [m]
|
|
max_rotation = 50e-6; % Wanted rotation mobility [rad]
|
|
|
|
Dxs = linspace(-max_translation, max_translation, 3);
|
|
Dys = linspace(-max_translation, max_translation, 3);
|
|
Dzs = linspace(-max_translation, max_translation, 3);
|
|
Rxs = linspace(-max_rotation, max_rotation, 3);
|
|
Rys = linspace(-max_rotation, max_rotation, 3);
|
|
|
|
max_angles_F = zeros(1,6); % Maximum bending angle - Fixed joints [rad]
|
|
max_angles_M = zeros(1,6); % Maximum bending angle - Mobile joints [rad]
|
|
|
|
% Compute initial strut orientation
|
|
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', zeros(3,1), 'ARB', eye(3));
|
|
As = nano_hexapod.geometry.As; % Fixed joints
|
|
Bs = nano_hexapod.geometry.Bs; % Mobile joints
|
|
|
|
for Dx = Dxs
|
|
for Dy = Dys
|
|
for Dz = Dzs
|
|
for Rx = Rxs
|
|
for Ry = Rys
|
|
ARB = [ cos(Ry) 0 sin(Ry);
|
|
0 1 0;
|
|
-sin(Ry) 0 cos(Ry)] * ...
|
|
[ 1 0 0;
|
|
0 cos(Rx) -sin(Rx);
|
|
0 sin(Rx) cos(Rx)];
|
|
|
|
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);
|
|
|
|
angles_M = acos(dot(Bs, nano_hexapod.geometry.Bs));
|
|
max_angles_M(angles_M > max_angles_M) = angles_M(angles_M > max_angles_M);
|
|
|
|
angles_F = acos(dot(Bs, nano_hexapod.geometry.As));
|
|
max_angles_F(angles_F > max_angles_F) = angles_F(angles_F > max_angles_F);
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
|
|
sprintf('Fixed joint stroke should be %.1f mrad', 1e3*max(max_angles_F))
|
|
sprintf('Mobile joint stroke should be %.1f mrad', 1e3*max(max_angles_M))
|