tdehaeze/phd-nass-geometry
Archived
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add simscape model

Browse Source
This commit is contained in:
Thomas Dehaeze 2025-04-01 14:06:53 +02:00
parent 45bf5c8f94
commit e19528610c
28 changed files with 2241 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

231
matlab/detail_kinematics_1_geometry.m Normal file
View File

@ -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);

690
matlab/detail_kinematics_2_cubic.m Normal file
View File

@ -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');

191
matlab/detail_kinematics_3_nano_hexapod.m Normal file
View File

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

BIN
matlab/nano_hexapod_model.slx Normal file
View File

Binary file not shown.

35
matlab/src/computeJacobian.m Normal file
View File

@ -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;

90
matlab/src/computeJointsPose.m Normal file
View File

@ -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;

79
matlab/src/describeStewartPlatform.m Normal file
View File

@ -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

252
matlab/src/displayArchitecture.m Normal file
View File

@ -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

34
matlab/src/forwardKinematicsApprox.m Normal file
View File

@ -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)];

57
matlab/src/generateCubicConfiguration.m Normal file
View File

@ -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;

39
matlab/src/generateGeneralConfiguration.m Normal file
View File

@ -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;

17
matlab/src/initializeController.m Normal file
View File

@ -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

59
matlab/src/initializeCylindricalPlatforms.m Normal file
View File

@ -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;

50
matlab/src/initializeCylindricalStruts.m Normal file
View File

@ -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

35
matlab/src/initializeFramesPositions.m Normal file
View File

@ -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;

129
matlab/src/initializeJointDynamics.m Normal file
View File

@ -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

26
matlab/src/initializeSample.m Normal file
View File

@ -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

31
matlab/src/initializeStewartPlatform.m Normal file
View File

@ -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();

27
matlab/src/initializeStewartPose.m Normal file
View File

@ -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;

60
matlab/src/initializeStrutDynamics.m Normal file
View File

@ -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

36
matlab/src/inverseKinematics.m Normal file
View File

@ -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;

58
matlab/src/plotCube.m Normal file
View File

@ -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

15
matlab/src/plotCylindricalPayload.m Normal file
View File

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

BIN
matlab/subsystems/metrology_6dof.slx Normal file
View File

Binary file not shown.

BIN
matlab/subsystems/nano_hexapod_fixed_base.slx Normal file
View File

Binary file not shown.

BIN
matlab/subsystems/nano_hexapod_mobile_platform.slx Normal file
View File

Binary file not shown.

BIN
matlab/subsystems/nano_hexapod_simscape.slx Normal file
View File

Binary file not shown.

BIN
matlab/subsystems/nano_hexapod_strut.slx Normal file
View File

Binary file not shown.