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