%% 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))