Add tangled files
This commit is contained in:
64
matlab/approximate_inverse_kinematics_validity.m
Normal file
64
matlab/approximate_inverse_kinematics_validity.m
Normal file
@@ -0,0 +1,64 @@
|
||||
|
||||
|
||||
simulinkproject('./');
|
||||
|
||||
% Stewart architecture definition
|
||||
% We first define some general Stewart architecture.
|
||||
|
||||
stewart = initializeFramesPositions('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.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');
|
||||
67
matlab/mobility_from_actuator_stroke.m
Normal file
67
matlab/mobility_from_actuator_stroke.m
Normal file
@@ -0,0 +1,67 @@
|
||||
|
||||
|
||||
simulinkproject('./');
|
||||
|
||||
% Stewart architecture definition
|
||||
% Let's first define the Stewart platform architecture that we want to study.
|
||||
|
||||
stewart = initializeFramesPositions('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, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
|
||||
|
||||
|
||||
% Let's now define the actuator stroke.
|
||||
|
||||
L_min = -50e-6; % [m]
|
||||
L_max = 50e-6; % [m]
|
||||
|
||||
% Pure translations
|
||||
% Let's first estimate the mobility in translation when the orientation of the Stewart platform stays the same.
|
||||
|
||||
% As shown previously, for such small stroke, we can use the approximate Forward Dynamics solution using the Jacobian matrix:
|
||||
% \begin{equation*}
|
||||
% \delta\bm{\mathcal{L}} = \bm{J} \delta\bm{\mathcal{X}}
|
||||
% \end{equation*}
|
||||
|
||||
% To obtain the mobility "volume" attainable by the Stewart platform when it's orientation is set to zero, we use the spherical coordinate $(r, \theta, \phi)$.
|
||||
|
||||
% For each possible value of $(\theta, \phi)$, we compute the maximum radius $r$ attainable with the constraint that the stroke of each actuator should be between =L_min= and =L_max=.
|
||||
|
||||
thetas = linspace(0, pi, 50);
|
||||
phis = linspace(0, 2*pi, 50);
|
||||
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.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
|
||||
|
||||
rs(i, j) = max([dL(dL<0)*L_min; dL(dL>0)*L_max]);
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
% #+RESULTS:
|
||||
% | =L_min= [$\mu m$] | =L_max= [$\mu m$] | =R= [$\mu m$] |
|
||||
% |-------------------+-------------------+---------------|
|
||||
% | -50.0 | 50.0 | 31.5 |
|
||||
|
||||
|
||||
figure;
|
||||
plot3(reshape(rs.*(sin(thetas)'*cos(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(sin(thetas)'*sin(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(cos(thetas)'*ones(1, length(phis))), [1, length(thetas)*length(phis)]))
|
||||
xlabel('X Translation [m]');
|
||||
ylabel('Y Translation [m]');
|
||||
zlabel('Z Translation [m]');
|
||||
119
matlab/required_stroke_from_mobility.m
Normal file
119
matlab/required_stroke_from_mobility.m
Normal file
@@ -0,0 +1,119 @@
|
||||
|
||||
|
||||
simulinkproject('./');
|
||||
|
||||
% Stewart architecture definition
|
||||
% Let's first define the Stewart platform architecture that we want to study.
|
||||
|
||||
stewart = initializeFramesPositions('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, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
|
||||
% Wanted translations and rotations
|
||||
% Let's now define the wanted extreme translations and rotations.
|
||||
|
||||
Tx_max = 50e-6; % Translation [m]
|
||||
Ty_max = 50e-6; % Translation [m]
|
||||
Tz_max = 50e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
|
||||
% Needed stroke for "pure" rotations or translations
|
||||
% As a first estimation, we estimate the needed actuator stroke for "pure" rotations and translation.
|
||||
% We do that using either the Inverse Kinematic solution or the Jacobian matrix as an approximation.
|
||||
|
||||
|
||||
LTx = stewart.J*[Tx_max 0 0 0 0 0]';
|
||||
LTy = stewart.J*[0 Ty_max 0 0 0 0]';
|
||||
LTz = stewart.J*[0 0 Tz_max 0 0 0]';
|
||||
LRx = stewart.J*[0 0 0 Rx_max 0 0]';
|
||||
LRy = stewart.J*[0 0 0 0 Ry_max 0]';
|
||||
LRz = stewart.J*[0 0 0 0 0 Rz_max]';
|
||||
|
||||
|
||||
|
||||
% The obtain required stroke is:
|
||||
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', min(min([LTx,LTy,LTz,LRx,LRy])), max(max([LTx,LTy,LTz,LRx,LRy])), 1e6*(max(max([LTx,LTy,LTz,LRx,LRy]))-min(min([LTx,LTy,LTz,LRx,LRy]))))
|
||||
|
||||
% Needed stroke for "combined" rotations or translations
|
||||
% We know would like to have a more precise estimation.
|
||||
|
||||
% To do so, we may estimate the required actuator stroke for all possible combination of translation and rotation.
|
||||
|
||||
% Let's first generate all the possible combination of maximum translation and rotations.
|
||||
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
|
||||
|
||||
|
||||
% #+RESULTS:
|
||||
% | *Tx [m]* | *Ty [m]* | *Tz [m]* | *Rx [rad]* | *Ry [rad]* | *Rz [rad]* |
|
||||
% |----------+----------+----------+------------+------------+------------|
|
||||
% | -5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | -5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | -5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | -5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | 5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | 5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | 5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | -5.0e-05 | 5.0e-05 | 5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
% | 5.0e-05 | 5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
|
||||
% For all possible combination, we compute the required actuator stroke using the inverse kinematic solution.
|
||||
|
||||
L_min = 0;
|
||||
L_max = 0;
|
||||
|
||||
for i = 1:size(Ps,1)
|
||||
Rx = [1 0 0;
|
||||
0 cos(Ps(i, 4)) -sin(Ps(i, 4));
|
||||
0 sin(Ps(i, 4)) cos(Ps(i, 4))];
|
||||
|
||||
Ry = [ cos(Ps(i, 5)) 0 sin(Ps(i, 5));
|
||||
0 1 0;
|
||||
-sin(Ps(i, 5)) 0 cos(Ps(i, 5))];
|
||||
|
||||
Rz = [cos(Ps(i, 6)) -sin(Ps(i, 6)) 0;
|
||||
sin(Ps(i, 6)) cos(Ps(i, 6)) 0;
|
||||
0 0 1];
|
||||
|
||||
ARB = Rz*Ry*Rx;
|
||||
[~, Ls] = inverseKinematics(stewart, 'AP', Ps(i, 1:3)', 'ARB', ARB);
|
||||
|
||||
if min(Ls) < L_min
|
||||
L_min = min(Ls)
|
||||
end
|
||||
if max(Ls) > L_max
|
||||
L_max = max(Ls)
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
% We obtain the required actuator stroke:
|
||||
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', L_min, L_max, 1e6*(L_max-L_min))
|
||||
Reference in New Issue
Block a user