80 lines
2.8 KiB
Matlab
80 lines
2.8 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
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Data directory
|
|
data_dir = './mat/';
|
|
|
|
% Simulink Model name
|
|
mdl = 'nano_hexapod_model';
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector [Hz]
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
%% Estimate the errors associated with approximate forward kinematics using the Jacobian matrix
|
|
stewart = initializeSimplifiedNanoHexapod('H', 100e-3);
|
|
|
|
Xrs = logspace(-6, -1, 10); % Wanted X translation of the mobile platform [m]
|
|
phis = linspace(-pi, pi, 100); % Tested azimutal angles [rad]
|
|
thetas = linspace(0, pi, 100); % Tested polar angles [rad]
|
|
|
|
% Compute the strut exact length for each X-position
|
|
Xrs_errors = zeros(1, length(Xrs)); % Maximum distance error [m]
|
|
Rrs_errors = zeros(1, length(Xrs)); % Maximum angular error [rad]
|
|
for i = 1:length(Xrs)
|
|
Xrs_error_min = 0;
|
|
Rrs_error_min = 0;
|
|
for theta = thetas
|
|
for phi = phis
|
|
ix = [sin(theta)*cos(phi); sin(theta)*sin(phi);cos(theta)]; % Unit vector for the displacement direction
|
|
[~, L_exact] = inverseKinematics(stewart, 'AP', Xrs(i)*ix); % Compute exact strut length for the wanted position
|
|
Xrs_approx = inv(stewart.geometry.J)*L_exact; % Approximate the position using the Jacobian
|
|
Xrs_error = norm(Xrs(i)*ix - Xrs_approx(1:3), 2); % Compute the position estimation error
|
|
Rrs_error = norm(Xrs_approx(4:6), 2); % Compute the angular estimation error
|
|
if Xrs_error > Xrs_error_min
|
|
Xrs_error_min = Xrs_error;
|
|
end
|
|
if Rrs_error > Rrs_error_min
|
|
Rrs_error_min = Rrs_error;
|
|
end
|
|
end
|
|
end
|
|
Xrs_errors(i) = Xrs_error_min;
|
|
Rrs_errors(i) = Rrs_error_min;
|
|
end
|
|
|
|
%% Errors associated with the use of the Jacobian matrix to solve the forward kinematic problem
|
|
figure;
|
|
yyaxis left
|
|
hold on;
|
|
plot(1e6*Xrs, 1e9*Xrs_errors, 'DisplayName', '$\epsilon_D$');
|
|
plot(1e6*Xrs, 1e6*Xrs, '--', 'DisplayName', '$0.1\%$ error');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]); ylim([1, 1e4]);
|
|
xlabel('Motion Stroke');
|
|
ylabel('Kinematic Errors');
|
|
xticks([1, 10, 100, 1000, 10000]);
|
|
yticks([1, 10, 100, 1000, 10000]);
|
|
xticklabels({'$1\mu m$', '$10\mu m$', '$100\mu m$', '$1mm$', '$10mm$'});
|
|
yticklabels({'$1nm$', '$10nm$', '$100nm$', '$1\mu m$', '$10\mu m$'});
|
|
|
|
yyaxis right
|
|
plot(1e6*Xrs, 1e9*Rrs_errors, 'DisplayName', '$\epsilon_R$');
|
|
set(gca, 'YScale', 'log');
|
|
ylim([1, 1e4]);
|
|
yticks([1, 10, 100, 1000, 10000]);
|
|
yticklabels({'$1$nrad', '$10$nrad', '$100$nrad', '$1\mu$rad', '$10\mu$rad'});
|