From 843fb79413ecf805859f43577f54c517b4ee3b6f Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 2 Apr 2025 16:56:24 +0200 Subject: [PATCH] Verify that matlab scripts are working --- matlab/detail_kinematics_1_geometry.m | 26 +- matlab/detail_kinematics_2_cubic.m | 1068 +++++++++++---------- matlab/detail_kinematics_3_nano_hexapod.m | 148 ++- nass-geometry.org | 16 +- 4 files changed, 681 insertions(+), 577 deletions(-) diff --git a/matlab/detail_kinematics_1_geometry.m b/matlab/detail_kinematics_1_geometry.m index d9846c3..12c58d3 100644 --- a/matlab/detail_kinematics_1_geometry.m +++ b/matlab/detail_kinematics_1_geometry.m @@ -7,6 +7,10 @@ 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; @@ -20,7 +24,7 @@ stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ... 'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180)); stewart = computeJointsPose(stewart); -stewart = initializeStrutDynamics(stewart, 'K', ones(6,1)); +stewart = initializeStrutDynamics(stewart, 'k', 1); stewart = computeJacobian(stewart); stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3); @@ -75,7 +79,7 @@ stewart_vert = generateGeneralConfiguration(stewart_vert, 'FH', 0, 'FR', 100e-3, 'FTh', [-25, 25, 120-25, 120+25, 240-25, 240+25]*(pi/180), ... 'MTh', [-60+25, 60-25, 60+25, 180-25, 180+25, -60-25]*(pi/180)); stewart_vert = computeJointsPose(stewart_vert); -stewart_vert = initializeStrutDynamics(stewart_vert, 'K', ones(6,1)); +stewart_vert = initializeStrutDynamics(stewart_vert, 'k', 1); stewart_vert = computeJacobian(stewart_vert); stewart_vert = initializeCylindricalPlatforms(stewart_vert, 'Fpr', 110e-3, 'Mpr', 110e-3); @@ -88,15 +92,15 @@ stewart_hori = generateGeneralConfiguration(stewart_hori, 'FH', 0, 'FR', 100e-3, 'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ... 'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180)); stewart_hori = computeJointsPose(stewart_hori); -stewart_hori = initializeStrutDynamics(stewart_hori, 'K', ones(6,1)); +stewart_hori = initializeStrutDynamics(stewart_hori, 'k', 1); stewart_hori = computeJacobian(stewart_hori); stewart_hori = initializeCylindricalPlatforms(stewart_hori, 'Fpr', 110e-3, 'Mpr', 110e-3); displayArchitecture(stewart_hori, 'labels', false, 'frames', false, 'F_color', colors(2,:), 'M_color', colors(2,:), 'L_color', colors(2,:)); %% Translation mobility for two Stewart platform geometry -thetas = linspace(0, pi, 50); -phis = linspace(0, 2*pi, 50); +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 100); rs_vert = zeros(length(thetas), length(phis)); rs_hori = zeros(length(thetas), length(phis)); @@ -131,8 +135,8 @@ Zs = 1e6*L_max*Zs; figure; hold on; -surf(X_vert, Y_vert, Z_vert, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:)); -surf(X_hori, Y_hori+400, Z_hori, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(2,:)); +surf(X_vert, Y_vert, Z_vert, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:)); +surf(X_hori, Y_hori+400, Z_hori, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:)); quiver3(0, 0, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); @@ -158,7 +162,7 @@ stewart_close = generateGeneralConfiguration(stewart_close, 'FH', 0, 'FR', 50e-3 'FTh', [-6, 6, 120-6, 120+6, 240-6, 240+6]*(pi/180), ... 'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180)); stewart_close = computeJointsPose(stewart_close); -stewart_close = initializeStrutDynamics(stewart_close, 'K', ones(6,1)); +stewart_close = initializeStrutDynamics(stewart_close, 'k', 1); stewart_close = computeJacobian(stewart_close); stewart_close = initializeCylindricalPlatforms(stewart_close, 'Fpr', 110e-3, 'Mpr', 110e-3); @@ -172,7 +176,7 @@ stewart_space = generateGeneralConfiguration(stewart_space, 'FH', 0, 'FR', 100e- 'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180)); stewart_space.platform_F.Fa = stewart_space.platform_M.Mb - (stewart_close.platform_M.Mb - stewart_close.platform_F.Fa); stewart_space = computeJointsPose(stewart_space); -stewart_space = initializeStrutDynamics(stewart_space, 'K', ones(6,1)); +stewart_space = initializeStrutDynamics(stewart_space, 'k', 1); stewart_space = computeJacobian(stewart_space); stewart_space = initializeCylindricalPlatforms(stewart_space, 'Fpr', 110e-3, 'Mpr', 110e-3); @@ -210,8 +214,8 @@ Z_space = 1e6 * rs_space .* cos(theta_grid); figure; hold on; -surf(X_close, Y_close, Z_close, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:)); -surf(X_space, Y_space+6000, Z_space, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(2,:)); +surf(X_close, Y_close, Z_close, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:)); +surf(X_space, Y_space+6000, Z_space, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:)); quiver3(0, 0, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); quiver3(0, 0, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); diff --git a/matlab/detail_kinematics_2_cubic.m b/matlab/detail_kinematics_2_cubic.m index 6e30455..15f2487 100644 --- a/matlab/detail_kinematics_2_cubic.m +++ b/matlab/detail_kinematics_2_cubic.m @@ -1,16 +1,3 @@ -%% 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; - %% Example of a typical "cubic" architecture MO_B = -50e-3; % Position {B} with respect to {M} [m] @@ -25,9 +12,12 @@ 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', ones(6,1)); -stewart = computeJacobian(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.5], 'link_to_struts', true); @@ -46,47 +36,95 @@ 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', ones(6,1)); +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.5], 'link_to_struts', true); -%% Cubic Architecture - Effect of the position of frame {A} and {B} -H = 100e-3; % Height of the Stewart platform [m] -Hc = 100e-3; % Size of the useful part of the cube [m] -FOc = H/2; % Center of the cube at the Stewart platform center +%% Clear Workspace and Close figures +clear; close all; clc; -%% Frames {A} and {B} at the cube's center -MO_B = -50e-3; % Position {B} with respect to {M} [m] +%% Intialize Laplace variable +s = zpk('s'); -stewart_center = initializeStewartPlatform(); -stewart_center = initializeFramesPositions(stewart_center, 'H', H, 'MO_B', MO_B); -stewart_center = generateCubicConfiguration(stewart_center, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); -stewart_center = computeJointsPose(stewart_center); -stewart_center = initializeStrutDynamics(stewart_center, 'K', ones(6,1)); -stewart_center = computeJacobian(stewart_center); -stewart_center = initializeCylindricalPlatforms(stewart_center, 'Fpr', 150e-3, 'Mpr', 150e-3); +%% Path for functions, data and scripts +addpath('./mat/'); % Path for data +addpath('./src/'); % Path for functions +addpath('./subsystems/'); % Path for Subsystems Simulink files -displayArchitecture(stewart_center, 'labels', false, 'frames', true); -plotCube(stewart_center, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +% Simulink Model name +mdl = 'nano_hexapod_model'; -%% Frames {A} and {B} offset from the cube's center -MO_B = 50e-3; % Position {B} with respect to {M} [m] +%% Colors for the figures +colors = colororder; -stewart_offset = initializeStewartPlatform(); -stewart_offset = initializeFramesPositions(stewart_offset, 'H', H, 'MO_B', MO_B); -stewart_offset = generateCubicConfiguration(stewart_offset, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); -stewart_offset = computeJointsPose(stewart_offset); -stewart_offset = initializeStrutDynamics(stewart_offset, 'K', 2*ones(6,1)); -stewart_offset = computeJacobian(stewart_offset); -stewart_offset = initializeCylindricalPlatforms(stewart_offset, 'Fpr', 150e-3, 'Mpr', 150e-3); +%% Analytical formula for Stiffness matrix of the Cubic Stewart platform +% Define symbolic variables +syms k Hc alpha H -displayArchitecture(stewart_offset, 'labels', false, 'frames', true); -plotCube(stewart_offset, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +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 -%% With cubic configuration +% 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 @@ -97,13 +135,14 @@ 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', ones(6,1)); +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)); @@ -121,522 +160,446 @@ for i = 1:length(thetas) end end -% Get circle that fits inside the accessible space -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); figure; -s = surf(X, Y, Z, 'FaceColor', 'white'); -s.EdgeColor = colors(1,:); +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 on; -xlabel('X Translation [$\mu$m]'); ylabel('Y Translation [$\mu$m]'); zlabel('Z Translation [$\mu$m]'); -xlim(5e6*[-L_max, L_max]); ylim(5e6*[-L_max, L_max]); zlim(5e6*[-L_max, L_max]); +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 = -10e-3; % Position {B} with respect to {M} [m] - -Hc = 2.5*H; % Size of the useful part of the cube [m] -FOc = H + MO_B; % Center of the cube with respect to {F} +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', Hc, 'FOc', FOc, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); -stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1)); -stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical'); +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 ... +); -stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ... - 'Mpm', 10, ... - 'Mph', 20e-3, ... - 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb))); +% 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'}; -stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3); -stewart = initializeInertialSensor(stewart); +%% 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 ... +); -ground = initializeGround('type', 'none'); -payload = initializePayload('type', 'none'); -controller = initializeController('type', 'open-loop'); - -displayArchitecture(stewart, 'labels', false, 'view', 'all'); - -open('stewart_platform_model.slx') - -%% Options for Linearized -options = linearizeOptions; -options.SampleTime = 0; - -%% Name of the Simulink File -mdl = 'stewart_platform_model'; - -%% Input/Output definition -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, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m] - -%% Run the linearization -G = linearize(mdl, io, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}; - -Gc = inv(stewart.kinematics.J)*G*inv(stewart.kinematics.J'); -Gc = inv(stewart.kinematics.J)*G*stewart.kinematics.J; -Gc.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; -Gc.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; - -freqs = logspace(1, 3, 500); +% 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; - -ax1 = subplot(2, 2, 1); +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); hold on; -for i = 1:6 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(Gc(1, 1), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(2, 2), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(3, 3), freqs, 'Hz')))); +% 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'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +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]) -ax3 = subplot(2, 2, 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:6 - for j = i+1:6 - p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(1, 1), freqs, 'Hz')))); -p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(2, 2), freqs, 'Hz')))); -p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(3, 3), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2, p3, p4], {'$D_x/F_x$','$D_y/F_y$', '$D_z/F_z$', '$D_i/F_j$'}) - -ax2 = subplot(2, 2, 2); -hold on; -for i = 1:6 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(Gc(4, 4), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(5, 5), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(6, 6), freqs, 'Hz')))); +% 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'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +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]) -ax4 = subplot(2, 2, 4); -hold on; -for i = 1:6 - for j = i+1:6 - p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(4, 4), freqs, 'Hz')))); -p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(5, 5), freqs, 'Hz')))); -p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(6, 6), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2, p3, p4], {'$R_x/M_x$','$R_y/M_y$', '$R_z/M_z$', '$R_i/M_j$'}) - -linkaxes([ax1,ax2,ax3,ax4],'x'); - -H = 200e-3; % height of the Stewart platform [m] +%% Cubic Stewart platform with payload above the top platform +H = 200e-3; MO_B = -100e-3; % Position {B} with respect to {M} [m] -Hc = 2.5*H; % Size of the useful part of the cube [m] -FOc = H + MO_B; % Center of the cube with respect to {F} - stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); -stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); -stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1)); -stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical'); +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 ... +); -stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ... - 'Mpm', 10, ... - 'Mph', 20e-3, ... - 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb))); +% Sample at the Center of the cube +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3); -stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3); -stewart = initializeInertialSensor(stewart); - -ground = initializeGround('type', 'none'); -payload = initializePayload('type', 'none'); -controller = initializeController('type', 'open-loop'); - -displayArchitecture(stewart, 'labels', false, 'view', 'all'); - -open('stewart_platform_model.slx') - -%% Options for Linearized -options = linearizeOptions; -options.SampleTime = 0; - -%% Name of the Simulink File -mdl = 'stewart_platform_model'; - -%% Input/Output definition -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, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m] - -%% Run the linearization -G = linearize(mdl, io, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}; - -Gc = inv(stewart.kinematics.J)*G*inv(stewart.kinematics.J'); -Gc.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; -Gc.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; - -freqs = logspace(1, 3, 500); +% 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; - -ax1 = subplot(2, 2, 1); +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); hold on; -for i = 1:6 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(Gc(1, 1), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(2, 2), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(3, 3), freqs, 'Hz')))); +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'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +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]) -ax3 = subplot(2, 2, 3); -hold on; -for i = 1:6 - for j = i+1:6 - p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(1, 1), freqs, 'Hz')))); -p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(2, 2), freqs, 'Hz')))); -p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(3, 3), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2, p3, p4], {'$D_x/F_x$','$D_y/F_y$', '$D_z/F_z$', '$D_i/F_j$'}) +%% 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] -ax2 = subplot(2, 2, 2); -hold on; -for i = 1:6 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(Gc(4, 4), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(5, 5), freqs, 'Hz')))); -plot(freqs, abs(squeeze(freqresp(Gc(6, 6), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - -ax4 = subplot(2, 2, 4); -hold on; -for i = 1:6 - for j = i+1:6 - p4 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(4, 4), freqs, 'Hz')))); -p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(5, 5), freqs, 'Hz')))); -p3 = plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(6, 6), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2, p3, p4], {'$R_x/M_x$','$R_y/M_y$', '$R_z/M_z$', '$R_i/M_j$'}) - -linkaxes([ax1,ax2,ax3,ax4],'x'); +% 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 = -10e-3; % Position {B} with respect to {M} [m] -Hc = 2.5*H; % Size of the useful part of the cube [m] -FOc = H + MO_B; % Center of the cube with respect to {F} +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', Hc, 'FOc', FOc, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); stewart = computeJointsPose(stewart); -stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e1*ones(6,1)); -stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical'); +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, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ... - 'Mpm', 10, ... - 'Mph', 20e-3, ... - 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb))); -stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3); -stewart = initializeInertialSensor(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 ... +); -ground = initializeGround('type', 'none'); -payload = initializePayload('type', 'none'); -controller = initializeController('type', 'open-loop'); - -disturbances = initializeDisturbances(); -references = initializeReferences(stewart); - -displayArchitecture(stewart, 'labels', false, 'view', 'all'); - -open('stewart_platform_model.slx') - -%% Options for Linearized -options = linearizeOptions; -options.SampleTime = 0; - -%% Name of the Simulink File -mdl = 'stewart_platform_model'; - -%% Input/Output definition -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, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m] - -%% Run the linearization -G = linearize(mdl, io, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}; - -freqs = logspace(1, 3, 1000); - -figure; - -ax1 = subplot(2, 1, 1); -hold on; -for i = 1:6 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - -ax3 = subplot(2, 1, 2); -hold on; -for i = 1:6 - for j = i+1:6 - p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2], {'$L_i/\tau_i$', '$L_i/\tau_j$'}) - -linkaxes([ax1,ax2],'x'); - -%% Input/Output definition -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, '/Stewart Platform'], 1, 'openoutput', [], 'Taum'); io_i = io_i + 1; % Force Sensor Outputs [N] - -%% Run the linearization -G = linearize(mdl, io, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}; - -freqs = logspace(1, 3, 500); - -figure; - -ax1 = subplot(2, 1, 1); -hold on; -for i = 1:6 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); - -ax3 = subplot(2, 1, 2); -hold on; -for i = 1:6 - for j = i+1:6 - p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2], {'$F_{m,i}/\tau_i$', '$F_{m,i}/\tau_j$'}) - -linkaxes([ax1,ax2],'x'); - -H = 200e-3; % height of the Stewart platform [m] -MO_B = -10e-3; % Position {B} with respect to {M} [m] +% 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 = generateGeneralConfiguration(stewart, 'FR', 250e-3, 'MR', 150e-3); +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*ones(6,1), 'C', 1e1*ones(6,1)); -stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical'); +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, 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa)), ... - 'Mpm', 10, ... - 'Mph', 20e-3, ... - 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb))); -stewart = initializeCylindricalStruts(stewart, 'Fsm', 1e-3, 'Msm', 1e-3); -stewart = initializeInertialSensor(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 ... +); -ground = initializeGround('type', 'none'); -payload = initializePayload('type', 'none'); -controller = initializeController('type', 'open-loop'); - -displayArchitecture(stewart, 'labels', false, 'view', 'all'); - -open('stewart_platform_model.slx') - -%% Options for Linearized -options = linearizeOptions; -options.SampleTime = 0; - -%% Name of the Simulink File -mdl = 'stewart_platform_model'; - -%% Input/Output definition -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, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m] - -%% Run the linearization -G = linearize(mdl, io, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}; - -freqs = logspace(1, 3, 1000); +% 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; - -ax1 = subplot(2, 1, 1); +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); hold on; -for i = 1:6 +for i = 1:5 for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); + 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 -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); +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'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); - -ax3 = subplot(2, 1, 2); -hold on; -for i = 1:6 - for j = i+1:6 - p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); - end -end -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2], {'$L_i/\tau_i$', '$L_i/\tau_j$'}) - -linkaxes([ax1,ax2],'x'); - -%% Input/Output definition -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, '/Stewart Platform'], 1, 'openoutput', [], 'Taum'); io_i = io_i + 1; % Force Sensor Outputs [N] - -%% Run the linearization -G = linearize(mdl, io, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}; - -freqs = logspace(1, 3, 500); +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; - -ax1 = subplot(2, 1, 1); +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); hold on; -for i = 1:6 +for i = 1:5 for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); + 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 -set(gca,'ColorOrderIndex',1); -plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); +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'); -ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); +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]) -ax3 = subplot(2, 1, 2); +%% 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:6 +for i = 1:5 for j = i+1:6 - p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'k-'); + 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 -set(gca,'ColorOrderIndex',1); -p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz')))); +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', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); -legend([p1, p2], {'$F_{m,i}/\tau_i$', '$F_{m,i}/\tau_j$'}) +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]); -linkaxes([ax1,ax2],'x'); +%% 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] @@ -646,45 +609,140 @@ 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 = 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', ones(6,1)); -stewart = computeJacobian(stewart); -stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.1*max(vecnorm(stewart.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart.platform_M.Mb))); +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))); -%% Example of a cubic architecture with cube's center above the top platform - Small cube size -displayArchitecture(stewart, 'labels', false, 'frames', false); -plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +%% 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 = 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', ones(6,1)); -stewart = computeJacobian(stewart); -stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.1*max(vecnorm(stewart.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart.platform_M.Mb))); +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))); -displayArchitecture(stewart, 'labels', false, 'frames', false); -plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +%% 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 = 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', ones(6,1)); -stewart = computeJacobian(stewart); -stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 1.1*max(vecnorm(stewart.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart.platform_M.Mb))); +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))); -displayArchitecture(stewart, 'labels', false, 'frames', false); -plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +%% 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)) diff --git a/matlab/detail_kinematics_3_nano_hexapod.m b/matlab/detail_kinematics_3_nano_hexapod.m index c3a13bb..8474a29 100644 --- a/matlab/detail_kinematics_3_nano_hexapod.m +++ b/matlab/detail_kinematics_3_nano_hexapod.m @@ -7,6 +7,10 @@ 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; @@ -25,14 +29,39 @@ nano_hexapod = generateGeneralConfiguration(nano_hexapod, ... '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 = initializeStrutDynamics(nano_hexapod, 'k', 1); nano_hexapod = computeJacobian(nano_hexapod); -nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 130e-3, 'Mpr', 120e-3); +nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 125e-3, 'Mpr', 115e-3); -displayArchitecture(nano_hexapod, 'labels', true); +%% Obtained architecture for the Nano Hexapod +figure; +displayArchitecture(nano_hexapod, 'labels', true, 'frames', false); +% 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 -max_translation = 50e-6; -max_rotation = 50e-6; +%% 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); @@ -40,40 +69,38 @@ 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; +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)]; + 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); + [~, Ls] = inverseKinematics(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB); - if min(Ls) < L_min - L_min = min(Ls); + if min(Ls) < L_min + L_min = min(Ls); + end + if max(Ls) > L_max + L_max = max(Ls); + end + end + end + end 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); @@ -103,28 +130,25 @@ for i = 1:length(thetas) dL_rot_max = zeros(6,1); % Perform (small) rotations, and find the (worst) case requiring maximum strut motion for Rx = Rxs - for Ry = Rys + 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 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)) - +%% 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); @@ -163,29 +187,51 @@ 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]); +xlim([-150 150]); ylim([-150, 150]); zlim([-120, 120]); +view(-160, 20) -%% Estimation of the required flexible joint angular stroke -max_angles = zeros(1,6); +%% 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; +As = nano_hexapod.geometry.As; % Fixed joints +Bs = nano_hexapod.geometry.Bs; % Mobile joints -% 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)); +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', [Tx; Ty; Tz], 'ARB', eye(3)); + nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB); - angles = acos(dot(As, nano_hexapod.geometry.As)); - larger_angles = abs(angles) > max_angles; - max_angles(larger_angles) = angles(larger_angles); + 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('Maximum flexible joint angle is %.1f mrad', 1e3*max(max_angles)) +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)) diff --git a/nass-geometry.org b/nass-geometry.org index 3a04041..c0e38ef 100644 --- a/nass-geometry.org +++ b/nass-geometry.org @@ -1646,11 +1646,11 @@ 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 = initializeJointDynamics(stewart); stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); stewart = initializeCylindricalStruts(stewart); stewart = computeJacobian(stewart); -stewart = initializeStewartPose(stewart) +stewart = initializeStewartPose(stewart); displayArchitecture(stewart, 'labels', false, 'frames', false); plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); @@ -3149,16 +3149,12 @@ 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('Verification bi_z = 0:'); -disp(simplify(bi_z_0)); +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('Verification bi_z = H:'); -disp(simplify(bi_z_H)); - -% Compute radius -simplify(sqrt(bi_z_H(:,1).^2 + bi_z_H(:,2).^2)) -simplify(sqrt(bi_z_0(:,1).^2 + bi_z_0(:,2).^2)) +disp('Radius for mobile platform:'); +simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2)) #+end_src In order to determine the approximate size of the platform as a function of