%% 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; %% Example of a typical "cubic" architecture MO_B = -50e-3; % Position {B} with respect to {M} [m] H = 100e-3; % Height of the Stewart platform [m] Hc = 100e-3; % Size of the useful part of the cube [m] FOc = H + MO_B; % Center of the cube with respect to {F} % here positionned at the frame {B} stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1); stewart = initializeJointDynamics(stewart); stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); stewart = initializeCylindricalStruts(stewart); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart); displayArchitecture(stewart, 'labels', false, 'frames', false); plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false); view([40, 5]); %% Example of a typical "cubic" architecture MO_B = -20e-3; % Position {B} with respect to {M} [m] H = 40e-3; % Height of the Stewart platform [m] Hc = 100e-3; % Size of the useful part of the cube [m] FOc = H + MO_B; % Center of the cube with respect to {F} % here positionned at the frame {B} stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1); stewart = computeJacobian(stewart); stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); displayArchitecture(stewart, 'labels', false, 'frames', false); plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false); view([40, 5]); %% Analytical formula for Stiffness matrix of the Cubic Stewart platform % Define symbolic variables syms k Hc alpha H assume(k > 0); % k is positive real assume(Hc, 'real'); % Hc is real assume(H, 'real'); % H is real assume(alpha, 'real'); % alpha is real % Define si matrix (edges of the cubes) si = 1/sqrt(3)*[ [ sqrt(2), 0, 1]; ... [-sqrt(2)/2, -sqrt(3/2), 1]; ... [-sqrt(2)/2, sqrt(3/2), 1]; ... [ sqrt(2), 0, 1]; ... [-sqrt(2)/2, -sqrt(3/2), 1]; ... [-sqrt(2)/2, sqrt(3/2), 1] ... ]; % Define ci matrix (vertices of the cubes) ci = Hc * [ [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... [-sqrt(2), 0, 0.5]; ... [-sqrt(2), 0, 0.5] ... ]; % Apply vertical shift to ci ci = ci + H * [0, 0, 1]; % Calculate bi vectors (Stewart platform top joints) bi = ci + alpha * si; % Initialize stiffness matrix K = sym(zeros(6,6)); % Calculate elements of the stiffness matrix for i = 1:6 % Extract vectors for each leg s_i = si(i,:)'; b_i = bi(i,:)'; % Calculate cross product vector cross_bs = cross(b_i, s_i); % Build matrix blocks K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs'; K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i'; K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i'; K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs'; end % Scale by stiffness coefficient K = k * K; % Simplify the expressions K = simplify(K); % Display the analytical stiffness matrix disp('Analytical Stiffness Matrix:'); pretty(K); %% Cubic configuration H = 100e-3; % Height of the Stewart platform [m] Hc = 100e-3; % Size of the useful part of the cube [m] FOc = 50e-3; % Center of the cube at the Stewart platform center MO_B = -50e-3; % Position {B} with respect to {M} [m] MHb = 0; stewart_cubic = initializeStewartPlatform(); stewart_cubic = initializeFramesPositions(stewart_cubic, 'H', H, 'MO_B', MO_B); stewart_cubic = generateCubicConfiguration(stewart_cubic, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb); stewart_cubic = computeJointsPose(stewart_cubic); stewart_cubic = initializeStrutDynamics(stewart_cubic, 'k', 1); stewart_cubic = computeJacobian(stewart_cubic); stewart_cubic = initializeCylindricalPlatforms(stewart_cubic, 'Fpr', 150e-3, 'Mpr', 150e-3); % Let's now define the actuator stroke. L_max = 50e-6; % [m] %% Mobility of a Stewart platform with Cubic architecture - Translations thetas = linspace(0, pi, 100); phis = linspace(0, 2*pi, 200); rs = zeros(length(thetas), length(phis)); for i = 1:length(thetas) for j = 1:length(phis) Tx = sin(thetas(i))*cos(phis(j)); Ty = sin(thetas(i))*sin(phis(j)); Tz = cos(thetas(i)); dL = stewart_cubic.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction rs(i, j) = L_max/max(abs(dL)); % rs(i, j) = max(abs([dL(dL<0)*L_min; dL(dL>=0)*L_max])); end end [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); figure; hold on; surf(X, Y, Z, 'FaceColor', 'white', 'EdgeColor', colors(1,:)); quiver3(0, 0, 0, 150, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 150, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 0, 150, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); text(150, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); text(0, 150, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); text(0, 0, 150, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); hold off; axis equal; grid off; axis off; view(105, 15); %% Mobility of a Stewart platform with Cubic architecture - Rotations thetas = linspace(0, pi, 100); phis = linspace(0, 2*pi, 200); rs_cubic = zeros(length(thetas), length(phis)); for i = 1:length(thetas) for j = 1:length(phis) Rx = sin(thetas(i))*cos(phis(j)); Ry = sin(thetas(i))*sin(phis(j)); Rz = cos(thetas(i)); dL = stewart_cubic.kinematics.J*[0; 0; 0; Rx; Ry; Rz;]; rs_cubic(i, j) = L_max/max(abs(dL)); end end [phi_grid, theta_grid] = meshgrid(phis, thetas); X_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* cos(phi_grid); Y_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* sin(phi_grid); Z_cubic = 1e6 * rs_cubic .* cos(theta_grid); figure; hold on; surf(X_cubic, Y_cubic, Z_cubic, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:)); quiver3(0, 0, 0, 1500, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 1500, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 0, 1500, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); text(1500, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); text(0, 1500, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); text(0, 0, 1500, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); hold off; axis equal; grid off; axis off; view(105, 15); %% Input/Output definition of the Simscape model clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N] io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External metrology [m,rad] % Prepare simulation controller = initializeController('type', 'open-loop'); sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3); %% Cubic Stewart platform with payload above the top platform - B frame at the CoM H = 200e-3; % height of the Stewart platform [m] MO_B = 50e-3; % Position {B} with respect to {M} [m] stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart); stewart = initializeCylindricalPlatforms(stewart, ... 'Mpm', 1e-6, ... % Massless platform 'Fpm', 1e-6, ... % Massless platform 'Mph', 20e-3, ... % Thin platform 'Fph', 20e-3, ... % Thin platform 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', 1e-6, ... % Massless strut 'Msm', 1e-6, ... % Massless strut 'Fsh', stewart.geometry.l(1)/2, ... 'Msh', stewart.geometry.l(1)/2 ... ); % Run the linearization G_CoM = linearize(mdl, io)*inv(stewart.kinematics.J).'; G_CoM.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G_CoM.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; %% Same geometry but B Frame at cube's center (CoK) MO_B = -100e-3; % Position {B} with respect to {M} [m] stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart); stewart = initializeCylindricalPlatforms(stewart, ... 'Mpm', 1e-6, ... % Massless platform 'Fpm', 1e-6, ... % Massless platform 'Mph', 20e-3, ... % Thin platform 'Fph', 20e-3, ... % Thin platform 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', 1e-6, ... % Massless strut 'Msm', 1e-6, ... % Massless strut 'Fsh', stewart.geometry.l(1)/2, ... 'Msh', stewart.geometry.l(1)/2 ... ); % Run the linearization G_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.'); G_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; %% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of mass of the payload freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; % for i = 1:5 % for j = i+1:6 % plot(freqs, abs(squeeze(freqresp(G_CoM(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... % 'HandleVisibility', 'off'); % end % end plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... 'DisplayName', '$D_x/F_x$'); plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... 'DisplayName', '$D_y/F_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... 'DisplayName', '$D_z/F_z$'); plot(freqs, abs(squeeze(freqresp(G_CoM(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... 'DisplayName', '$R_x/M_x$'); plot(freqs, abs(squeeze(freqresp(G_CoM(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... 'DisplayName', '$R_y/M_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... 'DisplayName', '$R_z/M_z$'); plot(freqs, abs(squeeze(freqresp(G_CoM(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$R_x/F_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$R_y/F_x$'); plot(freqs, abs(squeeze(freqresp(G_CoM(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_x/M_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_y/M_x$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude'); leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-10, 2e-3]) %% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of the cube freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; % for i = 1:5 % for j = i+1:6 % plot(freqs, abs(squeeze(freqresp(G_CoK(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... % 'HandleVisibility', 'off'); % end % end plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... 'DisplayName', '$D_x/F_x$'); plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... 'DisplayName', '$D_y/F_y$'); plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... 'DisplayName', '$D_z/F_z$'); plot(freqs, abs(squeeze(freqresp(G_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... 'DisplayName', '$R_x/M_x$'); plot(freqs, abs(squeeze(freqresp(G_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... 'DisplayName', '$R_y/M_y$'); plot(freqs, abs(squeeze(freqresp(G_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... 'DisplayName', '$R_z/M_z$'); plot(freqs, abs(squeeze(freqresp(G_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$R_x/F_y$'); plot(freqs, abs(squeeze(freqresp(G_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$R_y/F_x$'); plot(freqs, abs(squeeze(freqresp(G_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_x/M_y$'); plot(freqs, abs(squeeze(freqresp(G_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_y/M_x$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude'); leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-10, 2e-3]) %% Cubic Stewart platform with payload above the top platform H = 200e-3; MO_B = -100e-3; % Position {B} with respect to {M} [m] stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart); stewart = initializeCylindricalPlatforms(stewart, ... 'Mpm', 1e-6, ... % Massless platform 'Fpm', 1e-6, ... % Massless platform 'Mph', 20e-3, ... % Thin platform 'Fph', 20e-3, ... % Thin platform 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', 1e-6, ... % Massless strut 'Msm', 1e-6, ... % Massless strut 'Fsh', stewart.geometry.l(1)/2, ... 'Msh', stewart.geometry.l(1)/2 ... ); % Sample at the Center of the cube sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3); % Run the linearization G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.'); G_CoM_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... 'DisplayName', '$D_x/F_x$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... 'DisplayName', '$D_y/F_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... 'DisplayName', '$D_z/F_z$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... 'DisplayName', '$R_x/M_x$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... 'DisplayName', '$R_y/M_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... 'DisplayName', '$R_z/M_z$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$R_x/F_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$R_y/F_x$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_x/M_y$'); plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$D_y/M_x$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-10, 2e-3]) %% Input/Output definition of the Simscape model clear io; io_i = 1; io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N] io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Displacement sensors [m] io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensor [N] % Prepare simulation : Payload above the top platform controller = initializeController('type', 'open-loop'); sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3); %% Cubic Stewart platform H = 200e-3; % height of the Stewart platform [m] MO_B = 50e-3; % Position {B} with respect to {M} [m] stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart); stewart = initializeCylindricalPlatforms(stewart, ... 'Mpm', 1e-6, ... % Massless platform 'Fpm', 1e-6, ... % Massless platform 'Mph', 20e-3, ... % Thin platform 'Fph', 20e-3, ... % Thin platform 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', 1e-6, ... % Massless strut 'Msm', 1e-6, ... % Massless strut 'Fsh', stewart.geometry.l(1)/2, ... 'Msh', stewart.geometry.l(1)/2 ... ); % Run the linearization G_cubic = linearize(mdl, io); G_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; G_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; %% Non-Cubic Stewart platform stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = generateGeneralConfiguration(stewart, 'FH', 25e-3, 'FR', 250e-3, 'MH', 25e-3, 'MR', 250e-3, ... 'FTh', [-22, 22, 120-22, 120+22, 240-22, 240+22]*(pi/180), ... 'MTh', [-60+22, 60-22, 60+22, 180-22, 180+22, -60-22]*(pi/180)); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart); stewart = initializeCylindricalPlatforms(stewart, ... 'Mpm', 1e-6, ... % Massless platform 'Fpm', 1e-6, ... % Massless platform 'Mph', 20e-3, ... % Thin platform 'Fph', 20e-3, ... % Thin platform 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', 1e-6, ... % Massless strut 'Msm', 1e-6, ... % Massless strut 'Fsh', stewart.geometry.l(1)/2, ... 'Msh', stewart.geometry.l(1)/2 ... ); % Run the linearization G_non_cubic = linearize(mdl, io); G_non_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; G_non_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; %% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... 'HandleVisibility', 'off'); end end plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ... 'DisplayName', '$l_i/f_i$'); plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... 'DisplayName', '$l_i/f_j$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-10, 1e-4]) %% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... 'HandleVisibility', 'off'); end end plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ... 'DisplayName', '$l_i/f_i$'); plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... 'DisplayName', '$l_i/f_j$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-10, 1e-4]) %% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... 'HandleVisibility', 'off'); end end plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ... 'DisplayName', '$f_{m,i}/f_i$'); plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... 'DisplayName', '$f_{m,i}/f_j$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]'); leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-4, 1e2]); %% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... 'HandleVisibility', 'off'); end end plot(freqs, abs(squeeze(freqresp(G_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ... 'DisplayName', '$f_{m,i}/f_i$'); plot(freqs, abs(squeeze(freqresp(G_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... 'DisplayName', '$f_{m,i}/f_j$'); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]'); leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); leg.ItemTokenSize(1) = 15; xlim([1, 1e4]); ylim([1e-4, 1e2]); %% Cubic configurations with center of the cube above the top platform H = 100e-3; % height of the Stewart platform [m] MO_B = 20e-3; % Position {B} with respect to {M} [m] FOc = H + MO_B; % Center of the cube with respect to {F} %% Small cube Hc = 2*MO_B; % Size of the useful part of the cube [m] stewart_small = initializeStewartPlatform(); stewart_small = initializeFramesPositions(stewart_small, 'H', H, 'MO_B', MO_B); stewart_small = generateCubicConfiguration(stewart_small, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); stewart_small = computeJointsPose(stewart_small); stewart_small = initializeStrutDynamics(stewart_small, 'k', 1); stewart_small = computeJacobian(stewart_small); stewart_small = initializeCylindricalPlatforms(stewart_small, 'Fpr', 1.1*max(vecnorm(stewart_small.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_small.platform_M.Mb))); %% ISO View displayArchitecture(stewart_small, 'labels', false, 'frames', false); plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); %% Side view displayArchitecture(stewart_small, 'labels', false, 'frames', false); plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); view([90,0]) %% Top view displayArchitecture(stewart_small, 'labels', false, 'frames', false); plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); view([0,90]) %% Example of a cubic architecture with cube's center above the top platform - Medium cube size Hc = H + 2*MO_B; % Size of the useful part of the cube [m] stewart_medium = initializeStewartPlatform(); stewart_medium = initializeFramesPositions(stewart_medium, 'H', H, 'MO_B', MO_B); stewart_medium = generateCubicConfiguration(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); stewart_medium = computeJointsPose(stewart_medium); stewart_medium = initializeStrutDynamics(stewart_medium, 'k', 1); stewart_medium = computeJacobian(stewart_medium); stewart_medium = initializeCylindricalPlatforms(stewart_medium, 'Fpr', 1.1*max(vecnorm(stewart_medium.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_medium.platform_M.Mb))); %% ISO View displayArchitecture(stewart_medium, 'labels', false, 'frames', false); plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); %% Side view displayArchitecture(stewart_medium, 'labels', false, 'frames', false); plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); view([90,0]) %% Top view displayArchitecture(stewart_medium, 'labels', false, 'frames', false); plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); view([0,90]) %% Example of a cubic architecture with cube's center above the top platform - Large cube size Hc = 2*(H + MO_B); % Size of the useful part of the cube [m] stewart_large = initializeStewartPlatform(); stewart_large = initializeFramesPositions(stewart_large, 'H', H, 'MO_B', MO_B); stewart_large = generateCubicConfiguration(stewart_large, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); stewart_large = computeJointsPose(stewart_large); stewart_large = initializeStrutDynamics(stewart_large, 'k', 1); stewart_large = computeJacobian(stewart_large); stewart_large = initializeCylindricalPlatforms(stewart_large, 'Fpr', 1.1*max(vecnorm(stewart_large.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_large.platform_M.Mb))); %% ISO View displayArchitecture(stewart_large, 'labels', false, 'frames', false); plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); %% Side view displayArchitecture(stewart_large, 'labels', false, 'frames', false); plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); view([90,0]) %% Top view displayArchitecture(stewart_large, 'labels', false, 'frames', false); plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); scatter3(0, 0, FOc, 200, 'kh'); view([0,90]) %% Get the analytical formula for the location of the top and bottom joints % Define symbolic variables syms k Hc Hcom alpha H assume(k > 0); % k is positive real assume(Hcom > 0); % k is positive real assume(Hc > 0); % Hc is real assume(H > 0); % H is real assume(alpha, 'real'); % alpha is real % Define si matrix (edges of the cubes) si = 1/sqrt(3)*[ [ sqrt(2), 0, 1]; ... [-sqrt(2)/2, -sqrt(3/2), 1]; ... [-sqrt(2)/2, sqrt(3/2), 1]; ... [ sqrt(2), 0, 1]; ... [-sqrt(2)/2, -sqrt(3/2), 1]; ... [-sqrt(2)/2, sqrt(3/2), 1] ... ]; % Define ci matrix (vertices of the cubes) ci = Hc * [ [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... [-sqrt(2), 0, 0.5]; ... [-sqrt(2), 0, 0.5] ... ]; % Apply vertical shift to ci ci = ci + (H + Hcom) * [0, 0, 1]; % Calculate bi vectors (Stewart platform top joints) bi = ci + alpha * si; % Extract the z-component value from the first row of ci % (all rows have the same z-component) ci_z = ci(1, 3); % The z-component of si is 1 for all rows si_z = si(1, 3); alpha_for_0 = solve(ci_z + alpha * si_z == 0, alpha); alpha_for_H = solve(ci_z + alpha * si_z == H, alpha); % Verify the results % Substitute alpha values and check the resulting bi_z values bi_z_0 = ci + alpha_for_0 * si; disp('Radius for fixed base:'); simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2)) bi_z_H = ci + alpha_for_H * si; disp('Radius for mobile platform:'); simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2))