From 950302e5d6d710274e920df875f7492bca1aa302 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Mon, 27 Jan 2020 17:42:09 +0100 Subject: [PATCH] Reworked index.org: better filenames Removed few unused functions --- cubic-configuration.html | 530 +++--- identification.org | 468 ++++- index.html | 212 +-- index.org | 93 +- readme.org | 12 + simscape-model.html | 962 +--------- simscape-model.org | 1593 ---------------- .../stewart_strut_no_sensors.slx | Bin 26560 -> 26523 bytes simulink/stewart_platform_identification.slx | Bin 0 -> 29077 bytes ...stewart_platform_identification_simple.slx | Bin 0 -> 28855 bytes src/computeJointsPose.m | 4 +- src/forwardKinematics.m | 30 - src/generateGeneralConfiguration.m | 6 +- src/getMaxPositions.m | 18 - src/getMaxPureDisplacement.m | 9 - src/getStiffnessMatrix.m | 5 - src/identifyPlant.m | 67 - src/initializeHexapod.m | 171 -- src/initializeSimscapeData.m | 59 - src/initializeStewartPlatform.m | 94 - src/initializeStrutDynamics.m | 2 +- static-analysis.org | 87 + stewart-architecture.html | 1622 +++++++++++++++++ stewart-architecture.org | 1615 ++++++++++++++++ 24 files changed, 4041 insertions(+), 3618 deletions(-) create mode 100644 readme.org create mode 100644 simulink/stewart_platform_identification.slx create mode 100644 simulink/stewart_platform_identification_simple.slx delete mode 100644 src/forwardKinematics.m delete mode 100644 src/getMaxPositions.m delete mode 100644 src/getMaxPureDisplacement.m delete mode 100644 src/getStiffnessMatrix.m delete mode 100644 src/identifyPlant.m delete mode 100644 src/initializeHexapod.m delete mode 100644 src/initializeSimscapeData.m delete mode 100644 src/initializeStewartPlatform.m create mode 100644 static-analysis.org create mode 100644 stewart-architecture.html create mode 100644 stewart-architecture.org diff --git a/cubic-configuration.html b/cubic-configuration.html index 1d16096..8f79349 100644 --- a/cubic-configuration.html +++ b/cubic-configuration.html @@ -1,14 +1,15 @@ + - + Cubic configuration for the Stewart Platform - + - + + + + + - -

Stewart Platforms

-preumont07_six_axis_singl_stage_activ +The goal here is to provide a Matlab/Simscape Toolbox to study Stewart platforms.

-
-

1 Simscape Model

-
- -
+
+

1 Simulink Project (link)

-
-

2 Architecture Study

-
- -
+
+

2 Stewart Platform Architecture Definition (link)

-
-

3 Motion Control

-
-
    -
  • Active Damping
    • -
  • Inertial Control
    • -
  • Decentralized Control
    • -
+ +
+

3 Simscape Model of the Stewart Platform (link)

-
-
-

4 Notes about Stewart platforms

+ + +
+

4 Kinematic Analysis (link)

-
-
-

4.1 Jacobian

-
-
-
-

4.1.1 Relation to platform parameters

-
-

-A Jacobian is defined by: -

    -
  • the orientations of the struts \(\hat{s}_i\) expressed in a frame \(\{A\}\) linked to the fixed platform.
    • -
  • the vectors from \(O_B\) to \(b_i\) expressed in the frame \(\{A\}\)
    • +
  • Jacobian Analysis
    • +
  • Stiffness Analysis
    • +
  • Static Forces
- -

-Then, the choice of \(O_B\) changes the Jacobian. -

-
-

4.1.2 Jacobian for displacement

-
-

-\[ \dot{q} = J \dot{X} \] -With: -

+
+

5 Identification of the Stewart Dynamics (link)

+
    -
  • \(q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]\) vector of linear displacement of actuated joints
    • -
  • \(X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]\) position and orientation of \(O_B\) expressed in the frame \(\{A\}\)
    • +
  • Extraction of State Space models
    • +
  • Resonant Frequencies and Modal Damping
    • +
  • Mode Shapes
- -

-For very small displacements \(\delta q\) and \(\delta X\), we have \(\delta q = J \delta X\). -

-
-

4.1.3 Jacobian for forces

-
-

-\[ F = J^T \tau \] -With: -

+
+

6 Active Damping (link)

+
    -
  • \(\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]\) vector of actuator forces
    • -
  • \(F = [f_x\ f_y\ f_z\ n_x\ n_y\ n_z]\) force and torque acting on point \(O_B\)
    • +
  • Inertial Sensor
    • +
  • Force Sensor
    • +
  • Relative Motion Sensor
+ +
+

7 Control of the Stewart Platform (link)

-
-

4.2 Stiffness matrix \(K\)

-
-

-\[ K = J^T \text{diag}(k_i) J \] -

- -

-If all the struts have the same stiffness \(k\), then \(K = k J^T J\) -

- -

-\(K\) only depends of the geometry of the stewart platform: it depends on the Jacobian, that is on the orientations of the struts, position of the joints and choice of frame \(\{B\}\). -

- -

-\[ F = K X \] -

- -

-With \(F\) forces and torques applied to the moving platform at the origin of \(\{B\}\) and \(X\) the translations and rotations of \(\{B\}\) with respect to \(\{A\}\). -

- -

-\[ C = K^{-1} \] -

- -

-The compliance element \(C_{ij}\) is then the stiffness -\[ X_i = C_{ij} F_j \] -

-
+
+

8 Cubic Configuration (link)

-
-

4.3 Coupling

-
-

-What causes the coupling from \(F_i\) to \(X_i\) ? -

- -
-
\begin{tikzpicture}
-  \node[block] (Jt) at (0, 0) {$J^{-T}$};
-  \node[block, right= of Jt] (G) {$G$};
-  \node[block, right= of G] (J) {$J^{-1}$};
-
-  \draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$};
-  \draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$};
-  \draw[->] (G.east) -- (J.west) node[above left]{$q_i$};
-  \draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$};
-\end{tikzpicture}
-
-
- - -
-

coupling.png -

-

Figure 1: Block diagram to control an hexapod

-
- -

-There is no coupling from \(F_i\) to \(X_j\) if \(J^{-1} G J^{-T}\) is diagonal. -

- -

-If \(G\) is diagonal (cubic configuration), then \(J^{-1} G J^{-T} = G J^{-1} J^{-T} = G (J^{T} J)^{-1} = G K^{-1}\) -

- -

-Thus, the system is uncoupled if \(G\) and \(K\) are diagonal. -

-
-
+
+

9 Architecture Optimization

-

Bibliography

-
  • [preumont07_six_axis_singl_stage_activ] Preumont, Horodinca, Romanescu, de, Marneffe, Avraam, Deraemaeker, Bossens, & Abu Hanieh, A Six-Axis Single-Stage Active Vibration Isolator Based on Stewart Platform, Journal of Sound and Vibration, 300(3-5), 644-661 (2007). link. doi.
    • -
+ref.bib

diff --git a/index.org b/index.org index fa42ab1..8c1208a 100644 --- a/index.org +++ b/index.org @@ -22,90 +22,37 @@ :END: * Introduction :ignore: -The goal here is to +The goal here is to provide a Matlab/Simscape Toolbox to study Stewart platforms. -* Simscape Model of the Stewart Platform -- [[file:simscape-model.org][Model of the Stewart Platform]] -- [[file:identification.org][Identification of the Simscape Model]] +* Simulink Project ([[file:simulink-project.org][link]]) -* Architecture Study -- [[file:kinematic-study.org][Kinematic Study]] -- [[file:stiffness-study.org][Stiffness Matrix Study]] -- Jacobian Study -- [[file:cubic-configuration.org][Cubic Architecture]] +* Stewart Platform Architecture Definition ([[file:stewart-architecture.org][link]]) -* Motion Control -- Active Damping -- Inertial Control -- Decentralized Control -* Notes about Stewart platforms :noexport: -** Jacobian -*** Relation to platform parameters -A Jacobian is defined by: -- the orientations of the struts $\hat{s}_i$ expressed in a frame $\{A\}$ linked to the fixed platform. -- the vectors from $O_B$ to $b_i$ expressed in the frame $\{A\}$ +* Simscape Model of the Stewart Platform ([[file:simscape-model.org][link]]) -Then, the choice of $O_B$ changes the Jacobian. -*** Jacobian for displacement -\[ \dot{q} = J \dot{X} \] -With: -- $q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]$ vector of linear displacement of actuated joints -- $X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]$ position and orientation of $O_B$ expressed in the frame $\{A\}$ +* Kinematic Analysis ([[file:kinematic-study.org][link]]) +- Jacobian Analysis +- Stiffness Analysis +- Static Forces -For very small displacements $\delta q$ and $\delta X$, we have $\delta q = J \delta X$. +* Identification of the Stewart Dynamics ([[file:identification.org][link]]) +- Extraction of State Space models +- Resonant Frequencies and Modal Damping +- Mode Shapes -*** Jacobian for forces -\[ F = J^T \tau \] -With: -- $\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]$ vector of actuator forces -- $F = [f_x\ f_y\ f_z\ n_x\ n_y\ n_z]$ force and torque acting on point $O_B$ +* Active Damping ([[file:active-damping.org][link]]) +- Inertial Sensor +- Force Sensor +- Relative Motion Sensor -** Stiffness matrix $K$ +* Control of the Stewart Platform ([[file:control-study.org][link]]) -\[ K = J^T \text{diag}(k_i) J \] +* Cubic Configuration ([[file:cubic-configuration.org][link]]) -If all the struts have the same stiffness $k$, then $K = k J^T J$ +* Architecture Optimization -$K$ only depends of the geometry of the stewart platform: it depends on the Jacobian, that is on the orientations of the struts, position of the joints and choice of frame $\{B\}$. - -\[ F = K X \] - -With $F$ forces and torques applied to the moving platform at the origin of $\{B\}$ and $X$ the translations and rotations of $\{B\}$ with respect to $\{A\}$. - -\[ C = K^{-1} \] - -The compliance element $C_{ij}$ is then the stiffness -\[ X_i = C_{ij} F_j \] - -** Coupling -What causes the coupling from $F_i$ to $X_i$ ? - -#+begin_src latex :file coupling.pdf :post pdf2svg(file=*this*, ext="png") :exports both - \begin{tikzpicture} - \node[block] (Jt) at (0, 0) {$J^{-T}$}; - \node[block, right= of Jt] (G) {$G$}; - \node[block, right= of G] (J) {$J^{-1}$}; - - \draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$}; - \draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$}; - \draw[->] (G.east) -- (J.west) node[above left]{$q_i$}; - \draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$}; - \end{tikzpicture} -#+end_src - -#+name: fig:block_diag_coupling -#+caption: Block diagram to control an hexapod -#+RESULTS: -[[file:figs/coupling.png]] - -There is no coupling from $F_i$ to $X_j$ if $J^{-1} G J^{-T}$ is diagonal. - -If $G$ is diagonal (cubic configuration), then $J^{-1} G J^{-T} = G J^{-1} J^{-T} = G (J^{T} J)^{-1} = G K^{-1}$ - -Thus, the system is uncoupled if $G$ and $K$ are diagonal. - -* Bibliography :ignore: +* Bibliography :ignore: bibliographystyle:unsrt bibliography:ref.bib diff --git a/readme.org b/readme.org new file mode 100644 index 0000000..1ba8fa4 --- /dev/null +++ b/readme.org @@ -0,0 +1,12 @@ +#+TITLE: Stewart Platforms Toolbox +:DRAWER: +#+OPTIONS: toc:nil +#+OPTIONS: html-postamble:nil + +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +:END: diff --git a/simscape-model.html b/simscape-model.html index 6bd34d8..c39f8ff 100644 --- a/simscape-model.html +++ b/simscape-model.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Stewart Platform - Simscape Model @@ -246,31 +246,6 @@ for the JavaScript code in this tag. } /*]]>*///--> - -
@@ -279,944 +254,11 @@ for the JavaScript code in this tag. HOME

Stewart Platform - Simscape Model

-
-

Table of Contents

-
- -
-
-

-Stewart platforms are generated in multiple steps. -

- -

-We define 4 important frames: -

-
    -
  • \(\{F\}\): Frame fixed to the Fixed base and located at the center of its bottom surface. -This is used to fix the Stewart platform to some support.
    • -
  • \(\{M\}\): Frame fixed to the Moving platform and located at the center of its top surface. -This is used to place things on top of the Stewart platform.
    • -
  • \(\{A\}\): Frame fixed to the fixed base. -It defined the center of rotation of the moving platform.
    • -
  • \(\{B\}\): Frame fixed to the moving platform. -The motion of the moving platforms and forces applied to it are defined with respect to this frame \(\{B\}\).
    • -
- -

-Then, we define the location of the spherical joints: -

-
    -
  • \(\bm{a}_{i}\) are the position of the spherical joints fixed to the fixed base
    • -
  • \(\bm{b}_{i}\) are the position of the spherical joints fixed to the moving platform
    • -
- -

-We define the rest position of the Stewart platform: -

-
    -
  • For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.
    • -
  • Thus, to define the rest position of the Stewart platform, we just have to defined its total height \(H\). -\(H\) corresponds to the distance from the bottom of the fixed base to the top of the moving platform.
    • -
- -

-From \(\bm{a}_{i}\) and \(\bm{b}_{i}\), we can determine the length and orientation of each strut: -

-
    -
  • \(l_{i}\) is the length of the strut
    • -
  • \({}^{A}\hat{\bm{s}}_{i}\) is the unit vector align with the strut
    • -
- -

-The position of the Spherical joints can be computed using various methods: -

-
    -
  • Cubic configuration
    • -
  • Circular configuration
    • -
  • Arbitrary position
    • -
  • These methods should be easily scriptable and corresponds to specific functions that returns \({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\). -The input of these functions are the parameters corresponding to the wanted geometry.
    • -
- -

-For Simscape, we need: -

-
    -
  • The position and orientation of each spherical joint fixed to the fixed base: \({}^{F}\bm{a}_{i}\) and \({}^{F}\bm{R}_{a_{i}}\)
    • -
  • The position and orientation of each spherical joint fixed to the moving platform: \({}^{M}\bm{b}_{i}\) and \({}^{M}\bm{R}_{b_{i}}\)
    • -
  • The rest length of each strut: \(l_{i}\)
    • -
  • The stiffness and damping of each actuator: \(k_{i}\) and \(c_{i}\)
    • -
  • The position of the frame \(\{A\}\) with respect to the frame \(\{F\}\): \({}^{F}\bm{O}_{A}\)
    • -
  • The position of the frame \(\{B\}\) with respect to the frame \(\{M\}\): \({}^{M}\bm{O}_{B}\)
    • -
- - -
-

1 Procedure

-
-

-The procedure to define the Stewart platform is the following: -

-
    -
  1. Define the initial position of frames {A}, {B}, {F} and {M}. -We do that using the initializeFramesPositions function. -We have to specify the total height of the Stewart platform \(H\) and the position \({}^{M}O_{B}\) of {B} with respect to {M}.
    2. -
  2. Compute the positions of joints \({}^{F}a_{i}\) and \({}^{M}b_{i}\). -We can do that using various methods depending on the wanted architecture: -
      -
    • generateCubicConfiguration permits to generate a cubic configuration
      • -
    3. -
  3. Compute the position and orientation of the joints with respect to the fixed base and the moving platform. -This is done with the computeJointsPose function.
    4. -
  4. Define the dynamical properties of the Stewart platform. -The output are the stiffness and damping of each strut \(k_{i}\) and \(c_{i}\). -This can be done we simply choosing directly the stiffness and damping of each strut. -The stiffness and damping of each actuator can also be determine from the wanted stiffness of the Stewart platform for instance.
    5. -
  5. Define the mass and inertia of each element of the Stewart platform.
    6. -
- -

-By following this procedure, we obtain a Matlab structure stewart that contains all the information for the Simscape model and for further analysis. -

-
-
- -
-

2 Matlab Code

-
-
-
-

2.1 Simscape Model

-
-
-
open('stewart_platform.slx')
-
-
-
-
- -
-

2.2 Test the functions

-
-
-
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
-stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
-stewart = computeJointsPose(stewart);
-stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
-stewart = computeJacobian(stewart);
-[Li, dLi] = inverseKinematics(stewart, 'AP', [0;0;0.00001], 'ARB', eye(3));
-[P, R] = forwardKinematicsApprox(stewart, 'dL', dLi)
-
-
-
-
-
- -
-

3 initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

3.1 Function description

-
-
-
function [stewart] = initializeFramesPositions(args)
-% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
-%
-% Syntax: [stewart] = initializeFramesPositions(args)
-%
-% Inputs:
-%    - args - Can have the following fields:
-%        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
-%        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
-%
-% Outputs:
-%    - stewart - A structure with the following fields:
-%        - H    [1x1] - Total Height of the Stewart Platform [m]
-%        - FO_M [3x1] - Position of {M} with respect to {F} [m]
-%        - MO_B [3x1] - Position of {B} with respect to {M} [m]
-%        - FO_A [3x1] - Position of {A} with respect to {F} [m]
-
-
-
-
- -
-

3.2 Documentation

-
- -
-

stewart-frames-position.png -

-

Figure 1: Definition of the position of the frames

-
-
-
- -
-

3.3 Optional Parameters

-
-
-
arguments
-    args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
-    args.MO_B (1,1) double {mustBeNumeric} = 50e-3
-end
-
-
-
-
- -
-

3.4 Initialize the Stewart structure

-
-
-
stewart = struct();
-
-
-
-
- -
-

3.5 Compute the position of each frame

-
-
-
stewart.H = args.H; % Total Height of the Stewart Platform [m]
-
-stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
-
-stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
-
-stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
-
-
-
-
-
- -
-

4 generateCubicConfiguration: Generate a Cubic Configuration

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

4.1 Function description

-
-
-
function [stewart] = generateCubicConfiguration(stewart, args)
-% generateCubicConfiguration - Generate a Cubic Configuration
-%
-% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - H   [1x1] - Total height of the platform [m]
-%    - args - Can have the following fields:
-%        - Hc  [1x1] - Height of the "useful" part of the cube [m]
-%        - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
-%        - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
-%        - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%        - Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
-%        - Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
-
-
-
-
- -
-

4.2 Documentation

-
- -
-

cubic-configuration-definition.png -

-

Figure 2: Cubic Configuration

-
-
-
- -
-

4.3 Optional Parameters

-
-
-
arguments
-    stewart
-    args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
-    args.FOc (1,1) double {mustBeNumeric} = 50e-3
-    args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-end
-
-
-
-
- -
-

4.4 Position of the Cube

-
-

-We define the useful points of the cube with respect to the Cube’s center. -\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is -located at the center of the cube and aligned with {F} and {M}. -

- -
-
sx = [ 2; -1; -1];
-sy = [ 0;  1; -1];
-sz = [ 1;  1;  1];
-
-R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
-
-L = args.Hc*sqrt(3);
-
-Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
-
-CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
-CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
-
-
-
-
- -
-

4.5 Compute the pose

-
-

-We can compute the vector of each leg \({}^{C}\hat{\bm{s}}_{i}\) (unit vector from \({}^{C}C_{f}\) to \({}^{C}C_{m}\)). -

-
-
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
-
-
- -

-We now which to compute the position of the joints \(a_{i}\) and \(b_{i}\). -

-
-
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
-stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
-
-
-
-
-
- -
-

5 generateGeneralConfiguration: Generate a Very General Configuration

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

5.1 Function description

-
-
-
function [stewart] = generateGeneralConfiguration(stewart, args)
-% generateGeneralConfiguration - Generate a Very General Configuration
-%
-% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - H   [1x1] - Total height of the platform [m]
-%    - args - Can have the following fields:
-%        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
-%        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
-%        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
-%        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
-%        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
-%        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%        - Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
-%        - Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
-
-
-
-
- -
-

5.2 Documentation

-
-

-Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}. -The radius of the circles can be chosen as well as the angles where the joints are located. -

-
-
- -
-

5.3 Optional Parameters

-
-
-
arguments
-    stewart
-    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
-    args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
-    args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
-    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 70e-3;
-    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
-end
-
-
-
-
- -
-

5.4 Compute the pose

-
-
-
stewart.Fa = zeros(3,6);
-stewart.Mb = zeros(3,6);
-
-
- -
-
for i = 1:6
-  stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
-  stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
-end
-
-
-
-
-
- -
-

6 computeJointsPose: Compute the Pose of the Joints

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

6.1 Function description

-
-
-
function [stewart] = computeJointsPose(stewart)
-% computeJointsPose -
-%
-% Syntax: [stewart] = computeJointsPose(stewart)
-%
-% Inputs:
-%    - stewart - A structure with the following fields
-%        - Fa   [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
-%        - Mb   [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
-%        - FO_A [3x1] - Position of {A} with respect to {F}
-%        - MO_B [3x1] - Position of {B} with respect to {M}
-%        - FO_M [3x1] - Position of {M} with respect to {F}
-%
-% Outputs:
-%    - stewart - A structure with the following added fields
-%        - Aa  [3x6]   - The i'th column is the position of ai with respect to {A}
-%        - Ab  [3x6]   - The i'th column is the position of bi with respect to {A}
-%        - Ba  [3x6]   - The i'th column is the position of ai with respect to {B}
-%        - Bb  [3x6]   - The i'th column is the position of bi with respect to {B}
-%        - l   [6x1]   - The i'th element is the initial length of strut i
-%        - As  [3x6]   - The i'th column is the unit vector of strut i expressed in {A}
-%        - Bs  [3x6]   - The i'th column is the unit vector of strut i expressed in {B}
-%        - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
-%        - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
-
-
-
-
- -
-

6.2 Documentation

-
- -
-

stewart-struts.png -

-

Figure 3: Position and orientation of the struts

-
-
-
- -
-

6.3 Compute the position of the Joints

-
-
-
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
-stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
-
-stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
-stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
-
-
-
-
- -
-

6.4 Compute the strut length and orientation

-
-
-
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
-
-stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
-
-
- -
-
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
-
-
-
-
- -
-

6.5 Compute the orientation of the Joints

-
-
-
stewart.FRa = zeros(3,3,6);
-stewart.MRb = zeros(3,3,6);
-
-for i = 1:6
-  stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
-  stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
-
-  stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
-  stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
-end
-
-
-
-
-
- -
-

7 initializeStrutDynamics: Add Stiffness and Damping properties of each strut

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

7.1 Function description

-
-
-
function [stewart] = initializeStrutDynamics(stewart, args)
-% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
-%
-% Syntax: [stewart] = initializeStrutDynamics(args)
-%
-% Inputs:
-%    - args - Structure with the following fields:
-%        - Ki [6x1] - Stiffness of each strut [N/m]
-%        - Ci [6x1] - Damping of each strut [N/(m/s)]
-%
-% Outputs:
-%    - stewart - updated Stewart structure with the added fields:
-%        - Ki [6x1] - Stiffness of each strut [N/m]
-%        - Ci [6x1] - Damping of each strut [N/(m/s)]
-
-
-
-
- -
-

7.2 Optional Parameters

-
-
-
arguments
-    stewart
-    args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
-    args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
-end
-
-
-
-
- -
-

7.3 Add Stiffness and Damping properties of each strut

-
-
-
stewart.Ki = args.Ki;
-stewart.Ci = args.Ci;
-
-
-
-
-
- -
-

8 computeJacobian: Compute the Jacobian Matrix

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

8.1 Function description

-
-
-
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
-
-
-
-
- -
-

8.2 Compute Jacobian Matrix

-
-
-
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
-
-
-
-
- -
-

8.3 Compute Stiffness Matrix

-
-
-
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
-
-
-
-
- -
-

8.4 Compute Compliance Matrix

-
-
-
stewart.C = inv(stewart.K);
-
-
-
-
-
- -
-

9 inverseKinematics: Compute Inverse Kinematics

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

9.1 Function description

-
-
-
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}
-
-
-
-
- -
-

9.2 Optional Parameters

-
-
-
arguments
-    stewart
-    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
-    args.ARB (3,3) double {mustBeNumeric} = eye(3)
-end
-
-
-
-
- -
-

9.3 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. -

-
-
- -
-

9.4 Compute

-
-
-
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));
-
-
- -
-
dLi = Li-stewart.l;
-
-
-
-
-
- -
-

10 forwardKinematicsApprox: Compute the Forward Kinematics

-
-

- -

- -

-This Matlab function is accessible here. -

-
- -
-

10.1 Function description

-
-
-
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}
-
-
-
-
- -
-

10.2 Optional Parameters

-
-
-
arguments
-    stewart
-    args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
-end
-
-
-
-
- -
-

10.3 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}} \] -

-
-
X = stewart.J\args.dL;
-
-
- -

-The position vector corresponds to the first 3 elements. -

-
-
P = X(1:3);
-
-
- -

-The next 3 elements are the orientation of {B} with respect to {A} expressed -using the screw axis. -

-
-
theta = norm(X(4:6));
-s = X(4:6)/theta;
-
-
- -

-We then compute the corresponding rotation matrix. -

-
-
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)];
-
-
-
-
-

Author: Dehaeze Thomas

-

Created: 2020-01-22 mer. 11:35

+

Created: 2020-01-27 lun. 17:41

diff --git a/simscape-model.org b/simscape-model.org index 30675e7..a1f2556 100644 --- a/simscape-model.org +++ b/simscape-model.org @@ -20,1596 +20,3 @@ #+PROPERTY: header-args:matlab+ :output-dir figs :END: -* Introduction :ignore: -Stewart platforms are generated in multiple steps. - -We define 4 important *frames*: -- $\{F\}$: Frame fixed to the *Fixed* base and located at the center of its bottom surface. - This is used to fix the Stewart platform to some support. -- $\{M\}$: Frame fixed to the *Moving* platform and located at the center of its top surface. - This is used to place things on top of the Stewart platform. -- $\{A\}$: Frame fixed to the fixed base. - It defined the center of rotation of the moving platform. -- $\{B\}$: Frame fixed to the moving platform. - The motion of the moving platforms and forces applied to it are defined with respect to this frame $\{B\}$. - -Then, we define the *location of the spherical joints*: -- $\bm{a}_{i}$ are the position of the spherical joints fixed to the fixed base -- $\bm{b}_{i}$ are the position of the spherical joints fixed to the moving platform - -We define the *rest position* of the Stewart platform: -- For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position. -- Thus, to define the rest position of the Stewart platform, we just have to defined its total height $H$. - $H$ corresponds to the distance from the bottom of the fixed base to the top of the moving platform. - -From $\bm{a}_{i}$ and $\bm{b}_{i}$, we can determine the *length and orientation of each strut*: -- $l_{i}$ is the length of the strut -- ${}^{A}\hat{\bm{s}}_{i}$ is the unit vector align with the strut - -The position of the Spherical joints can be computed using various methods: -- Cubic configuration -- Circular configuration -- Arbitrary position -- These methods should be easily scriptable and corresponds to specific functions that returns ${}^{F}\bm{a}_{i}$ and ${}^{M}\bm{b}_{i}$. - The input of these functions are the parameters corresponding to the wanted geometry. - -For Simscape, we need: -- The position and orientation of each spherical joint fixed to the fixed base: ${}^{F}\bm{a}_{i}$ and ${}^{F}\bm{R}_{a_{i}}$ -- The position and orientation of each spherical joint fixed to the moving platform: ${}^{M}\bm{b}_{i}$ and ${}^{M}\bm{R}_{b_{i}}$ -- The rest length of each strut: $l_{i}$ -- The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$ -- The position of the frame $\{A\}$ with respect to the frame $\{F\}$: ${}^{F}\bm{O}_{A}$ -- The position of the frame $\{B\}$ with respect to the frame $\{M\}$: ${}^{M}\bm{O}_{B}$ - - -* Procedure -The procedure to define the Stewart platform is the following: -1. Define the initial position of frames {A}, {B}, {F} and {M}. - We do that using the =initializeFramesPositions= function. - We have to specify the total height of the Stewart platform $H$ and the position ${}^{M}O_{B}$ of {B} with respect to {M}. -2. Compute the positions of joints ${}^{F}a_{i}$ and ${}^{M}b_{i}$. - We can do that using various methods depending on the wanted architecture: - - =generateCubicConfiguration= permits to generate a cubic configuration -3. Compute the position and orientation of the joints with respect to the fixed base and the moving platform. - This is done with the =computeJointsPose= function. -4. Define the dynamical properties of the Stewart platform. - The output are the stiffness and damping of each strut $k_{i}$ and $c_{i}$. - This can be done we simply choosing directly the stiffness and damping of each strut. - The stiffness and damping of each actuator can also be determine from the wanted stiffness of the Stewart platform for instance. -5. Define the mass and inertia of each element of the Stewart platform. - -By following this procedure, we obtain a Matlab structure =stewart= that contains all the information for the Simscape model and for further analysis. - -* Matlab Code -** Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab - simulinkproject('./'); -#+end_src - -** Simscape Model -#+begin_src matlab - open('stewart_platform.slx') -#+end_src - -** Test the functions -#+begin_src matlab - stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); - % stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3); - stewart = generateGeneralConfiguration(stewart); - stewart = computeJointsPose(stewart); - stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); - stewart = initializeCylindricalPlatforms(stewart); - stewart = initializeCylindricalStruts(stewart); - stewart = computeJacobian(stewart); - stewart = initializeStewartPose(stewart, 'AP', [0;0;0.01], 'ARB', eye(3)); - - [Li, dLi] = inverseKinematics(stewart, 'AP', [0;0;0.00001], 'ARB', eye(3)); - [P, R] = forwardKinematicsApprox(stewart, 'dL', dLi); -#+end_src - -* =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M} -:PROPERTIES: -:header-args:matlab+: :tangle src/initializeFramesPositions.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/initializeFramesPositions.m][here]]. - -** Function description -#+begin_src matlab - function [stewart] = initializeFramesPositions(args) - % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} - % - % Syntax: [stewart] = initializeFramesPositions(args) - % - % Inputs: - % - args - Can have the following fields: - % - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m] - % - MO_B [1x1] - Height of the frame {B} with respect to {M} [m] - % - % Outputs: - % - stewart - A structure with the following fields: - % - H [1x1] - Total Height of the Stewart Platform [m] - % - FO_M [3x1] - Position of {M} with respect to {F} [m] - % - MO_B [3x1] - Position of {B} with respect to {M} [m] - % - FO_A [3x1] - Position of {A} with respect to {F} [m] -#+end_src - -** Documentation - -#+name: fig:stewart-frames-position -#+caption: Definition of the position of the frames -[[file:figs/stewart-frames-position.png]] - -** Optional Parameters -#+begin_src matlab - arguments - args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 - args.MO_B (1,1) double {mustBeNumeric} = 50e-3 - end -#+end_src - -** Initialize the Stewart structure -#+begin_src matlab - stewart = struct(); -#+end_src - -** Compute the position of each frame -#+begin_src matlab - stewart.H = args.H; % Total Height of the Stewart Platform [m] - - stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m] - - stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] - - stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m] -#+end_src - -* Initialize the position of the Joints -** =generateCubicConfiguration=: Generate a Cubic Configuration -:PROPERTIES: -:header-args:matlab+: :tangle src/generateCubicConfiguration.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/generateCubicConfiguration.m][here]]. - -*** Function description -#+begin_src matlab - function [stewart] = generateCubicConfiguration(stewart, args) - % generateCubicConfiguration - Generate a Cubic Configuration - % - % Syntax: [stewart] = generateCubicConfiguration(stewart, args) - % - % Inputs: - % - stewart - A structure with the following fields - % - H [1x1] - Total height of the platform [m] - % - args - Can have the following fields: - % - Hc [1x1] - Height of the "useful" part of the cube [m] - % - FOc [1x1] - Height of the center of the cube with respect to {F} [m] - % - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m] - % - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m] - % - % Outputs: - % - stewart - updated Stewart structure with the added fields: - % - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} - % - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} -#+end_src - -*** Documentation -#+name: fig:cubic-configuration-definition -#+caption: Cubic Configuration -[[file:figs/cubic-configuration-definition.png]] - -*** Optional Parameters -#+begin_src matlab - arguments - stewart - args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 - args.FOc (1,1) double {mustBeNumeric} = 50e-3 - args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 - args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 - end -#+end_src - -*** Position of the Cube -We define the useful points of the cube with respect to the Cube's center. -${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is -located at the center of the cube and aligned with {F} and {M}. - -#+begin_src matlab - sx = [ 2; -1; -1]; - sy = [ 0; 1; -1]; - sz = [ 1; 1; 1]; - - R = [sx, sy, sz]./vecnorm([sx, sy, sz]); - - L = args.Hc*sqrt(3); - - Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc]; - - CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg - CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg -#+end_src - -*** Compute the pose -We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). -#+begin_src matlab - CSi = (CCm - CCf)./vecnorm(CCm - CCf); -#+end_src - -We now which to compute the position of the joints $a_{i}$ and $b_{i}$. -#+begin_src matlab - stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; - stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; -#+end_src - -** =generateGeneralConfiguration=: Generate a Very General Configuration -:PROPERTIES: -:header-args:matlab+: :tangle src/generateGeneralConfiguration.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/generateGeneralConfiguration.m][here]]. - -*** Function description -#+begin_src matlab - function [stewart] = generateGeneralConfiguration(stewart, args) - % generateGeneralConfiguration - Generate a Very General Configuration - % - % Syntax: [stewart] = generateGeneralConfiguration(stewart, args) - % - % Inputs: - % - args - Can have the following fields: - % - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] - % - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] - % - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad] - % - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m] - % - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m] - % - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad] - % - % Outputs: - % - stewart - updated Stewart structure with the added fields: - % - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} - % - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} -#+end_src - -*** Documentation -Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}. -The radius of the circles can be chosen as well as the angles where the joints are located. - -*** Optional Parameters -#+begin_src matlab - arguments - stewart - args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 - args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; - args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); - args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 - args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e-3; - args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); - end -#+end_src - -*** Compute the pose -#+begin_src matlab - stewart.Fa = zeros(3,6); - stewart.Mb = zeros(3,6); -#+end_src - -#+begin_src matlab - for i = 1:6 - stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; - stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; - end -#+end_src - -* =computeJointsPose=: Compute the Pose of the Joints -:PROPERTIES: -:header-args:matlab+: :tangle src/computeJointsPose.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/computeJointsPose.m][here]]. - -** Function description -#+begin_src matlab - function [stewart] = computeJointsPose(stewart) - % computeJointsPose - - % - % Syntax: [stewart] = computeJointsPose(stewart) - % - % Inputs: - % - stewart - A structure with the following fields - % - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} - % - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} - % - FO_A [3x1] - Position of {A} with respect to {F} - % - MO_B [3x1] - Position of {B} with respect to {M} - % - FO_M [3x1] - Position of {M} with respect to {F} - % - % Outputs: - % - stewart - A structure with the following added fields - % - Aa [3x6] - The i'th column is the position of ai with respect to {A} - % - Ab [3x6] - The i'th column is the position of bi with respect to {A} - % - Ba [3x6] - The i'th column is the position of ai with respect to {B} - % - Bb [3x6] - The i'th column is the position of bi with respect to {B} - % - l [6x1] - The i'th element is the initial length of strut i - % - As [3x6] - The i'th column is the unit vector of strut i expressed in {A} - % - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} - % - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} - % - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} -#+end_src - -** Documentation - -#+name: fig:stewart-struts -#+caption: Position and orientation of the struts -[[file:figs/stewart-struts.png]] - -** Compute the position of the Joints -#+begin_src matlab - stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]); - stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]); - - stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]); - stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]); -#+end_src - -** Compute the strut length and orientation -#+begin_src matlab - stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As - - stewart.l = vecnorm(stewart.Ab - stewart.Aa)'; -#+end_src - -#+begin_src matlab - stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba); -#+end_src - -** Compute the orientation of the Joints -#+begin_src matlab - stewart.FRa = zeros(3,3,6); - stewart.MRb = zeros(3,3,6); - - for i = 1:6 - stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)]; - stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i)); - - stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)]; - stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i)); - end -#+end_src - -* =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut -:PROPERTIES: -:header-args:matlab+: :tangle src/initializeStrutDynamics.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/initializeStrutDynamics.m][here]]. - -** Function description -#+begin_src matlab - function [stewart] = initializeStrutDynamics(stewart, args) - % initializeStrutDynamics - Add Stiffness and Damping properties of each strut - % - % Syntax: [stewart] = initializeStrutDynamics(args) - % - % Inputs: - % - args - Structure with the following fields: - % - Ki [6x1] - Stiffness of each strut [N/m] - % - Ci [6x1] - Damping of each strut [N/(m/s)] - % - % Outputs: - % - stewart - updated Stewart structure with the added fields: - % - Ki [6x1] - Stiffness of each strut [N/m] - % - Ci [6x1] - Damping of each strut [N/(m/s)] -#+end_src - -** Optional Parameters -#+begin_src matlab - arguments - stewart - args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1) - args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1) - end -#+end_src - -** Add Stiffness and Damping properties of each strut -#+begin_src matlab - stewart.Ki = args.Ki; - stewart.Ci = args.Ci; -#+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: -<> - -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 - -* Initialize the Geometry of the Mechanical Elements -** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms -:PROPERTIES: -:header-args:matlab+: :tangle src/initializeCylindricalPlatforms.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/initializeCylindricalPlatforms.m][here]]. - -*** Function description -#+begin_src matlab - function [stewart] = initializeCylindricalPlatforms(stewart, args) - % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms - % - % Syntax: [stewart] = initializeCylindricalPlatforms(args) - % - % Inputs: - % - args - Structure with the following fields: - % - Fpm [1x1] - Fixed Platform Mass [kg] - % - Fph [1x1] - Fixed Platform Height [m] - % - Fpr [1x1] - Fixed Platform Radius [m] - % - Mpm [1x1] - Mobile Platform Mass [kg] - % - Mph [1x1] - Mobile Platform Height [m] - % - Mpr [1x1] - Mobile Platform Radius [m] - % - % Outputs: - % - stewart - updated Stewart structure with the added fields: - % - platforms [struct] - structure with the following fields: - % - Fpm [1x1] - Fixed Platform Mass [kg] - % - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2] - % - Fph [1x1] - Fixed Platform Height [m] - % - Fpr [1x1] - Fixed Platform Radius [m] - % - Mpm [1x1] - Mobile Platform Mass [kg] - % - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2] - % - Mph [1x1] - Mobile Platform Height [m] - % - Mpr [1x1] - Mobile Platform Radius [m] -#+end_src - -*** Optional Parameters -#+begin_src matlab - arguments - stewart - args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 - args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 - args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3 - args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 - args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 - args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 - end -#+end_src - -*** Create the =platforms= struct -#+begin_src matlab - platforms = struct(); - - platforms.Fpm = args.Fpm; - platforms.Fph = args.Fph; - platforms.Fpr = args.Fpr; - platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... - 1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... - 1/2 * platforms.Fpm * platforms.Fpr^2]); - - platforms.Mpm = args.Mpm; - platforms.Mph = args.Mph; - platforms.Mpr = args.Mpr; - platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... - 1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... - 1/2 * platforms.Mpm * platforms.Mpr^2]); -#+end_src - -*** Save the =platforms= struct -#+begin_src matlab - stewart.platforms = platforms; -#+end_src - -** =initializeCylindricalStruts=: Define the mass and moment of inertia of cylindrical struts -:PROPERTIES: -:header-args:matlab+: :tangle src/initializeCylindricalStruts.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/initializeCylindricalStruts.m][here]]. - -*** Function description -#+begin_src matlab - function [stewart] = initializeCylindricalStruts(stewart, args) - % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts - % - % Syntax: [stewart] = initializeCylindricalStruts(args) - % - % Inputs: - % - args - Structure with the following fields: - % - Fsm [1x1] - Mass of the Fixed part of the struts [kg] - % - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m] - % - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m] - % - Msm [1x1] - Mass of the Mobile part of the struts [kg] - % - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m] - % - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m] - % - % Outputs: - % - stewart - updated Stewart structure with the added fields: - % - struts [struct] - structure with the following fields: - % - Fsm [6x1] - Mass of the Fixed part of the struts [kg] - % - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] - % - Msm [6x1] - Mass of the Mobile part of the struts [kg] - % - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] - % - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m] - % - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m] - % - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m] - % - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m] -#+end_src - -*** Optional Parameters -#+begin_src matlab - arguments - stewart - args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 - args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 - args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 - args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 - args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 - args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 - end -#+end_src - -*** Create the =struts= structure -#+begin_src matlab - struts = struct(); - - struts.Fsm = ones(6,1).*args.Fsm; - struts.Msm = ones(6,1).*args.Msm; - - struts.Fsh = ones(6,1).*args.Fsh; - struts.Fsr = ones(6,1).*args.Fsr; - struts.Msh = ones(6,1).*args.Msh; - struts.Msr = ones(6,1).*args.Msr; - - struts.Fsi = zeros(3, 3, 6); - struts.Msi = zeros(3, 3, 6); - for i = 1:6 - struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... - 1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... - 1/2 * struts.Fsm(i) * struts.Fsr(i)^2]); - struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... - 1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... - 1/2 * struts.Msm(i) * struts.Msr(i)^2]); - end -#+end_src - -#+begin_src matlab - stewart.struts = struts; -#+end_src - -* =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose -:PROPERTIES: -:header-args:matlab+: :tangle src/initializeStewartPose.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/initializeStewartPose.m][here]]. - -** Function description -#+begin_src matlab - function [stewart] = initializeStewartPose(stewart, args) - % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose - % It uses the inverse kinematic - % - % Syntax: [stewart] = initializeStewartPose(stewart, args) - % - % 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: - % - stewart - updated Stewart structure with the added fields: - % - 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 - -** Use the Inverse Kinematic function -#+begin_src matlab - [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); - - stewart.dLi = dLi; -#+end_src - -* Utility Functions -** =inverseKinematics=: Compute Inverse Kinematics -:PROPERTIES: -:header-args:matlab+: :tangle src/inverseKinematics.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -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 Forward Kinematics -:PROPERTIES: -:header-args:matlab+: :tangle src/forwardKinematicsApprox.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -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 - -* Other Elements -** Z-Axis Geophone -:PROPERTIES: -:header-args:matlab+: :tangle ./src/initializeZAxisGeophone.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:../src/initializeZAxisGeophone.m][here]]. - -#+begin_src matlab - function [geophone] = initializeZAxisGeophone(args) - arguments - args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] - args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz] - end - - %% - geophone.m = args.mass; - - %% The Stiffness is set to have the damping resonance frequency - geophone.k = geophone.m * (2*pi*args.freq)^2; - - %% We set the damping value to have critical damping - geophone.c = 2*sqrt(geophone.m * geophone.k); - - %% Save - save('./mat/geophone_z_axis.mat', 'geophone'); - end -#+end_src - -** Z-Axis Accelerometer -:PROPERTIES: -:header-args:matlab+: :tangle ./src/initializeZAxisAccelerometer.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:../src/initializeZAxisAccelerometer.m][here]]. - -#+begin_src matlab - function [accelerometer] = initializeZAxisAccelerometer(args) - arguments - args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] - args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz] - end - - %% - accelerometer.m = args.mass; - - %% The Stiffness is set to have the damping resonance frequency - accelerometer.k = accelerometer.m * (2*pi*args.freq)^2; - - %% We set the damping value to have critical damping - accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k); - - %% Gain correction of the accelerometer to have a unity gain until the resonance - accelerometer.gain = -accelerometer.k/accelerometer.m; - - %% Save - save('./mat/accelerometer_z_axis.mat', 'accelerometer'); - end -#+end_src - -* OLD :noexport: -** Define the Height of the Platform :noexport: -#+begin_src matlab - %% 1. Height of the platform. Location of {F} and {M} - H = 90e-3; % [m] - FO_M = [0; 0; H]; -#+end_src - -** Define the location of {A} and {B} :noexport: -#+begin_src matlab - %% 2. Location of {A} and {B} - FO_A = [0; 0; 100e-3] + FO_M;% [m,m,m] - MO_B = [0; 0; 100e-3];% [m,m,m] -#+end_src - -** Define the position of $a_{i}$ and $b_{i}$ :noexport: -#+begin_src matlab - %% 3. Position of ai and bi - Fa = zeros(3, 6); % Fa_i is the i'th vector of Fa - Mb = zeros(3, 6); % Mb_i is the i'th vector of Mb -#+end_src - -#+begin_src matlab - Aa = Fa - repmat(FO_A, [1, 6]); - Bb = Mb - repmat(MO_B, [1, 6]); - - Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); - Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]); - - As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As - l = vecnorm(Ab - Aa); - - Bs = (Bb - Ba)./vecnorm(Bb - Ba); - - FRa = zeros(3,3,6); - MRb = zeros(3,3,6); - - for i = 1:6 - FRa(:,:,i) = [cross([0;1;0],As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)]; - FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i)); - - MRb(:,:,i) = [cross([0;1;0],Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)]; - MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); - end -#+end_src - -** Define the dynamical properties of each strut :noexport: -#+begin_src matlab - %% 4. Stiffness and Damping of each strut - Ki = 1e6*ones(6,1); - Ci = 1e2*ones(6,1); -#+end_src - -** Old Introduction :noexport: -First, geometrical parameters are defined: -- ${}^A\bm{a}_i$ - Position of the joints fixed to the fixed base w.r.t $\{A\}$ -- ${}^A\bm{b}_i$ - Position of the joints fixed to the mobile platform w.r.t $\{A\}$ -- ${}^B\bm{b}_i$ - Position of the joints fixed to the mobile platform w.r.t $\{B\}$ -- $H$ - Total height of the mobile platform - -These parameter are enough to determine all the kinematic properties of the platform like the Jacobian, stroke, stiffness, ... -These geometrical parameters can be generated using different functions: =initializeCubicConfiguration= for cubic configuration or =initializeGeneralConfiguration= for more general configuration. - -A function =computeGeometricalProperties= is then used to compute: -- $\bm{J}_f$ - Jacobian matrix for the force location -- $\bm{J}_d$ - Jacobian matrix for displacement estimation -- $\bm{R}_m$ - Rotation matrices to position the leg vectors - -Then, geometrical parameters are computed for all the mechanical elements with the function =initializeMechanicalElements=: -- Shape of the platforms - - External Radius - - Internal Radius - - Density - - Thickness -- Shape of the Legs - - Radius - - Size of ball joint - - Density - -Other Parameters are defined for the Simscape simulation: -- Sample mass, volume and position (=initializeSample= function) -- Location of the inertial sensor -- Location of the point for the differential measurements -- Location of the Jacobian point for velocity/displacement computation - -** Cubic Configuration :noexport: -To define the cubic configuration, we need to define 4 parameters: -- The size of the cube -- The location of the cube -- The position of the plane joint the points $a_{i}$ -- The position of the plane joint the points $b_{i}$ - -To do so, we specify the following parameters: -- $H_{C}$ the height of the useful part of the cube -- ${}^{F}O_{C}$ the position of the center of the cube with respect to $\{F\}$ -- ${}^{F}H_{A}$: the height of the plane joining the points $a_{i}$ with respect to the frame $\{F\}$ -- ${}^{M}H_{B}$: the height of the plane joining the points $b_{i}$ with respect to the frame $\{M\}$ - -We define the parameters -#+begin_src matlab - Hc = 60e-3; % [m] - FOc = 50e-3; % [m] - FHa = 15e-3; % [m] - MHb = 15e-3; % [m] -#+end_src - -We define the useful points of the cube with respect to the Cube's center. -${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}. -#+begin_src matlab - sx = [ 2; -1; -1]; - sy = [ 0; 1; -1]; - sz = [ 1; 1; 1]; - - R = [sx, sy, sz]./vecnorm([sx, sy, sz]); - - L = Hc*sqrt(3); - - Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*Hc]; - - CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg - CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg -#+end_src - -We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). -#+begin_src matlab - CSi = (CCm - CCf)./vecnorm(CCm - CCf); -#+end_src - -We now which to compute the position of the joints $a_{i}$ and $b_{i}$. -#+begin_src matlab - Fa = zeros(3, 6); % Fa_i is the i'th vector of Fa - Mb = zeros(3, 6); % Mb_i is the i'th vector of Mb -#+end_src - -#+begin_src matlab - Fa = CCf + [0; 0; FOc] + ((FHa-(FOc-Hc/2))./CSi(3,:)).*CSi; - Mb = CCf + [0; 0; FOc-H] + ((H-MHb-(FOc-Hc/2))./CSi(3,:)).*CSi; % TODO -#+end_src - -** initializeGeneralConfiguration :noexport: -:PROPERTIES: -:HEADER-ARGS:matlab+: :exports code -:HEADER-ARGS:matlab+: :comments no -:HEADER-ARGS:matlab+: :eval no -:HEADER-ARGS:matlab+: :tangle src/initializeGeneralConfiguration.m -:END: - -*** Function description -The =initializeGeneralConfiguration= function takes one structure that contains configurations for the hexapod and returns one structure representing the Hexapod. - -#+begin_src matlab - function [stewart] = initializeGeneralConfiguration(opts_param) -#+end_src - -*** Optional Parameters -Default values for opts. -#+begin_src matlab - opts = struct(... - 'H_tot', 90, ... % Height of the platform [mm] - 'H_joint', 15, ... % Height of the joints [mm] - 'H_plate', 10, ... % Thickness of the fixed and mobile platforms [mm] - 'R_bot', 100, ... % Radius where the legs articulations are positionned [mm] - 'R_top', 80, ... % Radius where the legs articulations are positionned [mm] - 'a_bot', 10, ... % Angle Offset [deg] - 'a_top', 40, ... % Angle Offset [deg] - 'da_top', 0 ... % Angle Offset from 0 position [deg] - ); -#+end_src - -Populate opts with input parameters -#+begin_src matlab - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end -#+end_src - -*** Geometry Description -#+name: fig:stewart_bottom_plate -#+caption: Schematic of the bottom plates with all the parameters -[[file:./figs/stewart_bottom_plate.png]] - -*** Compute Aa and Ab -We compute $[a_1, a_2, a_3, a_4, a_5, a_6]^T$ and $[b_1, b_2, b_3, b_4, b_5, b_6]^T$. - -#+begin_src matlab - Aa = zeros(6, 3); % [mm] - Ab = zeros(6, 3); % [mm] - Bb = zeros(6, 3); % [mm] -#+end_src - -#+begin_src matlab - for i = 1:3 - Aa(2*i-1,:) = [opts.R_bot*cos( pi/180*(120*(i-1) - opts.a_bot) ), ... - opts.R_bot*sin( pi/180*(120*(i-1) - opts.a_bot) ), ... - opts.H_plate+opts.H_joint]; - Aa(2*i,:) = [opts.R_bot*cos( pi/180*(120*(i-1) + opts.a_bot) ), ... - opts.R_bot*sin( pi/180*(120*(i-1) + opts.a_bot) ), ... - opts.H_plate+opts.H_joint]; - - Ab(2*i-1,:) = [opts.R_top*cos( pi/180*(120*(i-1) + opts.da_top - opts.a_top) ), ... - opts.R_top*sin( pi/180*(120*(i-1) + opts.da_top - opts.a_top) ), ... - opts.H_tot - opts.H_plate - opts.H_joint]; - Ab(2*i,:) = [opts.R_top*cos( pi/180*(120*(i-1) + opts.da_top + opts.a_top) ), ... - opts.R_top*sin( pi/180*(120*(i-1) + opts.da_top + opts.a_top) ), ... - opts.H_tot - opts.H_plate - opts.H_joint]; - end - - Bb = Ab - opts.H_tot*[0,0,1]; -#+end_src - -*** Returns Stewart Structure -#+begin_src matlab :results none - stewart = struct(); - stewart.Aa = Aa; - stewart.Ab = Ab; - stewart.Bb = Bb; - stewart.H_tot = opts.H_tot; -end -#+end_src - -** initializeCubicConfiguration :noexport: -:PROPERTIES: -:HEADER-ARGS:matlab+: :exports code -:HEADER-ARGS:matlab+: :comments no -:HEADER-ARGS:matlab+: :eval no -:HEADER-ARGS:matlab+: :tangle src/initializeCubicConfiguration.m -:END: -<> - -*** Function description -#+begin_src matlab - function [stewart] = initializeCubicConfiguration(opts_param) -#+end_src - -*** Optional Parameters -Default values for opts. -#+begin_src matlab - opts = struct(... - 'H_tot', 90, ... % Total height of the Hexapod [mm] - 'L', 110, ... % Size of the Cube [mm] - 'H', 40, ... % Height between base joints and platform joints [mm] - 'H0', 75 ... % Height between the corner of the cube and the plane containing the base joints [mm] - ); -#+end_src - -Populate opts with input parameters -#+begin_src matlab - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end -#+end_src - -*** Cube Creation -#+begin_src matlab :results none - points = [0, 0, 0; ... - 0, 0, 1; ... - 0, 1, 0; ... - 0, 1, 1; ... - 1, 0, 0; ... - 1, 0, 1; ... - 1, 1, 0; ... - 1, 1, 1]; - points = opts.L*points; -#+end_src - -We create the rotation matrix to rotate the cube -#+begin_src matlab :results none - sx = cross([1, 1, 1], [1 0 0]); - sx = sx/norm(sx); - - sy = -cross(sx, [1, 1, 1]); - sy = sy/norm(sy); - - sz = [1, 1, 1]; - sz = sz/norm(sz); - - R = [sx', sy', sz']'; -#+end_src - -We use to rotation matrix to rotate the cube -#+begin_src matlab :results none - cube = zeros(size(points)); - for i = 1:size(points, 1) - cube(i, :) = R * points(i, :)'; - end -#+end_src - -*** Vectors of each leg -#+begin_src matlab :results none - leg_indices = [3, 4; ... - 2, 4; ... - 2, 6; ... - 5, 6; ... - 5, 7; ... - 3, 7]; -#+end_src - -Vectors are: -#+begin_src matlab :results none - legs = zeros(6, 3); - legs_start = zeros(6, 3); - - for i = 1:6 - legs(i, :) = cube(leg_indices(i, 2), :) - cube(leg_indices(i, 1), :); - legs_start(i, :) = cube(leg_indices(i, 1), :); - end -#+end_src - -*** Verification of Height of the Stewart Platform -If the Stewart platform is not contained in the cube, throw an error. - -#+begin_src matlab :results none - Hmax = cube(4, 3) - cube(2, 3); - if opts.H0 < cube(2, 3) - error(sprintf('H0 is not high enought. Minimum H0 = %.1f', cube(2, 3))); - else if opts.H0 + opts.H > cube(4, 3) - error(sprintf('H0+H is too high. Maximum H0+H = %.1f', cube(4, 3))); - error('H0+H is too high'); - end -#+end_src - -*** Determinate the location of the joints -We now determine the location of the joints on the fixed platform w.r.t the fixed frame $\{A\}$. -$\{A\}$ is fixed to the bottom of the base. -#+begin_src matlab :results none - Aa = zeros(6, 3); - for i = 1:6 - t = (opts.H0-legs_start(i, 3))/(legs(i, 3)); - Aa(i, :) = legs_start(i, :) + t*legs(i, :); - end -#+end_src - -And the location of the joints on the mobile platform with respect to $\{A\}$. -#+begin_src matlab :results none - Ab = zeros(6, 3); - for i = 1:6 - t = (opts.H0+opts.H-legs_start(i, 3))/(legs(i, 3)); - Ab(i, :) = legs_start(i, :) + t*legs(i, :); - end -#+end_src - -And the location of the joints on the mobile platform with respect to $\{B\}$. -#+begin_src matlab :results none - Bb = zeros(6, 3); - Bb = Ab - (opts.H0 + opts.H_tot/2 + opts.H/2)*[0, 0, 1]; -#+end_src - -#+begin_src matlab :results none - h = opts.H0 + opts.H/2 - opts.H_tot/2; - Aa = Aa - h*[0, 0, 1]; - Ab = Ab - h*[0, 0, 1]; -#+end_src - -*** Returns Stewart Structure -#+begin_src matlab :results none - stewart = struct(); - stewart.Aa = Aa; - stewart.Ab = Ab; - stewart.Bb = Bb; - stewart.H_tot = opts.H_tot; -end -#+end_src - -** computeGeometricalProperties :noexport: -:PROPERTIES: -:HEADER-ARGS:matlab+: :exports code -:HEADER-ARGS:matlab+: :comments no -:HEADER-ARGS:matlab+: :eval no -:HEADER-ARGS:matlab+: :tangle src/computeGeometricalProperties.m -:END: - -*** Function description -#+begin_src matlab - function [stewart] = computeGeometricalProperties(stewart, opts_param) -#+end_src - -*** Optional Parameters -Default values for opts. -#+begin_src matlab - opts = struct(... - 'Jd_pos', [0, 0, 30], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm] - 'Jf_pos', [0, 0, 30] ... % Position of the Jacobian for force location from the top of the mobile platform [mm] - ); -#+end_src - -Populate opts with input parameters -#+begin_src matlab - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end -#+end_src - -*** Rotation matrices -We initialize $l_i$ and $\hat{s}_i$ -#+begin_src matlab - leg_length = zeros(6, 1); % [mm] - leg_vectors = zeros(6, 3); -#+end_src - -We compute $b_i - a_i$, and then: -\begin{align*} - l_i &= \left|b_i - a_i\right| \\ - \hat{s}_i &= \frac{b_i - a_i}{l_i} -\end{align*} - -#+begin_src matlab - legs = stewart.Ab - stewart.Aa; - - for i = 1:6 - leg_length(i) = norm(legs(i,:)); - leg_vectors(i,:) = legs(i,:) / leg_length(i); - end -#+end_src - -We compute rotation matrices to have the orientation of the legs. -The rotation matrix transforms the $z$ axis to the axis of the leg. The other axis are not important here. -#+begin_src matlab - stewart.Rm = struct('R', eye(3)); - - for i = 1:6 - sx = cross(leg_vectors(i,:), [1 0 0]); - sx = sx/norm(sx); - - sy = -cross(sx, leg_vectors(i,:)); - sy = sy/norm(sy); - - sz = leg_vectors(i,:); - sz = sz/norm(sz); - - stewart.Rm(i).R = [sx', sy', sz']; - end -#+end_src - -*** Jacobian matrices -Compute Jacobian Matrix -#+begin_src matlab - Jd = zeros(6); - - for i = 1:6 - Jd(i, 1:3) = leg_vectors(i, :); - Jd(i, 4:6) = cross(0.001*(stewart.Bb(i, :) - opts.Jd_pos), leg_vectors(i, :)); - end - - stewart.Jd = Jd; - stewart.Jd_inv = inv(Jd); -#+end_src - -#+begin_src matlab - Jf = zeros(6); - - for i = 1:6 - Jf(i, 1:3) = leg_vectors(i, :); - Jf(i, 4:6) = cross(0.001*(stewart.Bb(i, :) - opts.Jf_pos), leg_vectors(i, :)); - end - - stewart.Jf = Jf; - stewart.Jf_inv = inv(Jf); -#+end_src - -#+begin_src matlab - end -#+end_src - -** initializeMechanicalElements :noexport: -:PROPERTIES: -:HEADER-ARGS:matlab+: :exports code -:HEADER-ARGS:matlab+: :comments no -:HEADER-ARGS:matlab+: :eval no -:HEADER-ARGS:matlab+: :tangle src/initializeMechanicalElements.m -:END: - -*** Function description -#+begin_src matlab - function [stewart] = initializeMechanicalElements(stewart, opts_param) -#+end_src - -*** Optional Parameters -Default values for opts. -#+begin_src matlab - opts = struct(... - 'thickness', 10, ... % Thickness of the base and platform [mm] - 'density', 1000, ... % Density of the material used for the hexapod [kg/m3] - 'k_ax', 1e8, ... % Stiffness of each actuator [N/m] - 'c_ax', 1000, ... % Damping of each actuator [N/(m/s)] - 'stroke', 50e-6 ... % Maximum stroke of each actuator [m] - ); -#+end_src - -Populate opts with input parameters -#+begin_src matlab - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end -#+end_src - -*** Bottom Plate -#+name: fig:stewart_bottom_plate -#+caption: Schematic of the bottom plates with all the parameters -[[file:./figs/stewart_bottom_plate.png]] - -The bottom plate structure is initialized. -#+begin_src matlab - BP = struct(); -#+end_src - -We defined its internal radius (if there is a hole in the bottom plate) and its outer radius. -#+begin_src matlab - BP.Rint = 0; % Internal Radius [mm] - BP.Rext = 150; % External Radius [mm] -#+end_src - -We define its thickness. -#+begin_src matlab - BP.H = opts.thickness; % Thickness of the Bottom Plate [mm] -#+end_src - -We defined the density of the material of the bottom plate. -#+begin_src matlab - BP.density = opts.density; % Density of the material [kg/m3] -#+end_src - -And its color. -#+begin_src matlab - BP.color = [0.7 0.7 0.7]; % Color [RGB] -#+end_src - -Then the profile of the bottom plate is computed and will be used by Simscape -#+begin_src matlab - BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm] -#+end_src - -The structure is added to the stewart structure -#+begin_src matlab - stewart.BP = BP; -#+end_src - -*** Top Plate -The top plate structure is initialized. -#+begin_src matlab - TP = struct(); -#+end_src - -We defined the internal and external radius of the top plate. -#+begin_src matlab - TP.Rint = 0; % [mm] - TP.Rext = 100; % [mm] -#+end_src - -The thickness of the top plate. -#+begin_src matlab - TP.H = 10; % [mm] -#+end_src - -The density of its material. -#+begin_src matlab - TP.density = opts.density; % Density of the material [kg/m3] -#+end_src - -Its color. -#+begin_src matlab - TP.color = [0.7 0.7 0.7]; % Color [RGB] -#+end_src - -Then the shape of the top plate is computed -#+begin_src matlab - TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H]; -#+end_src - -The structure is added to the stewart structure -#+begin_src matlab - stewart.TP = TP; -#+end_src - -*** Legs -#+name: fig:stewart_legs -#+caption: Schematic for the legs of the Stewart platform -[[file:./figs/stewart_legs.png]] - -The leg structure is initialized. -#+begin_src matlab - Leg = struct(); -#+end_src - -The maximum Stroke of each leg is defined. -#+begin_src matlab - Leg.stroke = opts.stroke; % [m] -#+end_src - -The stiffness and damping of each leg are defined -#+begin_src matlab - Leg.k_ax = opts.k_ax; % Stiffness of each leg [N/m] - Leg.c_ax = opts.c_ax; % Damping of each leg [N/(m/s)] -#+end_src - -The radius of the legs are defined -#+begin_src matlab - Leg.Rtop = 10; % Radius of the cylinder of the top part of the leg[mm] - Leg.Rbot = 12; % Radius of the cylinder of the bottom part of the leg [mm] -#+end_src - -The density of its material. -#+begin_src matlab - Leg.density = opts.density; % Density of the material used for the legs [kg/m3] -#+end_src - -Its color. -#+begin_src matlab - Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB] -#+end_src - -The radius of spheres representing the ball joints are defined. -#+begin_src matlab - Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm] -#+end_src - -We estimate the length of the legs. -#+begin_src matlab - legs = stewart.Ab - stewart.Aa; - Leg.lenght = norm(legs(1,:))/1.5; -#+end_src - -Then the shape of the bottom leg is estimated -#+begin_src matlab - Leg.shape.bot = ... - [0 0; ... - Leg.Rbot 0; ... - Leg.Rbot Leg.lenght; ... - Leg.Rtop Leg.lenght; ... - Leg.Rtop 0.2*Leg.lenght; ... - 0 0.2*Leg.lenght]; -#+end_src - -The structure is added to the stewart structure -#+begin_src matlab - stewart.Leg = Leg; -#+end_src - -*** Ball Joints -#+name: fig:stewart_ball_joints -#+caption: Schematic of the support for the ball joints -[[file:./figs/stewart_ball_joints.png]] - -=SP= is the structure representing the support for the ball joints at the extremity of each leg. - -The =SP= structure is initialized. -#+begin_src matlab - SP = struct(); -#+end_src - -We can define its rotational stiffness and damping. For now, we use perfect joints. -#+begin_src matlab - SP.k = 0; % [N*m/deg] - SP.c = 0; % [N*m/deg] -#+end_src - -Its height is defined -#+begin_src matlab - SP.H = stewart.Aa(1, 3) - BP.H; % [mm] -#+end_src - -Its radius is based on the radius on the sphere at the end of the legs. -#+begin_src matlab - SP.R = Leg.R; % [mm] -#+end_src - -#+begin_src matlab - SP.section = [0 SP.H-SP.R; - 0 0; - SP.R 0; - SP.R SP.H]; -#+end_src - -The density of its material is defined. -#+begin_src matlab - SP.density = opts.density; % [kg/m^3] -#+end_src - -Its color is defined. -#+begin_src matlab - SP.color = [0.7 0.7 0.7]; % [RGB] -#+end_src - -The structure is added to the Hexapod structure -#+begin_src matlab - stewart.SP = SP; -#+end_src - -** initializeSample :noexport: -:PROPERTIES: -:HEADER-ARGS:matlab+: :exports code -:HEADER-ARGS:matlab+: :comments no -:HEADER-ARGS:matlab+: :eval no -:HEADER-ARGS:matlab+: :tangle src/initializeSample.m -:END: - -*** Function description -#+begin_src matlab - function [] = initializeSample(opts_param) -#+end_src - -*** Optional Parameters -Default values for opts. -#+begin_src matlab - sample = struct( ... - 'radius', 100, ... % radius of the cylinder [mm] - 'height', 100, ... % height of the cylinder [mm] - 'mass', 10, ... % mass of the cylinder [kg] - 'measheight', 50, ... % measurement point z-offset [mm] - 'offset', [0, 0, 0], ... % offset position of the sample [mm] - 'color', [0.9 0.1 0.1] ... - ); -#+end_src - -Populate opts with input parameters -#+begin_src matlab - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - sample.(opt{1}) = opts_param.(opt{1}); - end - end -#+end_src - -*** Save the Sample structure -#+begin_src matlab - save('./mat/sample.mat', 'sample'); -#+end_src - -#+begin_src matlab - end -#+end_src diff --git a/simscape_subsystems/stewart_strut_no_sensors.slx b/simscape_subsystems/stewart_strut_no_sensors.slx index bddaa6fe3da18b18e2a7104ff16c005900443a52..2d9d91f9eab32fa0a60463d61a3590dee622658a 100644 GIT binary patch delta 9389 zcmZX41yEc~vo`MT76=;LgS!NRdvJGH+yk5d!JP$yy9Njr+!Nd(5Q1!Q5AJ@Kyx+b5 zeg8XEXM5+Fe!BZi*UZ$Zp6&_Q%n4WwHAQ#?0vH$=6qtm-S_}d>xP-uZ8n^^l)Pz8O zwcgLUvnJp+ct&7Eo`#$4GiVxGA+kLpiZXp4`Hn3; z5c>&D;hl?E@wKD8#N38CboCYY{~eaK#;?}1j99j%I&EV$+0fwHBBYDIHxoCE_@cw zikG2@I-lAh_T5KrZi|z1+Mcf8{9zEi%+Z6dxdDZw`3#yvRR*`q5Q2o|*}^>zo-nZJ zrLz!OI2Y=!rEH1goB5A9#pDWg6sjP>J|{yB8UC--MzKKso0NgKX^MW}9$W@s#HolANA~IlZKLA^9-aZN zPM*7df^L6s!l{DmO~2@mo4ZM+ozdV;M^AUMFfdb~jnnJH28pG$mSo$> zWKh!_RYFMkDcF2+*=oL(;VSS3NBNPY-;$k4kqW%qor6$!HouEiGi=^p z6iPTi#{GpYSv!(?Q^ugr*2untT~~+XKqI0nN!~jC{+`YGytO0)tS@P<2&b3Nxbd|v zL7ypf!?U;oDUZV+gSsQ?=IcP`CBLl&k@XMxGQOT~|tt=%4oBk{v0J-NNwXnv9bI^LE*$Lq5LT z&E=Ip6i;W&j-?Kj(FIJbyff^njIsH7SyGMNinPUaSv&SjN>c=WCz(WEu`bfvQKpwyMl(>OIz(w*lPH$o9nZnMjVyw}dHGtHr>PR+ zN>@v63j^t{o>E@fINC4PzWZp1Equx0c~GWTO}*%% zeydp#f;_#jNH?+@t$zSm;)(c?Psk~rVs36R1NDBJ-%Sn%VxK;z2 z(!RXcS%hSTq&OTUKB&oKh1PA0j(k^kM|tlI9SZ^|QwfNF6}lM!M8#Jh?D;Gbb~{yM zV$j2#jwfR+c77tR8iiBlIKUTB`K*GC6@o$&M&e}T+*V<;Yq{W?~?rjycW$2LdBvd{>@lEUT7GdC^1G1}cDXT12KK<%nbQ{Y7E}&rIDmmc zRg}B3i9(7F=?}Z=I_~2+*DhEjuO!OO628QkkZY})NFv&m7%lZGvxi0l@i29IWybc=*I>4$JmtT4{Z^EiFE6sdw&E#jRaa z`2`Z>3*X92NTsb#)c#al<7=bEu;|pmOJJ`_qalYE;lb0*&U^>Q>6MBEJ321z#ETJE z9-Rp~iiJjC{fA((YCs5;5d`ufNTjE|uFiwxB~O;A648UOn!mYn@GY_FgM=$A-mjb* zHz~iP=0K5y2qrTmI8UQpcM@aw;O#~y$WHv=nmheSoIY{<+~l`bxl*CUg=2djErC<0VyVsD7eN zFNm|v@Y^^^zcEVLY-8Oa5RmZRElrDZtrGY?c7%YQ7Emri7WGyI;uE)ZVR30ct+KtD z7$gKsdefYZzfK%UIjuT0hg|lUntjbPc5Sl|af3yNYkb9G>9Wf)H<-s3R1omBL^Q!c z@#UxqFq80ufo%tYW0LKjxV6!2A*?^H)Q09Z@lXtrg|aVwlSFlcwt>EVElLs9JNhGm zpl#La8ll~EuXm4EQFGmQ@BptT#lF(Ez3}L5;0h|rxZyUbA2&t(LCny*(~54s0?Edn zK$$a7Enbq%ALQg(Zbk2dgXMxY*LT0kw)!4+1KinRFED@7xWQQl$)<#LwK~3)TCpls z2PK`}ZavO1C3wvn9UI?iYJ7_XyX~XqsBpab9x)?`}5@AJ0C`Ib%cw z3w$_`+xR`kAf^u++fT6c^WE=nClkT*7|Hm5UZ7;s3_GN`FR8Zn6-GN0`v|NBEu??V zzw@Xj*FyDCzg%L{roT$Pw%*U7X4V2K(SM{kP#kPm1Z&^_s-@8hbPE-muXZvuH)CUHJT@f{rg#|&->^uh5N5t zrCza8ZPgWKKjJ^q+6$;N^^#W)T+quD65ZnvUMaq4M^2M?+Mn`xF`z1+w&FtLE58c} z_roMev?1DGGK{_UFbV+MrfrUfQ|cMfPJ9^NkrYH23^Jk_`8?OyjIV!Nn2M8m%_7m0 zm@1;61bQXZ$Oj8<)qOT5hw#+clg+EuYLCBmqZ^2a@SS+EsnVvw;mjmK;QZiz~G_+NGz2ZYEbi`jJQK($a!lGiu;RMSt-D zBKzm`-Z4FM^7mqV_JLZG2b&ru(hrJ0SeN(k@98(R1<&2~K9d9bvBXL zlDHqI0R`vMEh4n;@3Fjs`!Q%DV66gaA|3pszoLQkwXP;8;DetS=m+IFQ1sr>yhcPEiIu3nAXM zZ7nxx!y~F}s*8H9r7=?N0{DgAf3ZOKF$)9K;@xiRu(|Gfrd>5>FRkjZq1pC?Y|tYn zlgS4mx)YpNns>6mD^ja3mj+L>F|&7}F3vMM_1z$4fLbjV+xc8s9s6VLuja?xhIp@W z$&&87`t39di1h>V_Rk~s5}kJV+I%4{qk$?0JaK$c&?A*SGw!|vXzFH(s!5WaqoArS zFN}>pkeU{^w|5+#yb?nH^xFBxbakLV-NMNpm-US7eE>5v<5xoZRK_p3`f?OwWm98d zIUJebl#lQeOcZ3w(;IV*+xvM`mg;YVZXhz;s=}J{Z>eP;SVmLO9Wg$|lO(=ii%z>A z*h>6ab?|5_HzrPi2Kc}T!?fE8j1V>#iwV3Gv7S~sW6y|t!%2UJGJ!iU)SMZ+^ZRhX z8uM|0km)T2$dJRzc5^e1EFj3HzFXS0T9RK9m;!tuDI3Yvd|FZ){XH_e@;PQ~LZ|-x z#~p<%&L?;p3?bSg6ehSy{~G**Mk^fs^ouv7CWEnkY-34F1$7 z$L&dG`4yOC7m=^of2s|Iv^N%aH}T$6HY*5ALq2E4q?G*H+@zmweJnl**}r0OOC5Zb zs1@`Tf5V&%meXz|5BaSR_q|eZ(xUUpCkL>9fwKg5Dr;hK*9VCp2mMh{*>C}cKr+JHpM(?KVvs!3wav3h7 zP>_nk2?QZ4One%!P~KAPvCPp8$lao$xgm<9h+00hPTZi=lgRB_!bk7A-wHsR5d-pw{guhZ#Mwnk&c<(avL9!aMcY&O}E?GUm;j_(yRaPo6=o1xN zw1n*9+c3Bd^2GXcb}6O$$>+i7wEV~DiSHf79ME;@%aYp^F+tvfm9l8??chbggVRx& z+k2L#D+llQ0XEtV$w;`9usOpeB?OR}#lUg8^f z6HtMewLOY*4_&r3!x(Ds&%7R+KhwKTuW4UI+VO8oF z8hQNvQsC}oj~f<9it^_{bGBeKOm0I$?|QTT;ccU8*7o{=I|1H7NR%yAvh1Cj)Yo0O zo7N~F)G&D&d{=6sOjYAapU4PI4ls3p5%6Nc9ep#J2t2~<_u1!`tcjr5&mjDe_7XBE z7ZD@BPKFCs?3`v9s0$;?S^Nk>IRH2o*cH_u*|oRQBeqDT?s+-aF(H0Nd$L?N9Bn9$D}*!lIXFtT+dhuBRU~1ZLjkT;N>) z`u13oX!{Ti8abOPJw`C%hKfBj-KyCZeH3%k9G;UdyiP+5=$!>9a@h9=D$n>`#h7@8WV)uunBb7ef6bh+72EW%=V-T99 zH_banGo3IP%0a(!ut8^VV*bJNm-u$ShCRj1z${Lck<@^t(^AV$MvSIBS&s4>?a5M@ z;-O7H3n4L4HFko&Q7Q3D51?8sk+ki;B^*dRD=9#&Tz>b~!tt%LSvG@`VK%HD&5mMB z0Hvx(DCTYM_fL<=cGk@E$A}M10Gy`zO&EpkLh7+)$l@PVe7lo`gHWhAFyO-2K&7(}dWS?Dr@XFj4iA_3`TDd9BbSrjjUPUQ~U&76!~wd%Sz=XhyvJ zgLQRmcMnz6qWc)O7m`{qI%x5C*5KNNueQb+8kvVktqAb2ZWG&BpFD4DU_R_m_4T;% z$M(P1+`=TXyx~kg^I}a?LTh0Mlyxixv#)*l(0SgPn?1>~I)rf~oi$P?@P03#*$l;! zxx8PJ{6K`ERKH{)IRW$qAJ#ir1++N&C}c6ygrf=Z-oJFsA(JIq#TVNi%+JKOQz=Lc zQdMD~?m`tx*Bw;3K=_F})hSyuGi~_UU*B(#I4YiKxD?wULwrnjgTI&4`55mmD#lrd z#j4-PYBlJc?~9s3ad$Yk&UuGUj!gRTrZtR^r~-Pjx_WcW6EF~`OTm3PV#>l`W2q~n z@L19#)=>#L*q$w> zm@)psdW&e|GRP~F!rscJY~Qt)Lp%r))Y)tR^X1c@Q@ zTdEj&>RE^~936Xi!^#E4(goKbTz>x1F9#Q{_1p^W>%HROx<+>t{24`Pwv$l+t z`H_JfM*D!(hbvuJpLjy3!1alr{czRHcuYx*_}fBu1rW7{ISP_E_%&!?iyvBhl@{L7 zvi}M!{nJZv^@s}}Q1rA!${tK}Mnw@D4__IheKa6RLNTl>RFq1tr9q-PvT#ZKQZLzD zV;5?6ore4!v6cP9s&A_?T2y?xn&D?!^c_D`Ua@hxbt$7y@asu0jP&PxT?~tkWRHDP zS+G60cLA&05&{$|VwhDu%K1k%_*JD_BjDIf0DNK~@3za^0AH-H7)ke1A$rCEP@Z2l zsyZV*b?RO{-idYFHT6_YbJpb44ZtW8P13GzFqsu^*-M=vNZ zaVK_2KJU^9CrvoI#i`ZfNT14__CQ58%8onC4&V&3H+br>MpRLJV{iA>5%4$S^)a%( ztF&Ed?LADt|DAq6p1#nTKK(_nN3=dz*xLTi3eC*E@1^G|hL-@!&Ry;9*26nz;u>Kw*irX?n`60jDab!Er7&RGaQ6!hc#q@Kdn?3{)+Q69%X)a-aEl9 z;l4_btl$stv|(-v38nmQ;E6~!(c|?@lQ~2jsd@T`<=AU(LXkOKJ|_zYO7>P(P9esd z52l$U!jtOCGE&SwmKNNUlF(I&_t84s-w~pMfZTS|S7mx)x!SdUzHOx&r`BC2t-$2o zl_D`&fS+v&C})VB*Vil5jfi7KTYD||GKa5;W^0@=UxpykK_xV)8kbtB9Z1N0;sOw z4Lq7xL-f7ucofNQfkh9LcH4C_hJahq#94Zs`ddJR!bDkbz`TztOH=@Wr@s; zvTM44&r5KdR`THSQ&z2#WWW3F(qlD!+We~hS2vZX{PlD^SOM8sF&`yx#T5PNwYZJ0 zX5LgW&$YOwp=RH%mAispm!q{Fzph%tBo)zd$^qAFY`ub*0=S{OQ9Ci;Z9q>gcQTr2 z=9NdOU~9pjchMubLcauQ27MQ}LkSwQf;;&K?xO`(3krs}^-zq>I1+4oMp*_)JA<7k zlKny-6fN}(7i+ot`0cIXdQ5tZ3=U|k3O$jljGG{$l;_BxbjuGq?k;)Bk#0Ar^qKh= zUzeRIt>kzxw3{Wly%sP-4pej}YS_0Qi>8K}Bj z)aSn(j-08e|V1m|1e%BEzO_H(r!_nDajrJCOcd*#I?XAs z6j^*1B~z7Ug&GWeNjA{lz2JmyiELgh6Ro3FA`YPjzCY2LPJv27=S{r|$Z$QoA9Q2~ zI?@Lnd2g^#VBlLf!>wa*uancTH_rFGC5}MVbQYGa6Drc2)1g{cUM=n|vjhcylN@_K}*P$eG;L8|_12E7;$3-W+0)DrVbjQ_-UwG5~D3 z4O^NfYj&(|!+J`lsSuqu^@v+jY^r=%u%nIlpau%1;vEUh1fq@2mvkMOHJH1PS*teA zQ>`=;RWQSN1_`(LkeZ^(`b@7&Fff)GyN_expyJzcEE&c!LFj5`0jGZ<%6EPRP7y^m zi-tb4>o=rf^#YclCJKz@vYulgz>V;X4gNcSv8fMbQ-)M>Cw9OwJ8ry9sXfsO*<=>}*G9iD8K>Z@OG0};vO$emP1 z%eY|z7$UvuOYeUCGFt{+3@Dd%KmxUY@It>-CL&k87XEDGD{-D;X3)cz54+`pHk~$y zL|?95#+%F{Z76RX=B2#y34bu$GxTJ(c!SBKOci1lwGbEr(xL^vrREAZ#>Uie`R6yN zST`uh%M|>ki~ryyQ+nfm0C?ug90@{PRwdzYJ088+)5;$-89D=zG&^g{WTis!RQk8E zi9dnuvdeq$=RRQVdUVy3Y}Ri0D=Rs3EK*lCx6|pjaj>BVxEiqzz72`d+O1umM@wv8 z7PnRy-D$DtZ?3zIJXe2Ac)_FnLEE+Mok#BmojO_FWtZ8YUkBE984&C;=;s*u<4uWO z43=1ohtarYjW%P6YiY4VfY`3jTj6{U*JW30b!H<~6@glkcDsym{AIx)jz4+7y|3#% z;(XW>)LFR~0~>4CE8`4KPG3ilvfjnYM|n@DIf_uR4e?)7i1%Sb3zSX0d z&gr}s|DOsQJhaeRfA+eJ^0`uztyTS8sZr5Rf~W_teVx*jwY2<&A1bbBYfv7Pspr^FQ?`xse%?gDOUA3)v~)^NZ$%;Oj;i zyR_G?$o7f+UliUT{m-mGCxu_lw#xpUCbX#K3?7E_X9V$XbMzlUP zyxx*WG3)+@nAQB-FIioMab+xLWJ^F4mBx#R!nZeniFC@!P^Hzz>Ha#*I!e5;R_wV=xFIPJ#qyK$L;i`W%yM$_-lRxKT!~p#2N z{jE7>CVbuXC_WO)R-2rp6jbZo>dxolTK{r&BMpqNG38xlunA{l`!hVX#`+70+sX0U zyjTAK5pFM)Zj{FWxD>+`s9LZU1v z-7g=$!@#D1&?97w?^sqUdh;j1%F)C#-SC-(DeTeqcf}JEQK^|f$?a5X6sTF_W1qgj zX_0pXl~9nNKYx)xiP0ZlEQb9vSqJ)S!J}TjxRbj$Guk%^9j{xC4YwW?Nvd-!)Vf9)kWB|UZzPJmq4WbZUGUBk9#Du- z7o-3*Y}8Xz&QRBB{Iu<4}yndOi*`JT;k*)Y}KZZ9D-sMA8PPv0}5Z6>2tM}4VP z$4)?|+Ei&Ns@_8xqRl$y8N;u+9eU3Y_Xp2RsY+-TwJNQb-jD?Ui=7amaN8?3`{i|3=9!;C;ER)9_(yR z|MGdi`FsBX76xV?8W*&v{ygHPniF812Lz7GsXu8;J*NqSYZ66eu;IH zih=4%LQD5_e;7!3u;7V0(+dR38fpxEmrc3Gpj%$3Msj6gegaW0R2npP2ur zx{n6?|HUH*X0v3ZdXCvXOsUC}61uZyz`zjy%W*H*!;;~7R~3hDE_5W4_2{adG_$$iUMv{7Yu8N53aPL zdPc^eh`rulD~!MX{(XAA79<|R=9He-B_^%IB!zLKxd#n|e-w??eSs0W)L zl8>9|yMpv7J*SEB$)phB^7vAu5dcRS`*afaHllu>T12)X8Mj{F%|hnZJx^ho!kWp( zTZHyUH4_4E|&wtZPUI*!&+RHx<@|s^!WABx_8!rf?Q?~ z1&V8&4CV-#{+cN_*l z6|_M0qWkz7|E13mr6qjgAz50@CBKl|dvn5au9r6R!}Z4%q#*y3Z-jZN0=G7Q{>=3% z5=ENwvx%vYQ_&-ZeNiI2_1yWsKPbTT-ST0K-7<3-OMbW)2uEeIu~ykLGliH6yAJu? zhm2Xo@E36x1>TZR+A}L-FbPKKD4^^B$lcO=_4ARe29Qq{W8(PTO$8XMf2n_+I%ZV0 zZqT%k*1-qhGL6`Zd%ic9%Kt{9p|9B&6|aUm4Bd*F`a$G`VM=JEJKM3H^3EplkmCAU zb7?1IQsRXRl^b7h88<%KoZ#CpGYaTS7|{qj@L3i^91cQ<-#~YmEUwlcOEIRj0eWh@ zISG0{_^=3`1Vj4oUNFpoh*{lCjp?SMD<93%mPp(hDJBs_nL-7TRu0_M}@TJf{3 zvmu6o0YOa2Xn-4!dH%Y4Q6NmV)rawrVaBk^K=*YOd41c@PofE@cHG9;Y?8P$_{FC< zim5lf%kXiZ-=e8%*h7p*T#Q6VkjY@SVU}UBpw)nrj?N~YyXe8JxGpK8R~I!g@HA8Zo`egSm8Cv};zIveCI?WHaJIy>8Y zKdwTDBJ)>Ccqkhyb2>{u2uzkH7uwb~M_sYb`+gmB<(JA7@EXCyZuMA@133?=d9BQb zzwaNuLAR~-mY~2-bNNYinYLrkWo0`;5=7%MyfI5jxG zoBpC`DG89-6pc$8CAI*AUJ$HzLPo+@52)2&!Kece1X z-@nIv73Jnf5US58B(?p9a${m(Pa^1Pt9Xlw!&-#Z7p}^FF;2a==OT>dblN2Ro-h$9 z*UXZqsim>=Wm5-#lf*@*sSa{9gC&9s#%SS;R4H&dK7?(= z(zYA49HKg74wqO5u3_I_w4QV!+`2TLb9$oHdhH}e(x-{~ca#X*d8HLb``0hJ9HQZ^ zcxxY^+X<*P7oJa=wB~BP-%sfnLOx{l<&&(=H(yV^1WtO=iT9=Jf9kObH@hAPnM7gC z>rnv$6ssQ;mdlj8)3ESEKxcn1TNC7YbN=q^vnF;`jy`imp<7ky?p$W0U6|8DuM3Cv zzP|F9scoea26^Y<57ZrN>YTMOOCgt@{nQXZYTi3gIQ%ppbn?Nm!O%5ME=6}H2^QW8 zj2G3P03d@^TjbRqa9b$p*d^fS@pM$AkI8<<7LtFX8+v0_;b;ll<*0Jf$zMjg4yzxi zRdrD0_1hPd&B@oZq`I7~QkI)8kJhx&Wp=$z>GaF$oYj?FsOe z`TO%%HP|k;Ve2BYnU;vsn9M6|vZ{RqkrF;MXES{9gVU$ws}@}s9p>gW*(PqymSXOx z=x}w6j=bvw?Q_}0&Zzi@xtNEr#T>x^cUCcNp|Qajs)?E26;D&0`5d?>rf{3)I(}6Q z$uaXsy)Hc)q}SSJqd5|F9f0(imlm(!<~k{7?{m%WR zfShlJ#vREbSSA|W-gF=>xn^Cc@2qk9ckAdc_^nKsHnQy7&{=d@CUNMI#JpT!yYR`E z^G~mG;od6b7grYyRv6Xt#)Q1rKzH-y1-=q}mdPRLK?f8(cuYT0q7!BdW0@EP{(NS`| z%a%vVyaL}?w5%LY*!`UY6^=GYZ>* zWf*(#=QQs9jYq=pLM0EI83@A0c#m3>W@ZDLD1z>j%u9E@vgP{Iw?`s_q$P(vKrM}R zi>t3wTebdWY=V*mwN}0b4wuZiU-x5)V~vZmF#}|Oumvqu<>Zai&iXN~^N7Q$Sr0@E z^iDjW78X?BWfvLzr@dWD3#%aKmN_DAQ93sqzm!k<#h)cP|a5Q*~V z7`t0~f=D4huN4dInz+8@H&McTjihu38Se-IJMfi*7N1=P@XrZ)J5806}iWDZ&!QZ6FY*>75X1g7B zxHZ>Q=dWpRZL0!!(T%QpmJwx;%PrVlG=F{-W#VZp;rmrO$B_h_ndvdVu+(qDAar7p zT>t2?Ax>?jT6f{3aP8|q?Q0zn9Ld6f>Y+2+lNoDEAYPJv8ffj5eNPaGeSa$&K)<0a z^oPz%Qj_JYNe*JlL)}_lJqJDum78rQe}~*BXSjK<=vusZ`wb3qO^wTaj7>#0S7u}b zLtk)Ew!fJ3Bh;FDj~df}AX?A#09{@NhGU~gi=E@< zy8WF@{=Acfsz6_<&aqEja8Lxlv#|2Tr4^5LeYAXvWOo6q@<3ft(=Ee+O=$xGp`l&f zZi4gKz>jZVi3LigEd>VYCs*X`-kv^Ssz7jC#}0yxniZ8|J*5hl;=Q_e3P_IhnH3T;uu4@L$*dqeOfWCA))$M()Spbhpy;$ar?bq$g+M z7krJ0_U}fGKdM*vU}>vzt#Ohc^u+BwS#HFmA%Bla0K|n4lrYN~LZy76`C}PyhI@rd@J(eg%c_8zJ$B6T z>r%8A6GCMwT4J;k;)=Sy+ZHRl^Q%b51}LHU^L-15LmOx9K(^kDu6^xfU5A=*U@uF? zZ_M1p1#d0`ZV5Bi0)?|Bpnt1XT=p!*xj4V9EU9XEZpp>(LPh0k-*uG4%(s4jRAop6 zKyj4hkyk^Q@5Wt39q!KoyqMsCa7d8jjNJM{I5z4|%JnpT?h z@>{(LSjl2MEVa2-PJPK&AX*F6)|iqq;2oPIiZ5H&BFE>@LGT0q%K?0&=F|FFT0>c= zB`mS2A(IFmV^)HRlzmQsj%D><9^Bz?;jnEek=wEOA4lzPQZL>|+PWScHx8zpxY>_8&fr!tN zxQ?@M&b$qIHJm5~w!#qma$`O24x5Kh3LmId8KosamGjdm{^<&CpynmG@plk}_QH*+ zOD){Xi{^u4c`RmB`XQx>@$nEnU=Gj3`KRdU4)W=&Hv9>uOhpZwdD_ChQ%TL+cg6OO44=KV&l)*Z$B|~+jBqMYbB_kv?3pmg9K7II>tgQxc zlx1I;=xJL|$Ec!|vXn0@qS}9(+n~1Ro`(8ZfsNtpksNmPlFmXG4AZYl02*#IRn(BS zjh>GH0?`%=A#&**87Us`84@}QEj&x)1M!RVZvQF#=u4`9$e{Y zM9u>f%?1ZAeb62DVar3L%M-$@^II}+joP(v^Gthq@_R8WDapF)7V<}Aw~9lr7s@Ds z8p@Ofit7=Fh+^_~^jWB)fH}yDJIgQ9j4S_-A%lDtnuHsB(*mLV4L)NP>6)*AZ&(o9 zZ2GwYUR@`ellp|)3=qFyK$0DCSV^SN-d!_)1C zwYyX^e%l@2yEN4@0Lq;V0@%vC#L5vUBH(~em+_Xox=%^RPtu|jIKX0J>gclc*rB(s$|F`aVaZX9Ina; zTY%1v`7&c11bWI#+>}xfzKWuK-3l~K-U#rbdATAaw5)>gcs6| zYO>%|rD6J@fIGLxRqYn5DtVdOT_|S@{i2iQI*N6>+X0kT{0W{jiM$KymsEUdP&Jki zmh`H|3QCVoK&|-l?{L)^R^v&5$6sydu*|wVmfh;HD~9xBAG`q>4ze4(M*5%8MMa$R zmGKNY4yui{bALiet19Pr^H%h==?Aulf~(e#_>vIC{hVa-IM%XU~hc%En{y`3$kQ{vnOaG z)^A1rvO^D9$<<#M$ z)JF$SF%ieCW!+QbU*WLxBs~0y+Grj{bWcb%v$w%O&x>dMBV=lG3agSS=nXSI&h7J# z8@(h+wnuWWC__N;`p6?##CpeSQ)%<94zsRMg7%0`%!W2k1g+a$)Fw(%R(0T|K8K?a zGsm!@*o&k9x1)tkEve5FVyP$u4{V=1GD1rMI)Hma`>yT{g$Hg1Eut3MPp;4$`|Paq z(^SZ5Rn!)|zF)JuEnndr)NZAbJ3aMlpnnOm@5rqU5@%LYBSwo^Qgsc?mx3KzVPGV2 z|F-dM9S{1e<9tctQJ<`HkIZVK>5B2e0Gok9o~%^PXtp4Jwrxs&)Rcyu4JZ*IC8ayy z77Ql4zw0VtnBo*brsNT7cq1Wpj6|OGAt8i0CTvDMvA@;ywZ)n!yJSl*$&Y|CiI(29 zM-X_ekmEOnBknQR2j5FXn_inG$*jKN)?SgLVPo-B6zyfUAML$L_vKUZ#NIh; zZHP=)dMX|JjJJxi_{0qsa)fzImdXH@nApY&jiD{X)qK)@@`NiEjP7Lorpvm{B;r$S zs+LhPt;eiCEWcP%wHvg&G~TT{hTUmjM3}wmQY26$G9g~2phaOBpJsKPj7}s{{SaFe zcjWW(C2|&+CwN&hL9ujMRHNh^hf?Ao;d5Li^G&nnF%aBB8f&E9AYRtiJ_nHNUdCqr zUEGAae87J*expgGy)9n$ zcIPI1P3X16=-IW}@ZR)Y=BLu;JM&%1O7U05JAZbe(-(dWMmwhmXwxDl@2C0JGVU|I z>qU-6n4sT9lZ<=@yu`ewZdk8N1A`&OG}{`h9k?3M2xfIIaix!+fPQlYt=mACO!ykO zUfb7kEQdpW)P-Vi$Zs^`Xh|&QHG~YvyCK$|8L5`w-!k=z4s=)tr>o2L zNMa38c$K_2D^TOx2}v&EZx9D%2T}PSIBF3b-Ax%W#Jo#n@hN6Z_Fyx(nLV2o07_VlU`y=B<=V9q9mf%4gxfc|OW-2e1N*MSTy&;U{eQjf-bu#P z!Tf-zjlOOUTVWTwE7ON&y`lY9)JA1}>VvIBP$!dtS{7%<*6b&5BQ|z9A~QuTwa6N6 zwEjaOAxluq%{FSMj0{2hu72ERI_+Xesu2}&TLvmpSXJP|Cml3;{rF zubUROuOa>Nu6o|yOXa5ZogzJ9wmwL)l2*r2_S>J(kYPK|Cesz2O(%seCIx%Jdt>ib z8%71Yop|&^d|VPipI?cnnAVa~sbkr&%y=XNp_N!~)Ed5~9RhAip`CN=b_9pdH zHX^eo&b^c-4$wn79H6LwibiPs&5VT5*qQf;ZFqoX2{}1z*RIe9#4KgB&Mk;!AU7R& zndI_m!LVt(q24ps4YVtJwlopF)S92(@;Gt2^f=K{@9FV1pRK8$m$iJY_WGD@ap%a1 z2dAXX@Zb?TbI&hj)EgA}NWok+$R@K-EIr;#1_-1*nqzr4%hXi>Ap4gI)y$vjG9Q*S zUh-yDDAbv3EKNwcipiYK`u`44w3gYakpX@(N2<3J@_a~Gn(R-|-6cg*Din8rWbOBQ zUX(^s%S3Dbt8Cd=Cq8>YMHBM{Jx+2qK099>9XJ2MPS2Xj+*J&>=*H;a8FfBN0CV{z zbkz`(q-{ET)lm6SYvjdon7gKgKfjK(~`2lpjvxDbg09Ax80RaESU1s{;WR^NgJDV zm$jYf2PUU|A-gMn!bi>tz%}V9e?2-SdvqH4uY^wQd~_cO{cBpKG93zI>wNZqEt5xZHG8b^-`t#%HHaW@(GGZ+H9GlW8D+ws znQaQW`e6+vv5f`~daRt1>7lUy??_Of=@C1a3~Ybj6yiiS$%IEAdj>z6B|qy_AIWUK zKbt*{k_I0Pv*0JgLGu^RnT~9qo3@ zZA@mglus+&dd4uHVED&(b!E~eq5j5gX-dcaSuB&T_O-(oyFTo>20Gp4N9L`yU)OT& zCM>;S6!CW6CrLx9NdV&PPN^9JO#S-agJ!9qfG?IYdUB0fCJ92PgP8ic`FvKx;^uvG z!&tWN5iLsGR>_%^!rkW0F?l9(Ihn4w5mWn|Bpqt1Wf6Tfq{~9V99lUyT5+Fr;BC+F4(d0euP>98%30KRt~w8_p`krq@j6+N0gXCX*$y|= zZPRIfWP0s;@$jE@x#}^#B8sOdRYN)PWiHQH z)9N|yU<%4%Z3I4|RbP1RvoF}*ODy1tx|h)z$=*jR+58gjB zI&5|G+o)@IGlHHh&G_6k@1dQDYTuw0>$iG(q*yaa=a}J}GjL%(HlHWgh)*cOq);VK zrR60TTXI}#Ki;y&DLFNun!%lKI+^&{19cb+x_3>IbH-${ZM6h5)^t&aqvi1u?4Vz*QLHqozK>#D&u zg6{Z!fhE3Nb}l53D~;nn4@E%DuG~Ra^RY+?f`J_Bg+HB-w|?f6cxjOFBx($zo`j^~ zrzbIDSo~bEw0e`@&;xmIL=PByIjBwm2?5tWtLYo7Uqg z{S}6O>nBLSLHdURH{abDT&Bn0h5338-uCvgAal8#6tIM%7E2XV985SnW)hoQx^DvW zI}l(mHufEJ(wE5B{3eo$7Z4mpVC+UX>lVBryC1HdmTpI6ZMG0(EJU| zBd}pFqs7f~Q-SUVC?svzOM6TF(;O$uDc}b!RqKH&2I7CSd#e>C#}>Q&$z5ZV*mx|( z-BQ=QUr0cxgB)dYX82&!^NF8r&~@MoT^HGDc0pEnsuH4Xy8b?B80Pv z#s3ami4in7(K~P-!dXqun7n$4g07%p<6gHM*IAMAYNT^G0yu~;9ia@X3jiJWjj@b@ z@RcshtDV3B*V)Al$KMz)?(29dsn`|wUzOuF%6(@VV^Q9=Q*gvQ1veViF&y8VL zyD&xn6_A)nMb`Xjr9JM@@mJlbn%*in4ZwK0$#3KGxc_i}ahrUlaiC zGUKa$L{zifw9{V>C&WpvWaRH++f*i0ybYtTZ($ah0YPSe@H>o1eCKTOuwMLY&Xi8U zL-!%Yo)2eN1Lxza4e&J|?uicY@8ItUMBvb}#gPf$h*6cZP}&w)pZK*&!|xP)P069q zlOlpkFy{>IWj0K1yh(L8CUz~T{Jm{>Wc&G^go70C=*LEnz4w>Rh~A%mn}mOvIVeWa z)1(NyFn}?*CJ!PGD3&Npi&Yc+N>f0EV!;ka8f7Vsd9t!u-gP_?j`G~)w)pE05L3acp-z(2@D+pd!09*mziy1A(oROl1gY7b z_{%!Z*gIpGeg~<4-AO|T2WeenZw9WRCGsy%Sm>7v&z?^;RVY*$YSIsPG+E8hCX$Wv zdFi?oRamy9Hoe*qaI;E8Vf9iiEymPO&>n{H4nyc6X0LGYEJv-gLetjB!$zxy_qvn+ zJZ4K+I2f3JKPQ2BnTbDL`8qR!|9DMuVfN-pDQHdxCxHlonv+5p%wJGGrBS%IaT;xp zi3H*C)dRz$d}@!w$0iBMHs_;y!py|_er$i#@IFp3JgDy9P7V!0c%MH%MPKieNnWNM-O z_qN)t$Y3QQ3s&sU3{StXokvTd$8C}QgJDJ&GO9}fd25aF4E+CW4+BH@58#y^5^(A2oQ8U)FR_nukXXo58b&(}wn$3?a%9L+z2o^08C>gKY3$z@G-# zkTmOm9agMsb&l#I$n3FG{sXKJ`EJAhZ2kmDpqFhT`N}R zR*(h;K>+{&fB?|7XID^(T^qjw1OT9g0suh%_f%Wh-p<9;&P89v)4|kPm(Ii1W+ctt zdXoWRv@hy@6Onp5no%rADG3d#6xw-AFRt39P4o#~(k{#QBR835`lNJ93%xJpY3mzr zTT&?bQM@(@{7FzNx!io6Lw3%Ax}@^q>2iDd%s49qLM)&O?9nBbPb``<$jQS*izEJ4JkLRcV4=bW9&Ip@0SMJ}&5U~-Hwp@GV z)5@jVolGiOI($nn>jvCrV#K|iBpf2h^Q-6TD?HWUKQ*Zlo`;f@KG$z=sv; z%XC6$C3dMS)3VTuBb%|52jDsZx$CiuY;oMu$HxKBnCWcr<5D|k~hsIUY z-x0r!Swc5QAl3Svq^|nobFde}Z?5+sD8ju~%IkAIxFEfYlXIWcL%6q9qG!E5od-6& zua9M#&hpY4sF+zq_asOEmOiw0>8sM5RB-iVFx`oKd+C-u*^?WHBzWn=DKD=Nzf z5u6QySGL;;ZBqmEFui@YVA_fWvrpFqC`qpghy zLk2HL=4@E7->_HE@>dq0z9v{-F)yAJ&~Gh~Vh1t%7f0gos-1*%A+gI}*zF6w*dFyX z`b}(6SAIBMN#ZLi9KL#+$Ia{?5QYB|hkr}`lE*)B0D}PlK>rhmzLTkqGdiD3F-ph>!{;OvXg=<_+UWO*CviqyefV8bLGjWFf_& zFfZqT`&Jd|>Uzs2ueeQe4f(P)nm>VVgewctD|6tj zmu=w7GEdBwDq5YXm-ec)J)E$urWP{*usXFH{v~jLdVG4XpH~9h>|ydPW9yMG`{$x; zFD`NR^f$WL7i(>fTiK?I_b9SHfYXmNMHRb$2sOS@AGPuix;g@8mkBYtFX$~0`0_|? zGFD?+p55g80RNjHwn{Kj-G2mi0s#PE|0BrO)Wy)m(8ZA6*xt!h!O7mi)XBxt)cHTF z*jGtceux0&n;Q0(wsb13*0M`vI#0)_MeGoJp{uS@0QgVcuii(fGCDHeUd~WQ_>^p$ zxt=$N(aQFJm(LB`}m#^%3C16mwlp)y+1+E({ko{(<_>&+`3B zKhh61Mge30ia@0*y2GXbu7Pv~rqsAhplH-QT7>uk^Al4?@AO**v`~}7^SdACg+s4+n!ND-@GiV=5~JXih@7qIT!SYgZGf1?7Ao)g!jdwaz!2; zZI?vbHVjMZ4pkqFQxXV?g>}MSURZ-yjsAC^{j+mxUHFG-zW)a+^>Q70qyN|t|HlmO z|IUi7`~S_0Y5bHO5F^aLg*4fVY#>@Kv({K#Mez-gtRce^i9E8;=r4~(V|KQXHhbUW z^%^0#)zBcnH09c!1T;Z*dcH8#xAPWW)riXguK2{tCDzOIcY&~x<|W|Cbd02&2x3g z>Vegjfdf{1vZT{Uk2eu|j!l%ci%tUfTQHJq=MR(4W8J#d_FmiV3H0Bx zM1*D6?fF+Y$Ugwc{$Buydi?v>I9uA={YRm?annwN1Q??Fp@y^i1!RsAI_I6xo4kSa zWX1t72Ceq?8+BJ7dEO^yLqqgu&`Np6XAK27#1L$qoH|@zj(mw=#X2$$W|4qgU$B_P z!u8+Vqae4GbqSL&n6`Nd+&rmGH<(jyK=w?}WiqcBb(tGy>sD9GIsvLW6fYnAd{80z z5ZxWJnt$)!(U;p>%5tKlCN6Bt*DovV+t;6)QtRw;NhV{WDs&IW8CrtY9BZ%$MXxzl zuh6$aekVW4{~Lgnc#p`we}%LB1AyZH1%Q&Njj5rtsid8m{eK{EjpIfRVn7J-_8XDJ z-$BTPENosJM5M(Hp@4~JfaLn|LH3M^-QH>U_7-1oUmc8w)O_S))SY4sdcD$CsHPy0#YRB5@3h;4;z2S6`SAq&?SUoo!-7jfdB@b~fo~ zsznb}V(7<~J1wl?`{Z>n++C+HHf|`hHV!|%&*wg5N5vcj=mWoafmq)Nc8nb0o_7V- zo&@LqDpkauG@wAUe&kEI`$K(vwOnenNq~W9{{czUO1&0~g7O7t<>vi}g*tZig0tm# zCYB15c=LVx4GvDq>Cl7t{!lZs9MgTfjBn999YqmDd8c~fYD1gZvOGOjeZQ2qx38yQ zY9y79dGx1O%-Iv!-`l%GIAoW)$w-EPgF{=X#az#*5p$dItlY~1Nqr78_r;RcT8P$M zGq$8kgsd?CEFD`P{@~P^YwN#;jyJ`J{J~?SJxE2@)h(%3@Nw#-^@1RT-2SnTQ zbYBsUU!HsKtxQPs`5-HZ|LMr(WBr8K+&cl}IiBe&jLo-k&H|D%=q4UzIe zPrLbgK{=0I$%WlM&8V_3EPW+vuejp6o=}McQDOA5zC~I+&;8C~NVJQFS3Bp&TWND{ z@9qpyv{!*IaeQ_Q@>E8koak&wzoosHJUf_Ucp+1PLM7jMI=kJTIDy2=YSKU0%?{`H zT^Y53^V;TPSj!fC=5s+KBaDN&*-Rf=T0-K_ppr6Trt2{sd(iX5Z|gmOUtjjZcJ2M{ z-kvTH`*o}JYFc_a{-?CoU4yl<>Cwps7HhOm_vEZUvbNN@x+wm=#>H5c0Idy4cN(`o zg!#y0<;FBY!mqLM_`GfJX?XlrSyo-krfM2^sw3*+ABie#kr%oxE;;h=nwr{J8=Dkw z{wpW@Q@$YHHh07Bom_W+U!qYMOkc_Okn5cuZx&-^+Sxw@dQxXlD+U#~aB^Jfv(vs5Rpfcd|utjhNoCDSES@C{7v8w1C zIK+9Pvhtf8Ahs{h^QRy)%o?m8F775P6Et=*)lW|^Sxry9E8hj1hn9whh}vR%3tN0(U;Vlw*I zGBld}of=Q*#2UsG{jpM_1&(sLrnI!QtN+x$^1aMZyif$YuBoYejp#yG&)^e*vVjHN zj*SJ@857~M@NDxv!`a;Yv*ZWj&Z5OZoAacsx|bT2mWEbFwR*HVb8rcs5)PpS*<#kP zk$@uY!}sd>>`vmH+ie-?e-BtKkIS}=q_%79FD7f$)%pXw-G0PU}iBZ5%hjQO3i#B zQ6yd~)cVI>iO_vm>PnSU0b*EL8PYOn7)*KBE848wZveLrMv9Tr6Kw?SG*~=(fmk&} zxuRO9Z>1m;qPYO=+Q-Moo4&i+zXv`&%oAmwNt%d=aVOhEgT5cUj` zEpwOn&>fyM?tC7X>JbkM8ylFIEaK9gtI?p7#P)g2#*ODkkIHb+X-e%4%r$(Y*K-hb z{+~<6^|n6|7FJe9(aQBMXQr;*zRQAOsd7Tb4Coh3Iu+(-W@SM~D#6W_6c3i6p@^io zcqvU9Y4fH>M;?3pOfZyuKf=D5E7= zPK~EL5NAdg3R_=>_PE{C@5rUeLwS8fKZ!g?YFuCL<(%HE0P4s3P=jDn3I7VopZ29M z%&Qx1(j7*dWHS#OJt3^*g178ITRTFf}5>M5+lh$nN8z`T!oi= zjbiv%GgXdr$81hAlwf36#d&FIDd6UvM$Fsf78V%L%QKcKr|7^S+2)_XR=H>=LXMknhPr~7Y?pQ=1RTRsDj(?!!&!0bJog<+Hd=4Tl zx<3gDKXZqbxt+*$?3$VvgO<1eXj`0X8*EYc_fUY?ckTJtW0SS4<9(BM2$n8UVa1w% z&MFg6N6d_Dz!VIS*j}HXPQaQ_3FsR%Y?7N)dbdKQ;@H8r0WYM>*=YzVB?HZp6H-~R zUuedlFHfmRqqJ7ArE8C$xMyxal?iy;p@wy5em=D)Di(&*Vzq+S&n}lJHST2H+A4Ia zJcDH*-nB77(1nYAx@rEL5raDnW$o#|wB=Z36UyZ>>Hf`fasX$Mm0!J0L_CX+goHZE ze<@x(?LQQ#%p`O2@bOuD&&M2xrq^W8piBW<@qV7CX3hb#(9qO?S0KHys8Q%P>x4+T zuiA@l%Qktc*Y7*jL$@GxcGA{4 zP)?b1c6J?kmBcWt8`e2MUzGD7p7uFMx>bsf<;V~TiK7a_rM;{2Nd22XM+>NviO|6L z>vnP^hScY@uvi*M?c^pEsC~PqBwYZyc33fDlXX6CqaB$?2>KZ1_)?I4mV|bYT6DQ{ z&fwo_u!P2&0GLJr;tE^ipr4H{9VlSX6yZr@dOy=B%ISPJQ&2R|ZBiK+HwiS@>pv?8=Seb0owZ|R20IkY;0DA#U zEUYAx@UqX0?ZT;{JzcO`jX|Xu-LO{-l}oVAA=>>p41)DS&5t!=3G{Hjr!5S`)Wa*v z`U)z`!e+F5)7x7=Znhmb>Z<)b+3iEG*+3!-a3H-KKnFNTTL1f_R#?|731hTA#T&AL zfSsBAn^y>T3ff3 znXKd663`Q1IN-D65IYDa&0p;E7Si}6LaEf%1Z}+d`g&F>8&bAdu_`_A41VYm=m(=H z1O_xL%yyI`2a8Fu+OU>N3kMj6AUAU>sZJTV>IaFsSY~S6870Cs%!Hz%>uhkHCL;5` z#YkPiTJF^hP0ee3{<8Sl{9LGvNG#&?3I3ESl_TEz30_k?l`T(B6F`#@DVnk>sRi4j zyyGrxasYT%R#t(~!&bWbo=69;YwUXX(~bzU)Dtizo}#1p1tz9>`O(SKKM~YYokirEfYq74It%NckAmM4+o2f&agC7@1^k1tRL=SAy7r$22)VWv4u}F@O4+DiI z>~}5`4ARsU0Jh9J^M+CE+&R!nv(o$af4)MsxFDPR?gAwj(zJW?s8>4s1cxw+9l3)$ zl?UUp(dNP(c1~at`Sm5rs!^*LhqrHpGajHD1Prc)gySanhI5J^XTQ|kws^Y)f5+{w zK)r)iz$QvxX^dq#vg}?g2>#%``G**99yPYg`jW-yUjB3wX$xE;aA^vBm=uVU7tVoeOIHv(^)KP zx_HWyc?+7#w`p0U=m-RAlIR|lI$gA2?L1{%`1CuvzYI)99_h!Cw=G!&QBJR86w^Wu zo&!9KgK4t!(PFneLdwxRo56M9V_-n|ZkA&*&l$_HvFaD2MUjtmwt5WXi0s;$M=7yVXn6@%5h2fpOcCpg5bmoGlXzkwuQiTXYAohB zM=T)6%I5tUjfjxUeUJI&T>Sko`;04+U8}@bz$+&Du6_x;bTzhJmiX7n*Y8Gi1@(HF zC!Ld&PXrZ_=8HvcwqIwr2L;EIGGa*HeR^|AB>K65Ly!k@_W1fX(qRVVAaX4f8-Gwz z!DFl9OnVFTT!2ZeJKbwKCn!FI3Pjy(Y$UyP5`_-!@9c*Hq6fUi&Yc7N*8J#Q_~M^} zpg>tP8D!nK&V*F=bVYS_^(lH1ELLFa_#pAj&Yt!)2|Bq2^;OIZo# zwHQ|?hsY%NMm5|fsET-#=o4aKU}OYTpRcI=H6TsixP9ABCzjJ4W7#Q3zdY)=a`p^h zQ5AVBM@g)OpNX*KT*6D6O!Ai6%>L`o4*A9py(ZFoL{#3TreEpBXySKj-hFX8=+PY- z-)6GrH4WBr^2+-a{LtdfJTALxGR2auW_^J$1WxPEK<5Jo2d5nv)t|m>y5mMq4;^?- z-f?@_`jY5hDk3@$px>E^J@`Y!*aF2spWtpye{}h?9!Ej8mpIRgCPhFAi4}<;a^(L~ zlP2h`o36vZ3p3>(;S8nnx3cYAAM0-^v8pw&N@v%5V-agF1$hP|5fM<%15xtJ)04+_ zZJZe!cw#XdA2kJq#)loi`sv9rs-~pF2ZfoFHGwo8P=v0Tm<3>uYcneL15@jg|#&Ugtg3QRqRD>KHdC+BIDN2=DwLr;os4q>9-m$PvaL1 zR}f`CxN0#cQM$$btzDWYGG|A3{QP|U7brGx2FMU`&z(s%)}ze~PZ3GEUlo_4yXM4i&<^tseMrMOv48t=O-&qh>fG z%Z{v$GEkhSj(KM41{V?vdAGg;NI>Y~lRva+b8^J}OsiY!cw}8`^2&z;@)KO#Dq+P^ zUEd9MYrwy>wXq>NY~G6f@>${sCz>ESFz9Q>Uki5O4Vm7m59~?QuU=0XhlYl1uy(9s zZpCezVZLwBVT~aW(XCOUCWg^E1K>(B*R*84{07vs;Enw;w}P#xmsX-uNE)_!oymcQ zZEpO;-MG{=uoZv<%+u1kQ#2sR$;oxb5{_PW2y^u#GeC-l|IJDmiv0oR13dB$me$&p zm^W^9lqDD>sS%2GG&K{u@8Rxrkpl$+VJfWJ5Y?xpE3KC5E1g`4$5nNac8`f1% z0nJo8(@hE7t;#SqO{{h+jP}bD9hXCXzSZ5cPRmWSr^v9L!c+56SK4Gu7sJpZDM+d2 z$60nt5d~HFw967Yff@BE-S4cOD|^eIHY6B82Fxh=wZ()5B4rp9)n`cBo$9X5Ydghq zY}oKTXz@zx&E^Wk7qTw)J+#&|L9K54;9hjED@Ja}Jb%e=O!iYq`Ht6~9%VCR_vFvk zmDFJi>?FM#=W5-!=519^H`TMeuROulojaC*+nSsNVgA1Fv*UI5=h_LRi z4`12%nNkf*Dcxe|Wy`N46R_#-@is+t^mbH6OW!$H&hoBOqtj_Z1&!&Il=|OACUXHg+g0pALV7w9f*ji`*}%eUf2o8%w}ruB9oMiT5Od2Bv^b*#9_WC-%An zi?4(WINN-LJV;6S2t!Zfayo7kQ5*LNF0Y-pr@QDDv3nPKG=*JENr=At5_ng!;&KQ)HlB8mq$yR%`xWi1nv13<#1(k? zSdIUixe=e>_`6^6lg@4!wff96U*i6YnZo-}wVRyPvA^~h@qgw~xfwcF!UxxGh>%02vkAvbVmtJ&{5?h4pc9T;;a^3;f?6 zK@#E%ve!TJlI5Sn{Ldx;aJIB{wXw9brdKw$Fg0v(nI$Z;OH+)OWn>ye?XEu=QN(!Cxq4IM!?a~LJl6Sz6QZVC zzf&F3eJ*C^?n2T9#tz*BEVbO@6+@)X3n!0qL>j>eeV0YRNz-JF%{Mr@5MJeaKLC2& zn3DCNMFsopp<_2pbik}E0)T0RNf1)#H>wJVK^>wQBq5wP1nd*BcQJ4A?pYLrI%QAt zpWnRL?4>MupRCm$YDYbk(X?941Ru-o9z8$gt^Vk|`f%~?P}NFxiNCIgf+qC!oN zj2cc%A6tFdZ$(W#*ZNjY>tOco2tyx8gv?Ro43k;y5Vvz*&49g+L{hd)f(iwE%>md? z2`Dl_M2IS36sL&=Y&#N?9P_s>jO z{#G!*cp*>;h%tji+&c7wH6n+AD4E7Usr?n;wHc_zL5Lg84|~k|Js#8dWMZ?-6z`>; z61_8+6)-qoTA&Tu$zo}%yFIA(+L?_@qLL&b3JUnE0TV$1SpY*C5S8K}3XMkGJ+}z_ z*mnJeh$(j`VTY!n7FBm66PIiPJ2q?DdGO-99huAP<%~yv8J5fQr1WLSEPNAmRFLpg zmGVw7`_>8)ihLou#l`y0v1M8&zpMx^ z<}g>o+mHqD5VSk``+~BuO>oq0hXo5>U0g5TY_c|IpLRa@U9?flGQV01_{g&_R5WS+ z{SL?1S2)={Qg5znsur)6vfijkKyiEVg+GcJEmXca}8u+ zt%DA-m)2-e*o75%^Om3p7k|xGlv292@4Cf}{FF7`_v})A(&mhWC}lbF^E>tgo-e2i zw!4Go%O1Tjzg(Y|n*BZ$+tsd@W_>js6%-c@!q|sl97RqEB@xY{7$iaxFH#UjKrdq* z^|Li><%#3T5ZmH7R!Nc|Cgl{BL4gq@f=J3i-h)Ar7S{=d*|oa&~D)B&U58%-U$kg{3oxf zQYPDTam0QeL(G2@Y+ca)IC;sJElXO%eY^lAmy30lu?*RPDn)r zAd8G?0^|9fM4=rmn)#WC3GD%bM9Q=V0XkBM$kmAEAZScP0qc6$Z)TmI>#&YjoG7G3 zlL|VjrA*GiQ34SmvrxgTMnP1pgn%9{a!mPVIYylGcGu>8`vPcX*}C&rPBo#yfflWB z&2(m&I5M%IK{7VW$T;xmb^M%*dlX3>>*~KbPX4O-( zqr2yxG}+do!!|)3b1)nwb_4&oRa<}Zr-r||aen9K8F|?JO$Bqi9U{*s=Exu5zZG8> z>m(HO&&L)72LK@X-?!-hl?F1hu{X9hu{1PyGPM1V>i=gts3%oh8ki9w^mj7JJ=<#l zdpk-6I+V%?6`xeLtgCQcX56Iqc@>*}Zre?}VVv*e?>Z-bGQ(z<7ro2_SRTtp3a(ox z4Is6mWp?S+yQPQ23=-5JmEoMwWSRg?$Hgn&Km~y4RB&apjiw{XHyEMGt=G)AtU~0) zBFl4wVA72T&<{>m60{r_MJ(G>_lkazjG<&Mm|sW_1U(dl0~ZYY{;qkVW(mP5$@3g= zqNnT_*}62ru?$O3+fYrvfL*pJFy*O?SB!wRebp#hW7*N*0k$gDTHA^>?Nl(ZGs93- z)nAyW5vT4^rRen$$7+{~-k;8&dYV2(Sr&SR-0?qdXRZ;ZTbf;NV{Z0S>YS_;r~2AN zopuIfQpgse+^QGqo7NHL)*`WnvMTd?PPCzz>k@p(O%H$Btbf^?!nVk|L))OxS(=!2 z994(E1L;-*sw5LbR)2iH<^P+_ypG%GmVYeb{$rH%|7O$J-pd|$;yyxq<$oRCq^moWQZv>E z>n;ZnK6ymp&}PbK)7=>saOCZ#Wzg(M4B!s$WUH;ch@EMSPzH?*?P`4pqVg#@&z%TY zrb$w5>R<<`+qTq4%IQGhxJ;49FY>3TW;lzJi_B{~*z`sbs|KDdAnV6WG8M@lI z{LhfowU(_taXaEqo_^yRyx``rd6z{XSt9Cqnk$=?W>6_NAY@R$25zl}@=@54l#`#Y z*|@G|lf*=CTe$H2XThQqK3w?YM}pJk*s~Cc^jsLVXJTF-w={A>=41yA-NNLI+)v`q z+E0}$rOM0Nam}2~d?;ph26EtG<1pWfQcOw zj$JOC@LnNPPJ(Z$@P=8cD&pQ-rHFtjhz2wl&<8;Wt7Z0-4V)Q1^9hg>_!YCRL*!ik z?9TUh%#Uw2&c}V@8rYM#-CGK@ zUA`V*-rd&B;*0+53{bT>mVh2*q05HebO-F)9(gGmZ<=OMPIw4RY8xHeUvOY`D@CF= zJ=PuA3w8>_Q`JisezS`=C~-x3OA40t8Z|#Ejh8=tbEI)Mdqmk+R;qC)H0sq6qfeZ} ztOkLB*=?l@&}1)$yri_g(wpoAX$GF7g9V7cxj;k)a$ekkdVk|-785~7RA7Vxk69I+ zey?F?3xhLT1n5m2Gn@y8M$-Xpaf@bK$qwzfU1q&&<5F5nB9!e@PWldKGR`3MB|i%yat%l{!~A30``OO!f*P`xuFdF}MbuukJZr4DtZy=qwrYr$hu z0`^VPq0g+?vQe_snb7j~`2$XZMBG-xp3I?MMD~0L+~7knqWtY~ZFb^Ub?FqF)2L+h zSNheVlMxcR>W`4NHR%^d+{$cl(%|5g)OzA?!5kHmn&pFzhZtHhO}2Nxf-!TmJC!q{ zTcS~@WHRlAQ{oIe^5XFGTeM0+aHd!&V=;wr13%Pgska#D3TacWSG!7)w@!iJmscM2 zl335~ACDpzzWKkmypE`|)@I!~qd4W>FBZ9W;;YWe+cXbEl3P>5zFy0UX^k;e*2dei zfMW5t3|YAcGByLWMl)U9xu#u$jiQmGR6MoL@(D7Tfjl92Ds8j>l4f7hSG3yr9-F3V z^J#N^mY-@fg3h15O8$~&7BffIYE+NH&4(dESYH|vWMU)6(-&>hD^uSC8OpW$Oc0Kq zJR+zncq1^Z1d-Z8#cOY*f={_@Cj)deRnNVjb0nssJa6`ete0Ac$`Mi-AqZDZOd1Vy z>)1{|U?W}@kG?%S)YYRk5}UbmK`KdB+Y4-iLKrwZWn+K#+yeP(7QMv?+sc!%Va2@g zlf#Ws%vl9H(c1n%j5=-pVr3##`M$E(%G9v;(F;< z7ykQNnf#p|sjeGRd34rK9}*Jx*b}=bkQ!>e&sf1YRkI>VBXU)?-Go6=fp6!Tza%@R z899`jRwh{!`@x9VFMG%$8xS`$avs{)OW)>mk(!Z&5M6#}0Ai>?OZWhAb~-+5c(#i) zWyIfl@C4-OS7#ZQr1BLh1EXs^z<5!TG6FMV+*G}&Zawd)*MQim(JpXB6%v}YRVViz zW~I~_o%*LmeP;7diB4O{-n|uYMIMjIq=?FDN{R{DNVa|MR;U1ZA`2pG;!9dAy&MG6 zBe)}!%5#;8_`+N=)(;XGa;*27>Uu=RI3WcjZ-nJcEK(|HV^RU;Ok9YqO`utBg-vpz z8}hjeDQaV*&}z{Ph$NJJrmZh0)R#%2$<<-=wff~BV9UoW2 zJMaMx+Kj(}Z&BRP<=31Mdx_TKPBa)Fsf`+EsTxL}ccUCh2(wf^2^r@|jU^-0bDF_* z?U*&+o8hx?c>Im%a4#U;rOU9N?ZNxi50?=IimcD=Tc1f=6Zoaujg$(_WkZO($c->y zoM)iyt|5!KlOp!|>b)Wx*m0rS=FXi=t~*serk4>0*|^pQ8Bw^^XgrUY=#KDciE$J+ zDOBK^YL>8TxCbW26{?;+VKGoVHMYXmRh5<3NYT$WaGsc6F^52Lz}R9p9;-2?3{-^Do6;kX`saspIgss-h$%60f+~TB z;g$C#@nQinW=dG`j8JT$Gy3jVdXQse$?a4`O!hg_7p&}PC&^~)=%M*6p3Vc$7p>wx z_lZML)h_61Jkjk{WzX~H%Fa=^C4esC6k-O6Z73(K&0M5zAPmOGPItXp1yPuQLCuU) z4fyP*+uW)se`YSxUfhT7zf4aO{h*yN&vj6x2X@MmO0Pc$zFkYIom@P8fzLO$pF5ZK zq?Q9Q=S_EmL7-j>!<49NXvNN9Klxqi1goo18K-q~;a17i_LOFkzo_mhT{C^^RYRY0 zv}A}udJvJ2O}i|-w)T;L8+r@l@>lId*iAG`bqfOvzVh;V_~x;gH|DQqgr`R@pj%`#2Ld~8<5B7%(AvxOlU6TI3vgAUN`G6PybibGg<^MlgaR-^1%L5Dm0Qi5|(*K&4{GUzuwXV1GCP&gQf4)#@M|ReB ztZ^dBbB0F?srJiom)liyiU(OGFFNfY5NmUk)vwFFKACZPyI*kG z!%@?S^qTFi`E^njEs<%k&ubSDc`LR!AMz*+$s_te9xrD1@6ahec1`!WeFm7^avgsG zM@dSoa^bqr5v6q4e%=FGORmm~L=2K+2#gFa4xxY6p(Gs>v>1B3i8I#PYm zM)c2d+9bRy<p-6X;Ytg66nI)ToUp|X_xOk?cJQOA@VRKk*5!%gBP-N|*=ax@3yhq&+_A^` z^~lp=W_kg0ij0xRlYRzHW``3I##VxlHw+h3RG~3ueh(L}YpQQ>lZ4569N9jRYEfqcH<_sh*v&~iD$8^}qRaj>Y}&;p z+Rc>(xL-l5cRKaEbg zm>o~Wm?84Hh_C!xL>b0-<-NJC#{};;7oo%k&bL|n?>Ab+kM`!b+8Rg;#sxbU)(*XP zVlwu)+>s{E25qtO)ypT?0q2DLf`<>z(d;M5@&OhR0QdXXGDQz6f)ous@Uc2@AVerf z7@zlu29FS5oV1!eX$}}5oJCd<8Q+ubFu6Anum?~F({pYHK{{ZJba8*!DES)6hc8YI zeua_*dh(uO!|pI#PErB{P0buknR@OQv%tfA&h~Ni_Q}`W7&~5l;az7UDA&MW5-*}m zB%<*{1`{NBm6TOEjD+*WLfp<*y%6L@)yXsAy%DD>#3%s@bvXv*X}GZGccOw~Bt|bT zXom9pQkxs zsACfXxg)(0I;c<`KNq1{NFxS~3|fympDk;5QxNDN^q6!DsuF%b5oIorgsTM<(eb%YJ)5pCx92? zq016$hDmySAcYBM@+23Z-UnlQCiuidJVTALXuB~!dqhrvrd0V6|ICGjqZ*=4i6IqdURH-T zdqodN_Bh0KJ4f6^$32>2&Nz5=clUxEgIj^MB*i?SlaSXq`XX)q)Q=%aY!9yVG~0Z- zTuN1-c_bPCUZ87)m_h*i)A<+(K7p+|As_JG?juD2o^v?67qd~~6 z69Yj1t&n?Q^iH9J76i&T`Lcnt5~iQYw7dYrre7&`|BmS%&9yD%R&W~c z1!&IL@!R#)8b#<{OSIR+U&qTZj4@6)XIh6r_;X(CKS1*ROoG#VDB7vBXg)mHVkWM# zZeS^6XJSrE21jcrd3YsTX)~A)W8kbFnDO%B*-S?V%e3=_v%d-s%3L#R==+}o_rx0B z{chek;v{6d_+0lcGYB$gjS50$H{^V77#EM|lOY~RUd(QMKv2C0Ll$u=p?^4)4xph6 zNW@*Q7I85!O!}X1oJ)#T&luom$MI*zBbgZ!M&#$ua9_5gySNc!PQ1ijeoINTKaZvb z&qVt}gMMIM)_S`)2P>m>;FD-?L<9P7G*xDfYqQ^ zgjw;c42&L6&w?lLlA9d^*}H&)d}en><9Hxb>=CCs(!86l(>n;H&QMT#E8Byi(~}Wb zdEm9c%+{fX%b;F=5qV(Qgy3`2W2fOsdwPs3z^QCzPmWjZO_){HV!is*T-q${%7p3 z^(AJ#-Rhy1e=2!7zGTSmkmFc{0K&XiYhwR?h2r)qIgI_tEkt+S}e zB;nB|%gbI%%p}MnbKLO1u`y`uz3yH4gg4b7j<3;5Ua682%gn%GvDxJ5F*2bm>d~-X zNuJT$K*wXEQi5v=0>uJ9}kZv2v$5hy2tzxr$J?}_Jq zl*WMxjYEqE`yk;A;aC6{*J$9>FV!@*xzeD9iBoX)i0@cBTCRVtezlrv=<<;0Vw<=> zme*=NIGNb|S*L40;F;LkYqOwYtL;zJ>fvqLT1q-HKbv1_U+Kn{UIVwscspKTl~>0W z9O#d+Y4=Go_U<*v{_xvcEZtao^6@e%u0J;JrnqAgjgbo^`U6j;-;T(Cawc|5jTD}q z+AO799xlDujAnbWd3{bl(2xxnT+(U<9Dw8abqEgpg`SI*?n`q8*z9`xTg#~-#;ymz z4WJp#?1FAyRz}kIVD^Z^TgDA@ob zLWCrO625aCb?$dwl(b6%9j?F|DDp<3VCvxx&ODTo0&E$w$j7iSqlXgo!`qee0;gl{)h zk$FA2Ne`Xv#vV9bR|=Z_jcapb;0&HE`haWmBWAEDW4P$2xpr0CGu@lkZYh=E0G`dg zwlz=|U;>`456o7J!p8mYZ87U>JFK#_K1-=O9Jl@s;C~4yC<(Xzch-RBTp;t;&_HJ1 z9hRpV_UQ}7S2zy$eofL{K=VWs@Em<$j%ySSQb1L0z?r9j#X0VMmg;nS?!7HQ^Hd-3 z9ML3P2Rpc&^wVKND z`!(MWK`A0Eq<>$mpqqF&v7wsiJ&BHMw{I+i6n~FFDIufeLDp+&$C-3&(v2tbNcY#O zrti~M3gfca&4iHzWH~PF96hfpSe&NQLcT+2wZm(Jzm+ES=lQETLn?!{oFHT6^7Cuc zNOxPbNbr)(a$6&;EnKAn^HCjBRQSHXiw~0>Ui1|rif_5f5#z zVft+nb9j=EJ~P1HE!Hn+hWPRWmH-1~xz@Lb9}()!ocVB%QQ5{`cLUk@ISju}?1N-< zvUG%5&JF#_FrLf2PZIjO4e2L%Z{=8}&FirzPsq-DYx->?$ zFU+n*N;ZPJASM*CGOt;Xp%PI`wmp}5G_5EKCwOL^R|pzs!+IG@@xP>SZCF{$Z@14N ziQBs)udN|vjLckcI;hPhNk#!v^_z1TxN2MDSfoFa{Yzsqw+8}~if)|Gx1o)Uv@M<4 zG^!QLX@|1uXh&BZeSupgfN}iY1+X2JTpUP;AJP-d7QTYN}s7`2>f#DtW28 zW0$( z%dvLg?)IbI)^TPWj4V7VUeShpp+p_VCdh|r@iL+3`cKU#Fxpw_cw6r{4+#kz2Av5BSjV|MU)bKsv8zH> zX=RqfB>Nw1CE2Ffc;%(i1jbv&^i|T773%W_i^G@$QyZi0xwU%N=L^<{%{-g6sK8!mP(t1voP?ZM>`m?YL)2^Ib38)bBeNhcW&Gu6c=Q0F zv|DT1OM~n{2=fuW{Et?aeKSo40_UzF5WE=QbA?EvX4Hxt1>m77X0e&=I<9V$C$@g% zb|FEzQ7a9~uqA1SO$hrd9KyL=)53K4$ZR6La>J?V{U{QUSt?iKjRbvFFA6R?f$DL0 z)%ND~v5*lAM6fA~YAXFrMZPfIH6&YtZB+6E=JFrJ=SroI9b`};^eK7o7p(Dy(XMxx z;ocu!6Xz@|4j0c5D;vRlS!;IR!h(xxlCDaQ6NQEMsu<8gV(4QG$Lp<6@^@^Y`|c@|~a3`#zQI?aY0lu$SZ9hC4k zybCu2|2#$3)ueHPp^fP?3=bnAyG5}lnU0YBWrLA(DSLVf4=VVC`FU=dmt;D!Q#?vSa zNQsObMb~jHJ_JQ};+ombIuT|y; z>Qw!!b;H!~{HbYXt|`NO)z4~mT-KG5@$Z-#H6-dbttum%YWp@>?95tO>s2LBm*2^1 zCm;}aKWKYjMc7GHut-A4&#+qkVB0hncGxM`HmPcWk0SYK+z`} zxyhRCE*(xSy3We0+f=y07Fu(^J6_9IiZ68ds`R5>IH*nh=gqp2FY)BEitEWnfNRI>GyVew6@sg_bUV&LlL7=A%$7$j|1wz2qyNkGvO&yP`o-o6Ul5Sh8#I<`-9X zJhEFbOj+Y`BXEeZm#7;cf6jmph)7w74jZ(e zCmms*i~T@(r%r3DdkB^0WuI*}iI;KgCS1>!pTm{V5-YZW;>YJ6I)k#Qz7AIsuY&&J zZsaRDB$&G;TO8kUvt}Hf;hF8G>D$HH2AM4FfI}MHPa?p_#f_U^Em~_DSMI7Vu~=T)d)kX=MusM)dZQ^Lq4ak_KM`ZDVg)Bn7# zs+rg6(nQnE?M^E<&C=6an}yHH_^4~}g~3E~Q=RWT_zyo|hEtvCV3VltK9I^=0G`&r z{wVaiGj^-{7~t0Tfs5Qi>QbTyZOg%hFY8BSb#&SfSota_-NykyJ(6pd(DQ~MDQ@n* z8A*mLJ=vRlZQOTZg?6J+c8hgGEX$9{*6O&;bj@oMv6_-sepQFTI8hd=uMf4qgysqV zBwY__pEZ`MsxnW1QJ_IwL3yWGXzHsjEe3;5^Z1@Jw?qn>$*Isb+>P5>+yMtS`;9Q_ z54GyDYHQl_$u#B;buZ#9EAK`MctBi5`|m^iWQjsDs>Lh|}x2 zs$t*1BW;HvvB>V{2Ylfti{PpmR{k-kUbJNOhD@F7H8XWmNDiy=ck96-a@|jMNb{En zciIxmY^8@Yl|JGXy4z|P@M)&Q(2%=%kgV=3`ZQ_WLgA&Kb-^RkU7q=HOM$nL z_2mYI4BOCL@@;PgA>9limI-P-R>CK1QSoX7}^9p}*4pMLbZX*YW}mLVmp-&_*pO= z;3h#;+`Kl{e_6~6z>Y|GSzXCYzV(q)jJ7&|?sd62dX!Ej1Q2qzdKZQndZD)q+LWpg z`u18>uky0ny|hnR0`6Sua?XP+^7{tR5jHnpc=LCh^s-N-Y`#>sg^}{ZE+?XXQ;slo ztz;E1OVpIV?l*Xkuh`#_eCO%?wL+iQAt7Q%BqN9^daB5s)X|c(Rj4r5Om??IzQK#x zE&`ctYj0*N;UI`AqS?pCNeo^9V6ydU?~rNPO?r+?`#v)N zv)I6wm&fY}z=!9ZzVbZ>!;a^J#fg_6%li7AK5(k+hLfg__Wkg3v{jAg8%`3wU$9kE zw0TXUsN1xR)zwLtih@TdZ5C8!#H(Fnir^y!GsXo@4XA=C(Yfn^ zpG{G@sc7CQ&Bv#oudr6iy!Nha#k6h}b_tkYZLB6k;AVVpV zrkN2BRlA-Tfe9a7{^pB7qQgF|O^cR!%s#8iciK15JAr52@x$BsEH1M8skDW4@!~T3 zeU!=KZf9;mDGb|R5U2DG4tun#s^)5l^nDNO`}{=Te%sx|Ljsi51$khrZ-g)~fQ?hb zJGjhfx2>0Z>K(W19gj5LProx1XmkUpPmsB=J%8{Usoy$>%!&o(y&$JT7l*uqv z1E0C_Xez!Tlc0)lw^VnSQi{q2;U8^;r*nE4`AuIH&WL_UK`6YRvV9~myCFJg{Da!(9q??99 zDnx#)yJWj_*zWC3RIc!Q#E7(1L#?o?Qz9KbIsXmv^7nn~ILsY&$eAW86pWwco z5D3rlIH%h8&5yXeA#z+AgtDd$T)0TDxg0^kE6Zv47ozH5Mq zP|E~^E`M#bh$ELo$b>tJR!BUIZHonRIKyX}lJl9(Lt&>daL-}t zuy78WLk~0UEbJvep8^0+a^t4>L4P7K*=@D!UWgT|ynZsQ?V3F&t+r;Bhg?=abHZ%! z{=iW(EAYTcg9G2^eUI4>fz`GtsQJ`&rEZ#H6o;npk|^lldb*UPu_5q$?@2zo;JwQz ztgko|E%c{GpBa?ciR&69HO_36{C6oMD_57g!nP;r-2=VH+NXY4jPza2M^V_wDJz8c zgR#nXblGWKJ8^3QoND|AbK7eHyRXm2PMHND9Y655OBpo_PJmUK)?txgD0LR34Mt8Q zl9aj7jB9LtlG}4~fwNgDFIRS6acTa=;qLrC1;q23yuzp$!jt+Y3?P&=3;k$OSd zuuls?Vh6A7&zAuD3rLbZ*#Sm^#f$h^9XcoX7|(Gr!l;}&Yx|bt&B!v^I+f+9m^)=P zsGgjS@nWK9q%22e)Cfy2F^evqJ=a-d>I@}1&d2IaM>CaTWr%EVb*j_Dq!0KkPba=) z-0G}no$PG0VXuc;2i1w|z)PO1v}7!@7t`SJ8Uae_2+qQf9oX)uYHpUZjo~PIc{a*w zArKHS&W&X152#=%ZmF2M9YR_j3~X?TfC>2D_Wly9~=oyiQyf66+! z_~5@xz!cM;U^oF6soT7AXwH%(&b26=3bS)k7(B^<16P^m;#zHNwvVqC%J&du7#U}E zzZg6=du#tgdq6`p2^OQGhudk2)MiiI*g`f@{fKRQJbT$U+OwFwujR)jm!8aP!O729 z1V-PiyFzE_%){gy@~YXkw_%{hY}u~)GS~c~hsrz`^432QH8D$RoRF4`7jf?m-sKW> z8sxq>9iDFuZAe_M8B3dw)M<^M8olnbh&2ENZH!kmyfS?V$D82A7GY&PS4#7I=)KIE z`l=;az5HbAsAf;pt4ot>jYUf}6N3rXOU6x7h^T{~pld7Ils{C>@}s@0FOkn|Olr6E z27?AQpWSq75JsQna;lIi{lN_GLDE*BcX8tU>al+(pUK0(d@CXA>}IWVPg!u54Ug%k zhZq%=T>V|>nLFDa`fNHp;hU;&SyK*d__tr44fBx!13OQa{xO8C#92JGr&NXaLyVJ` zcL^OU*8+^*j7RU5Dv!|ss}0vSc2qxFfXCof;Bjc2)BZ_kz{~!}eIo~o1MsL&TG8AY z{-wNZ7gN}oPY}MyH-~8Q{Ng1U0zh?mA}_vTLj!1LI_%^Tc=LSw_4duC1G;O+*bk9c zC|Jdc5$h>BP|n|Gu}cRpl;WBw^6a4ob52W1@iTdH0*l2?+x%vE-fELiR-(Il?+h~d z)8xGqq76qFhj?%44~8LAY#WlY`6|3^4!00|Pv%CZlB0*dnLLqJ_z=rEFPHv>Ij>57 zxO5wzIt;Ls?$5Lko&uRs93QHNujbk$G+UU6=9)?#0_{aA3;9Wtj3CejyV_(@^kQ6I zY{mI?tE_KSCgWHTX`)H2XyAGzD~bFLfm{{X_=RiKJ+kGeTNX+imnOp2&(3SF=+J6y z3Q-An;v_bVecqHs+Y=wIj=*7Jt3jM{?$ON`k~ZtYzg!-((h7<|w1_L;3Vgy?{8$53 zU$xy&68bW?frI)Ui<@{w{U^rE)@Zjc*Z2xtWWVkptTM7@&HcTozBGvtMGy zYKnJ>qGOv`f-lEX*fxK=&|aeZriYc9N^Kuzt}u&N zcqAz%S+J@R?X+eQtIPY1ZZZ9%G6m{O>qC4dxLF#GNrCKAZSxo}uYHV(rm!~6#KpeI zQlg$GwbK(;fW@J`0i(u5CT*K)Rt)7#Uur{po8$9-{^$es<#Vd(YB|rI=+Ku1o@<{4 zRzUiJM;%&br}#+}79YLQjmcYIXMVSx2IvhXcHYeTcqggO?`Vw+>}G6ALd2oUwbHYg ziC`$*vby+zDXAx_-ms4Na9Y*Pn5sx+6z|-?pwLamgA-)Zzs*#abYK%YF6XsPU~$id zy;EMfz5GHdUMSYcL;JD#-0h5cH*m0Hj?1o5Auk694J#K8P+~A!rW2XlvFNof^XA>QY#?? zDeVfCNQUK_K3Rtp)WgIFgX3#Jr3R}lzShk|-F8LoOzkga&Tm+8($;D-MazBWnEjel zjIBM@On!gl)9uf&*M0J(GB!AUVY`I7o35|o>k@AM{8M3*5-wQjC`$PUYVDR11-|=d z9NO;a2>I>jv;sxKi~=Ztm8I^}1S5QQ+`gpePs&Qc;oc7hof$giVlhN)D<-cPGtyb-HKfA($njD{92$7l?ZQ8zG8*v1&n>+e0y*#Dt&=u89aq6 zpNC43(^?vA&$`q)wv#x1QQyGqMV1Dq=O-c6ru7xv-PAKjUs5_zNkz*1gkLpwN~S~s zt|JO*6z2P6*Ue-6RZeS-EkiEjRKra~*3^>AbHZ+UG8hRowA=%H~lBPmY#G2K+iD)~o7QdLBE})<1pOI74kE zdpP`Zm{N~Lq6YNwBLfo?M}r|&*1GrkxEF^LpdMo)*-ooRi1Z&%4h2BE+e8qp(*qbv zk_l{he+?=5v%CKjL!&=di`2u606E!VXzW=`gD)Bn$5KlSX{ZEMTxUr@<4P-#P5<$m zmwTJVfToU7ew`Cm$b+8{G3;G8D{lp!I}CxZg`zg;g!^>;w`1Oqh7b#-`(`rRlNA|< zJr`^nGf2L#P@;uBZQvP_CfBG{tPr#>JFG(QR9F8V0u%E6PGk#sYiMJFff4;xFEDcJ zy``0jv8^;P^S6_^vB|#zZg;fw9hR6eeD^f9rVQE>6YVm^N~sa?b!e0!%I<@HY(vbW zr7t1nKl}tZcj6SXOxnFB&l0*hzvkaF@QFN1zN8j=yB@&oWvsTAh4;E-0zzZ}TY_Ga zYOmicU*+Z7xr>Mofm;crK#u?$?d!QWE)irc%=TM@W^!)(iHUcRH=OBJMao`$+*We&yDdnHs%p{y4I)`PTjG%5!P52mTAz&=0 z>^#uBIWlWz%LGvp4WKHeBEa|RQOuZu2aSjg^V&+eqS0y;m?~l^aLBXK9bUlH=DC|nRd#tF19t-V?A5H2bV81 zM@hnGGkXc47e2{eK7}&HOpa`x?uZFvjHh-VDcDauN{S-0M{5mdrh2C^v81B#?DB9K zY^9_|h*1fD=vo!^$<@3gW2nqyrqwC^^y%!dXyLx@C%DzYR}CCEQi8I1*f1YfezZQ4 zTth{I>gjiCZ?tLWzwXyypj5)9*t~b+!&7S{osE_ZiqD!5_i*PUO=RDQ-4AHoMWmjsD7%|B6lB-QD)c?X`YCcjhXbOP5HZ1J zTGekKY{xz;e67w`xMVG)YP@JDBvC83nqYKzzq8`)+s^B9(sZ)a%;3%B>oeeiljCBK zen8~@wSn(t`AwnL%9cMK;`Bg%;X>~5XC=-NLdg#xq38qtuUPhgzYA{QBb4B;zYja$ z8lvsr7ZK%SWr5F7jNscR%-$8nT`QhA@)b}r=7q6b zI%9LM4d5s|$cnYpC33-Xu@>yVLwm~$jfN-FNw%G;s{>uOa!_0Vtixj>Ce# zEPvDn3i1Os8esb~9{WF{#DJlx_72W2Rwk;h@83I_IXnM5I{W`y*^r#v#0V4PkQ@>t z12we_1LHVT-Cy0Q99+zt{|~B0n4}qy{;5U;P4mC5CGEVVG9rkYnD_O0ia-@c!1lKh>Oa>~RZdj& ze=i31#?kF}T3*vF?(8k@!+<3nl9hwrSG3x6GX_8%vB1UzZ2!1!nr81*O@ImZoed=% zTQJh+RdaRfZLbvPZ<4~rYRho_H{3kr40|cPjiskrQA~YAMQBKtB+ls$akiH`sPEE zjuZwCD$cMhYI`{22cI`?&0Sn+?6Q_Zu?{(@_|x`if_43Hj&!Vdh3MmwUu!dJ3k_E_o9U2D~F%HPpr);(rx-NOor zD3JKT`-?$32Vq2-Xu=LB!f%c0of6Z;iUld>zIJQ|o3@7QvXdw!qtDZ$*)aB{4qJdL zbU)t9kyctLWHx{O3328c==7ai5^+c>3NQkOGF8M;B#Gr>*8_~gYDSvD7Q*p}5%h>d z_R?y6A$fnOM;dp71*TNQaNpUOobnxHr|_azMzx7#e$}||35CGMPIUT)<8J3fg2DSu z{WWWJ9;3_j7p->q!`I|EGZC;!{IQQCD&RCnK>FjE`qsd?h z64mG|#});I$2G2~y%_E|y9lZ>B=TJZs@({7F=>F26&h(i9%hu`71LJ!`g(8vl3W+N zNtDn%NVcmG|00MH1Q^+#)Bt{r0M9;i=m3n!>VQQk(VhaSFuVEXax1M)az?aTJE#FYiYy|rQ18`o5|d>+$Htgk}p#Md0b~K6M%5o z!yzNw^l@lmBT`k9Mxt<}iKk3YzWb`gDeRVR!}7!gum&NCe>`dM?m!HuLK*|@hj@mJ zUW*@{fL>2cfpQH^awMvMl0KE}CiQr2>@;S$Q`_9sH0_0?#pZX%QW1hY|IqcxVjNFK z4v+K-f* z!=rd@)A9GV*xQ{O89j~?{8XO32|Us+oTIqHF2$XF9Q}}W!7TGgAL-4cy!&W5l3+8- z4Ss%v!K1G&)MYNV{diJu%^pz=TPYbsDCc$;PrJWd&<3S7HFH-T#C4! z3O==r>Q8Jevr`>Ut67N1a<4cN^ zjkk|}rrCZCnlUE?t*)e!6QcafIx`<*9K5|f)y9%j({GT8tju}Z))a~0$6zSK&1pyW z5wrZFou$6CAnwH>&|t}?EYcP9n)aPYLC8w1R@O9a>wl9HGTR#}(asTsy|tEwRGnl0 zB%@{}OG8@)fmv5heQZo_2lMHD%4f=e;&<-lZ(p?Ukd@CNun|IQxruk;e2CxTYB`E_FFtN*da4JZh-5X&!+8q{AvzbnfE ziUKWZ@(ZO79D4t(y90{)TY(c$CTNwAUrY^Pu9g3m2~slzlnPpQ;}7b;U%J2q7=Kd# zR)hnT0h*TUkMsY1={^wuWr6{kqze=Snk?xThMDX)m|v-rKtZ56Xnui6f%RqnoSXgo zo&pVq{p~#lng`_<@r>#p#DA>l{1yqCW8@cd4>+>j(~ns&=B8WkV=-nLO`hwN7@Xl>ME%D7gOWgv@h=jc(O*daaWTKK$)H3~SNn_D^*iE! z=W#)?pl0+J*2wrbLjN0U3Q7evk-wCc83H(KieD@oTf4797Fi;ow3x;Fz q8`%Hs=|JJ2zUvpf_pfje7p5c&1spK`h~LKrvj9HkajgG%_x}L?xUpaW literal 0 HcmV?d00001 diff --git a/simulink/stewart_platform_identification_simple.slx b/simulink/stewart_platform_identification_simple.slx new file mode 100644 index 0000000000000000000000000000000000000000..f7cbad231d0f593eb8ed872c8c1dcb328b64f75f GIT binary patch literal 28855 zcmaI7V~{3M*DYAKZQHiH?CP?Or)=BRW!tvhW!tuGd;0xmCg#5PM$C`QADNLEan@RU z?-P5kqbLIkh6V%#1O+7F qy*hdc3 z>!B1os|_Zk;hsNt8_2XsZTJONWSjg1&!>pBeZ!Vf>4h@ltCSEtd z&gX;64<8lrB@W7hmc}1|+W~u7#ZTC$U6AEIdzNMD>Uv8iFZd1eb-8j?n%@Dg#LM%M z%d?Qp7cG!WvX86)6|Iis3p>^7E^hc{Q;TUJc%ABXzaj)+JpsL!k4qt5&QOJ>k+q2D zy)!Y6XXh9@#%o>N^VJrI%}mpUJ2W|O;Hih{g0h`9VvR5K2d&)w&UXKqC1R|<=ZqFe z0y*T?X)94pPp%3*!2eCqBk@TGCY=xoSnZ0BgI=xAqe>gWtG zb^1>f`~22b2x3C}qAj|a#b_8}??qwva*R-l8gA9P#)@w#sy6=gg09#I;{kf#lcC??Z?r$uTq7isN?uLgLYRJo5kZ4M6 zHt{^bRwzu(#6~2{9SIPXrN`bvH3PnpWM8id;eaaTmmrH@9(YUGcpRG7AmmgKxFH@N-rF<*_wWu``J$pHt_zFhoYi7L^#t zfE9t=CF{n4b%D#ZIFNC~COxX*CJyo56#OO_#l$j^=YTaON*k4Jx%8L z2D|Vqr}U-k@tHu}kk9QEOb@3^%2-yB8Ok^+pTy}s08Mf#@#lbX3!;-<`o$FKtc1W! z|E)@7cuw7}e+viu2LQ$Y9{^(R|4NM$z|QtR0@aO~a_lF<64MVcoYBvtaFEnF>ww+h z4`8G)_J=cQwzFHWxqQ#@IzAm3U_6EWonw4jmxo6R#nHj7!}IAN5D!tPBWrIK0nGCW zk6rky_G@bx?1s7~ZX6EVCMS-UFS+3wd%_jOj^(LD_9d+*ef@OJ@^VSXUsZ?d`CU)| zCO8-BZ@Zl4g|lXEc4$<>f@lmIMZ>Eu%TkX*T4 z3IUW398GV{opJ%rb^FVRQA1)-MXc1!h_UfmngUh{@f|S(-c*3)HvCJbhkEENpr^Ml zPew%q)dn4%7U%7$d`UN*Bj(b-U{H;BD5OjniA}J*h)phyqpLJ=KIS!yO{B=Rp^1$ra(s3& zYU4si;NL|U9avUdbeo_J1Pcpf}cJosn6HebFrS6qB*UY-WeF97p%j$4n^=>lZe51-#1Wc@1b~_->l9&ld+qHu4^@w0trvL@KH~5Wl?i(ADp; zCNSWm+itJ1gyjQ@42^z9S^YKLq^_Rv)^>@& z9F^uJvhwYJ)bWdJ0*oSuqAF&gknunB*z{H`01PltB&5na`@61trHahvKL;VMZh~bg zu}QWgsf?j%IQOfoYwK|Dt9F86*<|N*XPQIRJYHVS&Q{z9>^W!Xk~CBr2({7^bC%iP zb9f{-o%dS?t6e1(FmJLKv>XE2)SH3KO zno$9rs?yrO2_sbu*l82BK1YM-XV zjgW%u$?LR`Xa|FYH^^=u{34{I@V=>f7e{JpEb!dKM3VdL%ok=8cI>g7BhIbF!E;`n8hHukBzsUnfA3q~CL9<3ogDYXt#=+OA+>K&a zmPG_5n66I7^V?d($q>}tUKm10IxFIqYxVn(YM;!0q&jPeVf(n#R8`drE7hIYQO!9@ zcvsy$*mZS7C`DYEy3iF$OKsW@>$*yy8yngy^@Pb5PhIZ;kox*5o12@_y(Q6KzaA(0 zVFfQxVPHc7F>B1xA&e}H7d;3Ft&jO^>(B;_>cyAt5Vyg4=ba5c{M2|kIQYV5FLq#@ zBd=2g>6G6d*BMM$r#l`NDA$7&i=P1sO+m1MjlsH5QZE9UJSEO+jngPKdJN%JND*~% z)f&FYi)?0jm-j7sc?M(BR71ig^`^Xaf9EQ!PnEVCvNDA0mBmA6>zL~_uv-7#L^@34 zCL|c%TM;URQU3}TCBem1zr%VgMAsdP>&(Bm0X-^izjmKqtg50cQiTW4$tkDK{LY}T z{{Tt($OfrizPN+#YnZTbZrQ#1*0Pz+0sV&OPKZu4l5ybD+;-mXW9Cf3Z^&)7wZU)f zW|1&Ar$mQ?kU4kT{7lbW?LPOSwjv5dnFUau)%HH*__xN}BUz4@Y=FE&MnscF|CaU!@rB#;nKD2D> z;S6igw4Amu>SM99v4@QwzOIicPS0ZKxw*Y8HZ`??*}ftAUZ|zF25O)|I967sZ&>dZ z6cm(WW8?4qktHK1vyY6NHV~;j`ckx~37L|o*uj95!!5PQSB`6IYsX*A~%D4DVWK41Y6 z%KdEz1GV#}FqxyH;4kNh%7B`5*zxK;URhBFhoon9!1;rXMP$fw=I9{xWV}>U#n*>R z!_3W`7-r>LQc^;Q(=@vq6N@C^ZEX$rbAFdw51APWu30MZj#Qy5F@(mUrC@cMYJ?L- zE3kuX_oqofho!vxe5-b5Z7n4sVZH;Mj{`dsH8D;|0Tn@s)8eTCFY(by6*~oH+oJHh zy+i9h$IEQN&vJnfB>qJ^v4KT6i+d;FF#Nb6N}ng-VP<}@nUN00ix;T z)YRRa4%U`o`A4#u3Np=SlsTyqrl3~LU*@`+ksR>I+?``+QM!I42M91ZD=Ck3Yms26 zdateSEcj&jp9ZR<*WG{>71~Dxha}k&tMshTS&CN-^9uG3xml*TvAh^(rU`hdW#myk z1UxNS{Jb(fp9mDuysB!CVrQuyFXNsrCAgWxyF8>xuACSQ;E$@ClLI{WO6Tcq?^kezsi>kpddb6FE+vZzg;)JOVc2zBXtm z4DP5agg)}K)fR;;ipVv*aHY{8eW-|VPQ+)=s1AJ(>)F~ceEFR%PTQQgtpXE4^#a$j zV9}1$Y?7a@E%5D*0_Tt*JY-@ovXu`Cz1Pi64{vX8D8GNd)ceFFn-QIg4i52BX-^hh zbi)^tB$Eu1eK%%)>kvr`F~LjE1^m3?9nuK%V8fBr!HnjEhnU>n_7wL1prT$dd>Q3q!)ayahw#C+i`t5djM>S-V&v)c}jTUB0O-nqYAt=&u16_adW*_A}^Gl;FKx`f3p@DwhrY;$|8kp)(6=mXug1<5p-^#vu zQ9cj&F!0_gjAzbs4d}y@pA(6c(DZ6S$oW*Vi77y3S9#l|6~8o-uXJOf@F+@ zv=?Y3D0{;63bGC${?*L3a`{V+W3Q<@D|sZ^y~_0-0&qW?YE3%fQ-V6w-sX>d9P1c+ zo;D55_3$@VUz(FdLXmJPE2lQ$lL69o_jRn}mW`bsMc8XVE)W9>RT$@NwJ3EGG2x_G zUDPZJ29%DZWCEd$)m@=3jhVj@hr-mNEH1`(_c-d^!_!kBXHdraWaFgE&iiXM(0?SK zaF0e*2ef?$p273_V!wJ^Lq=M9S4Au0AH}hpb^Nj{bImd2)V=hM4ssUmb%tNS9Y=~V+0*4P`gZgC!FC{URD=QzC8Qrx zAs8oj`gz$9m-}1EP(u$uWR4XnxLDZ;p|gI8su3^06D}18!#rT0J(cwke1G(2OL^5u zk?D9$2xtg*P=at8TOky@txz~E1r~7@JsIF1u(B-jv$HF#R0ERFIxDKRoy01RMI0{7 zHg#ZS|J#dm$0vnj^nsHXgsJt3o5}x|Y*LwLrF{wo@JZcyxt1e}5;}g%Vj~-Nz17)E zU14d($iLv3nd$~X1$AoxA87~ZAWqa~EBU?5DfR^WIDB z$lLWzuo7J?0l&!X26C3}UbA#t-jlYxZ$fMKd?E1EF$bG{AQ|HRUy=hP(}Ai&U0* zlr@(kqZAsBCH80ocsB)E#@+Z^h;mQfx% za>orq2ijoR4$M0%`w~WcM#lPjF7bg&-9_=R8?ZHx+s$!H(=7(h9K@F<0F=%#UW52e zPfHT?d`L~^PpJw2ae%+*^zA;;;xfjcK7%YiK2IR_n0C-cB>jjPU&hv(6wm~yV^TDr zOsj2u+V!L#TZZtWiBzgcL%VcYFeaPda*Qg+lkHf5#~$rU$W9k$Atf4EozZ3Fv@80k zQ`~gar$iRwuDR-;TT+mNj-r9b@0-XQ^NJmZ-KT|_3I zLfo^7S69zY`^rjHYJu=P!otGm;0Qt={(QgeL*|tw=p`srnP6vfgD^*axsERN|2Ez} zV}&y0g5R$*^r-_~8DYnah?@#qdaH2+qtz-*6&oBBhbbl*G5_rV;M zCBxB(zu6g}NHwkNOLrOpdM0!!6Q(%DWsmZ-H?J*Hw)qfdxW8>0KS^0@IZB%k$RKEZ z?`R@5I1k*I#I%w^i$4A5yP2({A+bM#{q$bzASI{&Mp#wZ>EU)ZGa?0TC>K#16>Y6A z$X_=(o`Waa1=IV>dDA;~Al_D9uDH1P^q}NVzm@Iq3mtf+yqn}|T6EPK`PuR3#%=u@ zB57}2K!)4fL{+H_%EfwyP1wo8hUleX6w z&BV&QbI0d<)J3FNcwD8h-_u_Ft4qZ1%e0BXE}awd%@v=Tjyq|^K41vu8ki~4jv`#U zig47f&gY3bv*hi0zryv0GcSB&$E-j?bmZHdh7@$3dF+Den+Wuj9=2*-e$uW8^}EoX zZOxS_-29o{1o~y!>m0!Q#-m7gG8K*NH+GB2}GFm zPV9_jw48{$rl!V|BM#s4=4?LyqpLDjW89LZlJY)x=T<3aEvfi-AG~7CfQ8&a_`{w7 zD1ulOkR~5qIiC*sd5G+L@^skbaw#?e0YrGy2=rJ1&W`fWNn>HrUvE6c>(Lkhp#$de zY=bHH0-6gdsv5Pf;qOiUJDb3V-y7?DRObXsoeeCZadAjcwqf{E0iG|}kW1>$VCg6viJ#febnAN{RIJJz|*1H(Caw zJuZ$4aTXUGDMQz8IwZUZ!zTI`3m;yJRmNi&bm=13f&Y5qv!c1`!)=r>{!*xAAaQO_ zeBYU2$86$FG)k}W&d6ieX{0geD0m5+XzG4ks=mn;jf7(GsUeWt81@l#yO1F@6QqEx z?f9dWbPiOGB6n~{RUSui$hn{nwk`@zU`i9}|6bfgtQ>qQh|Bn(QfNx2goFq)NK$HC z{8b-N+D|4?Y=5|TGh_&qJz<1B_!hlaRaiR)3hLRhc}v3Jv9q65FCFZ;VKFhsAs}cc z`Ue5YK7D~Xz~J)<8%W51GNpI}?nDn6|9L~F@E*OBCWT9I%68vc*@w%|;R)yz#AyA_0B=FtNXZ#2PiqMJG(WtBj#YpGheqgZuneC_#Js{r)s!m;!2{f@;} zJ=PQ^an)xV`@2L}~{~GALAI18Yil<(~z*O*s)wWv4sU7+NEhn>f-So3W zO27!NwR ztn=XVNg4U4#!q?(it^Ih-zV!S^!c0If!?)Gm!Kf4z$w|!&c)JgUraC_`WkhLLp51f zPRCQ>`NW`cLXnY?$%$_#Eg(P+f$y6Q1REo+4+E_8x-*W{G{@i{5QCn#HbYM5@ahS3U#5KbblC zI&w%>_42=)QtU{i?HE(j|Mh12ESktx7mHXc)Q!=%6&s)#&Us~a6f-gwHKHiE3f9Yb z1TVJUjD;1hDTRHn*i*FFfGel}HBxfEI0jfEZ1+Z?8;@vj8Gdoctvb$#>0E%eKj%1}@pH^`-qS;ggu3XSiZ zLFf#ZohVyEaOPjVc2^tvc~e&!ZUjgb3!^;YlQS|*V%l$zHFfDB(NMrnN|V-XW`tr^ z_7VNOnd%#Xk#fidc3&Evm?7OEl3+*~zjARAnRF7V+XfTMGCpex4lup_=A7a-q2l9G zVgyELAOfnW4Qqu;C@2xCaNVeRU3*;?|CQlyd%qEppIpKO*v0i5GV+r37=#y7G`#fT z@BOE7;8{<()?5!9Lce%s*NP}}g<$%QDJ|kvlglG3g*~;ALwic*r+{L|`HJ94p{?bn}zab`g&OXi$1NxY!{2s;z@b+u?0OL8f^xKC@ zPG5U3b@InW#kGBsJTo+gecjqY^DScMEoEmWShB2}kYE2yij6^KKk~G5C>y_Y?a{O> zc9;?dMvhs#RHvo>;x-E_yJ~kzNlAMQZ8~OCIM>^1ar&s!0D1nA8IZBA{ew@dn}mdf zpT5s*W0vpgYwMJ-hUVV}i$d=vxMZc&gue5^+>R-fN#Jiy z%fW^{dFj<^%_q~Tu0M{Qit%7pHl_uDO#aeOYCFrlyw(fDkMovu%7C+wplEm{`UyDn zdGny$C#J2W7S^(8_~cM2_$L)~GV9#N8w>=edPSrDRnF& zqb=FOZQSne0)k6e*kzM8Kjv@NVTF(ohoB4!jH;dh?Oxn)=juG4->7hrQsv*CnD5Ga(Mk{BB13Pc7ra&q4pSG0F=Q zJ9W!DC)j7x($k`}635;T_<#5R>m-GPJ^#%m+5QcWvH$h|od7m2)&N^8MrC6QQxg|! z(_jB)l>eD}G^>s!WHKRj&8RDGnpPXcH>6jXG~oT_o^;GXDKMpKL{CUe_x1^9Zn@)1 z#!CQ&aB0=5-ML6nZ)J2r_YT`mv1vp&3k(Kk>zJA_YedK;5G2OlF5#rrZol?esI1{a zEHRv=?o!2dKbPr5fIaDO!_w9js>Wz`9Exq(Sr4(b(!mXU(LBf5VHHWd^mrxD5=&x5 zDd7~Z6Ess=@%@DuiD+}MI|P2+9OU9Yg$~iFn_l}X0ZR^us=xpXCBcM>beyQADD+q~ zBlz?S_&dCYaiiP0*3A@g{Y_+^@u=jrrIPiuhvq`<)n>2_^?ttjD@Ylk#_-+UQK2nO z3dJ~FRC*jMf-EME1)2!M0a{$x=kajZ?U!7 zOU2a-e$614Xe^5r6*dBnDkk`j&NRxQh_A$5s+b5zKJKw68Hf;dKt?^e!?|23`N@=E zMOmoOQu2mRs&ibXL0_&$Di@8JCPl9IBm<+ct8*M$HQKBHd(SDcqk`p8y+PhO6HI-x@?vi(}N^+;*_pjA(I;R zS7^}f$dSk7&-_6x8+Im}Lix?Ox7p?dD^MCOyShwVD@k~T}h2z)Z1bDp327=@3s$uy2DI&;VEdr{?_v7?Ay_dJfU}+Ben2UgGu6NPBuw-UE~9%)0ZGQPzCt>H;Q_?LEtEC_g8Ja>*g*kU(U|1SR*S( zNZfogI>ss-&wj_Y#ZapjH##;u-}rh9Z+1yDV90ITUPn)_SLB#0udMld`X{$nHkzH) zY`8ukABb1W0qX%@y{oGVvgK63=oEqJo7Yw3x1GW6-3a7^TwigPCJjz&ciPd1Z+q+PE%k@s1H}f zDIpw}?+W_i@dc7Pr_I|y3MvQPckcQT@NVSwbodqnOOcGjH zh-sE?ne-k1-YT)5!w=yb>K0tUHQHwwxHksp;T7gx*M6|J4G6Ue*+`UWFpWItgRCX7 z3>sDvYg8pRQA1sIBgl(Uuf$8~f^MamdIu1G>{#7c(Xt&^CtrPFL|B`jU3HXq<%Zi4 zieYxdQ>Lo7GzK~EhvBXHOJ>v6OxmG8)7a4+v9W0;tx%EOvJ~OTJq9jMeqP1;9Z5Ki7%`%Jt!tL_5Rlr#_WA?*ROB}|=XJ~0 z6;%G$8unQiY(`H!LGZr?UlZ*p67_FHEer_+ME1XC=>Oa2V`Ob-Y-Iv4GRl9Cf;COP4 zn=p}Sqtla7_8uaKZ9NI!wSx|rRtb<8CNA6&X(vqH8~YA?OQ7>dDvP zl{P|^p`Q3 zP8q*>b_074`n$Q>&S&NYN%jmhc}Xt&eZlh9#_}ITyj`x{k|lS@|6X=?nEo>Vzl>c+ z1O)WYZSenXGmK13Ow9~kteyYI{&c0aV@K1D?z_`t#D=J<&CWSik=#OWC??V-j0*y@ z8QE01G(L$3VHd^U%rEp@d&B)KL}dKSaiiw?*9~auowtC1fS_PzQ}+&?SObVnTY~!8 zQCK7!aC&*x#6C^+<>zJRfvf@Zec$Rmbq-6Wvbk$d;W58n_BWDb_O`6 z%7X{0WGCi|RyF+T%;qgN=B7XoFyB$TZqbz;BLhgKIgZdaRlf7O9%CE)>#js`3V(`b zpiXFTR8k8A#v~;ur==#ztFTQM?xLMWe_!RY2dmlnD~yDa0)UEbtxC<8LgRT+=M+Z* z(LP?zwV7rFHl@-<)c75552tRhS8n6?89Az)hJuvL&ipfeIJ!noJ(I~R z0A?iWgtLh-GkTDsfrng@b$8bRw=+{>%4snj!auGy-&pdhf5Ejan{4`oar|95m(-N` z!}TN&_{P1Ciip__c}&4zYMn9_45HY$2Cb&~75biiaCiX?RHd(=dS=%v3f5D%Wgw*59q?JRt*~kqTsEPCG@Gh1BLn~H zfn~B6XSwABJ=cH7@bmfAbCI+BX0@aRYUT<^h!UQxcYWYCXAN+)uLaO|xT?(0BwYep zVM>a;@KP@I(fK=n77ptZd@dcaJ|3Tu9jBJnsS{=S4stfpQWvWQ%z!+O~Dvq zZp}R25CR#+#=+`U%YtdBPOX;4wpPfXerCkUETsXD$X_C#5SZFMJY@97<=xRYX51bv&>r0Gju<0U6m3*DSXNU6&;bOJVOT-fZu zI09EoUaoIUmuV)cUFRgrI?6kz&C3x8TkDVHIpY}KRQhK?^7yAy=RvRt&ZmYXh4_&2 z)cLc_^2JwQuyXYt3zUNgp9s1N!B7+%QG|A%TK$++;3==oWRR|gT4u?|ESaeo-)n6? z`-Rqla=3I_Fw$iQi$>k-8n5Fw!jPxMLx~ zM~HCcjIhw@NC|;oVa1U!pQ!dUnmAC9#9Ote-lWf$H738{D5N7F;nD z-u#ZU6gUc)_0q`iROduPgo53?WB`emvEJpQp9I$xX&o!;9>Br+Crp>ol01~`@Vy-t zO9@Kc3yZJL<4wt_P0B}F*ucuUTI}HglI5X~9*`wCs)v)qk z32PmWZT|2A62rdkl+KIn`U>Y`&*I$g6z&g+c^{cu@>YTH{XXRsQCVF%VJTbbCg`0? z7a(s8VN^|AZM(VK{a`9$H=>Zk~GTcbK1HJCH-v*miV2ke?yR6b%au?hLXb0SfH zot)3GPsrFAl`6BI{&!=bq1DbbH1K!Krm&xt&)Ul49cg7iS@s&I z3)7(TO7g_sUxtWnZx_Q`$UZLmGzS0IKfJJ|SK#5h@fSjlbXX9{^%|$i8ipRX!(8!5 zj5OW}X=f<)MMJo=YZ$tAtQsFLh`IQDzDA6A9AJM-m!cuQ_D<#ATtsHuGTyYWOUT^r zB>*qh5Q|l3_M$VQbV0%ho?~+Od(L4jOCKxAweim}#(T?afi<$(tu*>;JS3}TlsH%= zMk5vEGv32un?Yhlq<_4M#|=|dwLo3NxYn=$s@t|ig&>HyoyF`+lZ&nqL!K@Ee^F+c zJ0n3Nxv_(Jq)H#29ax@baZ{7BC4ZAtrAocWO!eZ8H~^OhXp3LFuVk4rF%Zja&<;K5 zpB}7cQMX1iCq>B%s{|Z`m5(|5i~C2J{l-yXf^iL*)_23xgC3zsY^5P#xyh0_x3Qy} z#GKmJL-UVTOooDrT}vU!txj z^-rft4MJQ(QJsWB%8J$u{c`u(?ot;ExcF;h=*NjSqi>dT+DeofKdoAic6mjkx$cW4 zuqoa_BN4X38P~nFWh)I|qk^A~touwVnP*Q3pla@4H%i*B|1mznX{yjdUEo_hbGI=9 zXLw@it3}EEojMk)r7Ag7K4cw|$0=e|V2vpk%d2Auw9L79_mGj^KiR$yWy0m89{Rqj zgiQP7R21_2%27R0G~PCClm_{gnX_y$Tng#Ns8%JT&6!flEnas3RDE${2TlKHsPBI_ zUAxcO`fdGlG%8m8Z;g%reZzdE>*KV+k@zE+D^j@W^1iuPFWJRelM_$hd6#ASK}T+z zc;;b&lHi^X#ZPvWy1nJr11@xjZkKdaG!tEt9YyyA#DLKmsSDJ}_x`akU&)_G!M#98R6||B;iK2LLpl0z-W=a4>dW`X zk2P_5Hs^Di;8|u1;{F0O&!V9lF?RrpJ{Do@=J)G*?B?h%c>+&oN$5xkpV3e?gSB$I zEiVg;4Jx?O*HxG~dW7y!+6nO|;AiC$a=z}@D=PXmZ}-ghq$;*9uKZn^ABRppK!lbMrN$Qkz78%Ye&@aSQ;@d@3nfA<3UdgCDy9M) z(SOytstWYf{rXKotyFc`k7!a;BCV`Ut{djT3A_-YOp91?m zYkDyIBgM^di&SBgX2XrIW6B$ze+8zz{GrQy4s|RENjwJv_@J#;WLqmhp+o_a*O_1w zX{4g^a86<;`mhWcCV-kVMNoYTA@u2OytEJn*;C-f!9PZK1ilxmpAMukc|iae7-Fns zq7jWt%+NVnSCFSQ-5V->==cpHWK|oLB?(@j{S+D85vvZBTRk8Mgg803s<-(Q3Jt>y zHiwq*Wmp%xFL3ug5xpVgb%D}kAzo+&*|Pvo@f-!|fl~GGue3}WQVspg7i#v0L5pD> z=Ylb|uQj;-3N7;f>cF%5ef)VsCrW}$z!^zCIAnP~}CuM`Ji#1IvSM#Ee(7=Hc7W(;K+(Q1%iehEwn>EX069ndj^s z@>VS($zQDmQ#LF3r|)6ij4BsNOhDQ>2O`k)Ge1m?St3-t{lqTHwRWX_K;glYHAFx3 zJ3?jL2O3rmSzzX_i_m`CoxLYBpM3W*I~j7`2${1bxD#6T+=#~Egu!9y9CZ&0cAP;SH586Dc zG1iE_)NLyC^^}gi!VrP=TBpjl)Q6jj!@l0Attf}tPMkRVYfVn<$j{L>G*b)^^j@|}W#4Gr)E2*F8T}=zNh>1FEK7P; zYzz5l--Q>+Fa2*tMdj6LF0{3m!Uf7%*IY3#JP9(A9YXE+E&~kN(|ScB;#01iIrr?6oOuZZ1<$ z%}=yw0fU+Lj44qwwgujTyHu2xyRnO)UibhQ@NW+>X!qjC{ zK0oN5Ac3Fg9+W=>+;HeK!TE~@U7}7%KPNcTcKQ~4{)5>YQPADmB1gZ{_zj{CRWi#5+vwW!mJ(q28g3qB`**A3Wz*#@2V#rrZM1N0Y5%;nVuP9qd z%IQS6wNh0DQ0ipv?-E*tM_8g3ENtcUEvW5-_~*YFeX3X;{&0Tp!XC3D9Oz~-ijM&8 z3P2Yn}1gaX5W~4~E1aZ4{fbVG4q*#@2v7?iNTKt8;@3 z(?p!96=*4uwIa#nTRh{4@HByiGm4%F#9u~(Wdl#3Dlt3du>0N1rQHbFSIL>}P}Tq8 zd^6LABk^(@tKp6x&rmox*=IsHgE}O|%|CiC&%yfrIv^RkjD|=*7aU{4qTH;Zl1S*9~=pFiotb2^-r^GoFd=E;cB`7RnfAKmv*Mr(d!b}=$5t4ZoBoPhH`#|c z_8VVY^zelZW}I7-(_>kDwAiQ1Gj1CSI2p|t3Sa|#YLA^q*7vCQ`qucXA|y^7v>cdJ zvqO&eW{{d1Oyv)+PjmhbGXmx&IcL1g{2V>pNs1RTur_D!aq0s$s|rmVj z21zSz7|<5&22Y-%QF-pcWSodB4VE5w)tjXI;AUaz$uvp%g2Ybm#E=UaR8@a4t3@>7 zXliTE{%E|bMy#P+dwr4yFNU=wey>-u^XcHvcwtQ;h%gw=3dGubt z`J)s%dUvOEy481o)IRrM7)ox3;(GeVt<8S)&XvrcIo6UBt@l?mYv{FPl|uAE=`lR0 z)@?MmQ=r4?bfp1-=&uaN;dzj%`^h^J@Titfo@P=-28!>&G$uA*338~jB(WCnD-MbC zWm_>W4*9F5(7xA>>lpS`YQw2nUg|3R7{mqK(Gi+(vC&igA^b+dm6Jdxd_Ouxj zt1Oj=cGnSb2La#u)5)(L?7lEadmqA419f0udwl9QTxv5PYiIT4{j5pKphj(L6=zaU z#uU^ZlacG)a6Lg26YjUdC@i|iJy&b?g|fd79cGPN(Nkuv<)$1TUXAUXnO=?QVK-C9 z`h(UP5o?#-HEWkPf2X=rS^?5@d)wC6Q^(uZFhpLBZ}gL9tjw8RE9{x##s{`IB`G}s z=@`7{P$xUCmr!M(HQZ@CLoV%zz0014J=qpc_^vOx#=lEloLOE2nxQxF<^&C#Sv&pq zEu5i^JMN|f>DKl*p1fIRZ?`u8`9UMx4(>Fci5*ufXV%KWL*>O~^1>h=4$h05_TjmH z&zo}fk~worN_L`7d{I)guKckt04LluFGCUNE>tdo$?7nd%PyL{bC#JZJXsORpTfl} zyEOoQ>x)h}MpDGSb;a{S$z)DYfQKw8`BYVr;;zAXJj1S!<&&;`9cN+5>mp@rCx&Va zCWTabs>&6-pd~|dEm>dOQpU~fBNV7z#*=)d$=I_qnY^A$cSEj2G}|eWp%Z)Rb=Cb! zDpiLkM(Rq{%t$G&#kk<`w_;2LN^|Vl`CkrHa>%A%HrCJLg19e&@Ahl!dH@pIc;pzMc`5D$2QWMF)=xK0XG@ z&wY&I+KUX|T=Y$<6z6&7=VTMxT!~l%KLQ-iS<7k9IA0?H;qBP{=+cv*lK%i9uON1N zAW4OA63I{7xOUs-;1{b~w^V?65UV!5=DO}L^k=6v!??7bUu%X)wQUP(_l2apVJtr( zgl8k|jD&zH#QF`x{s>NPhS3fqYU3DHzicXRrn7k$zH zoTlyE)^P8^8uL4;;_%<7W<T+hm zd@PHOSk!-@Wmuag59chpIEI@%FTvi3 zw>O_`n{i>LI2tXhOV_R_+HHRIZgI#lTvnoWZtQ_RLpO`Xm^yWJlHZUwp4!4XxoT@CtIo&NIef4x`Kx(j=g|eVhLdW{?`{}UPVzw|yKGi-m z2mi5Y8Ud#^S0!OjYN;`(I^%ismq9aDPh5CmDU>(Jd&t{nwT!l03g?fBxr{7j0Zu zuWiP-^H{1zfc!p;^Haj4{?GIKHpHb(?>2Qzu7y7DpKo@$MEx1Ot4PG5DL_4YEf+B?px*ev! za;-T?$5-3DS&y(llPSB>f}HT!-53ImO@h~^L-7>6a{s{L0q!s^zqVrYlAr@%in&3O z-SPKt0R~Q~(YOl1IxzSzk2i2;kkdqMPhu7!}DS_SRJbYtlv z$>7||Wes@dFiR%XLzWFmMlPB>G(BA+y{a~cv)c4j#xDO)Em=ZW5B#6&*arOA@)tXH z<(RUZ>$XM=h2OWc3?tVbsgyt*!TqMX7km_ZRD|W!hc4!;CMFcz{0ic3JV#oc6t3(x z>;Ki+SpdbAZEYKZy95bNaCZqF+}$m>yE{RHyIXK~w?KddcXxMfBm{@Q=iWP$nVz{{ z{omJB`&4yTou}X4=X9U5R`2z$e0uSFlACt^A5KtdFv+Uhn(RfYTj{8eDRdDlr(eac z7qe1aPC4&)a2MV3l$t$R3+o;_+)K4P(v&RNInvLb(C2IW(O0r*Yj&&rj2F}tLLp#W6j(BC*)gF?n|w}=RHH0jU>YRD1D^2t zGDV9A2T|&do6aeWb=1gX-I7x>>UG!JZ?YGq`plZHr;5mVScmSs!66!0vQ)ToiWaA& zzOc$B6mVPY)lv^wa<-QR$zSq9>Hgq5`+@u}Rp@gEKXO-y4s4}`l&oIuOy9W0`TDMc zF{T6_BW@L(iWMN>OAu^XBMzfdnYqBva6>h!9*l1f1pSPlq=o9Ds1;h{p ziB+3qIkm^~Bn^foTWuL>&Dy99_9hqW=IOZ+E4dk4g93AEeq%Oe!Bbc%$Gb3JzN3BM z%N7qkw2)vxhFiDy>8KIHk5r|TGT1>Y)qq=M@5z7NyCF2MsU37hr7!xrKt>ue;wZi0 z81c;5Uw7(y_i**%d}rM*gfqW=ItII#)9u@2@#gN}VU!r9N-`StZAok#8oyI^tW&^U z$X;oxhJ4Bb%r$WWS?M(RyB^0+=yOB{o~%FiVAVqIyR^hLO6-Xxe{{6GTUc?(`;zuK zMjjwH9<(j=`5HfZ5*=o|-^wgn0%u(8P<6Sq3JgyAU{$WnN$T1ILMdfNW_J>vq^(De z1Yg66O>FX2U@MW_8H2pMcrmGo@YAGmdQHfR4#CGvnXBX1H(+g^k{C6FG3PBy>@vu$ z{LPEDb}q)zlDTcnrI%HksSUW0%j7AsUVQPm&<77Z3g~SS8Bx{D8VoBm4O$02; z1J{PA4Q20T_uS_n}Du2NL%qB8CE zmuYIdB1DEx*VJ$Nf^Y>d3mu@7PRP8TmAAuiJ`PyZA~s4#a(li_gU7}zKyR~?4z>ym-LOVo|Rv|D%`c$Iy9e4 zXz|OLvBox2k~68DkmdF=UW_g#aC#mh>l5+gH$+W!dV3>lM_3pIMaxl6^IXbj4JWr~ zmDG$lhoNh+1t=O~jK>d)HyFlKa8i1QZxOun&0XJmeW~rZET^PhFzn1)-&;MWYbxvI z#wtW6tT5H_g~vLc{RfnCZh@$?~7_Dz=W&R za$kF#yke>{DrS;5VFAqi=$6TF-F?zS#n41&S6C>{PNjGeEq9C3}kiJ!`E!D{y+EMloOn#bE2R0{;l^cSyE^M&h_cahe< z6OH+7IV+AEpYOeZ{qz>+rsEuVmU%II9C*vUT^6%D1#Td+637w(;K^lECKW=b5n{2lnULg(> zYk+Xs(L6|^>2fsl)(-IWbhDj5X~&xpQ=CxDa}>6Mb5S63?CyB$yuZ4fLUQ)#^U~fy zq&IY3JTq3}n7=!@g_NXs7@K{z!OB~{UDrj*_HbJ;72qmHS=_un+ILmV1Hg_*cv)M` zO1bftk&m{xc>d=6^Y~FRjSxV{+3Hmorsw&(UBIeTiO}bpS@jwZyUk1cq&eWuxi04- z$Sl9N|25*))(cPmuA@%&nS|As%C;~PKDgyXwC{=$#x9jCqGgHdve$hd?D6FLI#TXD zyuMcG^4KLr>tc_DUdnFf2Q}Nw%N> zxWJqieWNN*yH(}Owt4zt$&F7<>ey|$6Dm{R%$w-kjlg7MG%iYNLxovsO$qhuLwsHy z?sVTA@9!NDkT+MP>En#t_x>oQ-a21h))HiUi}5oRBB`4haM3jDi4hs`Ud#Uc!k=h& zKx5USVH$J5qO?Fm0KFS{-W@-*lh5odt(!(uSQjrUbnFmr*D<=G($U>;RXH+#;a=V5+27gJ(Vh1oBtmUon7pHrLq+Fm=H9!^)G#sA2p za249tYqGz_0lRsgq4NVnRTA4m?BJByZ6IVDm;z)jEZQWskxV$$3n3rCmKWe1^-2Bu z@ocFP0mhqyM~Co=-I7n78ww7+OHxec-jbp$a9XlyFr-3gWy3k!jl+6>f4p*)&pk${ zr7Ca#9y6AP3loCGNU8clGM|4gT?9$-{s^M=@gTu%Cm|4lT?#n!e< zXe|RbWx1e!SY%CI-2k+PNPNSkh@z#B1t;P{$#&%pcANu6M zr!U9rUhTs3^Zc{zI;EU1_4q{eeF=b%fVi>ihPkeOMnVlE4BGtlks``$Tqg@OBalShTK*c>{T>E~fD`T67kgttFk zq;?*3B9>8Mr@m5dR!_=%~hl=WX9(vckXC zHVHMCwxQ5XU5ski6kZYq9b8YFnlw5Hp6@lmOB=j@6@~Tn&3FslnSL_80y}YCgSgtc zwSxa1MP%jLQdii{1f5%;*J%6XirH}QkGUvvD;Y((@IEjW>5eWNwQEN%b%0}y??7&Q zEnpA!eDsWoAJX9?U%P~Uv%ol5rEwkBTMUKHg7ksNDWtb$&eUUS+n;3i9i8E=R!cdT zzCJWhhP(N`VO)zDduz-l5ZpMq?rP`rZj?-oDo+Z20hG zwfqz(;YT+Pf?HjeYh-Bzg-XE$zqHzj>~XikK|AbnvruVr4;*`UK`k(AY|aet5~5OKb(R9OJ_mlTUk0jG9&vCGumh zbd|-J?W|D;?FW&$7NA)5FJ4$wKd-hoGE>XEXJ||&5E~7$RdWl(d z_UO6(JgQ1xqUCg=+H^c!DN=^S#;R4F5hi)aYkoHVCG$pWFTt&ZnX{j#4vpI(0SU&d#Q=}XWXhmX{5UOh5p zP7-~$D47Pcds-MgLH`E6GTqsw+Q8%hPbHN1AxbYY&fDN-{`?IkNLcfPee^j5^AR;C1XWg`vZ5m_?;heUmTAvwg)%GuGS4CO^53= z#?JI%d(C1$0D?BhDjFb+AHs3Rd9a087%mjjJs!SYeV+WPAzr=wWbB|~OY}{fI@c15 zhH^Rv6YLu)*V{rQExZJ6YvHE+!D{A}_O9MUUXxLYz0#i;)M)wa#*+guy3ALTg^U>w zCh!m9*8JZV$1i?7_U+~~x_>a;PWXKObG>t4QDBA*m+{tJgpyLG{x0;~jcxz+Oa=lW zLDl!qlXh%)H(#Fh@=*Z&yHDo+F@!9{pSf$#C=2fg878jo5;|6|`5C+zjt!S8PtXBt z4cAsSlq)U3ZSWd!J2=K^`=r(HX}faY$bo7H+{%?!G0X%fs$_>sckrmf081JEjPv2CkeS8t zp(=PPE=})e3KP*?(#S%fJxQb?KdF=A2RdU{8%+pbj>(FwI>EL|`$T0ij0TY;8pR3+ zZbY)YmEFacsRA3jbcwo0G5>VKOkw5RMA(|_v<^XwUTal|Mz|X%wrSu^P!?@Ve6%(U zkBP1F>Wp)rcCL`5SsUTy>V$mc`?1O!I5kIuajwmpbppQuHEc7ub@ZJPZ;h_%u5b<3= zCpl<~tfpHvf#=}14~xU4R1b8iagi!Y)YJ>Np5%(G77>_7au}-KFDR9TN^!(F(!*oA zy7Cr$;!7>TLktx(BF8sb%)(K5D}uh*)5@xt5EygZ zs+%#Dk;}*pUBRGU8;u1gNM(GVt}f}oCUjWNYa7Snnhi5lT)nyaLLyoy(#TEYSA5}m z&a@Xe&@ubYrco|0=M6emEAEa0PDR+)ho3B(yTj(;7XbmA@C~1D7YL1M=Cb7I)Zp(-S2{)H$j_+vk+oXMP69YJvP(g?{I_pbp=Z#d#K^YEb z*ZJ~+)xNy3bTZ6?6J0KR%tD0DaM!j?Lb%F4gQ}9a0h^U^!I~Kgj>4Db5^h+%?Gl8+ zf&)wLIkifb3(p~O!MY;kgcV2CI%U2K^fyeZc|;yoQ_373x8~OngFStBVow!m@I(qKdQrN6stxpzt0)Qb_@xWd>^?((s#d)%&>3c6%MLLfgh>f@qJ?zC z@3r(=?u!fA#{0BN$Uq94LWQ?OGEJW>Lkj9);)B8Q z)S%LWRTg2jv(R>2&^pulN}2K-Rvk4p+Kkb2pE+i}<`iRVPBxR>AA5KE)9-hmeyNNN z&Y0gRq3WjXt@yfxlRx)V*rb31Ryu-O{*g+vr9_VR{`n0}cXWj8&U1Q!JYi-56u`n< z`)Qm3Av^9s+~by_Qef!Y#}6L2LP+fdN$z%TPdxk&jk75VpM6aZp546K1v(QSUO?P9 zA)p2(`GBkMOMpd0e_4wD$J%?Q-vZzyC0Xfj?@>E$)akqu(_`ig5hTNl#B00F`7q*a zSdgr4NJnhHKYMXTVOZoJm|(fCWq8<`=G}#0><~=YR~q7af|KXK>pH6THqzZXF`t;BhjKyjQhFfNCq55LpsVpIAtUb?ZO@s-JoT3HmELm60>(L0y|q_#-i zP#UA8z&))N!RrlVWY$D_B|Kl$l!`%T7QFZiV~L(){hh5@cM(tLown{*-0Z=$*Oo18 zP~~12E)zVuK)WY;*7u*;&tyjL@r@Bzt-w>{-d&>mW{)6|EGM|)2u!HUu-QcQ`>ftW zzopW&hb!X=+#uoA9!dazjENioL=UfKzMLNLgUq;*0D7l8aM6DjaNz!V@&6w){(H59 z?r1G?4-+EfM2DV%M=>>?a6CM7Eit5?0#tFGIX<-u4SzPB-*->9HnVMHU&6ggSNbO8iWFGZgvTaTy`#_*Z3wl@~ z&?il-Qz=;>YF>3%gx)Ey{WVPz+y;TFu}lx{;U^RV`Fb_VPs$}33Oj}G&L~# zS7qd`hOXTb6Nb;ex(3j0FEO%GB~K;s6H;X&6}YO8kgX%QRhXJ=yusOUAsu*MB<+3sa__gNygM*&X-SYxVojK{o7<~D@@BIcD zQX9f2!W}^hqk_%v0uZ)2qILR=GvwZ+^ps@J$rQs*MD0QcVGD$|#-g?|K~_bB!)uKB zkDDcIVt(3nuL;7v=0pKk$IJK}RGlil6QqHR6XQz<4Sm<;SLY8#^9K7a+VmoM7>QtH zR&Q54FlGz3MCMblLDlh54Oow=tmu{(alyCHI?nTf&@F{Pr*|3gU;%0}KBWOgPI!by zJ#Kz*u|HL^I$+P?A~?w7H)6d5F;o$cz++&0l_DJ8v1zi^nr-wCT2yHU!JUp$>d!o= z5E%FfYA@82DPL|EV_~5-iv~Uzk#wgphai>sRv)vk=yrwmAN$X?n2lqI1(%5mxyRSf z;_HM?>{cpbPZE=%m_|7C1_Hga`1**9P)+56rds3Eg%)QFizzLr`@g<^IqXTqz)Z-K zE%@-TRFbVqJ8o7-wboF*^2tk}rR|VIfbqq@;Zn~M5ef}ivi^Tb`oy_h}Mla~Tx#TViASWJIVz%e81 z=A*Mb1znk`K%;+&lUo;&k(5Z{0uxz&>UKfcf@!ZK&QcxStd%Nq-W%>UNy4Jlz_WlO z&p`}@5OVmq6pqev9qr>B6HFbrmt1dMxQ2>t*soRhnsb6b1gop`)NkpIfs4A-=c~&- zl3h?&$IFlBlX!-g)VC8pl2NReM8t?hmr4*a2em$G4v9SFVoKu{H7v&=(H^I z@#{nB?A`cMlDA6=j3t}IdYl?zc-^O5Q!Ku;XAm3N)iCj*L*SQ$3;2_Y|K~4@4KSx@ z{nw~!zHdz(b{maFMfFsa7-E)w!^p--sJ8 zeMfaJg25W=o=b?Fs9f-wkro60RD#i%<4*#Eac1m7&t=>0?G`^%afxfkYj6U%c@Nfg z@PW3UMV}hjY%GIwS**BZo#{%ZJmj1d_e9H4SWv`f^btXVpF)BSKKR6=YtV%;KIo+sG z*SuZlFfu$MO9x&CH$W{rK2pD}1oUTDIF}xgZB!LXP_7#uub)H#vz+RmfC9+{XAzb^ zY61oM0tYp4{82~yyC@N0O`)xwle2}9vWvaFqluH#zpHBhzm*Nj$czs&G7QQfGtg5} zNzpTmG1mQAUr*8dK?Z?wF#)g=U7~hh5PfW)$Imo?y1annH+SB@>65WDvoI30HL&*Z zG;#djPu#_z{J4zki1bKlg%myf-)FCfq}n(o+bo|5Y^EYcl@mlAWTX|M0#KC^a1j5K zIypOM6Q}=!s$oV+dgQ;W5kgo0&#|PL6IVn6Q4?cdpC=Dgp${B?xnTW%ER|)1h5z?r z;C?!|{*xA1+Qr@d#RC|yq$ARD(DRB$lXm(8P)96qFapQlj+?rPp|TOs^V~^K%+AFY z=!5=$I`84+W#Q$<7$xYV-G&)Qr6uXWX;A(?Z9SDT|2f6X6Z!e;L8h%O&a1NuSXC=V z4+i$;&r;niY>n;QoPNJ6S=G{XT9QEZUDp%5XtDafB0ltUZb1Kpo~QdFPPXZ%pkG&b zt;pNO?+?e#;`($oi4w6=G$VoUmE`4#)}7^JOkc(sZ?>261)>6}cuS06X03b|VxR@z zahC{?efw1Q;&HqD972pQQh&>aFBbB3*75=1nf>IebS#%Ys0~;XC}|HwlLc})LqKW+r6LJqvzm3Tu{WxzG6t5)zIG4+(w>2kodm_cPhm_suxLtHoM7Xg-~y6lrlEtkY$vhi8gId_0j8 z#QxY;g)}<9rpCzIm;|*L@Eq;Zcq#K+v(h z0J$e9{{AiFo6YYiV$G78N89?Dx6>|`S@Pb5LES#**#a5~kE`~njXqAci-vj)UuF6x zXR_PYXw_@H)hljFw%{Jw7e}0r_)J9cASKy!H=d3x>PSTHZJ*09R~|MEhn}M~)pCN{ z7`ZP-mWN&&Rt_i4ADf)KoHB6w3qUAV&{%!&nmeXR&%BFyZVCOSxr4;eA^y_P*awDr zZT?a6kts}1URNemhLUd1-KD4M#|wt&P5%d?$fs~9S1RR5)Se`zc*DW)R4^(dfx$R% zqa%e;W)dwD`s)vsxpAbSWalcZG&K|z3QfbEME8J7GhwSG**m~2+f?*k5X)d8nHDYh zg^Xf|eP2Oe4nb~s^-wMHI&VW*nK`Yq5%Us!yEmRq3QETz!U-%8!YWP}c zMPzjmB?czZH?e**sHc$4gnTK6<9(U)A z?N&tC&@D-3EQVfC_~<#*T9*uK5uHl#MyM)^HaZ*(3G@zKD0Mmwo*-@_a|rP(V*k`U zv|h`I7FEZf4qWA$0_+@wsc=E}{PRerAYxqU7U5Uu6!Wuhi`_6C?c7~vd}2z}2u$8M zqEEjHfKJLZ3gvYTh8&cJha8MpfQns%Hz61IyT+1>kc0y$xX>9il_YgN6~29gpITV{y<| z4raKm_jH1=G|Z4z=5kH2Qsm;d$o3>W2$V%8CcGA3$TMS=c`Y5FU>yP!E$8YMGL?_Vx+AmG)Dt{Zx3~jIVs;i*`&= zNi#59IY@J5q$9j`sOUmPAbpA9;5uXt)NLUrw7&_z$Hj2PJMr3DT@i*MeZqaOgi569 z>ZCX>aBF(Z(9uho9uHxSl)t(n(C?1-rj$s1+MqpnfS6nm(&Uu`(&Vf7YSNaRn!at7 z+SAUSM7)i{yx~dU%(^;ffAOTh;Y*fNF#5(TuEa!E(i42#ETjR4D2iL zO}t_LwnimfQWgqRJEsieBN(pGf`N)~We9tn*K=X#)hXvRDa!EjH3aHUzad3ufiR)t zfE6cHS~9Nww*%_(!W#kfxA=Z8V^zyLqr1jO5J9#+0@Oj*+)Y#YJoJng;flIPD{@8s zXcBR^;%n|JSNCpK+l#psJXyX{NoXDxB_y5mwE~T+>RcR77BT&CDUh2b0SkIu_B&#d z=&RT+BYe(~2}Pm&FDq!u!35!QuXRH%(u;>7C6#J$^k8WbJU*uLW_`N&-dvl<*7DhF zu5ESSergE-?xCKN3qRH)Scode!2^>Oe=Gx4}y7 zVMC=rm5do%^Z1IdU8VOq{@QrOi|y%E47N1PXg@d_makO@%yA1q=@n<;8@}aH{80ig zTQ%*6T>!op3{L3Rp-o+{^mlTP{%JB>`#*ekV)|*raG#>}dcH5wwfMr$6=Txz)CA1h z1|qgt*7%bsh=myh-Xy*uk-JwxN*1adYbAGTPWqOrKo7<)g8~1{C#|9w4Ah z&_)`+n5w|LtN+RbX{!NB1?^1n8};80EufLaAJo6}sQ_hwx?BCW|Gy8N7V)2MO+me` zKrx`6L%%Qxr2l~VSo(z#m-rLr@9UR9L7-XRUmzLv zzkvSZzw)mfFen5xk@*YK2@F;Ln3=y#Gbjf%!T5_~3k=Es;QVvC5flcRfcXW>(D@VW zAJQ?PB+xv?FH)!OpGdz4mH&{n040JZ8GaF!fidDAAM|(Pe@i)lVnK5Qzpz*Tg#9m> z0#GVwO#h1-r}qzHK_Yxm8fZ-Zi$ +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: + +#+PROPERTY: header-args:matlab :session *MATLAB* +#+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: + +* Jacobian +** Relation to platform parameters +A Jacobian is defined by: +- the orientations of the struts $\hat{s}_i$ expressed in a frame $\{A\}$ linked to the fixed platform. +- the vectors from $O_B$ to $b_i$ expressed in the frame $\{A\}$ + +Then, the choice of $O_B$ changes the Jacobian. + +** Jacobian for displacement +\[ \dot{q} = J \dot{X} \] +With: +- $q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]$ vector of linear displacement of actuated joints +- $X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]$ position and orientation of $O_B$ expressed in the frame $\{A\}$ + +For very small displacements $\delta q$ and $\delta X$, we have $\delta q = J \delta X$. + +** Jacobian for forces +\[ F = J^T \tau \] +With: +- $\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]$ vector of actuator forces +- $F = [f_x\ f_y\ f_z\ n_x\ n_y\ n_z]$ force and torque acting on point $O_B$ + +* Stiffness matrix $K$ + +\[ K = J^T \text{diag}(k_i) J \] + +If all the struts have the same stiffness $k$, then $K = k J^T J$ + +$K$ only depends of the geometry of the stewart platform: it depends on the Jacobian, that is on the orientations of the struts, position of the joints and choice of frame $\{B\}$. + +\[ F = K X \] + +With $F$ forces and torques applied to the moving platform at the origin of $\{B\}$ and $X$ the translations and rotations of $\{B\}$ with respect to $\{A\}$. + +\[ C = K^{-1} \] + +The compliance element $C_{ij}$ is then the stiffness +\[ X_i = C_{ij} F_j \] + +* Coupling +What causes the coupling from $F_i$ to $X_i$ ? + +#+begin_src latex :file coupling.pdf :post pdf2svg(file=*this*, ext="png") :exports both + \begin{tikzpicture} + \node[block] (Jt) at (0, 0) {$J^{-T}$}; + \node[block, right= of Jt] (G) {$G$}; + \node[block, right= of G] (J) {$J^{-1}$}; + + \draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$}; + \draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$}; + \draw[->] (G.east) -- (J.west) node[above left]{$q_i$}; + \draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$}; + \end{tikzpicture} +#+end_src + +#+name: fig:block_diag_coupling +#+caption: Block diagram to control an hexapod +#+RESULTS: +[[file:figs/coupling.png]] + +There is no coupling from $F_i$ to $X_j$ if $J^{-1} G J^{-T}$ is diagonal. + +If $G$ is diagonal (cubic configuration), then $J^{-1} G J^{-T} = G J^{-1} J^{-T} = G (J^{T} J)^{-1} = G K^{-1}$ + +Thus, the system is uncoupled if $G$ and $K$ are diagonal. diff --git a/stewart-architecture.html b/stewart-architecture.html new file mode 100644 index 0000000..1cb20d3 --- /dev/null +++ b/stewart-architecture.html @@ -0,0 +1,1622 @@ + + + + + + + + + +Stewart Platform - Definition of the Architecture + + + + + + + + + + + + + + +
+

Stewart Platform - Definition of the Architecture

+
+

Table of Contents

+
+ +
+
+ +

+Stewart platforms are generated in multiple steps. +

+ +

+We define 4 important frames: +

+
    +
  • \(\{F\}\): Frame fixed to the Fixed base and located at the center of its bottom surface. +This is used to fix the Stewart platform to some support.
    • +
  • \(\{M\}\): Frame fixed to the Moving platform and located at the center of its top surface. +This is used to place things on top of the Stewart platform.
    • +
  • \(\{A\}\): Frame fixed to the fixed base. +It defined the center of rotation of the moving platform.
    • +
  • \(\{B\}\): Frame fixed to the moving platform. +The motion of the moving platforms and forces applied to it are defined with respect to this frame \(\{B\}\).
    • +
+ +

+Then, we define the location of the spherical joints: +

+
    +
  • \(\bm{a}_{i}\) are the position of the spherical joints fixed to the fixed base
    • +
  • \(\bm{b}_{i}\) are the position of the spherical joints fixed to the moving platform
    • +
+ +

+We define the rest position of the Stewart platform: +

+
    +
  • For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.
    • +
  • Thus, to define the rest position of the Stewart platform, we just have to defined its total height \(H\). +\(H\) corresponds to the distance from the bottom of the fixed base to the top of the moving platform.
    • +
+ +

+From \(\bm{a}_{i}\) and \(\bm{b}_{i}\), we can determine the length and orientation of each strut: +

+
    +
  • \(l_{i}\) is the length of the strut
    • +
  • \({}^{A}\hat{\bm{s}}_{i}\) is the unit vector align with the strut
    • +
+ +

+The position of the Spherical joints can be computed using various methods: +

+
    +
  • Cubic configuration
    • +
  • Circular configuration
    • +
  • Arbitrary position
    • +
  • These methods should be easily scriptable and corresponds to specific functions that returns \({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\). +The input of these functions are the parameters corresponding to the wanted geometry.
    • +
+ +

+For Simscape, we need: +

+
    +
  • The position and orientation of each spherical joint fixed to the fixed base: \({}^{F}\bm{a}_{i}\) and \({}^{F}\bm{R}_{a_{i}}\)
    • +
  • The position and orientation of each spherical joint fixed to the moving platform: \({}^{M}\bm{b}_{i}\) and \({}^{M}\bm{R}_{b_{i}}\)
    • +
  • The rest length of each strut: \(l_{i}\)
    • +
  • The stiffness and damping of each actuator: \(k_{i}\) and \(c_{i}\)
    • +
  • The position of the frame \(\{A\}\) with respect to the frame \(\{F\}\): \({}^{F}\bm{O}_{A}\)
    • +
  • The position of the frame \(\{B\}\) with respect to the frame \(\{M\}\): \({}^{M}\bm{O}_{B}\)
    • +
+ + +
+

1 Procedure

+
+

+The procedure to define the Stewart platform is the following: +

+
    +
  1. Define the initial position of frames {A}, {B}, {F} and {M}. +We do that using the initializeFramesPositions function. +We have to specify the total height of the Stewart platform \(H\) and the position \({}^{M}O_{B}\) of {B} with respect to {M}.
    2. +
  2. Compute the positions of joints \({}^{F}a_{i}\) and \({}^{M}b_{i}\). +We can do that using various methods depending on the wanted architecture: +
      +
    • generateCubicConfiguration permits to generate a cubic configuration
      • +
    3. +
  3. Compute the position and orientation of the joints with respect to the fixed base and the moving platform. +This is done with the computeJointsPose function.
    4. +
  4. Define the dynamical properties of the Stewart platform. +The output are the stiffness and damping of each strut \(k_{i}\) and \(c_{i}\). +This can be done we simply choosing directly the stiffness and damping of each strut. +The stiffness and damping of each actuator can also be determine from the wanted stiffness of the Stewart platform for instance.
    5. +
  5. Define the mass and inertia of each element of the Stewart platform.
    6. +
+ +

+By following this procedure, we obtain a Matlab structure stewart that contains all the information for the Simscape model and for further analysis. +

+
+
+ +
+

2 Matlab Code

+
+
+
+

2.1 Simscape Model

+
+
+
open('stewart_platform.slx')
+
+
+
+
+ +
+

2.2 Test the functions

+
+
+
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
+% stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
+stewart = generateGeneralConfiguration(stewart);
+stewart = computeJointsPose(stewart);
+stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
+stewart = initializeCylindricalPlatforms(stewart);
+stewart = initializeCylindricalStruts(stewart);
+stewart = computeJacobian(stewart);
+stewart = initializeStewartPose(stewart, 'AP', [0;0;0.01], 'ARB', eye(3));
+
+[Li, dLi] = inverseKinematics(stewart, 'AP', [0;0;0.00001], 'ARB', eye(3));
+[P, R] = forwardKinematicsApprox(stewart, 'dL', dLi);
+
+
+
+
+
+ +
+

3 initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

3.1 Function description

+
+
+
function [stewart] = initializeFramesPositions(args)
+% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
+%
+% Syntax: [stewart] = initializeFramesPositions(args)
+%
+% Inputs:
+%    - args - Can have the following fields:
+%        - H    [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
+%        - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
+%
+% Outputs:
+%    - stewart - A structure with the following fields:
+%        - H    [1x1] - Total Height of the Stewart Platform [m]
+%        - FO_M [3x1] - Position of {M} with respect to {F} [m]
+%        - MO_B [3x1] - Position of {B} with respect to {M} [m]
+%        - FO_A [3x1] - Position of {A} with respect to {F} [m]
+
+
+
+
+ +
+

3.2 Documentation

+
+ +
+

stewart-frames-position.png +

+

Figure 1: Definition of the position of the frames

+
+
+
+ +
+

3.3 Optional Parameters

+
+
+
arguments
+    args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
+    args.MO_B (1,1) double {mustBeNumeric} = 50e-3
+end
+
+
+
+
+ +
+

3.4 Initialize the Stewart structure

+
+
+
stewart = struct();
+
+
+
+
+ +
+

3.5 Compute the position of each frame

+
+
+
stewart.H = args.H; % Total Height of the Stewart Platform [m]
+
+stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
+
+stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
+
+stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
+
+
+
+
+
+ +
+

4 Initialize the position of the Joints

+
+
+
+

4.1 generateCubicConfiguration: Generate a Cubic Configuration

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

4.1.1 Function description

+
+
+
function [stewart] = generateCubicConfiguration(stewart, args)
+% generateCubicConfiguration - Generate a Cubic Configuration
+%
+% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
+%
+% Inputs:
+%    - stewart - A structure with the following fields
+%        - H   [1x1] - Total height of the platform [m]
+%    - args - Can have the following fields:
+%        - Hc  [1x1] - Height of the "useful" part of the cube [m]
+%        - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
+%        - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
+%        - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
+%
+% Outputs:
+%    - stewart - updated Stewart structure with the added fields:
+%        - Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
+%        - Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
+
+
+
+
+ +
+

4.1.2 Documentation

+
+ +
+

cubic-configuration-definition.png +

+

Figure 2: Cubic Configuration

+
+
+
+ +
+

4.1.3 Optional Parameters

+
+
+
arguments
+    stewart
+    args.Hc  (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
+    args.FOc (1,1) double {mustBeNumeric} = 50e-3
+    args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+    args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+end
+
+
+
+
+ +
+

4.1.4 Position of the Cube

+
+

+We define the useful points of the cube with respect to the Cube’s center. +\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is +located at the center of the cube and aligned with {F} and {M}. +

+ +
+
sx = [ 2; -1; -1];
+sy = [ 0;  1; -1];
+sz = [ 1;  1;  1];
+
+R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
+
+L = args.Hc*sqrt(3);
+
+Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
+
+CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
+CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
+
+
+
+
+ +
+

4.1.5 Compute the pose

+
+

+We can compute the vector of each leg \({}^{C}\hat{\bm{s}}_{i}\) (unit vector from \({}^{C}C_{f}\) to \({}^{C}C_{m}\)). +

+
+
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
+
+
+ +

+We now which to compute the position of the joints \(a_{i}\) and \(b_{i}\). +

+
+
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
+stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
+
+
+
+
+
+ +
+

4.2 generateGeneralConfiguration: Generate a Very General Configuration

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

4.2.1 Function description

+
+
+
function [stewart] = generateGeneralConfiguration(stewart, args)
+% generateGeneralConfiguration - Generate a Very General Configuration
+%
+% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
+%
+% Inputs:
+%    - args - Can have the following fields:
+%        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
+%        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
+%        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
+%        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
+%        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
+%        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
+%
+% Outputs:
+%    - stewart - updated Stewart structure with the added fields:
+%        - Fa  [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
+%        - Mb  [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
+
+
+
+
+ +
+

4.2.2 Documentation

+
+

+Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}. +The radius of the circles can be chosen as well as the angles where the joints are located. +

+
+
+ +
+

4.2.3 Optional Parameters

+
+
+
arguments
+    stewart
+    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
+    args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
+    args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
+    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
+end
+
+
+
+
+ +
+

4.2.4 Compute the pose

+
+
+
stewart.Fa = zeros(3,6);
+stewart.Mb = zeros(3,6);
+
+
+ +
+
for i = 1:6
+  stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
+  stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
+end
+
+
+
+
+
+
+ +
+

5 computeJointsPose: Compute the Pose of the Joints

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

5.1 Function description

+
+
+
function [stewart] = computeJointsPose(stewart)
+% computeJointsPose -
+%
+% Syntax: [stewart] = computeJointsPose(stewart)
+%
+% Inputs:
+%    - stewart - A structure with the following fields
+%        - Fa   [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
+%        - Mb   [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
+%        - FO_A [3x1] - Position of {A} with respect to {F}
+%        - MO_B [3x1] - Position of {B} with respect to {M}
+%        - FO_M [3x1] - Position of {M} with respect to {F}
+%
+% Outputs:
+%    - stewart - A structure with the following added fields
+%        - Aa  [3x6]   - The i'th column is the position of ai with respect to {A}
+%        - Ab  [3x6]   - The i'th column is the position of bi with respect to {A}
+%        - Ba  [3x6]   - The i'th column is the position of ai with respect to {B}
+%        - Bb  [3x6]   - The i'th column is the position of bi with respect to {B}
+%        - l   [6x1]   - The i'th element is the initial length of strut i
+%        - As  [3x6]   - The i'th column is the unit vector of strut i expressed in {A}
+%        - Bs  [3x6]   - The i'th column is the unit vector of strut i expressed in {B}
+%        - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
+%        - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
+
+
+
+
+ +
+

5.2 Documentation

+
+ +
+

stewart-struts.png +

+

Figure 3: Position and orientation of the struts

+
+
+
+ +
+

5.3 Compute the position of the Joints

+
+
+
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
+stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
+
+stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
+stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
+
+
+
+
+ +
+

5.4 Compute the strut length and orientation

+
+
+
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
+
+stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
+
+
+ +
+
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
+
+
+
+
+ +
+

5.5 Compute the orientation of the Joints

+
+
+
stewart.FRa = zeros(3,3,6);
+stewart.MRb = zeros(3,3,6);
+
+for i = 1:6
+  stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
+  stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
+
+  stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
+  stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
+end
+
+
+
+
+
+ +
+

6 initializeStrutDynamics: Add Stiffness and Damping properties of each strut

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

6.1 Function description

+
+
+
function [stewart] = initializeStrutDynamics(stewart, args)
+% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
+%
+% Syntax: [stewart] = initializeStrutDynamics(args)
+%
+% Inputs:
+%    - args - Structure with the following fields:
+%        - Ki [6x1] - Stiffness of each strut [N/m]
+%        - Ci [6x1] - Damping of each strut [N/(m/s)]
+%
+% Outputs:
+%    - stewart - updated Stewart structure with the added fields:
+%        - Ki [6x1] - Stiffness of each strut [N/m]
+%        - Ci [6x1] - Damping of each strut [N/(m/s)]
+
+
+
+
+ +
+

6.2 Optional Parameters

+
+
+
arguments
+    stewart
+    args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
+    args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1)
+end
+
+
+
+
+ +
+

6.3 Add Stiffness and Damping properties of each strut

+
+
+
stewart.Ki = args.Ki;
+stewart.Ci = args.Ci;
+
+
+
+
+
+ +
+

7 computeJacobian: Compute the Jacobian Matrix

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

7.1 Function description

+
+
+
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
+
+
+
+
+ +
+

7.2 Compute Jacobian Matrix

+
+
+
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
+
+
+
+
+ +
+

7.3 Compute Stiffness Matrix

+
+
+
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
+
+
+
+
+ +
+

7.4 Compute Compliance Matrix

+
+
+
stewart.C = inv(stewart.K);
+
+
+
+
+
+ +
+

8 Initialize the Geometry of the Mechanical Elements

+
+
+
+

8.1 initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

8.1.1 Function description

+
+
+
function [stewart] = initializeCylindricalPlatforms(stewart, args)
+% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
+%
+% Syntax: [stewart] = initializeCylindricalPlatforms(args)
+%
+% Inputs:
+%    - args - Structure with the following fields:
+%        - Fpm [1x1] - Fixed Platform Mass [kg]
+%        - Fph [1x1] - Fixed Platform Height [m]
+%        - Fpr [1x1] - Fixed Platform Radius [m]
+%        - Mpm [1x1] - Mobile Platform Mass [kg]
+%        - Mph [1x1] - Mobile Platform Height [m]
+%        - Mpr [1x1] - Mobile Platform Radius [m]
+%
+% Outputs:
+%    - stewart - updated Stewart structure with the added fields:
+%      - platforms [struct] - structure with the following fields:
+%        - Fpm [1x1] - Fixed Platform Mass [kg]
+%        - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
+%        - Fph [1x1] - Fixed Platform Height [m]
+%        - Fpr [1x1] - Fixed Platform Radius [m]
+%        - Mpm [1x1] - Mobile Platform Mass [kg]
+%        - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
+%        - Mph [1x1] - Mobile Platform Height [m]
+%        - Mpr [1x1] - Mobile Platform Radius [m]
+
+
+
+
+ +
+

8.1.2 Optional Parameters

+
+
+
arguments
+    stewart
+    args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
+    args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
+    args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
+    args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
+    args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
+    args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
+end
+
+
+
+
+ +
+

8.1.3 Create the platforms struct

+
+
+
platforms = struct();
+
+platforms.Fpm = args.Fpm;
+platforms.Fph = args.Fph;
+platforms.Fpr = args.Fpr;
+platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
+                      1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
+                      1/2  * platforms.Fpm * platforms.Fpr^2]);
+
+platforms.Mpm = args.Mpm;
+platforms.Mph = args.Mph;
+platforms.Mpr = args.Mpr;
+platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
+                      1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
+                      1/2  * platforms.Mpm * platforms.Mpr^2]);
+
+
+
+
+ +
+

8.1.4 Save the platforms struct

+
+
+
stewart.platforms = platforms;
+
+
+
+
+
+ +
+

8.2 initializeCylindricalStruts: Define the mass and moment of inertia of cylindrical struts

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

8.2.1 Function description

+
+
+
function [stewart] = initializeCylindricalStruts(stewart, args)
+% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
+%
+% Syntax: [stewart] = initializeCylindricalStruts(args)
+%
+% Inputs:
+%    - args - Structure with the following fields:
+%        - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
+%        - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
+%        - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
+%        - Msm [1x1] - Mass of the Mobile part of the struts [kg]
+%        - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
+%        - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
+%
+% Outputs:
+%    - stewart - updated Stewart structure with the added fields:
+%      - struts [struct] - structure with the following fields:
+%        - Fsm [6x1]   - Mass of the Fixed part of the struts [kg]
+%        - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
+%        - Msm [6x1]   - Mass of the Mobile part of the struts [kg]
+%        - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
+%        - Fsh [6x1]   - Height of cylinder for the Fixed part of the struts [m]
+%        - Fsr [6x1]   - Radius of cylinder for the Fixed part of the struts [m]
+%        - Msh [6x1]   - Height of cylinder for the Mobile part of the struts [m]
+%        - Msr [6x1]   - Radius of cylinder for the Mobile part of the struts [m]
+
+
+
+
+ +
+

8.2.2 Optional Parameters

+
+
+
arguments
+    stewart
+    args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+    args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+    args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
+    args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
+    args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
+    args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
+end
+
+
+
+
+ +
+

8.2.3 Create the struts structure

+
+
+
struts = struct();
+
+struts.Fsm = ones(6,1).*args.Fsm;
+struts.Msm = ones(6,1).*args.Msm;
+
+struts.Fsh = ones(6,1).*args.Fsh;
+struts.Fsr = ones(6,1).*args.Fsr;
+struts.Msh = ones(6,1).*args.Msh;
+struts.Msr = ones(6,1).*args.Msr;
+
+struts.Fsi = zeros(3, 3, 6);
+struts.Msi = zeros(3, 3, 6);
+for i = 1:6
+  struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
+                            1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
+                            1/2  * struts.Fsm(i) * struts.Fsr(i)^2]);
+  struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
+                            1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
+                            1/2  * struts.Msm(i) * struts.Msr(i)^2]);
+end
+
+
+ +
+
stewart.struts = struts;
+
+
+
+
+
+
+ +
+

9 initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

9.1 Function description

+
+
+
function [stewart] = initializeStewartPose(stewart, args)
+% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
+%                         It uses the inverse kinematic
+%
+% Syntax: [stewart] = initializeStewartPose(stewart, args)
+%
+% 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:
+%    - stewart - updated Stewart structure with the added fields:
+%      - 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}
+
+
+
+
+ +
+

9.2 Optional Parameters

+
+
+
arguments
+    stewart
+    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+    args.ARB (3,3) double {mustBeNumeric} = eye(3)
+end
+
+
+
+
+ +
+

9.3 Use the Inverse Kinematic function

+
+
+
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
+
+stewart.dLi = dLi;
+
+
+
+
+
+ +
+

10 Utility Functions

+
+
+
+

10.1 inverseKinematics: Compute Inverse Kinematics

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

10.1.1 Function description

+
+
+
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}
+
+
+
+
+ +
+

10.1.2 Optional Parameters

+
+
+
arguments
+    stewart
+    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+    args.ARB (3,3) double {mustBeNumeric} = eye(3)
+end
+
+
+
+
+ +
+

10.1.3 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. +

+
+
+ +
+

10.1.4 Compute

+
+
+
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));
+
+
+ +
+
dLi = Li-stewart.l;
+
+
+
+
+
+ +
+

10.2 forwardKinematicsApprox: Compute the Forward Kinematics

+
+

+ +

+ +

+This Matlab function is accessible here. +

+
+ +
+

10.2.1 Function description

+
+
+
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}
+
+
+
+
+ +
+

10.2.2 Optional Parameters

+
+
+
arguments
+    stewart
+    args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
+end
+
+
+
+
+ +
+

10.2.3 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}} \] +

+
+
X = stewart.J\args.dL;
+
+
+ +

+The position vector corresponds to the first 3 elements. +

+
+
P = X(1:3);
+
+
+ +

+The next 3 elements are the orientation of {B} with respect to {A} expressed +using the screw axis. +

+
+
theta = norm(X(4:6));
+s = X(4:6)/theta;
+
+
+ +

+We then compute the corresponding rotation matrix. +

+
+
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)];
+
+
+
+
+
+
+ +
+

11 Other Elements

+
+
+
+

11.1 Z-Axis Geophone

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [geophone] = initializeZAxisGeophone(args)
+    arguments
+        args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
+        args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1    % [Hz]
+    end
+
+    %%
+    geophone.m = args.mass;
+
+    %% The Stiffness is set to have the damping resonance frequency
+    geophone.k = geophone.m * (2*pi*args.freq)^2;
+
+    %% We set the damping value to have critical damping
+    geophone.c = 2*sqrt(geophone.m * geophone.k);
+
+    %% Save
+    save('./mat/geophone_z_axis.mat', 'geophone');
+end
+
+
+
+
+ +
+

11.2 Z-Axis Accelerometer

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [accelerometer] = initializeZAxisAccelerometer(args)
+    arguments
+        args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
+        args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3  % [Hz]
+    end
+
+    %%
+    accelerometer.m = args.mass;
+
+    %% The Stiffness is set to have the damping resonance frequency
+    accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
+
+    %% We set the damping value to have critical damping
+    accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
+
+    %% Gain correction of the accelerometer to have a unity gain until the resonance
+    accelerometer.gain = -accelerometer.k/accelerometer.m;
+
+    %% Save
+    save('./mat/accelerometer_z_axis.mat', 'accelerometer');
+end
+
+
+
+
+
+
+
+

Author: Dehaeze Thomas

+

Created: 2020-01-27 lun. 17:41

+
+ + diff --git a/stewart-architecture.org b/stewart-architecture.org new file mode 100644 index 0000000..2b2bcb0 --- /dev/null +++ b/stewart-architecture.org @@ -0,0 +1,1615 @@ +#+TITLE: Stewart Platform - Definition of the Architecture +:DRAWER: +#+HTML_LINK_HOME: ./index.html +#+HTML_LINK_UP: ./index.html + +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: +#+HTML_HEAD: + +#+PROPERTY: header-args:matlab :session *MATLAB* +#+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: + +* Introduction :ignore: +Stewart platforms are generated in multiple steps. + +We define 4 important *frames*: +- $\{F\}$: Frame fixed to the *Fixed* base and located at the center of its bottom surface. + This is used to fix the Stewart platform to some support. +- $\{M\}$: Frame fixed to the *Moving* platform and located at the center of its top surface. + This is used to place things on top of the Stewart platform. +- $\{A\}$: Frame fixed to the fixed base. + It defined the center of rotation of the moving platform. +- $\{B\}$: Frame fixed to the moving platform. + The motion of the moving platforms and forces applied to it are defined with respect to this frame $\{B\}$. + +Then, we define the *location of the spherical joints*: +- $\bm{a}_{i}$ are the position of the spherical joints fixed to the fixed base +- $\bm{b}_{i}$ are the position of the spherical joints fixed to the moving platform + +We define the *rest position* of the Stewart platform: +- For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position. +- Thus, to define the rest position of the Stewart platform, we just have to defined its total height $H$. + $H$ corresponds to the distance from the bottom of the fixed base to the top of the moving platform. + +From $\bm{a}_{i}$ and $\bm{b}_{i}$, we can determine the *length and orientation of each strut*: +- $l_{i}$ is the length of the strut +- ${}^{A}\hat{\bm{s}}_{i}$ is the unit vector align with the strut + +The position of the Spherical joints can be computed using various methods: +- Cubic configuration +- Circular configuration +- Arbitrary position +- These methods should be easily scriptable and corresponds to specific functions that returns ${}^{F}\bm{a}_{i}$ and ${}^{M}\bm{b}_{i}$. + The input of these functions are the parameters corresponding to the wanted geometry. + +For Simscape, we need: +- The position and orientation of each spherical joint fixed to the fixed base: ${}^{F}\bm{a}_{i}$ and ${}^{F}\bm{R}_{a_{i}}$ +- The position and orientation of each spherical joint fixed to the moving platform: ${}^{M}\bm{b}_{i}$ and ${}^{M}\bm{R}_{b_{i}}$ +- The rest length of each strut: $l_{i}$ +- The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$ +- The position of the frame $\{A\}$ with respect to the frame $\{F\}$: ${}^{F}\bm{O}_{A}$ +- The position of the frame $\{B\}$ with respect to the frame $\{M\}$: ${}^{M}\bm{O}_{B}$ + + +* Procedure +The procedure to define the Stewart platform is the following: +1. Define the initial position of frames {A}, {B}, {F} and {M}. + We do that using the =initializeFramesPositions= function. + We have to specify the total height of the Stewart platform $H$ and the position ${}^{M}O_{B}$ of {B} with respect to {M}. +2. Compute the positions of joints ${}^{F}a_{i}$ and ${}^{M}b_{i}$. + We can do that using various methods depending on the wanted architecture: + - =generateCubicConfiguration= permits to generate a cubic configuration +3. Compute the position and orientation of the joints with respect to the fixed base and the moving platform. + This is done with the =computeJointsPose= function. +4. Define the dynamical properties of the Stewart platform. + The output are the stiffness and damping of each strut $k_{i}$ and $c_{i}$. + This can be done we simply choosing directly the stiffness and damping of each strut. + The stiffness and damping of each actuator can also be determine from the wanted stiffness of the Stewart platform for instance. +5. Define the mass and inertia of each element of the Stewart platform. + +By following this procedure, we obtain a Matlab structure =stewart= that contains all the information for the Simscape model and for further analysis. + +* Matlab Code +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab + simulinkproject('./'); +#+end_src + +** Simscape Model +#+begin_src matlab + open('stewart_platform.slx') +#+end_src + +** Test the functions +#+begin_src matlab + stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); + % stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3); + stewart = generateGeneralConfiguration(stewart); + stewart = computeJointsPose(stewart); + stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); + stewart = initializeCylindricalPlatforms(stewart); + stewart = initializeCylindricalStruts(stewart); + stewart = computeJacobian(stewart); + stewart = initializeStewartPose(stewart, 'AP', [0;0;0.01], 'ARB', eye(3)); + + [Li, dLi] = inverseKinematics(stewart, 'AP', [0;0;0.00001], 'ARB', eye(3)); + [P, R] = forwardKinematicsApprox(stewart, 'dL', dLi); +#+end_src + +* =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M} +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeFramesPositions.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeFramesPositions.m][here]]. + +** Function description +#+begin_src matlab + function [stewart] = initializeFramesPositions(args) + % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} + % + % Syntax: [stewart] = initializeFramesPositions(args) + % + % Inputs: + % - args - Can have the following fields: + % - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m] + % - MO_B [1x1] - Height of the frame {B} with respect to {M} [m] + % + % Outputs: + % - stewart - A structure with the following fields: + % - H [1x1] - Total Height of the Stewart Platform [m] + % - FO_M [3x1] - Position of {M} with respect to {F} [m] + % - MO_B [3x1] - Position of {B} with respect to {M} [m] + % - FO_A [3x1] - Position of {A} with respect to {F} [m] +#+end_src + +** Documentation + +#+name: fig:stewart-frames-position +#+caption: Definition of the position of the frames +[[file:figs/stewart-frames-position.png]] + +** Optional Parameters +#+begin_src matlab + arguments + args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 + args.MO_B (1,1) double {mustBeNumeric} = 50e-3 + end +#+end_src + +** Initialize the Stewart structure +#+begin_src matlab + stewart = struct(); +#+end_src + +** Compute the position of each frame +#+begin_src matlab + stewart.H = args.H; % Total Height of the Stewart Platform [m] + + stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m] + + stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] + + stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m] +#+end_src + +* Initialize the position of the Joints +** =generateCubicConfiguration=: Generate a Cubic Configuration +:PROPERTIES: +:header-args:matlab+: :tangle src/generateCubicConfiguration.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/generateCubicConfiguration.m][here]]. + +*** Function description +#+begin_src matlab + function [stewart] = generateCubicConfiguration(stewart, args) + % generateCubicConfiguration - Generate a Cubic Configuration + % + % Syntax: [stewart] = generateCubicConfiguration(stewart, args) + % + % Inputs: + % - stewart - A structure with the following fields + % - H [1x1] - Total height of the platform [m] + % - args - Can have the following fields: + % - Hc [1x1] - Height of the "useful" part of the cube [m] + % - FOc [1x1] - Height of the center of the cube with respect to {F} [m] + % - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m] + % - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} + % - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +#+end_src + +*** Documentation +#+name: fig:cubic-configuration-definition +#+caption: Cubic Configuration +[[file:figs/cubic-configuration-definition.png]] + +*** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 + args.FOc (1,1) double {mustBeNumeric} = 50e-3 + args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + end +#+end_src + +*** Position of the Cube +We define the useful points of the cube with respect to the Cube's center. +${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is +located at the center of the cube and aligned with {F} and {M}. + +#+begin_src matlab + sx = [ 2; -1; -1]; + sy = [ 0; 1; -1]; + sz = [ 1; 1; 1]; + + R = [sx, sy, sz]./vecnorm([sx, sy, sz]); + + L = args.Hc*sqrt(3); + + Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc]; + + CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg + CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg +#+end_src + +*** Compute the pose +We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). +#+begin_src matlab + CSi = (CCm - CCf)./vecnorm(CCm - CCf); +#+end_src + +We now which to compute the position of the joints $a_{i}$ and $b_{i}$. +#+begin_src matlab + stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; + stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; +#+end_src + +** =generateGeneralConfiguration=: Generate a Very General Configuration +:PROPERTIES: +:header-args:matlab+: :tangle src/generateGeneralConfiguration.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/generateGeneralConfiguration.m][here]]. + +*** Function description +#+begin_src matlab + function [stewart] = generateGeneralConfiguration(stewart, args) + % generateGeneralConfiguration - Generate a Very General Configuration + % + % Syntax: [stewart] = generateGeneralConfiguration(stewart, args) + % + % Inputs: + % - args - Can have the following fields: + % - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] + % - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] + % - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad] + % - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m] + % - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m] + % - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} + % - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +#+end_src + +*** Documentation +Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}. +The radius of the circles can be chosen as well as the angles where the joints are located. + +*** Optional Parameters +#+begin_src matlab + arguments + stewart + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3; + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); + end +#+end_src + +*** Compute the pose +#+begin_src matlab + stewart.Fa = zeros(3,6); + stewart.Mb = zeros(3,6); +#+end_src + +#+begin_src matlab + for i = 1:6 + stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; + stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; + end +#+end_src + +* =computeJointsPose=: Compute the Pose of the Joints +:PROPERTIES: +:header-args:matlab+: :tangle src/computeJointsPose.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/computeJointsPose.m][here]]. + +** Function description +#+begin_src matlab + function [stewart] = computeJointsPose(stewart) + % computeJointsPose - + % + % Syntax: [stewart] = computeJointsPose(stewart) + % + % Inputs: + % - stewart - A structure with the following fields + % - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} + % - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} + % - FO_A [3x1] - Position of {A} with respect to {F} + % - MO_B [3x1] - Position of {B} with respect to {M} + % - FO_M [3x1] - Position of {M} with respect to {F} + % + % Outputs: + % - stewart - A structure with the following added fields + % - Aa [3x6] - The i'th column is the position of ai with respect to {A} + % - Ab [3x6] - The i'th column is the position of bi with respect to {A} + % - Ba [3x6] - The i'th column is the position of ai with respect to {B} + % - Bb [3x6] - The i'th column is the position of bi with respect to {B} + % - l [6x1] - The i'th element is the initial length of strut i + % - As [3x6] - The i'th column is the unit vector of strut i expressed in {A} + % - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} + % - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} + % - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} +#+end_src + +** Documentation + +#+name: fig:stewart-struts +#+caption: Position and orientation of the struts +[[file:figs/stewart-struts.png]] + +** Compute the position of the Joints +#+begin_src matlab + stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]); + stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]); + + stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]); + stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]); +#+end_src + +** Compute the strut length and orientation +#+begin_src matlab + stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As + + stewart.l = vecnorm(stewart.Ab - stewart.Aa)'; +#+end_src + +#+begin_src matlab + stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba); +#+end_src + +** Compute the orientation of the Joints +#+begin_src matlab + stewart.FRa = zeros(3,3,6); + stewart.MRb = zeros(3,3,6); + + for i = 1:6 + stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)]; + stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i)); + + stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)]; + stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i)); + end +#+end_src + +* =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeStrutDynamics.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeStrutDynamics.m][here]]. + +** Function description +#+begin_src matlab + function [stewart] = initializeStrutDynamics(stewart, args) + % initializeStrutDynamics - Add Stiffness and Damping properties of each strut + % + % Syntax: [stewart] = initializeStrutDynamics(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Ki [6x1] - Stiffness of each strut [N/m] + % - Ci [6x1] - Damping of each strut [N/(m/s)] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - Ki [6x1] - Stiffness of each strut [N/m] + % - Ci [6x1] - Damping of each strut [N/(m/s)] +#+end_src + +** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1) + args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1) + end +#+end_src + +** Add Stiffness and Damping properties of each strut +#+begin_src matlab + stewart.Ki = args.Ki; + stewart.Ci = args.Ci; +#+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: +<> + +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 + +* Initialize the Geometry of the Mechanical Elements +** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeCylindricalPlatforms.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeCylindricalPlatforms.m][here]]. + +*** Function description +#+begin_src matlab + function [stewart] = initializeCylindricalPlatforms(stewart, args) + % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms + % + % Syntax: [stewart] = initializeCylindricalPlatforms(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Fpm [1x1] - Fixed Platform Mass [kg] + % - Fph [1x1] - Fixed Platform Height [m] + % - Fpr [1x1] - Fixed Platform Radius [m] + % - Mpm [1x1] - Mobile Platform Mass [kg] + % - Mph [1x1] - Mobile Platform Height [m] + % - Mpr [1x1] - Mobile Platform Radius [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - platforms [struct] - structure with the following fields: + % - Fpm [1x1] - Fixed Platform Mass [kg] + % - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2] + % - Fph [1x1] - Fixed Platform Height [m] + % - Fpr [1x1] - Fixed Platform Radius [m] + % - Mpm [1x1] - Mobile Platform Mass [kg] + % - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2] + % - Mph [1x1] - Mobile Platform Height [m] + % - Mpr [1x1] - Mobile Platform Radius [m] +#+end_src + +*** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + end +#+end_src + +*** Create the =platforms= struct +#+begin_src matlab + platforms = struct(); + + platforms.Fpm = args.Fpm; + platforms.Fph = args.Fph; + platforms.Fpr = args.Fpr; + platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... + 1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... + 1/2 * platforms.Fpm * platforms.Fpr^2]); + + platforms.Mpm = args.Mpm; + platforms.Mph = args.Mph; + platforms.Mpr = args.Mpr; + platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... + 1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... + 1/2 * platforms.Mpm * platforms.Mpr^2]); +#+end_src + +*** Save the =platforms= struct +#+begin_src matlab + stewart.platforms = platforms; +#+end_src + +** =initializeCylindricalStruts=: Define the mass and moment of inertia of cylindrical struts +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeCylindricalStruts.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeCylindricalStruts.m][here]]. + +*** Function description +#+begin_src matlab + function [stewart] = initializeCylindricalStruts(stewart, args) + % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts + % + % Syntax: [stewart] = initializeCylindricalStruts(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Fsm [1x1] - Mass of the Fixed part of the struts [kg] + % - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m] + % - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m] + % - Msm [1x1] - Mass of the Mobile part of the struts [kg] + % - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m] + % - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - struts [struct] - structure with the following fields: + % - Fsm [6x1] - Mass of the Fixed part of the struts [kg] + % - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] + % - Msm [6x1] - Mass of the Mobile part of the struts [kg] + % - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] + % - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m] + % - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m] + % - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m] + % - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m] +#+end_src + +*** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + end +#+end_src + +*** Create the =struts= structure +#+begin_src matlab + struts = struct(); + + struts.Fsm = ones(6,1).*args.Fsm; + struts.Msm = ones(6,1).*args.Msm; + + struts.Fsh = ones(6,1).*args.Fsh; + struts.Fsr = ones(6,1).*args.Fsr; + struts.Msh = ones(6,1).*args.Msh; + struts.Msr = ones(6,1).*args.Msr; + + struts.Fsi = zeros(3, 3, 6); + struts.Msi = zeros(3, 3, 6); + for i = 1:6 + struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... + 1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... + 1/2 * struts.Fsm(i) * struts.Fsr(i)^2]); + struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... + 1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... + 1/2 * struts.Msm(i) * struts.Msr(i)^2]); + end +#+end_src + +#+begin_src matlab + stewart.struts = struts; +#+end_src + +* =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeStewartPose.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeStewartPose.m][here]]. + +** Function description +#+begin_src matlab + function [stewart] = initializeStewartPose(stewart, args) + % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose + % It uses the inverse kinematic + % + % Syntax: [stewart] = initializeStewartPose(stewart, args) + % + % 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: + % - stewart - updated Stewart structure with the added fields: + % - 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 + +** Use the Inverse Kinematic function +#+begin_src matlab + [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); + + stewart.dLi = dLi; +#+end_src + +* Utility Functions +** =inverseKinematics=: Compute Inverse Kinematics +:PROPERTIES: +:header-args:matlab+: :tangle src/inverseKinematics.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +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 Forward Kinematics +:PROPERTIES: +:header-args:matlab+: :tangle src/forwardKinematicsApprox.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +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 + +* Other Elements +** Z-Axis Geophone +:PROPERTIES: +:header-args:matlab+: :tangle ./src/initializeZAxisGeophone.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/initializeZAxisGeophone.m][here]]. + +#+begin_src matlab + function [geophone] = initializeZAxisGeophone(args) + arguments + args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] + args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz] + end + + %% + geophone.m = args.mass; + + %% The Stiffness is set to have the damping resonance frequency + geophone.k = geophone.m * (2*pi*args.freq)^2; + + %% We set the damping value to have critical damping + geophone.c = 2*sqrt(geophone.m * geophone.k); + + %% Save + save('./mat/geophone_z_axis.mat', 'geophone'); + end +#+end_src + +** Z-Axis Accelerometer +:PROPERTIES: +:header-args:matlab+: :tangle ./src/initializeZAxisAccelerometer.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/initializeZAxisAccelerometer.m][here]]. + +#+begin_src matlab + function [accelerometer] = initializeZAxisAccelerometer(args) + arguments + args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] + args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz] + end + + %% + accelerometer.m = args.mass; + + %% The Stiffness is set to have the damping resonance frequency + accelerometer.k = accelerometer.m * (2*pi*args.freq)^2; + + %% We set the damping value to have critical damping + accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k); + + %% Gain correction of the accelerometer to have a unity gain until the resonance + accelerometer.gain = -accelerometer.k/accelerometer.m; + + %% Save + save('./mat/accelerometer_z_axis.mat', 'accelerometer'); + end +#+end_src + +* OLD :noexport: +** Define the Height of the Platform :noexport: +#+begin_src matlab + %% 1. Height of the platform. Location of {F} and {M} + H = 90e-3; % [m] + FO_M = [0; 0; H]; +#+end_src + +** Define the location of {A} and {B} :noexport: +#+begin_src matlab + %% 2. Location of {A} and {B} + FO_A = [0; 0; 100e-3] + FO_M;% [m,m,m] + MO_B = [0; 0; 100e-3];% [m,m,m] +#+end_src + +** Define the position of $a_{i}$ and $b_{i}$ :noexport: +#+begin_src matlab + %% 3. Position of ai and bi + Fa = zeros(3, 6); % Fa_i is the i'th vector of Fa + Mb = zeros(3, 6); % Mb_i is the i'th vector of Mb +#+end_src + +#+begin_src matlab + Aa = Fa - repmat(FO_A, [1, 6]); + Bb = Mb - repmat(MO_B, [1, 6]); + + Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); + Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]); + + As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As + l = vecnorm(Ab - Aa); + + Bs = (Bb - Ba)./vecnorm(Bb - Ba); + + FRa = zeros(3,3,6); + MRb = zeros(3,3,6); + + for i = 1:6 + FRa(:,:,i) = [cross([0;1;0],As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)]; + FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i)); + + MRb(:,:,i) = [cross([0;1;0],Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)]; + MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); + end +#+end_src + +** Define the dynamical properties of each strut :noexport: +#+begin_src matlab + %% 4. Stiffness and Damping of each strut + Ki = 1e6*ones(6,1); + Ci = 1e2*ones(6,1); +#+end_src + +** Old Introduction :noexport: +First, geometrical parameters are defined: +- ${}^A\bm{a}_i$ - Position of the joints fixed to the fixed base w.r.t $\{A\}$ +- ${}^A\bm{b}_i$ - Position of the joints fixed to the mobile platform w.r.t $\{A\}$ +- ${}^B\bm{b}_i$ - Position of the joints fixed to the mobile platform w.r.t $\{B\}$ +- $H$ - Total height of the mobile platform + +These parameter are enough to determine all the kinematic properties of the platform like the Jacobian, stroke, stiffness, ... +These geometrical parameters can be generated using different functions: =initializeCubicConfiguration= for cubic configuration or =initializeGeneralConfiguration= for more general configuration. + +A function =computeGeometricalProperties= is then used to compute: +- $\bm{J}_f$ - Jacobian matrix for the force location +- $\bm{J}_d$ - Jacobian matrix for displacement estimation +- $\bm{R}_m$ - Rotation matrices to position the leg vectors + +Then, geometrical parameters are computed for all the mechanical elements with the function =initializeMechanicalElements=: +- Shape of the platforms + - External Radius + - Internal Radius + - Density + - Thickness +- Shape of the Legs + - Radius + - Size of ball joint + - Density + +Other Parameters are defined for the Simscape simulation: +- Sample mass, volume and position (=initializeSample= function) +- Location of the inertial sensor +- Location of the point for the differential measurements +- Location of the Jacobian point for velocity/displacement computation + +** Cubic Configuration :noexport: +To define the cubic configuration, we need to define 4 parameters: +- The size of the cube +- The location of the cube +- The position of the plane joint the points $a_{i}$ +- The position of the plane joint the points $b_{i}$ + +To do so, we specify the following parameters: +- $H_{C}$ the height of the useful part of the cube +- ${}^{F}O_{C}$ the position of the center of the cube with respect to $\{F\}$ +- ${}^{F}H_{A}$: the height of the plane joining the points $a_{i}$ with respect to the frame $\{F\}$ +- ${}^{M}H_{B}$: the height of the plane joining the points $b_{i}$ with respect to the frame $\{M\}$ + +We define the parameters +#+begin_src matlab + Hc = 60e-3; % [m] + FOc = 50e-3; % [m] + FHa = 15e-3; % [m] + MHb = 15e-3; % [m] +#+end_src + +We define the useful points of the cube with respect to the Cube's center. +${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}. +#+begin_src matlab + sx = [ 2; -1; -1]; + sy = [ 0; 1; -1]; + sz = [ 1; 1; 1]; + + R = [sx, sy, sz]./vecnorm([sx, sy, sz]); + + L = Hc*sqrt(3); + + Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*Hc]; + + CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg + CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg +#+end_src + +We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). +#+begin_src matlab + CSi = (CCm - CCf)./vecnorm(CCm - CCf); +#+end_src + +We now which to compute the position of the joints $a_{i}$ and $b_{i}$. +#+begin_src matlab + Fa = zeros(3, 6); % Fa_i is the i'th vector of Fa + Mb = zeros(3, 6); % Mb_i is the i'th vector of Mb +#+end_src + +#+begin_src matlab + Fa = CCf + [0; 0; FOc] + ((FHa-(FOc-Hc/2))./CSi(3,:)).*CSi; + Mb = CCf + [0; 0; FOc-H] + ((H-MHb-(FOc-Hc/2))./CSi(3,:)).*CSi; % TODO +#+end_src + +** initializeGeneralConfiguration :noexport: +:PROPERTIES: +:HEADER-ARGS:matlab+: :exports code +:HEADER-ARGS:matlab+: :comments no +:HEADER-ARGS:matlab+: :eval no +:HEADER-ARGS:matlab+: :tangle src/initializeGeneralConfiguration.m +:END: + +*** Function description +The =initializeGeneralConfiguration= function takes one structure that contains configurations for the hexapod and returns one structure representing the Hexapod. + +#+begin_src matlab + function [stewart] = initializeGeneralConfiguration(opts_param) +#+end_src + +*** Optional Parameters +Default values for opts. +#+begin_src matlab + opts = struct(... + 'H_tot', 90, ... % Height of the platform [mm] + 'H_joint', 15, ... % Height of the joints [mm] + 'H_plate', 10, ... % Thickness of the fixed and mobile platforms [mm] + 'R_bot', 100, ... % Radius where the legs articulations are positionned [mm] + 'R_top', 80, ... % Radius where the legs articulations are positionned [mm] + 'a_bot', 10, ... % Angle Offset [deg] + 'a_top', 40, ... % Angle Offset [deg] + 'da_top', 0 ... % Angle Offset from 0 position [deg] + ); +#+end_src + +Populate opts with input parameters +#+begin_src matlab + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end +#+end_src + +*** Geometry Description +#+name: fig:stewart_bottom_plate +#+caption: Schematic of the bottom plates with all the parameters +[[file:./figs/stewart_bottom_plate.png]] + +*** Compute Aa and Ab +We compute $[a_1, a_2, a_3, a_4, a_5, a_6]^T$ and $[b_1, b_2, b_3, b_4, b_5, b_6]^T$. + +#+begin_src matlab + Aa = zeros(6, 3); % [mm] + Ab = zeros(6, 3); % [mm] + Bb = zeros(6, 3); % [mm] +#+end_src + +#+begin_src matlab + for i = 1:3 + Aa(2*i-1,:) = [opts.R_bot*cos( pi/180*(120*(i-1) - opts.a_bot) ), ... + opts.R_bot*sin( pi/180*(120*(i-1) - opts.a_bot) ), ... + opts.H_plate+opts.H_joint]; + Aa(2*i,:) = [opts.R_bot*cos( pi/180*(120*(i-1) + opts.a_bot) ), ... + opts.R_bot*sin( pi/180*(120*(i-1) + opts.a_bot) ), ... + opts.H_plate+opts.H_joint]; + + Ab(2*i-1,:) = [opts.R_top*cos( pi/180*(120*(i-1) + opts.da_top - opts.a_top) ), ... + opts.R_top*sin( pi/180*(120*(i-1) + opts.da_top - opts.a_top) ), ... + opts.H_tot - opts.H_plate - opts.H_joint]; + Ab(2*i,:) = [opts.R_top*cos( pi/180*(120*(i-1) + opts.da_top + opts.a_top) ), ... + opts.R_top*sin( pi/180*(120*(i-1) + opts.da_top + opts.a_top) ), ... + opts.H_tot - opts.H_plate - opts.H_joint]; + end + + Bb = Ab - opts.H_tot*[0,0,1]; +#+end_src + +*** Returns Stewart Structure +#+begin_src matlab :results none + stewart = struct(); + stewart.Aa = Aa; + stewart.Ab = Ab; + stewart.Bb = Bb; + stewart.H_tot = opts.H_tot; +end +#+end_src + +** initializeCubicConfiguration :noexport: +:PROPERTIES: +:HEADER-ARGS:matlab+: :exports code +:HEADER-ARGS:matlab+: :comments no +:HEADER-ARGS:matlab+: :eval no +:HEADER-ARGS:matlab+: :tangle src/initializeCubicConfiguration.m +:END: +<> + +*** Function description +#+begin_src matlab + function [stewart] = initializeCubicConfiguration(opts_param) +#+end_src + +*** Optional Parameters +Default values for opts. +#+begin_src matlab + opts = struct(... + 'H_tot', 90, ... % Total height of the Hexapod [mm] + 'L', 110, ... % Size of the Cube [mm] + 'H', 40, ... % Height between base joints and platform joints [mm] + 'H0', 75 ... % Height between the corner of the cube and the plane containing the base joints [mm] + ); +#+end_src + +Populate opts with input parameters +#+begin_src matlab + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end +#+end_src + +*** Cube Creation +#+begin_src matlab :results none + points = [0, 0, 0; ... + 0, 0, 1; ... + 0, 1, 0; ... + 0, 1, 1; ... + 1, 0, 0; ... + 1, 0, 1; ... + 1, 1, 0; ... + 1, 1, 1]; + points = opts.L*points; +#+end_src + +We create the rotation matrix to rotate the cube +#+begin_src matlab :results none + sx = cross([1, 1, 1], [1 0 0]); + sx = sx/norm(sx); + + sy = -cross(sx, [1, 1, 1]); + sy = sy/norm(sy); + + sz = [1, 1, 1]; + sz = sz/norm(sz); + + R = [sx', sy', sz']'; +#+end_src + +We use to rotation matrix to rotate the cube +#+begin_src matlab :results none + cube = zeros(size(points)); + for i = 1:size(points, 1) + cube(i, :) = R * points(i, :)'; + end +#+end_src + +*** Vectors of each leg +#+begin_src matlab :results none + leg_indices = [3, 4; ... + 2, 4; ... + 2, 6; ... + 5, 6; ... + 5, 7; ... + 3, 7]; +#+end_src + +Vectors are: +#+begin_src matlab :results none + legs = zeros(6, 3); + legs_start = zeros(6, 3); + + for i = 1:6 + legs(i, :) = cube(leg_indices(i, 2), :) - cube(leg_indices(i, 1), :); + legs_start(i, :) = cube(leg_indices(i, 1), :); + end +#+end_src + +*** Verification of Height of the Stewart Platform +If the Stewart platform is not contained in the cube, throw an error. + +#+begin_src matlab :results none + Hmax = cube(4, 3) - cube(2, 3); + if opts.H0 < cube(2, 3) + error(sprintf('H0 is not high enought. Minimum H0 = %.1f', cube(2, 3))); + else if opts.H0 + opts.H > cube(4, 3) + error(sprintf('H0+H is too high. Maximum H0+H = %.1f', cube(4, 3))); + error('H0+H is too high'); + end +#+end_src + +*** Determinate the location of the joints +We now determine the location of the joints on the fixed platform w.r.t the fixed frame $\{A\}$. +$\{A\}$ is fixed to the bottom of the base. +#+begin_src matlab :results none + Aa = zeros(6, 3); + for i = 1:6 + t = (opts.H0-legs_start(i, 3))/(legs(i, 3)); + Aa(i, :) = legs_start(i, :) + t*legs(i, :); + end +#+end_src + +And the location of the joints on the mobile platform with respect to $\{A\}$. +#+begin_src matlab :results none + Ab = zeros(6, 3); + for i = 1:6 + t = (opts.H0+opts.H-legs_start(i, 3))/(legs(i, 3)); + Ab(i, :) = legs_start(i, :) + t*legs(i, :); + end +#+end_src + +And the location of the joints on the mobile platform with respect to $\{B\}$. +#+begin_src matlab :results none + Bb = zeros(6, 3); + Bb = Ab - (opts.H0 + opts.H_tot/2 + opts.H/2)*[0, 0, 1]; +#+end_src + +#+begin_src matlab :results none + h = opts.H0 + opts.H/2 - opts.H_tot/2; + Aa = Aa - h*[0, 0, 1]; + Ab = Ab - h*[0, 0, 1]; +#+end_src + +*** Returns Stewart Structure +#+begin_src matlab :results none + stewart = struct(); + stewart.Aa = Aa; + stewart.Ab = Ab; + stewart.Bb = Bb; + stewart.H_tot = opts.H_tot; +end +#+end_src + +** computeGeometricalProperties :noexport: +:PROPERTIES: +:HEADER-ARGS:matlab+: :exports code +:HEADER-ARGS:matlab+: :comments no +:HEADER-ARGS:matlab+: :eval no +:HEADER-ARGS:matlab+: :tangle src/computeGeometricalProperties.m +:END: + +*** Function description +#+begin_src matlab + function [stewart] = computeGeometricalProperties(stewart, opts_param) +#+end_src + +*** Optional Parameters +Default values for opts. +#+begin_src matlab + opts = struct(... + 'Jd_pos', [0, 0, 30], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm] + 'Jf_pos', [0, 0, 30] ... % Position of the Jacobian for force location from the top of the mobile platform [mm] + ); +#+end_src + +Populate opts with input parameters +#+begin_src matlab + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end +#+end_src + +*** Rotation matrices +We initialize $l_i$ and $\hat{s}_i$ +#+begin_src matlab + leg_length = zeros(6, 1); % [mm] + leg_vectors = zeros(6, 3); +#+end_src + +We compute $b_i - a_i$, and then: +\begin{align*} + l_i &= \left|b_i - a_i\right| \\ + \hat{s}_i &= \frac{b_i - a_i}{l_i} +\end{align*} + +#+begin_src matlab + legs = stewart.Ab - stewart.Aa; + + for i = 1:6 + leg_length(i) = norm(legs(i,:)); + leg_vectors(i,:) = legs(i,:) / leg_length(i); + end +#+end_src + +We compute rotation matrices to have the orientation of the legs. +The rotation matrix transforms the $z$ axis to the axis of the leg. The other axis are not important here. +#+begin_src matlab + stewart.Rm = struct('R', eye(3)); + + for i = 1:6 + sx = cross(leg_vectors(i,:), [1 0 0]); + sx = sx/norm(sx); + + sy = -cross(sx, leg_vectors(i,:)); + sy = sy/norm(sy); + + sz = leg_vectors(i,:); + sz = sz/norm(sz); + + stewart.Rm(i).R = [sx', sy', sz']; + end +#+end_src + +*** Jacobian matrices +Compute Jacobian Matrix +#+begin_src matlab + Jd = zeros(6); + + for i = 1:6 + Jd(i, 1:3) = leg_vectors(i, :); + Jd(i, 4:6) = cross(0.001*(stewart.Bb(i, :) - opts.Jd_pos), leg_vectors(i, :)); + end + + stewart.Jd = Jd; + stewart.Jd_inv = inv(Jd); +#+end_src + +#+begin_src matlab + Jf = zeros(6); + + for i = 1:6 + Jf(i, 1:3) = leg_vectors(i, :); + Jf(i, 4:6) = cross(0.001*(stewart.Bb(i, :) - opts.Jf_pos), leg_vectors(i, :)); + end + + stewart.Jf = Jf; + stewart.Jf_inv = inv(Jf); +#+end_src + +#+begin_src matlab + end +#+end_src + +** initializeMechanicalElements :noexport: +:PROPERTIES: +:HEADER-ARGS:matlab+: :exports code +:HEADER-ARGS:matlab+: :comments no +:HEADER-ARGS:matlab+: :eval no +:HEADER-ARGS:matlab+: :tangle src/initializeMechanicalElements.m +:END: + +*** Function description +#+begin_src matlab + function [stewart] = initializeMechanicalElements(stewart, opts_param) +#+end_src + +*** Optional Parameters +Default values for opts. +#+begin_src matlab + opts = struct(... + 'thickness', 10, ... % Thickness of the base and platform [mm] + 'density', 1000, ... % Density of the material used for the hexapod [kg/m3] + 'k_ax', 1e8, ... % Stiffness of each actuator [N/m] + 'c_ax', 1000, ... % Damping of each actuator [N/(m/s)] + 'stroke', 50e-6 ... % Maximum stroke of each actuator [m] + ); +#+end_src + +Populate opts with input parameters +#+begin_src matlab + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end +#+end_src + +*** Bottom Plate +#+name: fig:stewart_bottom_plate +#+caption: Schematic of the bottom plates with all the parameters +[[file:./figs/stewart_bottom_plate.png]] + +The bottom plate structure is initialized. +#+begin_src matlab + BP = struct(); +#+end_src + +We defined its internal radius (if there is a hole in the bottom plate) and its outer radius. +#+begin_src matlab + BP.Rint = 0; % Internal Radius [mm] + BP.Rext = 150; % External Radius [mm] +#+end_src + +We define its thickness. +#+begin_src matlab + BP.H = opts.thickness; % Thickness of the Bottom Plate [mm] +#+end_src + +We defined the density of the material of the bottom plate. +#+begin_src matlab + BP.density = opts.density; % Density of the material [kg/m3] +#+end_src + +And its color. +#+begin_src matlab + BP.color = [0.7 0.7 0.7]; % Color [RGB] +#+end_src + +Then the profile of the bottom plate is computed and will be used by Simscape +#+begin_src matlab + BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm] +#+end_src + +The structure is added to the stewart structure +#+begin_src matlab + stewart.BP = BP; +#+end_src + +*** Top Plate +The top plate structure is initialized. +#+begin_src matlab + TP = struct(); +#+end_src + +We defined the internal and external radius of the top plate. +#+begin_src matlab + TP.Rint = 0; % [mm] + TP.Rext = 100; % [mm] +#+end_src + +The thickness of the top plate. +#+begin_src matlab + TP.H = 10; % [mm] +#+end_src + +The density of its material. +#+begin_src matlab + TP.density = opts.density; % Density of the material [kg/m3] +#+end_src + +Its color. +#+begin_src matlab + TP.color = [0.7 0.7 0.7]; % Color [RGB] +#+end_src + +Then the shape of the top plate is computed +#+begin_src matlab + TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H]; +#+end_src + +The structure is added to the stewart structure +#+begin_src matlab + stewart.TP = TP; +#+end_src + +*** Legs +#+name: fig:stewart_legs +#+caption: Schematic for the legs of the Stewart platform +[[file:./figs/stewart_legs.png]] + +The leg structure is initialized. +#+begin_src matlab + Leg = struct(); +#+end_src + +The maximum Stroke of each leg is defined. +#+begin_src matlab + Leg.stroke = opts.stroke; % [m] +#+end_src + +The stiffness and damping of each leg are defined +#+begin_src matlab + Leg.k_ax = opts.k_ax; % Stiffness of each leg [N/m] + Leg.c_ax = opts.c_ax; % Damping of each leg [N/(m/s)] +#+end_src + +The radius of the legs are defined +#+begin_src matlab + Leg.Rtop = 10; % Radius of the cylinder of the top part of the leg[mm] + Leg.Rbot = 12; % Radius of the cylinder of the bottom part of the leg [mm] +#+end_src + +The density of its material. +#+begin_src matlab + Leg.density = opts.density; % Density of the material used for the legs [kg/m3] +#+end_src + +Its color. +#+begin_src matlab + Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB] +#+end_src + +The radius of spheres representing the ball joints are defined. +#+begin_src matlab + Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm] +#+end_src + +We estimate the length of the legs. +#+begin_src matlab + legs = stewart.Ab - stewart.Aa; + Leg.lenght = norm(legs(1,:))/1.5; +#+end_src + +Then the shape of the bottom leg is estimated +#+begin_src matlab + Leg.shape.bot = ... + [0 0; ... + Leg.Rbot 0; ... + Leg.Rbot Leg.lenght; ... + Leg.Rtop Leg.lenght; ... + Leg.Rtop 0.2*Leg.lenght; ... + 0 0.2*Leg.lenght]; +#+end_src + +The structure is added to the stewart structure +#+begin_src matlab + stewart.Leg = Leg; +#+end_src + +*** Ball Joints +#+name: fig:stewart_ball_joints +#+caption: Schematic of the support for the ball joints +[[file:./figs/stewart_ball_joints.png]] + +=SP= is the structure representing the support for the ball joints at the extremity of each leg. + +The =SP= structure is initialized. +#+begin_src matlab + SP = struct(); +#+end_src + +We can define its rotational stiffness and damping. For now, we use perfect joints. +#+begin_src matlab + SP.k = 0; % [N*m/deg] + SP.c = 0; % [N*m/deg] +#+end_src + +Its height is defined +#+begin_src matlab + SP.H = stewart.Aa(1, 3) - BP.H; % [mm] +#+end_src + +Its radius is based on the radius on the sphere at the end of the legs. +#+begin_src matlab + SP.R = Leg.R; % [mm] +#+end_src + +#+begin_src matlab + SP.section = [0 SP.H-SP.R; + 0 0; + SP.R 0; + SP.R SP.H]; +#+end_src + +The density of its material is defined. +#+begin_src matlab + SP.density = opts.density; % [kg/m^3] +#+end_src + +Its color is defined. +#+begin_src matlab + SP.color = [0.7 0.7 0.7]; % [RGB] +#+end_src + +The structure is added to the Hexapod structure +#+begin_src matlab + stewart.SP = SP; +#+end_src + +** initializeSample :noexport: +:PROPERTIES: +:HEADER-ARGS:matlab+: :exports code +:HEADER-ARGS:matlab+: :comments no +:HEADER-ARGS:matlab+: :eval no +:HEADER-ARGS:matlab+: :tangle src/initializeSample.m +:END: + +*** Function description +#+begin_src matlab + function [] = initializeSample(opts_param) +#+end_src + +*** Optional Parameters +Default values for opts. +#+begin_src matlab + sample = struct( ... + 'radius', 100, ... % radius of the cylinder [mm] + 'height', 100, ... % height of the cylinder [mm] + 'mass', 10, ... % mass of the cylinder [kg] + 'measheight', 50, ... % measurement point z-offset [mm] + 'offset', [0, 0, 0], ... % offset position of the sample [mm] + 'color', [0.9 0.1 0.1] ... + ); +#+end_src + +Populate opts with input parameters +#+begin_src matlab + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + sample.(opt{1}) = opts_param.(opt{1}); + end + end +#+end_src + +*** Save the Sample structure +#+begin_src matlab + save('./mat/sample.mat', 'sample'); +#+end_src + +#+begin_src matlab + end +#+end_src