diff --git a/matlab/detail_kinematics_1_geometry.m b/matlab/detail_kinematics_1_geometry.m new file mode 100644 index 0000000..d9846c3 --- /dev/null +++ b/matlab/detail_kinematics_1_geometry.m @@ -0,0 +1,231 @@ +%% 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 one Stewart platform and associated translational mobility +L_max = 50e-6; % Maximum actuator stroke (+/-) [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', -50e-3); +stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 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 = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'K', ones(6,1)); +stewart = computeJacobian(stewart); +stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3); + +displayArchitecture(stewart, 'labels', false, 'frames', false, 'F_color', [0,0,0], 'M_color', [0,0,0], 'L_color', [0,0,0]); + +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 100); +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.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)); + end +end + +% Sphere with radius equal to actuator stroke +[Xs,Ys,Zs] = sphere; +Xs = 1e6*L_max*Xs; +Ys = 1e6*L_max*Ys; +Zs = 1e6*L_max*Zs; + +[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('Renderer', 'painters'); +hold on; +surf(X, Y, Z, 'FaceColor', 'none', 'LineWidth', 0.1, 'EdgeColor', [0, 0, 0]); +surf(Xs, Ys, Zs, 'FaceColor', colors(2,:), 'EdgeColor', 'none'); +quiver3(0, 0, 0, 100, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9); +quiver3(0, 0, 0, 0, 100, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9); +quiver3(0, 0, 0, 0, 0, 100, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9); +hold off; +axis equal; +grid off; +axis off; +view(55, 25); + +%% Stewart platform with Vertically oriented struts +L_max = 50e-6; % Maximum actuator stroke (+/-) [m] + +stewart_vert = initializeStewartPlatform(); +stewart_vert = initializeFramesPositions(stewart_vert, 'H', 40e-3, 'MO_B', -20e-3); +stewart_vert = generateGeneralConfiguration(stewart_vert, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 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 = computeJacobian(stewart_vert); +stewart_vert = initializeCylindricalPlatforms(stewart_vert, 'Fpr', 110e-3, 'Mpr', 110e-3); + +displayArchitecture(stewart_vert, 'labels', false, 'frames', false, 'F_color', colors(1,:), 'M_color', colors(1,:), 'L_color', colors(1,:)); + +%% Stewart platform with Horizontally oriented struts +stewart_hori = initializeStewartPlatform(); +stewart_hori = initializeFramesPositions(stewart_hori, 'H', 40e-3, 'MO_B', -20e-3); +stewart_hori = generateGeneralConfiguration(stewart_hori, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 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 = 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); +rs_vert = zeros(length(thetas), length(phis)); +rs_hori = 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_vert.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction + rs_vert(i, j) = L_max/max(abs(dL)); + + dL = stewart_hori.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction + rs_hori(i, j) = L_max/max(abs(dL)); + end +end + +[phi_grid, theta_grid] = meshgrid(phis, thetas); + +X_vert = 1e6 * rs_vert .* sin(theta_grid) .* cos(phi_grid); +Y_vert = 1e6 * rs_vert .* sin(theta_grid) .* sin(phi_grid); +Z_vert = 1e6 * rs_vert .* cos(theta_grid); + +X_hori = 1e6 * rs_hori .* sin(theta_grid) .* cos(phi_grid); +Y_hori = 1e6 * rs_hori .* sin(theta_grid) .* sin(phi_grid); +Z_hori = 1e6 * rs_hori .* cos(theta_grid); + +[Xs,Ys,Zs] = sphere; +Xs = 1e6*L_max*Xs; +Ys = 1e6*L_max*Ys; +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,:)); +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); +text(200, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 200, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 0, 200, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +quiver3(0, 400, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 400, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 400, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +text(200, 400, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 600, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 400, 200, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +hold off; +axis equal; +grid off; +axis off; +view(105, 15); + +%% Stewart platform with struts close to each other +stewart_close = initializeStewartPlatform(); +stewart_close = initializeFramesPositions(stewart_close, 'H', 100e-3, 'MO_B', 0); +stewart_close = generateGeneralConfiguration(stewart_close, 'FH', 0, 'FR', 50e-3, 'MH', 0, 'MR', 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 = computeJacobian(stewart_close); +stewart_close = initializeCylindricalPlatforms(stewart_close, 'Fpr', 110e-3, 'Mpr', 110e-3); + +displayArchitecture(stewart_close, 'labels', false, 'frames', false, 'F_color', colors(1,:), 'M_color', colors(1,:), 'L_color', colors(1,:)); + +%% Stewart platform with struts further away from each other +stewart_space = initializeStewartPlatform(); +stewart_space = initializeFramesPositions(stewart_space, 'H', 100e-3, 'MO_B', 0); +stewart_space = generateGeneralConfiguration(stewart_space, 'FH', 0, 'FR', 100e-3, 'MH', 0, 'MR', 100e-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_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 = computeJacobian(stewart_space); +stewart_space = initializeCylindricalPlatforms(stewart_space, 'Fpr', 110e-3, 'Mpr', 110e-3); + +displayArchitecture(stewart_space, '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); +rs_close = zeros(length(thetas), length(phis)); +rs_space = 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_close.kinematics.J*[0; 0; 0; Rx; Ry; Rz;]; + rs_close(i, j) = L_max/max(abs(dL)); + + dL = stewart_space.kinematics.J*[0; 0; 0; Rx; Ry; Rz;]; + rs_space(i, j) = L_max/max(abs(dL)); + end +end + +[phi_grid, theta_grid] = meshgrid(phis, thetas); + +X_close = 1e6 * rs_close .* sin(theta_grid) .* cos(phi_grid); +Y_close = 1e6 * rs_close .* sin(theta_grid) .* sin(phi_grid); +Z_close = 1e6 * rs_close .* cos(theta_grid); + +X_space = 1e6 * rs_space .* sin(theta_grid) .* cos(phi_grid); +Y_space = 1e6 * rs_space .* sin(theta_grid) .* sin(phi_grid); +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,:)); +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); +text(4000, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 4000, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 0, 4000, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +quiver3(0, 6000, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 6000, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 6000, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +text(4000, 6000, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top'); +text(0, 10000, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 6000, 4000, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top'); +hold off; +axis equal; +grid off; +axis off; +view(105, 15); diff --git a/matlab/detail_kinematics_2_cubic.m b/matlab/detail_kinematics_2_cubic.m new file mode 100644 index 0000000..6e30455 --- /dev/null +++ b/matlab/detail_kinematics_2_cubic.m @@ -0,0 +1,690 @@ +%% 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] + +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', ones(6,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); + +%% 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', ones(6,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 + +%% Frames {A} and {B} at the cube's center +MO_B = -50e-3; % Position {B} with respect to {M} [m] + +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); + +displayArchitecture(stewart_center, 'labels', false, 'frames', true); +plotCube(stewart_center, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); + +%% Frames {A} and {B} offset from the cube's center +MO_B = 50e-3; % Position {B} with respect to {M} [m] + +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); + +displayArchitecture(stewart_offset, 'labels', false, 'frames', true); +plotCube(stewart_offset, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); + +%% With 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', ones(6,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] + +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 + +% 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,:); +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]); + +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} + +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 = 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 = 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); + +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); + +figure; + +ax1 = subplot(2, 2, 1); +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')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +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$'}) + +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'); + +H = 200e-3; % height of the Stewart platform [m] +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 = 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 = 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); + +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); + +figure; + +ax1 = subplot(2, 2, 1); +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')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +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$'}) + +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'); + +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} + +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 = 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 = 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); + +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] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateGeneralConfiguration(stewart, 'FR', 250e-3, 'MR', 150e-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 = 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); + +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); + +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'); + +%% 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 = 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))); + +%% 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); +scatter3(0, 0, FOc, 200, 'kh'); + +%% 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))); + +displayArchitecture(stewart, 'labels', false, 'frames', false); +plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); + +%% 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))); + +displayArchitecture(stewart, 'labels', false, 'frames', false); +plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); diff --git a/matlab/detail_kinematics_3_nano_hexapod.m b/matlab/detail_kinematics_3_nano_hexapod.m new file mode 100644 index 0000000..c3a13bb --- /dev/null +++ b/matlab/detail_kinematics_3_nano_hexapod.m @@ -0,0 +1,191 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +%% Path for functions, data and scripts +addpath('./mat/'); % Path for data +addpath('./src/'); % Path for functions + +%% Colors for the figures +colors = colororder; + +%% Obtained Nano Hexapod Design +nano_hexapod = initializeStewartPlatform(); +nano_hexapod = initializeFramesPositions(nano_hexapod, ... + 'H', 95e-3, ... + 'MO_B', 150e-3); + +nano_hexapod = generateGeneralConfiguration(nano_hexapod, ... + 'FH', 15e-3, ... + 'FR', 120e-3, ... + 'FTh', [220, 320, 340, 80, 100, 200]*(pi/180), ... + 'MH', 15e-3, ... + 'MR', 110e-3, ... + 'MTh', [255, 285, 15, 45, 135, 165]*(pi/180)); +nano_hexapod = computeJointsPose(nano_hexapod); +nano_hexapod = initializeStrutDynamics(nano_hexapod, 'K', ones(6,1)); +nano_hexapod = computeJacobian(nano_hexapod); +nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 130e-3, 'Mpr', 120e-3); + +displayArchitecture(nano_hexapod, 'labels', true); + +max_translation = 50e-6; +max_rotation = 50e-6; + +Dxs = linspace(-max_translation, max_translation, 3); +Dys = linspace(-max_translation, max_translation, 3); +Dzs = linspace(-max_translation, max_translation, 3); +Rxs = linspace(-max_rotation, max_rotation, 3); +Rys = linspace(-max_rotation, max_rotation, 3); + +L_min = 0; +L_max = 0; + +for Dx = Dxs +for Dy = Dys +for Dz = Dzs +for Rx = Rxs +for Ry = Rys + ARB = [ cos(Ry) 0 sin(Ry); + 0 1 0; + -sin(Ry) 0 cos(Ry)] * ... + [ 1 0 0; + 0 cos(Rx) -sin(Rx); + 0 sin(Rx) cos(Rx)]; + + [~, Ls] = inverseKinematics(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB); + + if min(Ls) < L_min + L_min = min(Ls); + end + if max(Ls) > L_max + L_max = max(Ls); + end +end +end +end +end +end + +sprintf('Actuator stroke should be from %.0f um to %.0f um', 1e6*L_min, 1e6*L_max) + +%% Compute mobility in translation with combined angular motion +% L_max = 100e-6; % Actuator Stroke (+/-) + +% Direction of motion (spherical coordinates) +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 100); + +% Considered Rotations +Rxs = linspace(-max_rotation, max_rotation, 3); +Rys = linspace(-max_rotation, max_rotation, 3); + +% Maximum distance that can be reached in the direction of motion +% Considering combined angular motion and limited actuator stroke +rs = zeros(length(thetas), length(phis)); +worst_rx_ry = zeros(length(thetas), length(phis), 2); + +for i = 1:length(thetas) + for j = 1:length(phis) + % Compute unitary motion in the considered direction + Tx = sin(thetas(i))*cos(phis(j)); + Ty = sin(thetas(i))*sin(phis(j)); + Tz = cos(thetas(i)); + % Start without considering rotations + dL_lin = nano_hexapod.kinematics.J*[Tx; Ty; Tz; 0; 0; 0]; + % Strut motion for maximum displacement in the considered direction + dL_lin_max = L_max*dL_lin/max(abs(dL_lin)); + + % Find rotation that gives worst case stroke + dL_worst = max(abs(dL_lin_max)); % This should be equal to L_max + dL_rot_max = zeros(6,1); + % Perform (small) rotations, and find the (worst) case requiring maximum strut motion + for Rx = Rxs + for Ry = Rys + dL_rot = nano_hexapod.kinematics.J*[0; 0; 0; Rx; Ry; 0]; + if max(abs(dL_lin_max + dL_rot)) > dL_worst + dL_worst = max(abs(dL_lin_max + dL_rot)); + dL_rot_max = dL_rot; + worst_rx_ry(i,j,:) = [Rx, Ry]; + end + end + end + + stroke_ratio = min(abs([( L_max - dL_rot_max) ./ dL_lin_max; (-L_max - dL_rot_max) ./ dL_lin_max])); + dL_real = dL_lin_max*stroke_ratio + dL_rot_max; + + % % Obtained maximum displacement in the considered direction with angular motion + % rs(i, j) = stroke_ratio*max(abs(dL_lin_max)); + rs(i, j) = stroke_ratio*L_max/max(abs(dL_lin)); + end +end + +min(min(rs)) +max(max(rs)) + +[phi_grid, theta_grid] = meshgrid(phis, thetas); +X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid); +Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid); +Z = 1e6 * rs .* cos(theta_grid); + +vertices = 1e6*max_translation*[ + -1 -1 -1; % vertex 1 + 1 -1 -1; % vertex 2 + 1 1 -1; % vertex 3 + -1 1 -1; % vertex 4 + -1 -1 1; % vertex 5 + 1 -1 1; % vertex 6 + 1 1 1; % vertex 7 + -1 1 1 % vertex 8 +]; + +% Define the faces using the vertex indices +faces = [ + 1 2 3 4; % bottom face + 5 6 7 8; % top face + 1 2 6 5; % front face + 2 3 7 6; % right face + 3 4 8 7; % back face + 4 1 5 8 % left face +]; + +figure; +hold on; +s = surf(X, Y, Z, 'FaceColor', 'none'); +s.EdgeColor = colors(2, :); +patch('Vertices', vertices, 'Faces', faces, ... + 'FaceColor', [0.7 0.7 0.7], ... + 'EdgeColor', 'black', ... + 'FaceAlpha', 1); +hold off; +axis equal; +grid on; +xlabel('X Translation [$\mu$m]'); ylabel('Y Translation [$\mu$m]'); zlabel('Z Translation [$\mu$m]'); +xlim(2e6*[-L_max, L_max]); ylim(2e6*[-L_max, L_max]); zlim(2e6*[-L_max, L_max]); + +%% Estimation of the required flexible joint angular stroke +max_angles = zeros(1,6); + +% Compute initial strut orientation +nano_hexapod = computeJointsPose(nano_hexapod, 'AP', zeros(3,1), 'ARB', eye(3)); +As = nano_hexapod.geometry.As; + +% Only consider translations, but add maximum expected top platform rotation +for i = 1:length(thetas) + for j = 1:length(phis) + % Maximum translations + Tx = rs(i,j)*sin(thetas(i))*cos(phis(j)); + Ty = rs(i,j)*sin(thetas(i))*sin(phis(j)); + Tz = rs(i,j)*cos(thetas(i)); + + nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Tx; Ty; Tz], 'ARB', eye(3)); + + angles = acos(dot(As, nano_hexapod.geometry.As)); + larger_angles = abs(angles) > max_angles; + max_angles(larger_angles) = angles(larger_angles); + end +end + +sprintf('Maximum flexible joint angle is %.1f mrad', 1e3*max(max_angles)) diff --git a/matlab/nano_hexapod_model.slx b/matlab/nano_hexapod_model.slx new file mode 100644 index 0000000..b650045 Binary files /dev/null and b/matlab/nano_hexapod_model.slx differ diff --git a/matlab/src/computeJacobian.m b/matlab/src/computeJacobian.m new file mode 100644 index 0000000..a1c76d5 --- /dev/null +++ b/matlab/src/computeJacobian.m @@ -0,0 +1,35 @@ +function [stewart] = computeJacobian(stewart) +% computeJacobian - +% +% Syntax: [stewart] = computeJacobian(stewart) +% +% Inputs: +% - stewart - With at least the following fields: +% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A} +% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A} +% - actuators.K [6x1] - Total stiffness of the actuators +% +% Outputs: +% - stewart - With the 3 added field: +% - kinematics.J [6x6] - The Jacobian Matrix +% - kinematics.K [6x6] - The Stiffness Matrix +% - kinematics.C [6x6] - The Compliance Matrix + +assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As') +As = stewart.geometry.As; + +assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab') +Ab = stewart.geometry.Ab; + +assert(isfield(stewart.actuators, 'k'), 'stewart.actuators should have attribute k') +Ki = stewart.actuators.k*eye(6); + +J = [As' , cross(Ab, As)']; + +K = J'*Ki*J; + +C = inv(K); + +stewart.kinematics.J = J; +stewart.kinematics.K = K; +stewart.kinematics.C = C; diff --git a/matlab/src/computeJointsPose.m b/matlab/src/computeJointsPose.m new file mode 100644 index 0000000..5520000 --- /dev/null +++ b/matlab/src/computeJointsPose.m @@ -0,0 +1,90 @@ +function [stewart] = computeJointsPose(stewart, args) +% computeJointsPose - +% +% Syntax: [stewart] = computeJointsPose(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} +% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} +% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} +% - geometry.FO_M [3x1] - Position of {M} with respect to {F} +% - args - Can have the following fields: +% - AP [3x1] - The wanted position of {B} with respect to {A} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} +% +% Outputs: +% - stewart - A structure with the following added fields +% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A} +% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A} +% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B} +% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B} +% - geometry.l [6x1] - The i'th element is the initial length of strut i +% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A} +% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} +% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut +% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut +% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} +% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} + +arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) +end + +assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') +Fa = stewart.platform_F.Fa; + +assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb') +Mb = stewart.platform_M.Mb; + +assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A') +FO_A = stewart.platform_F.FO_A; + +assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B') +MO_B = stewart.platform_M.MO_B; + +assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M') +FO_M = stewart.geometry.FO_M; + +Aa = Fa - repmat(FO_A, [1, 6]); +Bb = Mb - repmat(MO_B, [1, 6]); + +Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); +Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]); + +Ab = args.ARB *Bb - repmat(-args.AP, [1, 6]); +Ba = args.ARB'*Aa - repmat( args.AP, [1, 6]); + +As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As + +l = vecnorm(Ab - Aa)'; + +Bs = (Bb - Ba)./vecnorm(Bb - Ba); + +FRa = zeros(3,3,6); +MRb = zeros(3,3,6); + +for i = 1:6 + FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)]; + FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i)); + + MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)]; + MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); +end + +stewart.geometry.Aa = Aa; +stewart.geometry.Ab = Ab; +stewart.geometry.Ba = Ba; +stewart.geometry.Bb = Bb; +stewart.geometry.As = As; +stewart.geometry.Bs = Bs; +stewart.geometry.l = l; + +stewart.struts_F.l = l/2; +stewart.struts_M.l = l/2; + +stewart.platform_F.FRa = FRa; +stewart.platform_M.MRb = MRb; diff --git a/matlab/src/describeStewartPlatform.m b/matlab/src/describeStewartPlatform.m new file mode 100644 index 0000000..3be8da3 --- /dev/null +++ b/matlab/src/describeStewartPlatform.m @@ -0,0 +1,79 @@ +function [] = describeStewartPlatform(stewart) +% describeStewartPlatform - Display some text describing the current defined Stewart Platform +% +% Syntax: [] = describeStewartPlatform(args) +% +% Inputs: +% - stewart +% +% Outputs: + +arguments + stewart +end + +fprintf('GEOMETRY:\n') +fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H) + +if stewart.platform_M.MO_B(3) > 0 + fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3)) +else + fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3)) +end + +fprintf('- The initial length of the struts are:\n') +fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l) +fprintf('\n') + +fprintf('ACTUATORS:\n') +if stewart.actuators.type == 1 + fprintf('- The actuators are classical.\n') + fprintf('- The Stiffness and Damping of each actuators is:\n') + fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.k, stewart.actuators.c) +elseif stewart.actuators.type == 2 + fprintf('- The actuators are mechanicaly amplified.\n') +end +fprintf('\n') + +fprintf('JOINTS:\n') + +switch stewart.joints_F.type + case 1 + fprintf('- The joints on the fixed based are universal joints\n') + case 2 + fprintf('- The joints on the fixed based are spherical joints\n') + case 3 + fprintf('- The joints on the fixed based are perfect universal joints\n') + case 4 + fprintf('- The joints on the fixed based are perfect spherical joints\n') +end + +switch stewart.joints_M.type + case 1 + fprintf('- The joints on the mobile based are universal joints\n') + case 2 + fprintf('- The joints on the mobile based are spherical joints\n') + case 3 + fprintf('- The joints on the mobile based are perfect universal joints\n') + case 4 + fprintf('- The joints on the mobile based are perfect spherical joints\n') +end + +fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n') +fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa) + +fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n') +fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb) +fprintf('\n') + +fprintf('KINEMATICS:\n') + +if isfield(stewart.kinematics, 'K') + fprintf('- The Stiffness matrix K is (in [N/m]):\n') + fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K) +end + +if isfield(stewart.kinematics, 'C') + fprintf('- The Damping matrix C is (in [m/N]):\n') + fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C) +end diff --git a/matlab/src/displayArchitecture.m b/matlab/src/displayArchitecture.m new file mode 100644 index 0000000..8b66821 --- /dev/null +++ b/matlab/src/displayArchitecture.m @@ -0,0 +1,252 @@ +function [] = displayArchitecture(stewart, args) +% displayArchitecture - 3D plot of the Stewart platform architecture +% +% Syntax: [] = displayArchitecture(args) +% +% Inputs: +% - stewart +% - args - Structure with the following fields: +% - AP [3x1] - The wanted position of {B} with respect to {A} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} +% - F_color [color] - Color used for the Fixed elements +% - M_color [color] - Color used for the Mobile elements +% - L_color [color] - Color used for the Legs elements +% - frames [true/false] - Display the Frames +% - legs [true/false] - Display the Legs +% - joints [true/false] - Display the Joints +% - labels [true/false] - Display the Labels +% - platforms [true/false] - Display the Platforms +% - views ['all', 'xy', 'yz', 'xz', 'default'] - +% +% Outputs: + +arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + args.F_color = [0 0.4470 0.7410] + args.M_color = [0.8500 0.3250 0.0980] + args.L_color = [0 0 0] + args.frames logical {mustBeNumericOrLogical} = true + args.legs logical {mustBeNumericOrLogical} = true + args.joints logical {mustBeNumericOrLogical} = true + args.labels logical {mustBeNumericOrLogical} = true + args.platforms logical {mustBeNumericOrLogical} = true + args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default' +end + +assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A') +FO_A = stewart.platform_F.FO_A; + +assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B') +MO_B = stewart.platform_M.MO_B; + +assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') +H = stewart.geometry.H; + +assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') +Fa = stewart.platform_F.Fa; + +assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb') +Mb = stewart.platform_M.Mb; + +% The reference frame of the 3d plot corresponds to the frame $\{F\}$. +if ~strcmp(args.views, 'all') + figure; +else + f = figure('visible', 'off'); +end + +hold on; + +% We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$. +FTa = [eye(3), FO_A; ... + zeros(1,3), 1]; +ATb = [args.ARB, args.AP; ... + zeros(1,3), 1]; +BTm = [eye(3), -MO_B; ... + zeros(1,3), 1]; + +FTm = FTa*ATb*BTm; + +% Let's define a parameter that define the length of the unit vectors used to display the frames. +d_unit_vector = H/4; + +% Let's define a parameter used to position the labels with respect to the center of the element. +d_label = H/20; +% Let's first plot the frame $\{F\}$. +Ff = [0, 0, 0]; +if args.frames + quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ... + [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color) + + if args.labels + text(Ff(1) + d_label, ... + Ff(2) + d_label, ... + Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color); + end +end + +% Now plot the frame $\{A\}$ fixed to the Base. +if args.frames + quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ... + [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color) + + if args.labels + text(FO_A(1) + d_label, ... + FO_A(2) + d_label, ... + FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color); + end +end + +% Let's then plot the circle corresponding to the shape of the Fixed base. +if args.platforms && stewart.platform_F.type == 1 + theta = [0:0.1:2*pi+0.1]; % Angles [rad] + v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal + center = [0; 0; 0]; % Center of the circle + radius = stewart.platform_F.R; % Radius of the circle [m] + + points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); + + plot3(points(1,:), ... + points(2,:), ... + points(3,:), '-', 'Color', args.F_color); +end + +% Let's now plot the position and labels of the Fixed Joints +if args.joints + scatter3(Fa(1,:), ... + Fa(2,:), ... + Fa(3,:), 'MarkerEdgeColor', args.F_color); + if args.labels + for i = 1:size(Fa,2) + text(Fa(1,i) + d_label, ... + Fa(2,i), ... + Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color); + end + end +end + +% Plot the frame $\{M\}$. +Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F} + +if args.frames + FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors + quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ... + FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color) + + if args.labels + text(Fm(1) + d_label, ... + Fm(2) + d_label, ... + Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color); + end +end + +% Plot the frame $\{B\}$. +FB = FO_A + args.AP; + +if args.frames + FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors + quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ... + FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color) + + if args.labels + text(FB(1) - d_label, ... + FB(2) + d_label, ... + FB(3) + d_label, '$\{B\}$', 'Color', args.M_color); + end +end + +% Let's then plot the circle corresponding to the shape of the Mobile platform. +if args.platforms && stewart.platform_M.type == 1 + theta = [0:0.1:2*pi+0.1]; % Angles [rad] + v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal + center = Fm(1:3); % Center of the circle + radius = stewart.platform_M.R; % Radius of the circle [m] + + points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); + + plot3(points(1,:), ... + points(2,:), ... + points(3,:), '-', 'Color', args.M_color); +end + +% Plot the position and labels of the rotation joints fixed to the mobile platform. +if args.joints + Fb = FTm*[Mb;ones(1,6)]; + + scatter3(Fb(1,:), ... + Fb(2,:), ... + Fb(3,:), 'MarkerEdgeColor', args.M_color); + + if args.labels + for i = 1:size(Fb,2) + text(Fb(1,i) + d_label, ... + Fb(2,i), ... + Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color); + end + end +end + +% Plot the legs connecting the joints of the fixed base to the joints of the mobile platform. +if args.legs + for i = 1:6 + plot3([Fa(1,i), Fb(1,i)], ... + [Fa(2,i), Fb(2,i)], ... + [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color); + + if args.labels + text((Fa(1,i)+Fb(1,i))/2 + d_label, ... + (Fa(2,i)+Fb(2,i))/2, ... + (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color); + end + end +end + +switch args.views + case 'default' + view([1 -0.6 0.4]); + case 'xy' + view([0 0 1]); + case 'xz' + view([0 -1 0]); + case 'yz' + view([1 0 0]); +end +axis equal; +axis off; + +if strcmp(args.views, 'all') + hAx = findobj('type', 'axes'); + + figure; + s1 = subplot(2,2,1); + copyobj(get(hAx(1), 'Children'), s1); + view([0 0 1]); + axis equal; + axis off; + title('Top') + + s2 = subplot(2,2,2); + copyobj(get(hAx(1), 'Children'), s2); + view([1 -0.6 0.4]); + axis equal; + axis off; + + s3 = subplot(2,2,3); + copyobj(get(hAx(1), 'Children'), s3); + view([1 0 0]); + axis equal; + axis off; + title('Front') + + s4 = subplot(2,2,4); + copyobj(get(hAx(1), 'Children'), s4); + view([0 -1 0]); + axis equal; + axis off; + title('Side') + + close(f); +end diff --git a/matlab/src/forwardKinematicsApprox.m b/matlab/src/forwardKinematicsApprox.m new file mode 100644 index 0000000..a6f3c23 --- /dev/null +++ b/matlab/src/forwardKinematicsApprox.m @@ -0,0 +1,34 @@ +function [P, R] = forwardKinematicsApprox(stewart, args) +% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using +% the Jacobian Matrix +% +% Syntax: [P, R] = forwardKinematicsApprox(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - kinematics.J [6x6] - The Jacobian Matrix +% - args - Can have the following fields: +% - dL [6x1] - Displacement of each strut [m] +% +% Outputs: +% - P [3x1] - The estimated position of {B} with respect to {A} +% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A} + +arguments + stewart + args.dL (6,1) double {mustBeNumeric} = zeros(6,1) +end + +assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') +J = stewart.kinematics.J; + +X = J\args.dL; + +P = X(1:3); + +theta = norm(X(4:6)); +s = X(4:6)/theta; + +R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta); + s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta); + s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)]; diff --git a/matlab/src/generateCubicConfiguration.m b/matlab/src/generateCubicConfiguration.m new file mode 100644 index 0000000..d08b019 --- /dev/null +++ b/matlab/src/generateCubicConfiguration.m @@ -0,0 +1,57 @@ +function [stewart] = generateCubicConfiguration(stewart, args) +% generateCubicConfiguration - Generate a Cubic Configuration +% +% Syntax: [stewart] = generateCubicConfiguration(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - geometry.H [1x1] - Total height of the platform [m] +% - args - Can have the following fields: +% - Hc [1x1] - Height of the "useful" part of the cube [m] +% - FOc [1x1] - Height of the center of the cube with respect to {F} [m] +% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m] +% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m] +% +% Outputs: + +% - stewart - updated Stewart structure with the added fields: +% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} +% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} + +arguments + stewart + args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 + args.FOc (1,1) double {mustBeNumeric} = 50e-3 + args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 + args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 +end + +assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') +H = stewart.geometry.H; + +% We define the useful points of the cube with respect to the Cube's center. +% ${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}. + +sx = [ 2; -1; -1]; +sy = [ 0; 1; -1]; +sz = [ 1; 1; 1]; + +R = [sx, sy, sz]./vecnorm([sx, sy, sz]); + +L = args.Hc*sqrt(3); + +Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc]; + +CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg +CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg + +% We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). + +CSi = (CCm - CCf)./vecnorm(CCm - CCf); + +% We now which to compute the position of the joints $a_{i}$ and $b_{i}$. +Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; +Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; + +stewart.platform_F.Fa = Fa; +stewart.platform_M.Mb = Mb; diff --git a/matlab/src/generateGeneralConfiguration.m b/matlab/src/generateGeneralConfiguration.m new file mode 100644 index 0000000..4cfcc24 --- /dev/null +++ b/matlab/src/generateGeneralConfiguration.m @@ -0,0 +1,39 @@ +function [stewart] = generateGeneralConfiguration(stewart, args) +% generateGeneralConfiguration - Generate a Very General Configuration +% +% Syntax: [stewart] = generateGeneralConfiguration(stewart, args) +% +% Inputs: +% - args - Can have the following fields: +% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] +% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] +% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad] +% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m] +% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m] +% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} +% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} + +arguments + stewart + args.FH (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3; + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); + args.MH (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); +end + +Fa = zeros(3,6); +Mb = zeros(3,6); + +for i = 1:6 + Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; + Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; +end + +stewart.platform_F.Fa = Fa; +stewart.platform_M.Mb = Mb; diff --git a/matlab/src/initializeController.m b/matlab/src/initializeController.m new file mode 100644 index 0000000..d0e0025 --- /dev/null +++ b/matlab/src/initializeController.m @@ -0,0 +1,17 @@ +function [controller] = initializeController(args) + + arguments + args.type char {mustBeMember(args.type,{'open-loop', 'iff'})} = 'open-loop' + end + + controller = struct(); + + switch args.type + case 'open-loop' + controller.type = 1; + controller.name = 'Open-Loop'; + case 'iff' + controller.type = 2; + controller.name = 'Decentralized Integral Force Feedback'; + end +end diff --git a/matlab/src/initializeCylindricalPlatforms.m b/matlab/src/initializeCylindricalPlatforms.m new file mode 100644 index 0000000..0b86fc1 --- /dev/null +++ b/matlab/src/initializeCylindricalPlatforms.m @@ -0,0 +1,59 @@ +function [stewart] = initializeCylindricalPlatforms(stewart, args) +% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms +% +% Syntax: [stewart] = initializeCylindricalPlatforms(args) +% +% Inputs: +% - args - Structure with the following fields: +% - Fpm [1x1] - Fixed Platform Mass [kg] +% - Fph [1x1] - Fixed Platform Height [m] +% - Fpr [1x1] - Fixed Platform Radius [m] +% - Mpm [1x1] - Mobile Platform Mass [kg] +% - Mph [1x1] - Mobile Platform Height [m] +% - Mpr [1x1] - Mobile Platform Radius [m] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - platform_F [struct] - structure with the following fields: +% - type = 1 +% - M [1x1] - Fixed Platform Mass [kg] +% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2] +% - H [1x1] - Fixed Platform Height [m] +% - R [1x1] - Fixed Platform Radius [m] +% - platform_M [struct] - structure with the following fields: +% - M [1x1] - Mobile Platform Mass [kg] +% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2] +% - H [1x1] - Mobile Platform Height [m] +% - R [1x1] - Mobile Platform Radius [m] + +arguments + stewart + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 +end + +I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/2 *args.Fpm * args.Fpr^2]); + +I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/2 *args.Mpm * args.Mpr^2]); + +stewart.platform_F.type = 1; + +stewart.platform_F.I = I_F; +stewart.platform_F.M = args.Fpm; +stewart.platform_F.R = args.Fpr; +stewart.platform_F.H = args.Fph; + +stewart.platform_M.type = 1; + +stewart.platform_M.I = I_M; +stewart.platform_M.M = args.Mpm; +stewart.platform_M.R = args.Mpr; +stewart.platform_M.H = args.Mph; diff --git a/matlab/src/initializeCylindricalStruts.m b/matlab/src/initializeCylindricalStruts.m new file mode 100644 index 0000000..16d4fd2 --- /dev/null +++ b/matlab/src/initializeCylindricalStruts.m @@ -0,0 +1,50 @@ +function [stewart] = initializeCylindricalStruts(stewart, args) +% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts +% +% Syntax: [stewart] = initializeCylindricalStruts(args) +% +% Inputs: +% - args - Structure with the following fields: +% - Fsm [1x1] - Mass of the Fixed part of the struts [kg] +% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m] +% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m] +% - Msm [1x1] - Mass of the Mobile part of the struts [kg] +% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m] +% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - struts_F [struct] - structure with the following fields: +% - M [6x1] - Mass of the Fixed part of the struts [kg] +% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] +% - H [6x1] - Height of cylinder for the Fixed part of the struts [m] +% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m] +% - struts_M [struct] - structure with the following fields: +% - M [6x1] - Mass of the Mobile part of the struts [kg] +% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] +% - H [6x1] - Height of cylinder for the Mobile part of the struts [m] +% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m] + + arguments + stewart + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + end + + stewart.struts_M.type = 1; + + stewart.struts_M.M = args.Msm; + stewart.struts_M.R = args.Msr; + stewart.struts_M.H = args.Msh; + + stewart.struts_F.type = 1; + + stewart.struts_F.M = args.Fsm; + stewart.struts_F.R = args.Fsr; + stewart.struts_F.H = args.Fsh; + +end diff --git a/matlab/src/initializeFramesPositions.m b/matlab/src/initializeFramesPositions.m new file mode 100644 index 0000000..f68b97d --- /dev/null +++ b/matlab/src/initializeFramesPositions.m @@ -0,0 +1,35 @@ +function [stewart] = initializeFramesPositions(stewart, args) +% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} +% +% Syntax: [stewart] = initializeFramesPositions(stewart, args) +% +% Inputs: +% - args - Can have the following fields: +% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m] +% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m] +% +% Outputs: +% - stewart - A structure with the following fields: +% - geometry.H [1x1] - Total Height of the Stewart Platform [m] +% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m] +% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m] +% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m] + +arguments + stewart + args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 + args.MO_B (1,1) double {mustBeNumeric} = 50e-3 +end + +H = args.H; % Total Height of the Stewart Platform [m] + +FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m] + +MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] + +FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m] + +stewart.geometry.H = H; +stewart.geometry.FO_M = FO_M; +stewart.platform_M.MO_B = MO_B; +stewart.platform_F.FO_A = FO_A; diff --git a/matlab/src/initializeJointDynamics.m b/matlab/src/initializeJointDynamics.m new file mode 100644 index 0000000..4c4d1b5 --- /dev/null +++ b/matlab/src/initializeJointDynamics.m @@ -0,0 +1,129 @@ +function [stewart] = initializeJointDynamics(stewart, args) +% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints +% +% Syntax: [stewart] = initializeJointDynamics(args) +% +% Inputs: +% - args - Structure with the following fields: +% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p' +% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p' +% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad] +% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad] +% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)] +% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)] +% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad] +% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad] +% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)] +% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - stewart.joints_F and stewart.joints_M: +% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect) +% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m] +% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad] +% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad] +% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)] +% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)] +% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)] + + arguments + stewart + args.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' + args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' + args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.K_M double {mustBeNumeric} = zeros(6,6) + args.M_M double {mustBeNumeric} = zeros(6,6) + args.n_xyz_M double {mustBeNumeric} = zeros(2,3) + args.xi_M double {mustBeNumeric} = 0.1 + args.step_file_M char {} = '' + args.K_F double {mustBeNumeric} = zeros(6,6) + args.M_F double {mustBeNumeric} = zeros(6,6) + args.n_xyz_F double {mustBeNumeric} = zeros(2,3) + args.xi_F double {mustBeNumeric} = 0.1 + args.step_file_F char {} = '' + end + + switch args.type_F + case '2dof' + stewart.joints_F.type = 1; + case '3dof' + stewart.joints_F.type = 2; + case '4dof' + stewart.joints_F.type = 3; + case '6dof' + stewart.joints_F.type = 4; + case 'flexible' + stewart.joints_F.type = 5; + otherwise + error("joints_F are not correctly defined") + end + + switch args.type_M + case '2dof' + stewart.joints_M.type = 1; + case '3dof' + stewart.joints_M.type = 2; + case '4dof' + stewart.joints_M.type = 3; + case '6dof' + stewart.joints_M.type = 4; + case 'flexible' + stewart.joints_M.type = 5; + otherwise + error("joints_M are not correctly defined") + end + + stewart.joints_M.Ka = args.Ka_M; + stewart.joints_M.Kr = args.Kr_M; + + stewart.joints_F.Ka = args.Ka_F; + stewart.joints_F.Kr = args.Kr_F; + + stewart.joints_M.Ca = args.Ca_M; + stewart.joints_M.Cr = args.Cr_M; + + stewart.joints_F.Ca = args.Ca_F; + stewart.joints_F.Cr = args.Cr_F; + + stewart.joints_M.Kf = args.Kf_M; + stewart.joints_M.Kt = args.Kt_M; + + stewart.joints_F.Kf = args.Kf_F; + stewart.joints_F.Kt = args.Kt_F; + + stewart.joints_M.Cf = args.Cf_M; + stewart.joints_M.Ct = args.Ct_M; + + stewart.joints_F.Cf = args.Cf_F; + stewart.joints_F.Ct = args.Ct_F; + + stewart.joints_F.M = args.M_F; + stewart.joints_F.K = args.K_F; + stewart.joints_F.n_xyz = args.n_xyz_F; + stewart.joints_F.xi = args.xi_F; + stewart.joints_F.xi = args.xi_F; + stewart.joints_F.step_file = args.step_file_F; + + stewart.joints_M.M = args.M_M; + stewart.joints_M.K = args.K_M; + stewart.joints_M.n_xyz = args.n_xyz_M; + stewart.joints_M.xi = args.xi_M; + stewart.joints_M.step_file = args.step_file_M; + +end diff --git a/matlab/src/initializeSample.m b/matlab/src/initializeSample.m new file mode 100644 index 0000000..a0b3eaa --- /dev/null +++ b/matlab/src/initializeSample.m @@ -0,0 +1,26 @@ +function [sample] = initializeSample(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none' + args.H_offset (1,1) double {mustBeNumeric} = 0 % Vertical offset [m] + args.H (1,1) double {mustBeNumeric, mustBePositive} = 200e-3 % Height [m] + args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m] + args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg] + end + + sample = struct(); + + switch args.type + case 'none' + sample.type = 0; + sample.m = 0; + case 'cylindrical' + sample.type = 1; + + sample.H_offset = args.H_offset; + + sample.H = args.H; + sample.R = args.R; + sample.m = args.m; + end +end diff --git a/matlab/src/initializeStewartPlatform.m b/matlab/src/initializeStewartPlatform.m new file mode 100644 index 0000000..0545b4d --- /dev/null +++ b/matlab/src/initializeStewartPlatform.m @@ -0,0 +1,31 @@ +function [stewart] = initializeStewartPlatform() +% initializeStewartPlatform - Initialize the stewart structure +% +% Syntax: [stewart] = initializeStewartPlatform(args) +% +% Outputs: +% - stewart - A structure with the following sub-structures: +% - platform_F - +% - platform_M - +% - joints_F - +% - joints_M - +% - struts_F - +% - struts_M - +% - actuators - +% - geometry - +% - properties - + +stewart = struct(); +stewart.platform_F = struct(); +stewart.platform_M = struct(); +stewart.joints_F = struct(); +stewart.joints_M = struct(); +stewart.struts_F = struct(); +stewart.struts_M = struct(); +stewart.actuators = struct(); +stewart.sensors = struct(); +stewart.sensors.inertial = struct(); +stewart.sensors.force = struct(); +stewart.sensors.relative = struct(); +stewart.geometry = struct(); +stewart.kinematics = struct(); diff --git a/matlab/src/initializeStewartPose.m b/matlab/src/initializeStewartPose.m new file mode 100644 index 0000000..325b501 --- /dev/null +++ b/matlab/src/initializeStewartPose.m @@ -0,0 +1,27 @@ +function [stewart] = initializeStewartPose(stewart, args) +% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose +% It uses the inverse kinematic +% +% Syntax: [stewart] = initializeStewartPose(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - Aa [3x6] - The positions ai expressed in {A} +% - Bb [3x6] - The positions bi expressed in {B} +% - args - Can have the following fields: +% - AP [3x1] - The wanted position of {B} with respect to {A} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A} + +arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) +end + +[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); + +stewart.actuators.Leq = dLi; diff --git a/matlab/src/initializeStrutDynamics.m b/matlab/src/initializeStrutDynamics.m new file mode 100644 index 0000000..79c2898 --- /dev/null +++ b/matlab/src/initializeStrutDynamics.m @@ -0,0 +1,60 @@ +function [stewart] = initializeStrutDynamics(stewart, args) +% initializeStrutDynamics - Add Stiffness and Damping properties of each strut +% +% Syntax: [stewart] = initializeStrutDynamics(args) +% +% Inputs: +% - args - Structure with the following fields: +% - K [6x1] - Stiffness of each strut [N/m] +% - C [6x1] - Damping of each strut [N/(m/s)] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - actuators.type = 1 +% - actuators.K [6x1] - Stiffness of each strut [N/m] +% - actuators.C [6x1] - Damping of each strut [N/(m/s)] + + arguments + stewart + args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof' + args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6 + args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6 + args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6 + args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1 + args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 + args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6 + args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10 + + args.F_gain (1,1) double {mustBeNumeric} = 1 + args.me (1,1) double {mustBeNumeric} = 0.01 + args.ma (1,1) double {mustBeNumeric} = 0.01 + end + + if strcmp(args.type, '1dof') + stewart.actuators.type = 1; + elseif strcmp(args.type, '2dof') + stewart.actuators.type = 2; + elseif strcmp(args.type, 'flexible') + stewart.actuators.type = 3; + end + + stewart.actuators.k = args.k; + stewart.actuators.c = args.c; + + % Parallel stiffness + stewart.actuators.kp = args.kp; + stewart.actuators.cp = args.cp; + + stewart.actuators.ka = args.ka; + stewart.actuators.ca = args.ca; + + stewart.actuators.ke = args.ke; + stewart.actuators.ce = args.ce; + + stewart.actuators.F_gain = args.F_gain; + + stewart.actuators.ma = args.ma; + stewart.actuators.me = args.me; + +end diff --git a/matlab/src/inverseKinematics.m b/matlab/src/inverseKinematics.m new file mode 100644 index 0000000..c1c0618 --- /dev/null +++ b/matlab/src/inverseKinematics.m @@ -0,0 +1,36 @@ +function [Li, dLi] = inverseKinematics(stewart, args) +% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A} +% +% Syntax: [stewart] = inverseKinematics(stewart) +% +% Inputs: +% - stewart - A structure with the following fields +% - geometry.Aa [3x6] - The positions ai expressed in {A} +% - geometry.Bb [3x6] - The positions bi expressed in {B} +% - geometry.l [6x1] - Length of each strut +% - args - Can have the following fields: +% - AP [3x1] - The wanted position of {B} with respect to {A} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} +% +% Outputs: +% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A} +% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A} + +arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) +end + +assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa') +Aa = stewart.geometry.Aa; + +assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb') +Bb = stewart.geometry.Bb; + +assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l') +l = stewart.geometry.l; + +Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa)); + +dLi = Li-l; diff --git a/matlab/src/plotCube.m b/matlab/src/plotCube.m new file mode 100644 index 0000000..7367775 --- /dev/null +++ b/matlab/src/plotCube.m @@ -0,0 +1,58 @@ +function [] = plotCube(stewart, args) + +arguments + stewart + args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 + args.FOc (1,1) double {mustBeNumeric} = 50e-3 + args.color (4,1) double {mustBeNumeric} = [0,0,0,0.5] + args.linewidth (1,1) double {mustBeNumeric, mustBePositive} = 2.5 + args.link_to_struts logical {mustBeNumericOrLogical} = false +end + +sx = [ 2; -1; -1]; +sy = [ 0; 1; -1]; +sz = [ 1; 1; 1]; + +R = [sx, sy, sz]./vecnorm([sx, sy, sz]); + +L = args.Hc*sqrt(3); + +p_xyz = R'*[[0;0;0],[L;0;0],[L;L;0],[0;L;0],[0;0;L],[L;0;L],[L;L;L],[0;L;L]] - [0;0;1.5*args.Hc]; + +% Position center of the cube +p_xyz = p_xyz + args.FOc*[0;0;1]*ones(1,8); + +edges_order = [1 2 3 4 1]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [5 6 7 8 5]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [1 5]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [2 6]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [3 7]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [4 8]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); + +if args.link_to_struts + Fb = stewart.platform_M.Mb + stewart.geometry.FO_M; + plot3([Fb(1,1), p_xyz(1,5)],... + [Fb(2,1), p_xyz(2,5)],... + [Fb(3,1), p_xyz(3,5)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,2), p_xyz(1,2)],... + [Fb(2,2), p_xyz(2,2)],... + [Fb(3,2), p_xyz(3,2)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,3), p_xyz(1,2)],... + [Fb(2,3), p_xyz(2,2)],... + [Fb(3,3), p_xyz(3,2)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,4), p_xyz(1,4)],... + [Fb(2,4), p_xyz(2,4)],... + [Fb(3,4), p_xyz(3,4)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,5), p_xyz(1,4)],... + [Fb(2,5), p_xyz(2,4)],... + [Fb(3,5), p_xyz(3,4)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,6), p_xyz(1,5)],... + [Fb(2,6), p_xyz(2,5)],... + [Fb(3,6), p_xyz(3,5)], '--', 'color', args.color, 'linewidth', args.linewidth); +end diff --git a/matlab/src/plotCylindricalPayload.m b/matlab/src/plotCylindricalPayload.m new file mode 100644 index 0000000..93cb975 --- /dev/null +++ b/matlab/src/plotCylindricalPayload.m @@ -0,0 +1,15 @@ +function [] = plotCylindricalPayload(stewart, args) + +arguments + stewart + args.H (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.R (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.H_offset (1,1) double {mustBeNumeric} = 0 + args.color (3,1) double {mustBeNumeric} = [0.5,0.5,0.5] +end + +[X,Y,Z] = cylinder(args.R); +Z = args.H*Z + args.H_offset; +surf(X, Y, Z, 'facecolor', args.color, 'edgecolor', 'none') +fill3(X(1,:), Y(1,:), Z(1,:), 'k', 'facecolor', args.color) +fill3(X(2,:), Y(2,:), Z(2,:), 'k', 'facecolor', args.color) diff --git a/matlab/subsystems/metrology_6dof.slx b/matlab/subsystems/metrology_6dof.slx new file mode 100644 index 0000000..d6bc758 Binary files /dev/null and b/matlab/subsystems/metrology_6dof.slx differ diff --git a/matlab/subsystems/nano_hexapod_fixed_base.slx b/matlab/subsystems/nano_hexapod_fixed_base.slx new file mode 100644 index 0000000..9b7075a Binary files /dev/null and b/matlab/subsystems/nano_hexapod_fixed_base.slx differ diff --git a/matlab/subsystems/nano_hexapod_mobile_platform.slx b/matlab/subsystems/nano_hexapod_mobile_platform.slx new file mode 100644 index 0000000..78ef458 Binary files /dev/null and b/matlab/subsystems/nano_hexapod_mobile_platform.slx differ diff --git a/matlab/subsystems/nano_hexapod_simscape.slx b/matlab/subsystems/nano_hexapod_simscape.slx new file mode 100644 index 0000000..2c93e23 Binary files /dev/null and b/matlab/subsystems/nano_hexapod_simscape.slx differ diff --git a/matlab/subsystems/nano_hexapod_strut.slx b/matlab/subsystems/nano_hexapod_strut.slx new file mode 100644 index 0000000..78da33f Binary files /dev/null and b/matlab/subsystems/nano_hexapod_strut.slx differ