Verify that matlab scripts are working
This commit is contained in:
parent
b6a32476b5
commit
843fb79413
@ -7,6 +7,10 @@ s = zpk('s');
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for functions
|
||||
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'nano_hexapod_model';
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
@ -20,7 +24,7 @@ stewart = generateGeneralConfiguration(stewart, 'FH', 0, 'FR', 100e-3, 'MH', 0,
|
||||
'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
|
||||
'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'K', ones(6,1));
|
||||
stewart = initializeStrutDynamics(stewart, 'k', 1);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 110e-3, 'Mpr', 110e-3);
|
||||
|
||||
@ -75,7 +79,7 @@ stewart_vert = generateGeneralConfiguration(stewart_vert, 'FH', 0, 'FR', 100e-3,
|
||||
'FTh', [-25, 25, 120-25, 120+25, 240-25, 240+25]*(pi/180), ...
|
||||
'MTh', [-60+25, 60-25, 60+25, 180-25, 180+25, -60-25]*(pi/180));
|
||||
stewart_vert = computeJointsPose(stewart_vert);
|
||||
stewart_vert = initializeStrutDynamics(stewart_vert, 'K', ones(6,1));
|
||||
stewart_vert = initializeStrutDynamics(stewart_vert, 'k', 1);
|
||||
stewart_vert = computeJacobian(stewart_vert);
|
||||
stewart_vert = initializeCylindricalPlatforms(stewart_vert, 'Fpr', 110e-3, 'Mpr', 110e-3);
|
||||
|
||||
@ -88,15 +92,15 @@ stewart_hori = generateGeneralConfiguration(stewart_hori, 'FH', 0, 'FR', 100e-3,
|
||||
'FTh', [-5, 5, 120-5, 120+5, 240-5, 240+5]*(pi/180), ...
|
||||
'MTh', [-60+5, 60-5, 60+5, 180-5, 180+5, -60-5]*(pi/180));
|
||||
stewart_hori = computeJointsPose(stewart_hori);
|
||||
stewart_hori = initializeStrutDynamics(stewart_hori, 'K', ones(6,1));
|
||||
stewart_hori = initializeStrutDynamics(stewart_hori, 'k', 1);
|
||||
stewart_hori = computeJacobian(stewart_hori);
|
||||
stewart_hori = initializeCylindricalPlatforms(stewart_hori, 'Fpr', 110e-3, 'Mpr', 110e-3);
|
||||
|
||||
displayArchitecture(stewart_hori, 'labels', false, 'frames', false, 'F_color', colors(2,:), 'M_color', colors(2,:), 'L_color', colors(2,:));
|
||||
|
||||
%% Translation mobility for two Stewart platform geometry
|
||||
thetas = linspace(0, pi, 50);
|
||||
phis = linspace(0, 2*pi, 50);
|
||||
thetas = linspace(0, pi, 100);
|
||||
phis = linspace(0, 2*pi, 100);
|
||||
rs_vert = zeros(length(thetas), length(phis));
|
||||
rs_hori = zeros(length(thetas), length(phis));
|
||||
|
||||
@ -131,8 +135,8 @@ Zs = 1e6*L_max*Zs;
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
surf(X_vert, Y_vert, Z_vert, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:));
|
||||
surf(X_hori, Y_hori+400, Z_hori, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(2,:));
|
||||
surf(X_vert, Y_vert, Z_vert, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:));
|
||||
surf(X_hori, Y_hori+400, Z_hori, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:));
|
||||
quiver3(0, 0, 0, 200, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
||||
quiver3(0, 0, 0, 0, 200, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
||||
quiver3(0, 0, 0, 0, 0, 200, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
||||
@ -158,7 +162,7 @@ stewart_close = generateGeneralConfiguration(stewart_close, 'FH', 0, 'FR', 50e-3
|
||||
'FTh', [-6, 6, 120-6, 120+6, 240-6, 240+6]*(pi/180), ...
|
||||
'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180));
|
||||
stewart_close = computeJointsPose(stewart_close);
|
||||
stewart_close = initializeStrutDynamics(stewart_close, 'K', ones(6,1));
|
||||
stewart_close = initializeStrutDynamics(stewart_close, 'k', 1);
|
||||
stewart_close = computeJacobian(stewart_close);
|
||||
stewart_close = initializeCylindricalPlatforms(stewart_close, 'Fpr', 110e-3, 'Mpr', 110e-3);
|
||||
|
||||
@ -172,7 +176,7 @@ stewart_space = generateGeneralConfiguration(stewart_space, 'FH', 0, 'FR', 100e-
|
||||
'MTh', [-60+6, 60-6, 60+6, 180-6, 180+6, -60-6]*(pi/180));
|
||||
stewart_space.platform_F.Fa = stewart_space.platform_M.Mb - (stewart_close.platform_M.Mb - stewart_close.platform_F.Fa);
|
||||
stewart_space = computeJointsPose(stewart_space);
|
||||
stewart_space = initializeStrutDynamics(stewart_space, 'K', ones(6,1));
|
||||
stewart_space = initializeStrutDynamics(stewart_space, 'k', 1);
|
||||
stewart_space = computeJacobian(stewart_space);
|
||||
stewart_space = initializeCylindricalPlatforms(stewart_space, 'Fpr', 110e-3, 'Mpr', 110e-3);
|
||||
|
||||
@ -210,8 +214,8 @@ Z_space = 1e6 * rs_space .* cos(theta_grid);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
surf(X_close, Y_close, Z_close, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:));
|
||||
surf(X_space, Y_space+6000, Z_space, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(2,:));
|
||||
surf(X_close, Y_close, Z_close, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(1,:));
|
||||
surf(X_space, Y_space+6000, Z_space, 'FaceColor', 'white', 'LineWidth', 0.1, 'EdgeColor', colors(2,:));
|
||||
quiver3(0, 0, 0, 4000, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
||||
quiver3(0, 0, 0, 0, 4000, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
||||
quiver3(0, 0, 0, 0, 0, 4000, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -7,6 +7,10 @@ s = zpk('s');
|
||||
%% Path for functions, data and scripts
|
||||
addpath('./mat/'); % Path for data
|
||||
addpath('./src/'); % Path for functions
|
||||
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
||||
|
||||
% Simulink Model name
|
||||
mdl = 'nano_hexapod_model';
|
||||
|
||||
%% Colors for the figures
|
||||
colors = colororder;
|
||||
@ -25,14 +29,39 @@ nano_hexapod = generateGeneralConfiguration(nano_hexapod, ...
|
||||
'MR', 110e-3, ...
|
||||
'MTh', [255, 285, 15, 45, 135, 165]*(pi/180));
|
||||
nano_hexapod = computeJointsPose(nano_hexapod);
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'K', ones(6,1));
|
||||
nano_hexapod = initializeStrutDynamics(nano_hexapod, 'k', 1);
|
||||
nano_hexapod = computeJacobian(nano_hexapod);
|
||||
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 130e-3, 'Mpr', 120e-3);
|
||||
nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpr', 125e-3, 'Mpr', 115e-3);
|
||||
|
||||
displayArchitecture(nano_hexapod, 'labels', true);
|
||||
%% Obtained architecture for the Nano Hexapod
|
||||
figure;
|
||||
displayArchitecture(nano_hexapod, 'labels', true, 'frames', false);
|
||||
% Bottom circle
|
||||
h = 15e-3;
|
||||
r = 120e-3;
|
||||
theta = linspace(0, 2*pi, 100);
|
||||
x = r * cos(theta);
|
||||
y = r * sin(theta);
|
||||
z = h * ones(size(theta)); % All points at same height
|
||||
plot3(x, y, z, '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
|
||||
for i = 1:6
|
||||
plot3([0, nano_hexapod.platform_F.Fa(1,i)], [0, nano_hexapod.platform_F.Fa(2,i)], [h,h], '--', 'color', [colors(1,:)], 'LineWidth', 0.5);
|
||||
end
|
||||
% Top circle
|
||||
h = 95e-3 - 15e-3;
|
||||
r = 110e-3;
|
||||
theta = linspace(0, 2*pi, 100);
|
||||
x = r * cos(theta);
|
||||
y = r * sin(theta);
|
||||
z = h * ones(size(theta)); % All points at same height
|
||||
plot3(x, y, z, '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
|
||||
for i = 1:6
|
||||
plot3([0, nano_hexapod.platform_M.Mb(1,i)], [0, nano_hexapod.platform_M.Mb(2,i)], [h,h], '--', 'color', [colors(2,:)], 'LineWidth', 0.5);
|
||||
end
|
||||
|
||||
max_translation = 50e-6;
|
||||
max_rotation = 50e-6;
|
||||
%% Estimate required actuator stroke for the wanted mobility
|
||||
max_translation = 50e-6; % Wanted translation mobility [m]
|
||||
max_rotation = 50e-6; % Wanted rotation mobility [rad]
|
||||
|
||||
Dxs = linspace(-max_translation, max_translation, 3);
|
||||
Dys = linspace(-max_translation, max_translation, 3);
|
||||
@ -40,14 +69,14 @@ Dzs = linspace(-max_translation, max_translation, 3);
|
||||
Rxs = linspace(-max_rotation, max_rotation, 3);
|
||||
Rys = linspace(-max_rotation, max_rotation, 3);
|
||||
|
||||
L_min = 0;
|
||||
L_max = 0;
|
||||
L_min = 0; % Required actuator negative stroke [m]
|
||||
L_max = 0; % Required actuator negative stroke [m]
|
||||
|
||||
for Dx = Dxs
|
||||
for Dy = Dys
|
||||
for Dz = Dzs
|
||||
for Rx = Rxs
|
||||
for Ry = Rys
|
||||
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)] * ...
|
||||
@ -63,17 +92,15 @@ for Ry = Rys
|
||||
if max(Ls) > L_max
|
||||
L_max = max(Ls);
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
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);
|
||||
@ -117,14 +144,11 @@ for i = 1:length(thetas)
|
||||
dL_real = dL_lin_max*stroke_ratio + dL_rot_max;
|
||||
|
||||
% % Obtained maximum displacement in the considered direction with angular motion
|
||||
% rs(i, j) = stroke_ratio*max(abs(dL_lin_max));
|
||||
rs(i, j) = stroke_ratio*L_max/max(abs(dL_lin));
|
||||
end
|
||||
end
|
||||
|
||||
min(min(rs))
|
||||
max(max(rs))
|
||||
|
||||
%% Wanted translation mobility of the Nano-Hexapod and computed Mobility
|
||||
[phi_grid, theta_grid] = meshgrid(phis, thetas);
|
||||
X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid);
|
||||
Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid);
|
||||
@ -163,29 +187,51 @@ hold off;
|
||||
axis equal;
|
||||
grid on;
|
||||
xlabel('X Translation [$\mu$m]'); ylabel('Y Translation [$\mu$m]'); zlabel('Z Translation [$\mu$m]');
|
||||
xlim(2e6*[-L_max, L_max]); ylim(2e6*[-L_max, L_max]); zlim(2e6*[-L_max, L_max]);
|
||||
xlim([-150 150]); ylim([-150, 150]); zlim([-120, 120]);
|
||||
view(-160, 20)
|
||||
|
||||
%% Estimation of the required flexible joint angular stroke
|
||||
max_angles = zeros(1,6);
|
||||
%% Estimate required actuator stroke for the wanted mobility
|
||||
max_translation = 50e-6; % Wanted translation mobility [m]
|
||||
max_rotation = 50e-6; % Wanted rotation mobility [rad]
|
||||
|
||||
Dxs = linspace(-max_translation, max_translation, 3);
|
||||
Dys = linspace(-max_translation, max_translation, 3);
|
||||
Dzs = linspace(-max_translation, max_translation, 3);
|
||||
Rxs = linspace(-max_rotation, max_rotation, 3);
|
||||
Rys = linspace(-max_rotation, max_rotation, 3);
|
||||
|
||||
max_angles_F = zeros(1,6); % Maximum bending angle - Fixed joints [rad]
|
||||
max_angles_M = zeros(1,6); % Maximum bending angle - Mobile joints [rad]
|
||||
|
||||
% Compute initial strut orientation
|
||||
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', zeros(3,1), 'ARB', eye(3));
|
||||
As = nano_hexapod.geometry.As;
|
||||
As = nano_hexapod.geometry.As; % Fixed joints
|
||||
Bs = nano_hexapod.geometry.Bs; % Mobile joints
|
||||
|
||||
% Only consider translations, but add maximum expected top platform rotation
|
||||
for i = 1:length(thetas)
|
||||
for j = 1:length(phis)
|
||||
% Maximum translations
|
||||
Tx = rs(i,j)*sin(thetas(i))*cos(phis(j));
|
||||
Ty = rs(i,j)*sin(thetas(i))*sin(phis(j));
|
||||
Tz = rs(i,j)*cos(thetas(i));
|
||||
for Dx = Dxs
|
||||
for Dy = Dys
|
||||
for Dz = Dzs
|
||||
for Rx = Rxs
|
||||
for Ry = Rys
|
||||
ARB = [ cos(Ry) 0 sin(Ry);
|
||||
0 1 0;
|
||||
-sin(Ry) 0 cos(Ry)] * ...
|
||||
[ 1 0 0;
|
||||
0 cos(Rx) -sin(Rx);
|
||||
0 sin(Rx) cos(Rx)];
|
||||
|
||||
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Tx; Ty; Tz], 'ARB', eye(3));
|
||||
nano_hexapod = computeJointsPose(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);
|
||||
|
||||
angles = acos(dot(As, nano_hexapod.geometry.As));
|
||||
larger_angles = abs(angles) > max_angles;
|
||||
max_angles(larger_angles) = angles(larger_angles);
|
||||
angles_M = acos(dot(Bs, nano_hexapod.geometry.Bs));
|
||||
max_angles_M(angles_M > max_angles_M) = angles_M(angles_M > max_angles_M);
|
||||
|
||||
angles_F = acos(dot(Bs, nano_hexapod.geometry.As));
|
||||
max_angles_F(angles_F > max_angles_F) = angles_F(angles_F > max_angles_F);
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
sprintf('Maximum flexible joint angle is %.1f mrad', 1e3*max(max_angles))
|
||||
sprintf('Fixed joint stroke should be %.1f mrad', 1e3*max(max_angles_F))
|
||||
sprintf('Mobile joint stroke should be %.1f mrad', 1e3*max(max_angles_M))
|
||||
|
||||
@ -1646,11 +1646,11 @@ stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B);
|
||||
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'k', 1);
|
||||
stewart = initializeJointDynamics(stewart)
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart)
|
||||
stewart = initializeStewartPose(stewart);
|
||||
|
||||
displayArchitecture(stewart, 'labels', false, 'frames', false);
|
||||
plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.5], 'link_to_struts', true);
|
||||
@ -3149,16 +3149,12 @@ alpha_for_H = solve(ci_z + alpha * si_z == H, alpha);
|
||||
% Verify the results
|
||||
% Substitute alpha values and check the resulting bi_z values
|
||||
bi_z_0 = ci + alpha_for_0 * si;
|
||||
disp('Verification bi_z = 0:');
|
||||
disp(simplify(bi_z_0));
|
||||
disp('Radius for fixed base:');
|
||||
simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2))
|
||||
|
||||
bi_z_H = ci + alpha_for_H * si;
|
||||
disp('Verification bi_z = H:');
|
||||
disp(simplify(bi_z_H));
|
||||
|
||||
% Compute radius
|
||||
simplify(sqrt(bi_z_H(:,1).^2 + bi_z_H(:,2).^2))
|
||||
simplify(sqrt(bi_z_0(:,1).^2 + bi_z_0(:,2).^2))
|
||||
disp('Radius for mobile platform:');
|
||||
simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2))
|
||||
#+end_src
|
||||
|
||||
In order to determine the approximate size of the platform as a function of
|
||||
|
||||
Reference in New Issue
Block a user