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

Verify that matlab scripts are working

Browse Source
This commit is contained in:
Thomas Dehaeze 2025-04-02 16:56:24 +02:00
parent b6a32476b5
commit 843fb79413
4 changed files with 681 additions and 577 deletions

26
matlab/detail_kinematics_1_geometry.m

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

1068
matlab/detail_kinematics_2_cubic.m

File diff suppressed because it is too large Load Diff

148
matlab/detail_kinematics_3_nano_hexapod.m

@ -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,40 +69,38 @@ 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
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)];
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);
[~, Ls] = inverseKinematics(nano_hexapod, 'AP', [Dx;Dy;Dz], 'ARB', ARB);
if min(Ls) < L_min
L_min = min(Ls);
if min(Ls) < L_min
L_min = min(Ls);
end
if max(Ls) > L_max
L_max = max(Ls);
end
end
end
end
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);
@ -103,28 +130,25 @@ for i = 1:length(thetas)
dL_rot_max = zeros(6,1);
% Perform (small) rotations, and find the (worst) case requiring maximum strut motion
for Rx = Rxs
for Ry = Rys
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
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))
%% 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))

16
nass-geometry.org

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