stewart-simscape/kinematic-study.org

#+TITLE: Kinematic Study of the Stewart Platform
:DRAWER:
#+HTML_LINK_HOME: ./index.html
#+HTML_LINK_UP: ./index.html

#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :tangle matlab/kinematic_study.m
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs
:END:

* Matlab Init                                               :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
  <<matlab-dir>>
#+end_src

#+begin_src matlab :exports none :results silent :noweb yes
  <<matlab-init>>
#+end_src

#+begin_src matlab :results none :exports none
  simulinkproject('./');
#+end_src

* Needed Actuator Stroke
The goal is to determine the needed stroke of the actuators to obtain wanted translations and rotations.

** Stewart architecture definition
We use a cubic architecture.

#+begin_src matlab :results silent
  opts = struct(...
      'H_tot', 90, ... % Total height of the Hexapod [mm]
      'L',     200/sqrt(3), ... % Size of the Cube [mm]
      'H',     60, ... % Height between base joints and platform joints [mm]
      'H0',    200/2-60/2 ... % Height between the corner of the cube and the plane containing the base joints [mm]
      );
  stewart = initializeCubicConfiguration(opts);
  opts = struct(...
      'Jd_pos', [0, 0, 100], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]
      'Jf_pos', [0, 0, -50]  ... % Position of the Jacobian for force location from the top of the mobile platform [mm]
      );
  stewart = computeGeometricalProperties(stewart, opts);
  opts = struct(...
      'stroke', 50e-6 ... % Maximum stroke of each actuator [m]
      );
  stewart = initializeMechanicalElements(stewart, opts);

  save('./mat/stewart.mat', 'stewart');
#+end_src

** Wanted translations and rotations
We define wanted translations and rotations
#+begin_src matlab :results silent
  Tx_max = 15e-6; % Translation [m]
  Ty_max = 15e-6; % Translation [m]
  Tz_max = 15e-6; % Translation [m]
  Rx_max = 30e-6; % Rotation [rad]
  Ry_max = 30e-6; % Rotation [rad]
#+end_src

** Needed stroke for "pure" rotations or translations
First, we estimate the needed actuator stroke for "pure" rotations and translation.
#+begin_src matlab :results silent
  LTx = stewart.Jd*[Tx_max 0 0 0 0 0]';
  LTy = stewart.Jd*[0 Ty_max 0 0 0 0]';
  LTz = stewart.Jd*[0 0 Tz_max 0 0 0]';
  LRx = stewart.Jd*[0 0 0 Rx_max 0 0]';
  LRy = stewart.Jd*[0 0 0 0 Ry_max 0]';
#+end_src

#+begin_src matlab :results value :exports results
  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]))))
#+end_src

#+RESULTS:
: From -1.2e-05[m] to 1.1e-05[m]: Total stroke = 22.9[um]

** Needed stroke for combined translations and rotations
Now, we combine translations and rotations, and we try to find the worst case (that we suppose to happen at the border).
#+begin_src matlab :results none
  Lmax = 0;
  Lmin = 0;
  pos = [0, 0, 0, 0, 0];
  for Tx = [-Tx_max,Tx_max]
  for Ty = [-Ty_max,Ty_max]
  for Tz = [-Tz_max,Tz_max]
  for Rx = [-Rx_max,Rx_max]
  for Ry = [-Ry_max,Ry_max]
      lmax = max(stewart.Jd*[Tx Ty Tz Rx Ry 0]');
      lmin = min(stewart.Jd*[Tx Ty Tz Rx Ry 0]');
      if lmax > Lmax
          Lmax = lmax;
          pos = [Tx Ty Tz Rx Ry];
      end
      if lmin < Lmin
          Lmin = lmin;
      end
  end
  end
  end
  end
  end
#+end_src

We obtain a needed stroke shown below (almost two times the needed stroke for "pure" rotations and translations).
#+begin_src matlab :results value :exports results
  ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', Lmin, Lmax, 1e6*(Lmax-Lmin))
#+end_src

#+RESULTS:
: From -3.1e-05[m] to 3.1e-05[m]: Total stroke = 61.5[um]

* Maximum Stroke
From a specified actuator stroke, we try to estimate the available maneuverability of the Stewart platform.

#+begin_src matlab :results silent
  [X, Y, Z] = getMaxPositions(stewart);
#+end_src

#+begin_src matlab :results silent
  figure;
  plot3(X, Y, Z, 'k-')
#+end_src

* Functions
  :PROPERTIES:
  :HEADER-ARGS:matlab+: :exports code
  :HEADER-ARGS:matlab+: :comments no
  :HEADER-ARGS:matlab+: :mkdir yes
  :HEADER-ARGS:matlab+: :eval no
  :END:
** getMaxPositions
  :PROPERTIES:
  :HEADER-ARGS:matlab+: :tangle src/getMaxPositions.m
  :END:
#+begin_src matlab
  function [X, Y, Z] = getMaxPositions(stewart)
      Leg = stewart.Leg;
      J = stewart.Jd;
      theta = linspace(0, 2*pi, 100);
      phi = linspace(-pi/2 , pi/2, 100);
      dmax = zeros(length(theta), length(phi));

      for i = 1:length(theta)
          for j = 1:length(phi)
              L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
              dmax(i, j) = Leg.stroke/max(abs(L));
          end
      end

      X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
      Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
      Z = dmax.*sin(repmat(phi,length(theta),1));
  end
#+end_src

** getMaxPureDisplacement
  :PROPERTIES:
  :HEADER-ARGS:matlab+: :tangle src/getMaxPureDisplacement.m
  :END:
#+begin_src matlab
  function [max_disp] = getMaxPureDisplacement(Leg, J)
      max_disp = zeros(6, 1);
      max_disp(1) = Leg.stroke/max(abs(J*[1 0 0 0 0 0]'));
      max_disp(2) = Leg.stroke/max(abs(J*[0 1 0 0 0 0]'));
      max_disp(3) = Leg.stroke/max(abs(J*[0 0 1 0 0 0]'));
      max_disp(4) = Leg.stroke/max(abs(J*[0 0 0 1 0 0]'));
      max_disp(5) = Leg.stroke/max(abs(J*[0 0 0 0 1 0]'));
      max_disp(6) = Leg.stroke/max(abs(J*[0 0 0 0 0 1]'));
  end
#+end_src
** =computeJacobian=: Compute the Jacobian Matrix
:PROPERTIES:
:header-args:matlab+: :tangle src/computeJacobian.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:computeJacobian>>

This Matlab function is accessible [[file:src/computeJacobian.m][here]].

*** Function description
#+begin_src matlab
  function [stewart] = computeJacobian(stewart)
  % computeJacobian -
  %
  % Syntax: [stewart] = computeJacobian(stewart)
  %
  % Inputs:
  %    - stewart - With at least the following fields:
  %        - As [3x6] - The 6 unit vectors for each strut expressed in {A}
  %        - Ab [3x6] - The 6 position of the joints bi expressed in {A}
  %
  % Outputs:
  %    - stewart - With the 3 added field:
  %        - J [6x6] - The Jacobian Matrix
  %        - K [6x6] - The Stiffness Matrix
  %        - C [6x6] - The Compliance Matrix
#+end_src

*** Compute Jacobian Matrix
#+begin_src matlab
  stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
#+end_src

*** Compute Stiffness Matrix
#+begin_src matlab
  stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
#+end_src

*** Compute Compliance Matrix
#+begin_src matlab
  stewart.C = inv(stewart.K);
#+end_src

** =inverseKinematics=: Compute Inverse Kinematics
:PROPERTIES:
:header-args:matlab+: :tangle src/inverseKinematics.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:inverseKinematics>>

This Matlab function is accessible [[file:src/inverseKinematics.m][here]].

*** Function description
#+begin_src matlab
  function [Li, dLi] = inverseKinematics(stewart, args)
  % inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
  %
  % Syntax: [stewart] = inverseKinematics(stewart)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - Aa   [3x6] - The positions ai expressed in {A}
  %        - Bb   [3x6] - The positions bi expressed in {B}
  %    - args - Can have the following fields:
  %        - AP   [3x1] - The wanted position of {B} with respect to {A}
  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
  %
  % Outputs:
  %    - Li   [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
  %    - dLi  [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
#+end_src

*** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
  end
#+end_src

*** Theory
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.

From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
\begin{align*}
  l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
                         &= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
\end{align*}

To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
\begin{equation}
  l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
\end{equation}

Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
\begin{equation}
  l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
\end{equation}

If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.

*** Compute
#+begin_src matlab
  Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
#+end_src

#+begin_src matlab
  dLi = Li-stewart.l;
#+end_src

** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
:PROPERTIES:
:header-args:matlab+: :tangle src/forwardKinematicsApprox.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:forwardKinematicsApprox>>

This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]].

*** Function description
#+begin_src matlab
  function [P, R] = forwardKinematicsApprox(stewart, args)
  % forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
  %                           the Jacobian Matrix
  %
  % Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
  %
  % Inputs:
  %    - stewart - A structure with the following fields
  %        - J  [6x6] - The Jacobian Matrix
  %    - args - Can have the following fields:
  %        - dL [6x1] - Displacement of each strut [m]
  %
  % Outputs:
  %    - P  [3x1] - The estimated position of {B} with respect to {A}
  %    - R  [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
#+end_src

*** Optional Parameters
#+begin_src matlab
  arguments
      stewart
      args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
  end
#+end_src

*** Computation
From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
position and orientation of {B} with respect to {A} using the following formula:
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
#+begin_src matlab
  X = stewart.J\args.dL;
#+end_src

The position vector corresponds to the first 3 elements.
#+begin_src matlab
  P = X(1:3);
#+end_src

The next 3 elements are the orientation of {B} with respect to {A} expressed
using the screw axis.
#+begin_src matlab
  theta = norm(X(4:6));
  s = X(4:6)/theta;
#+end_src

We then compute the corresponding rotation matrix.
#+begin_src matlab
  R = [s(1)^2*(1-cos(theta)) + cos(theta) ,        s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
       s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta),         s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
       s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
#+end_src