232 lines
11 KiB
Matlab
232 lines
11 KiB
Matlab
%% 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);
|