%% 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 %% 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', ones(6,1)); nano_hexapod = computeJacobian(nano_hexapod); nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 130e-3, 'Mpr', 120e-3); displayArchitecture(nano_hexapod, 'labels', true); max_translation = 50e-6; max_rotation = 50e-6; 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; L_max = 0; 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 % L_max = 100e-6; % Actuator Stroke (+/-) % 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*max(abs(dL_lin_max)); rs(i, j) = stroke_ratio*L_max/max(abs(dL_lin)); end end min(min(rs)) max(max(rs)) [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(2e6*[-L_max, L_max]); ylim(2e6*[-L_max, L_max]); zlim(2e6*[-L_max, L_max]); %% Estimation of the required flexible joint angular stroke max_angles = zeros(1,6); % Compute initial strut orientation nano_hexapod = computeJointsPose(nano_hexapod, 'AP', zeros(3,1), 'ARB', eye(3)); As = nano_hexapod.geometry.As; % Only consider translations, but add maximum expected top platform rotation for i = 1:length(thetas) for j = 1:length(phis) % Maximum translations Tx = rs(i,j)*sin(thetas(i))*cos(phis(j)); Ty = rs(i,j)*sin(thetas(i))*sin(phis(j)); Tz = rs(i,j)*cos(thetas(i)); nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Tx; Ty; Tz], 'ARB', eye(3)); angles = acos(dot(As, nano_hexapod.geometry.As)); larger_angles = abs(angles) > max_angles; max_angles(larger_angles) = angles(larger_angles); end end sprintf('Maximum flexible joint angle is %.1f mrad', 1e3*max(max_angles))