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 bddaa6f..2d9d91f 100644 Binary files a/simscape_subsystems/stewart_strut_no_sensors.slx and b/simscape_subsystems/stewart_strut_no_sensors.slx differ diff --git a/simulink/stewart_platform_identification.slx b/simulink/stewart_platform_identification.slx new file mode 100644 index 0000000..87bc8ff Binary files /dev/null and b/simulink/stewart_platform_identification.slx differ diff --git a/simulink/stewart_platform_identification_simple.slx b/simulink/stewart_platform_identification_simple.slx new file mode 100644 index 0000000..f7cbad2 Binary files /dev/null and b/simulink/stewart_platform_identification_simple.slx differ diff --git a/src/computeJointsPose.m b/src/computeJointsPose.m index 26619bb..5b5754e 100644 --- a/src/computeJointsPose.m +++ b/src/computeJointsPose.m @@ -1,10 +1,12 @@ function [stewart] = computeJointsPose(stewart) % computeJointsPose - % -% Syntax: [stewart] = computeJointsPose(stewart, opts_param) +% 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} diff --git a/src/forwardKinematics.m b/src/forwardKinematics.m deleted file mode 100644 index 8814647..0000000 --- a/src/forwardKinematics.m +++ /dev/null @@ -1,30 +0,0 @@ -function [P, R] = forwardKinematics(stewart, args) -% forwardKinematics - Computed the pose of {B} with respect to {A} from the length of each strut -% -% Syntax: [in_data] = forwardKinematics(stewart) -% -% Inputs: -% - stewart - A structure with the following fields -% - J [6x6] - The Jacobian Matrix -% - args - Can have the following fields: -% - L [6x1] - Length 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} - -arguments - stewart - args.L (6,1) double {mustBeNumeric} = zeros(6,1) -end - -X = stewart.J\args.L; - -P = X(1:3); - -theta = norm(X(4:6)); -s = X(4:6)/theta; - -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)]; diff --git a/src/generateGeneralConfiguration.m b/src/generateGeneralConfiguration.m index 99f9afa..ce9de8b 100644 --- a/src/generateGeneralConfiguration.m +++ b/src/generateGeneralConfiguration.m @@ -4,8 +4,6 @@ function [stewart] = generateGeneralConfiguration(stewart, args) % 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] @@ -22,10 +20,10 @@ function [stewart] = generateGeneralConfiguration(stewart, args) arguments stewart args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 - args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-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} = 70e-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 diff --git a/src/getMaxPositions.m b/src/getMaxPositions.m deleted file mode 100644 index 5038912..0000000 --- a/src/getMaxPositions.m +++ /dev/null @@ -1,18 +0,0 @@ -function [X, Y, Z] = getMaxPositions(stewart) - Leg = stewart.Leg; - J = stewart.Jd; - theta = linspace(0, 2*pi, 100); - phi = linspace(-pi/2 , pi/2, 100); - dmax = zeros(length(theta), length(phi)); - - for i = 1:length(theta) - for j = 1:length(phi) - L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]'; - dmax(i, j) = Leg.stroke/max(abs(L)); - end - end - - X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))'; - Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))'; - Z = dmax.*sin(repmat(phi,length(theta),1)); -end diff --git a/src/getMaxPureDisplacement.m b/src/getMaxPureDisplacement.m deleted file mode 100644 index 8a14569..0000000 --- a/src/getMaxPureDisplacement.m +++ /dev/null @@ -1,9 +0,0 @@ -function [max_disp] = getMaxPureDisplacement(Leg, J) - max_disp = zeros(6, 1); - max_disp(1) = Leg.stroke/max(abs(J*[1 0 0 0 0 0]')); - max_disp(2) = Leg.stroke/max(abs(J*[0 1 0 0 0 0]')); - max_disp(3) = Leg.stroke/max(abs(J*[0 0 1 0 0 0]')); - max_disp(4) = Leg.stroke/max(abs(J*[0 0 0 1 0 0]')); - max_disp(5) = Leg.stroke/max(abs(J*[0 0 0 0 1 0]')); - max_disp(6) = Leg.stroke/max(abs(J*[0 0 0 0 0 1]')); -end diff --git a/src/getStiffnessMatrix.m b/src/getStiffnessMatrix.m deleted file mode 100644 index 8b282d5..0000000 --- a/src/getStiffnessMatrix.m +++ /dev/null @@ -1,5 +0,0 @@ -function [K] = getStiffnessMatrix(k, J) -% k - leg stiffness -% J - Jacobian matrix - K = k*(J'*J); -end diff --git a/src/identifyPlant.m b/src/identifyPlant.m deleted file mode 100644 index 868b2a8..0000000 --- a/src/identifyPlant.m +++ /dev/null @@ -1,67 +0,0 @@ -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:1]] -function [sys] = identifyPlant(opts_param) -% identifyPlant:1 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:2]] -%% Default values for opts -opts = struct(); - -%% Populate opts with input parameters -if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end -end -% identifyPlant:2 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:3]] -options = linearizeOptions; -options.SampleTime = 0; -% identifyPlant:3 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:4]] -mdl = 'stewart'; -% identifyPlant:4 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:5]] -%% Inputs -io(1) = linio([mdl, '/F'], 1, 'input'); % Cartesian forces -io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces -io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces -io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion - -%% Outputs -io(5) = linio([mdl, '/Dm'], 1, 'output'); % Relative Motion -io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg -io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg -io(8) = linio([mdl, '/Xm'], 1, 'output'); % Absolute motion of platform -% identifyPlant:5 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:6]] -G = linearize(mdl, io, 0); -% identifyPlant:6 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:7]] -G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ... - 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ... - 'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ... - 'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}; -G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ... - 'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ... - 'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ... - 'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}; -% identifyPlant:7 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:8]] -sys.G_cart = G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}); -sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})); -sys.G_legs = minreal(G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})); -sys.G_tran = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'})); -sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'})); -sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'})); -% sys.G_all = minreal(G); -% identifyPlant:8 ends here - -% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:9]] -end -% identifyPlant:9 ends here diff --git a/src/initializeHexapod.m b/src/initializeHexapod.m deleted file mode 100644 index d7517da..0000000 --- a/src/initializeHexapod.m +++ /dev/null @@ -1,171 +0,0 @@ -function [stewart] = initializeHexapod(opts_param) - -opts = struct(... - 'height', 90, ... % Height of the platform [mm] - 'density', 10, ... % 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] - 'name', 'stewart' ... % Name of the file - ); - -if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end -end - -stewart = struct(); - -stewart.H = opts.height; % [mm] - -BP = struct(); - -BP.Rint = 0; % Internal Radius [mm] -BP.Rext = 150; % External Radius [mm] - -BP.H = 10; % Thickness of the Bottom Plate [mm] - -BP.Rleg = 100; % Radius where the legs articulations are positionned [mm] -BP.alpha = 30; % Angle Offset [deg] - -BP.density = opts.density; % Density of the material [kg/m3] - -BP.color = [0.7 0.7 0.7]; % Color [RGB] - -BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm] - -stewart.BP = BP; - -TP = struct(); - -TP.Rint = 0; % [mm] -TP.Rext = 100; % [mm] - -TP.H = 10; % [mm] - -TP.Rleg = 80; % Radius where the legs articulations are positionned [mm] -TP.alpha = 10; % Angle [deg] -TP.dalpha = 0; % Angle Offset from 0 position [deg] - -TP.density = opts.density; % Density of the material [kg/m3] - -TP.color = [0.7 0.7 0.7]; % Color [RGB] - -TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H]; - -stewart.TP = TP; - -Leg = struct(); - -Leg.stroke = opts.stroke; % [m] - -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)] - -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] - -Leg.density = 0.01*opts.density; % Density of the material used for the legs [kg/m3] - -Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB] - -Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm] - -stewart.Leg = Leg; - -SP = struct(); - -SP.k = 0; % [N*m/deg] -SP.c = 0; % [N*m/deg] - -SP.H = 15; % [mm] - -SP.R = Leg.R; % [mm] - -SP.section = [0 SP.H-SP.R; - 0 0; - SP.R 0; - SP.R SP.H]; - -SP.density = opts.density; % [kg/m^3] - -SP.color = [0.7 0.7 0.7]; % [RGB] - -stewart.SP = SP; - -stewart = initializeParameters(stewart); - -save('./mat/stewart.mat', 'stewart') - -function [stewart] = initializeParameters(stewart) - -stewart.Aa = zeros(6, 3); % [mm] -stewart.Ab = zeros(6, 3); % [mm] -stewart.Bb = zeros(6, 3); % [mm] - -for i = 1:3 - stewart.Aa(2*i-1,:) = [stewart.BP.Rleg*cos( pi/180*(120*(i-1) - stewart.BP.alpha) ), ... - stewart.BP.Rleg*sin( pi/180*(120*(i-1) - stewart.BP.alpha) ), ... - stewart.BP.H+stewart.SP.H]; - stewart.Aa(2*i,:) = [stewart.BP.Rleg*cos( pi/180*(120*(i-1) + stewart.BP.alpha) ), ... - stewart.BP.Rleg*sin( pi/180*(120*(i-1) + stewart.BP.alpha) ), ... - stewart.BP.H+stewart.SP.H]; - - stewart.Ab(2*i-1,:) = [stewart.TP.Rleg*cos( pi/180*(120*(i-1) + stewart.TP.dalpha - stewart.TP.alpha) ), ... - stewart.TP.Rleg*sin( pi/180*(120*(i-1) + stewart.TP.dalpha - stewart.TP.alpha) ), ... - stewart.H - stewart.TP.H - stewart.SP.H]; - stewart.Ab(2*i,:) = [stewart.TP.Rleg*cos( pi/180*(120*(i-1) + stewart.TP.dalpha + stewart.TP.alpha) ), ... - stewart.TP.Rleg*sin( pi/180*(120*(i-1) + stewart.TP.dalpha + stewart.TP.alpha) ), ... - stewart.H - stewart.TP.H - stewart.SP.H]; -end -stewart.Bb = stewart.Ab - stewart.H*[0,0,1]; - -leg_length = zeros(6, 1); % [mm] -leg_vectors = zeros(6, 3); - -legs = stewart.Ab - stewart.Aa; - -for i = 1:6 - leg_length(i) = norm(legs(i,:)); - leg_vectors(i,:) = legs(i,:) / leg_length(i); -end - -stewart.Leg.lenght = leg_length(1)/1.5; -stewart.Leg.shape.bot = ... - [0 0; ... - stewart.Leg.Rbot 0; ... - stewart.Leg.Rbot stewart.Leg.lenght; ... - stewart.Leg.Rtop stewart.Leg.lenght; ... - stewart.Leg.Rtop 0.2*stewart.Leg.lenght; ... - 0 0.2*stewart.Leg.lenght]; - -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 - -J = zeros(6); - -for i = 1:6 - J(i, 1:3) = leg_vectors(i, :); - J(i, 4:6) = cross(0.001*(stewart.Ab(i, :)- stewart.H*[0,0,1]), leg_vectors(i, :)); -end - -stewart.J = J; -stewart.Jinv = inv(J); - -stewart.K = stewart.Leg.k_ax*stewart.J'*stewart.J; - -end -end diff --git a/src/initializeSimscapeData.m b/src/initializeSimscapeData.m deleted file mode 100644 index ff0bfa5..0000000 --- a/src/initializeSimscapeData.m +++ /dev/null @@ -1,59 +0,0 @@ -function [stewart] = initializeSimscapeData(stewart, opts_param) - -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] - ); - -if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end -end - -leg_length = zeros(6, 1); % [mm] -leg_vectors = zeros(6, 3); - -legs = stewart.Ab - stewart.Aa; - -for i = 1:6 - leg_length(i) = norm(legs(i,:)); - leg_vectors(i,:) = legs(i,:) / leg_length(i); -end - -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 - -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); - -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 diff --git a/src/initializeStewartPlatform.m b/src/initializeStewartPlatform.m deleted file mode 100644 index 0d48c10..0000000 --- a/src/initializeStewartPlatform.m +++ /dev/null @@ -1,94 +0,0 @@ -function [stewart] = initializeStewartPlatform(stewart, opts_param) - -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] - ); - -if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end -end - -BP = struct(); - -BP.Rint = 0; % Internal Radius [mm] -BP.Rext = 150; % External Radius [mm] - -BP.H = opts.thickness; % Thickness of the Bottom Plate [mm] - -BP.density = opts.density; % Density of the material [kg/m3] - -BP.color = [0.7 0.7 0.7]; % Color [RGB] - -BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm] - -stewart.BP = BP; - -TP = struct(); - -TP.Rint = 0; % [mm] -TP.Rext = 100; % [mm] - -TP.H = 10; % [mm] - -TP.density = opts.density; % Density of the material [kg/m3] - -TP.color = [0.7 0.7 0.7]; % Color [RGB] - -TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H]; - -stewart.TP = TP; - -Leg = struct(); - -Leg.stroke = opts.stroke; % [m] - -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)] - -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] - -Leg.density = opts.density; % Density of the material used for the legs [kg/m3] - -Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB] - -Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm] - -legs = stewart.Ab - stewart.Aa; -Leg.lenght = norm(legs(1,:))/1.5; - -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]; - -stewart.Leg = Leg; - -SP = struct(); - -SP.k = 0; % [N*m/deg] -SP.c = 0; % [N*m/deg] - -SP.H = stewart.Aa(1, 3) - BP.H; % [mm] - -SP.R = Leg.R; % [mm] - -SP.section = [0 SP.H-SP.R; - 0 0; - SP.R 0; - SP.R SP.H]; - -SP.density = opts.density; % [kg/m^3] - -SP.color = [0.7 0.7 0.7]; % [RGB] - -stewart.SP = SP; diff --git a/src/initializeStrutDynamics.m b/src/initializeStrutDynamics.m index e4ba57f..6863fd0 100644 --- a/src/initializeStrutDynamics.m +++ b/src/initializeStrutDynamics.m @@ -16,7 +16,7 @@ function [stewart] = initializeStrutDynamics(stewart, args) arguments stewart args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1) - args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1) + args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1) end stewart.Ki = args.Ki; diff --git a/static-analysis.org b/static-analysis.org new file mode 100644 index 0000000..9069f8e --- /dev/null +++ b/static-analysis.org @@ -0,0 +1,87 @@ +#+TITLE: Stewart Platform - Static Analysis +: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: + +* 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 + + + + + + + + + + + + + + +
+ UP + | + HOME +
+

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