70 lines
2.4 KiB
Matlab
70 lines
2.4 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
simulinkproject('../');
|
|
|
|
% Stewart architecture definition
|
|
% We first define some general Stewart architecture.
|
|
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
|
stewart = generateGeneralConfiguration(stewart);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = initializeStrutDynamics(stewart);
|
|
stewart = initializeJointDynamics(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
|
|
% Comparison for "pure" translations
|
|
% Let's first compare the perfect and approximate solution of the inverse for pure $x$ translations.
|
|
|
|
% We compute the approximate and exact required strut stroke to have the wanted mobile platform $x$ displacement.
|
|
% The estimate required strut stroke for both the approximate and exact solutions are shown in Figure [[fig:inverse_kinematics_approx_validity_x_translation]].
|
|
% The relative strut length displacement is shown in Figure [[fig:inverse_kinematics_approx_validity_x_translation_relative]].
|
|
|
|
Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]
|
|
|
|
Ls_approx = zeros(6, length(Xrs));
|
|
Ls_exact = zeros(6, length(Xrs));
|
|
|
|
for i = 1:length(Xrs)
|
|
Xr = Xrs(i);
|
|
L_approx(:, i) = stewart.kinematics.J*[Xr; 0; 0; 0; 0; 0;];
|
|
[~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xr; 0; 0]);
|
|
end
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(Xrs, abs(L_approx(i, :)));
|
|
set(gca,'ColorOrderIndex',i);
|
|
plot(Xrs, abs(L_exact(i, :)), '--');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Wanted $x$ displacement [m]');
|
|
ylabel('Estimated required stroke');
|
|
|
|
|
|
|
|
% #+NAME: fig:inverse_kinematics_approx_validity_x_translation
|
|
% #+CAPTION: Comparison of the Approximate solution and True solution for the Inverse kinematic problem ([[./figs/inverse_kinematics_approx_validity_x_translation.png][png]], [[./figs/inverse_kinematics_approx_validity_x_translation.pdf][pdf]])
|
|
% [[file:figs/inverse_kinematics_approx_validity_x_translation.png]]
|
|
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(Xrs, abs(L_approx(i, :) - L_exact(i, :))./abs(L_approx(i, :) + L_exact(i, :)), 'k-');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Wanted $x$ displacement [m]');
|
|
ylabel('Relative Stroke Error');
|