From 19eecaaba8ddce2e07d6964ba817480d8dbf7fec Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Thu, 30 Jan 2020 11:12:06 +0100 Subject: [PATCH] Add tangled files --- kinematic-study.org | 14 +++ .../approximate_inverse_kinematics_validity.m | 64 ++++++++++ matlab/mobility_from_actuator_stroke.m | 67 ++++++++++ matlab/required_stroke_from_mobility.m | 119 ++++++++++++++++++ 4 files changed, 264 insertions(+) create mode 100644 matlab/approximate_inverse_kinematics_validity.m create mode 100644 matlab/mobility_from_actuator_stroke.m create mode 100644 matlab/required_stroke_from_mobility.m diff --git a/kinematic-study.org b/kinematic-study.org index bbf19ca..331353c 100644 --- a/kinematic-study.org +++ b/kinematic-study.org @@ -196,6 +196,12 @@ As the inverse kinematic can be easily solved exactly this is not much useful, h The function =forwardKinematicsApprox= (described [[sec:forwardKinematicsApprox][here]]) can be used to solve the forward kinematic problem using the Jacobian matrix. ** Estimation of the range validity of the approximate inverse kinematics +:PROPERTIES: +:header-args:matlab+: :tangle matlab/approximate_inverse_kinematics_validity.m +:header-args:matlab+: :comments org :mkdirp yes +:END: +<> + *** Introduction :ignore: As we know how to exactly solve the Inverse kinematic problem, we can compare the exact solution with the approximate solution using the Jacobian matrix. For small displacements, the approximate solution is expected to work well. @@ -299,6 +305,10 @@ The relative strut length displacement is shown in Figure [[fig:inverse_kinemati For small wanted displacements (up to $\approx 1\%$ of the size of the Hexapod), the approximate inverse kinematic solution using the Jacobian matrix is quite correct. * Estimated required actuator stroke from specified platform mobility +:PROPERTIES: +:header-args:matlab+: :tangle matlab/required_stroke_from_mobility.m +:header-args:matlab+: :comments org :mkdirp yes +:END: <> ** Introduction :ignore: Let's say one want to design a Stewart platform with some specified mobility (position and orientation). @@ -450,6 +460,10 @@ We obtain the required actuator stroke: This is probably a much realistic estimation of the required actuator stroke. * Estimated platform mobility from specified actuator stroke +:PROPERTIES: +:header-args:matlab+: :tangle matlab/mobility_from_actuator_stroke.m +:header-args:matlab+: :comments org :mkdirp yes +:END: <> ** Introduction :ignore: Here, from some value of the actuator stroke, we would like to estimate the mobility of the Stewart platform. diff --git a/matlab/approximate_inverse_kinematics_validity.m b/matlab/approximate_inverse_kinematics_validity.m new file mode 100644 index 0000000..5098e9d --- /dev/null +++ b/matlab/approximate_inverse_kinematics_validity.m @@ -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'); diff --git a/matlab/mobility_from_actuator_stroke.m b/matlab/mobility_from_actuator_stroke.m new file mode 100644 index 0000000..ff54742 --- /dev/null +++ b/matlab/mobility_from_actuator_stroke.m @@ -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]'); diff --git a/matlab/required_stroke_from_mobility.m b/matlab/required_stroke_from_mobility.m new file mode 100644 index 0000000..2c7a562 --- /dev/null +++ b/matlab/required_stroke_from_mobility.m @@ -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))