tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Test of position/orientation of an Hexapod

Browse Source
This commit is contained in:
Thomas Dehaeze 2019-12-10 18:06:33 +01:00
parent b14de47379
commit 9fb86964ef
7 changed files with 819 additions and 228 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

315
kinematics/index.html
View File

@ -3,7 +3,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2019-10-08 mar. 11:13 --> <!-- 2019-12-10 mar. 18:03 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Kinematics of the station</title> <title>Kinematics of the station</title>
@ -246,6 +246,31 @@ for the JavaScript code in this tag.
} }
/*]]>*///--> /*]]>*///-->
</script> </script>
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
displayAlign: "center",
displayIndent: "0em",
"HTML-CSS": { scale: 100,
linebreaks: { automatic: "false" },
webFont: "TeX"
},
SVG: {scale: 100,
linebreaks: { automatic: "false" },
font: "TeX"},
NativeMML: {scale: 100},
TeX: { equationNumbers: {autoNumber: "AMS"},
MultLineWidth: "85%",
TagSide: "right",
TagIndent: ".8em",
Macros: {
bm: ["{\\boldsymbol #1}",1],
}
}
});
</script>
<script type="text/javascript"
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
</head> </head>
<body> <body>
<div id="org-div-home-and-up"> <div id="org-div-home-and-up">
@ -254,17 +279,299 @@ for the JavaScript code in this tag.
<a accesskey="H" href="../index.html"> HOME </a> <a accesskey="H" href="../index.html"> HOME </a>
</div><div id="content"> </div><div id="content">
<h1 class="title">Kinematics of the station</h1> <h1 class="title">Kinematics of the station</h1>
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#orgc0809a7">1. Micro Hexapod</a>
<ul>
<li><a href="#org5ed8144">1.1. How the Symetrie Hexapod is controlled on the micro station</a></li>
<li><a href="#orge1f9456">1.2. Control of the Micro-Hexapod using Simscape</a>
<ul>
<li><a href="#orgaec6c7a">1.2.1. Using Bushing Joint</a></li>
<li><a href="#org964676d">1.2.2. Using Inverse Kinematics and Leg Actuators</a>
<ul>
<li><a href="#org3fcf5f8">1.2.2.1. Theory</a></li>
<li><a href="#orgc39da76">1.2.2.2. Matlab Implementation</a></li>
</ul>
</li>
</ul>
</li>
</ul>
</li>
</ul>
</div>
</div>
<div class="note">
<p> <p>
All the files (data and Matlab scripts) are accessible <a href="data/kinematics.zip">here</a>. In this document, we discuss the way the motion of each stage is defined.
</p> </p>
<div id="outline-container-orgc0809a7" class="outline-2">
<h2 id="orgc0809a7"><span class="section-number-2">1</span> Micro Hexapod</h2>
<div class="outline-text-2" id="text-1">
</div>
<div id="outline-container-org5ed8144" class="outline-3">
<h3 id="org5ed8144"><span class="section-number-3">1.1</span> How the Symetrie Hexapod is controlled on the micro station</h3>
<div class="outline-text-3" id="text-1-1">
<p>
For the Micro-Hexapod, the convention for the angles are defined in <code>MAN_A_Software API_4.0.150918_EN.pdf</code> on page 13 (section 2.4 - Rotation Vectors):
</p>
<blockquote>
<p>
The <b>Euler type II convention</b> is used to express the rotation vector.
This convention is mainly used in the aeronautics field (standard ISO 1151 concerning flight mechanics).
</p>
<p>
This convention uses the concepts of rotation of vehicles (ship, car and plane).
Generally, we consider that the main movement of the vehicle is following the X-axis and the Z-axis is parallel to the axis of gravity (at the initial position).
The roll rotation is around the X-axis, the pitch is around the Y-axis and yaw is the rotation around the Z-axis.
<b>The order of rotation is: Rx, Ry and then Rz.</b>
</p>
<p>
In most case, rotations are related to a reference with fixed axis; thus we say the rotations are around fixed axes.
The combination of these three rotations enables to write a rotation matrix.
This writing is unique and equal to:
\[ \bm{R} = \bm{R}_z(\gamma) \cdot \bm{R}_y(\beta) \cdot \bm{R}_x(\alpha) \]
</p>
<p>
The Euler type II convention corresponding to the <b>succession of rotations with respect to fixed axes</b>: first around X0, then Y0 and Z0.
This is equivalent to the succession of rotations with respect to mobile axes: first around Z0, then Y1' and X2'.
</p>
</blockquote>
<p>
More generally on the Control of the Micro-Hexapod:
</p>
<blockquote>
<p>
Note that for all control modes, <b>the rotation center coincides with Object coordinate system origin</b>.
Moreover, the movements are controlled with <b>translation components at first</b> (Tx, Ty, Tz) <b>then rotation components</b> (Rx, Ry, Rz).
</p>
</blockquote>
<p>
Thus, it does the translations and then the rotation around the new translated frame.
</p>
</div>
</div>
<div id="outline-container-orge1f9456" class="outline-3">
<h3 id="orge1f9456"><span class="section-number-3">1.2</span> Control of the Micro-Hexapod using Simscape</h3>
<div class="outline-text-3" id="text-1-2">
<p>
We can think of two main ways to position the Micro-Hexapod using Simscape.
</p>
<p>
The first one is to use only one Bushing Joint between the base and the mobile platform.
The advantage is that it is very easy to impose the wanted displacement, however, we loose the dynamical properties of the Hexapod.
</p>
<p>
The second way is to specify the wanted length of the legs of the Hexapod in order to have the wanted position of the mobile platform.
This require a little bit more of mathematical derivations but this is the chosen solution.
</p>
</div>
<div id="outline-container-orgaec6c7a" class="outline-4">
<h4 id="orgaec6c7a"><span class="section-number-4">1.2.1</span> Using Bushing Joint</h4>
<div class="outline-text-4" id="text-1-2-1">
<p>
In the documentation of the Bushing Joint (<code>doc "Bushing Joint"</code>) that is used to position the Hexapods, it is mention that the following frame is positioned with respect to the base frame in a way shown in figure <a href="#orgaa7f4a2">1</a>.
</p>
<div id="orgaa7f4a2" class="figure">
<p><img src="figs/bushing_joint_transform.png" alt="bushing_joint_transform.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Joint Transformation Sequence for the Bushing Joint</p>
</div>
<p>
Basically, it performs the translations, and then the rotation along the X, Y and Z axis of the moving frame.
The three rotations that we define thus corresponds to the Euler U-V-W angles.
</p>
<p>
We should have the <b>same behavior</b> for the Micro-Hexapod on Simscape (same inputs at least).
However, the Bushing Joint makes rotations around mobiles axes (X, Y' and then Z'') and not fixed axes (X, Y and Z).
</p>
</div>
</div>
<div id="outline-container-org964676d" class="outline-4">
<h4 id="org964676d"><span class="section-number-4">1.2.2</span> Using Inverse Kinematics and Leg Actuators</h4>
<div class="outline-text-4" id="text-1-2-2">
<p>
Here, we can use the Inverse Kinematic of the Hexapod to determine the length of each leg in order to obtain some defined translation and rotation of the mobile platform.
</p>
<p>
The advantages are:
</p>
<ul class="org-ul">
<li>we can position the Hexapod as we want by specifying a rotation matrix</li>
<li>the hexapod keeps its full flexibility as we don't specify any wanted displacements, only leg's rest position</li>
</ul>
</div>
<div id="outline-container-org3fcf5f8" class="outline-5">
<h5 id="org3fcf5f8"><span class="section-number-5">1.2.2.1</span> Theory</h5>
<div class="outline-text-5" id="text-1-2-2-1">
<p>
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\).
</p>
<p>
From the geometry of the manipulator, the loop closure for each limb, \(i = 1, 2, \dots, 6\) can be written as
</p>
\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*}
<p>
To obtain the length of each actuator and eliminate \(\hat{\bm{s}}_i\), it is sufficient to dot multiply each side by itself:
</p>
\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}
<p>
Hence, for \(i = 1, 2, \dots, 6\), each limb length can be uniquely determined by:
</p>
\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}
<p>
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.
</p>
</div>
</div>
<div id="outline-container-orgc39da76" class="outline-5">
<h5 id="orgc39da76"><span class="section-number-5">1.2.2.2</span> Matlab Implementation</h5>
<div class="outline-text-5" id="text-1-2-2-2">
<div class="org-src-container">
<pre class="src src-matlab">open <span class="org-string">'simscape/hexapod_tests.slx'</span>
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">load<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/conf_simscape.mat'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-matlab-simulink-keyword">set_param</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">conf_simscape</span>, <span class="org-string">'StopTime'</span>, '<span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">5</span><span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">tx = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">1</span>; <span class="org-comment">% [rad]</span>
ty = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">2</span>; <span class="org-comment">% [rad]</span>
tz = <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">05</span>; <span class="org-comment">% [rad]</span>
Rx = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span>;
<span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>tx<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>tx<span class="org-rainbow-delimiters-depth-2">)</span>;
<span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>tx<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>tx<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Ry = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ty<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> sin<span class="org-rainbow-delimiters-depth-2">(</span>ty<span class="org-rainbow-delimiters-depth-2">)</span>;
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span>;
<span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>ty<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>ty<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
Rz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>tz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>tz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;
sin<span class="org-rainbow-delimiters-depth-2">(</span>tz<span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>tz<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-highlight-numbers-number">0</span>;
<span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;
ARB = Rz<span class="org-type">*</span>Ry<span class="org-type">*</span>Rx;
AP = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">01</span>; <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">02</span>; <span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">03</span><span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
hexapod = initializeMicroHexapod<span class="org-rainbow-delimiters-depth-1">(</span>struct<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-string">'AP'</span>, AP, <span class="org-string">'ARB'</span>, ARB<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>;
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'simscape/hexapod_tests.slx'</span><span class="org-rainbow-delimiters-depth-1">)</span>
</pre>
</div>
<p>
And we verify that we indeed succeed to go to the wanted position.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-rainbow-delimiters-depth-1">[</span>simout.x.Data<span class="org-rainbow-delimiters-depth-2">(</span>end<span class="org-rainbow-delimiters-depth-2">)</span> ; simout.y.Data<span class="org-rainbow-delimiters-depth-2">(</span>end<span class="org-rainbow-delimiters-depth-2">)</span> ; simout.z.Data<span class="org-rainbow-delimiters-depth-2">(</span>end<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span> <span class="org-type">-</span> AP
</pre>
</div>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-right" />
</colgroup>
<tbody>
<tr>
<td class="org-right">-2.12e-06</td>
</tr>
<tr>
<td class="org-right">2.9787e-06</td>
</tr>
<tr>
<td class="org-right">-4.4341e-06</td>
</tr>
</tbody>
</table>
<div class="org-src-container">
<pre class="src src-matlab">simout.R.Data<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, end<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">-</span>ARB
</pre>
</div>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
</colgroup>
<tbody>
<tr>
<td class="org-right">-1.5714e-06</td>
<td class="org-right">1.4513e-06</td>
<td class="org-right">7.8133e-06</td>
</tr>
<tr>
<td class="org-right">8.4113e-07</td>
<td class="org-right">-7.1485e-07</td>
<td class="org-right">-7.4572e-06</td>
</tr>
<tr>
<td class="org-right">-7.5348e-06</td>
<td class="org-right">7.7112e-06</td>
<td class="org-right">-2.3088e-06</td>
</tr>
</tbody>
</table>
</div>
</div>
</div>
</div>
</div> </div>
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-10-08 mar. 11:13</p> <p class="date">Created: 2019-12-10 mar. 18:03</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p> <p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
</div> </div>
</body> </body>

156
kinematics/index.org
View File

@ -42,23 +42,91 @@
:END: :END:
* Introduction :ignore: * Introduction :ignore:
In this document, we discuss the way the motion of each stage is defined.
* ZIP file containing the data and matlab files :ignore: * Micro Hexapod
#+begin_src bash :exports none :results none ** How the Symetrie Hexapod is controlled on the micro station
if [ matlab/kinematics.m -nt data/kinematics.zip ]; then For the Micro-Hexapod, the convention for the angles are defined in =MAN_A_Software API_4.0.150918_EN.pdf= on page 13 (section 2.4 - Rotation Vectors):
cp matlab/kinematics.m kinematics.m;
zip data/kinematics \
mat/data.mat \
kinematics.m
rm kinematics.m;
fi
#+end_src
#+begin_note #+begin_quote
All the files (data and Matlab scripts) are accessible [[file:data/kinematics.zip][here]]. The *Euler type II convention* is used to express the rotation vector.
#+end_note This convention is mainly used in the aeronautics field (standard ISO 1151 concerning flight mechanics).
* Matlab Init :noexport:ignore: This convention uses the concepts of rotation of vehicles (ship, car and plane).
Generally, we consider that the main movement of the vehicle is following the X-axis and the Z-axis is parallel to the axis of gravity (at the initial position).
The roll rotation is around the X-axis, the pitch is around the Y-axis and yaw is the rotation around the Z-axis.
*The order of rotation is: Rx, Ry and then Rz.*
In most case, rotations are related to a reference with fixed axis; thus we say the rotations are around fixed axes.
The combination of these three rotations enables to write a rotation matrix.
This writing is unique and equal to:
\[ \bm{R} = \bm{R}_z(\gamma) \cdot \bm{R}_y(\beta) \cdot \bm{R}_x(\alpha) \]
The Euler type II convention corresponding to the *succession of rotations with respect to fixed axes*: first around X0, then Y0 and Z0.
This is equivalent to the succession of rotations with respect to mobile axes: first around Z0, then Y1' and X2'.
#+end_quote
More generally on the Control of the Micro-Hexapod:
#+begin_quote
Note that for all control modes, *the rotation center coincides with Object coordinate system origin*.
Moreover, the movements are controlled with *translation components at first* (Tx, Ty, Tz) *then rotation components* (Rx, Ry, Rz).
#+end_quote
Thus, it does the translations and then the rotation around the new translated frame.
** Control of the Micro-Hexapod using Simscape
*** Introduction :ignore:
We can think of two main ways to position the Micro-Hexapod using Simscape.
The first one is to use only one Bushing Joint between the base and the mobile platform.
The advantage is that it is very easy to impose the wanted displacement, however, we loose the dynamical properties of the Hexapod.
The second way is to specify the wanted length of the legs of the Hexapod in order to have the wanted position of the mobile platform.
This require a little bit more of mathematical derivations but this is the chosen solution.
*** Using Bushing Joint
In the documentation of the Bushing Joint (=doc "Bushing Joint"=) that is used to position the Hexapods, it is mention that the following frame is positioned with respect to the base frame in a way shown in figure [[fig:bushing_joint_transform]].
#+name: fig:bushing_joint_transform
#+caption: Joint Transformation Sequence for the Bushing Joint
[[file:figs/bushing_joint_transform.png]]
Basically, it performs the translations, and then the rotation along the X, Y and Z axis of the moving frame.
The three rotations that we define thus corresponds to the Euler U-V-W angles.
We should have the *same behavior* for the Micro-Hexapod on Simscape (same inputs at least).
However, the Bushing Joint makes rotations around mobiles axes (X, Y' and then Z'') and not fixed axes (X, Y and Z).
*** Using Inverse Kinematics and Leg Actuators
Here, we can use the Inverse Kinematic of the Hexapod to determine the length of each leg in order to obtain some defined translation and rotation of the mobile platform.
The advantages are:
- we can position the Hexapod as we want by specifying a rotation matrix
- the hexapod keeps its full flexibility as we don't specify any wanted displacements, only leg's rest position
**** 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.
**** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
#+end_src #+end_src
@ -66,3 +134,63 @@
#+begin_src matlab :exports none :results silent :noweb yes #+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>> <<matlab-init>>
#+end_src #+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
**** Matlab Implementation
#+begin_src matlab
open 'simscape/hexapod_tests.slx'
#+end_src
#+begin_src matlab
load('simscape/conf_simscape.mat');
set_param(conf_simscape, 'StopTime', '0.5');
#+end_src
#+begin_src matlab
tx = 0.1; % [rad]
ty = 0.2; % [rad]
tz = 0.05; % [rad]
Rx = [1 0 0;
0 cos(tx) -sin(tx);
0 sin(tx) cos(tx)];
Ry = [ cos(ty) 0 sin(ty);
0 1 0;
-sin(ty) 0 cos(ty)];
Rz = [cos(tz) -sin(tz) 0;
sin(tz) cos(tz) 0;
0 0 1];
ARB = Rz*Ry*Rx;
AP = [0.01; 0.02; 0.03]; % [m]
hexapod = initializeMicroHexapod(struct('AP', AP, 'ARB', ARB));
#+end_src
#+begin_src matlab
sim('simscape/hexapod_tests.slx')
#+end_src
And we verify that we indeed succeed to go to the wanted position.
#+begin_src matlab :results table replace
[simout.x.Data(end) ; simout.y.Data(end) ; simout.z.Data(end)] - AP
#+end_src
#+RESULTS:
| -2.12e-06 |
| 2.9787e-06 |
| -4.4341e-06 |
#+begin_src matlab :results table replace
simout.R.Data(:, :, end)-ARB
#+end_src
#+RESULTS:
| -1.5714e-06 | 1.4513e-06 | 7.8133e-06 |
| 8.4113e-07 | -7.1485e-07 | -7.4572e-06 |
| -7.5348e-06 | 7.7112e-06 | -2.3088e-06 |

BIN
simscape/hexapod_tests.slx Normal file
View File

Binary file not shown.

439
simscape/index.html
View File

@ -3,7 +3,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2019-12-06 ven. 12:04 --> <!-- 2019-12-10 mar. 18:05 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Simscape Model</title> <title>Simscape Model</title>
@ -258,64 +258,65 @@ for the JavaScript code in this tag.
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#org5c21e47">1. Simulink Project - Startup and Shutdown scripts</a></li> <li><a href="#org88d59ba">1. Simulink Project - Startup and Shutdown scripts</a></li>
<li><a href="#org5ca7221">2. Simscape Multibody - Presentation</a> <li><a href="#orga2206d5">2. Simscape Multibody - Presentation</a>
<ul> <ul>
<li><a href="#orga5fbe1d">2.1. Solid bodies</a></li> <li><a href="#org6b4540f">2.1. Solid bodies</a></li>
<li><a href="#org8017e71">2.2. Frames</a></li> <li><a href="#orge470a34">2.2. Frames</a></li>
<li><a href="#org486d9bf">2.3. Joints</a></li> <li><a href="#org474edd3">2.3. Joints</a></li>
<li><a href="#org5a1378a">2.4. Measurements</a></li> <li><a href="#org9171b40">2.4. Measurements</a></li>
<li><a href="#orga435910">2.5. Excitation</a></li> <li><a href="#org7df2aef">2.5. Excitation</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org289000e">3. Simulink files and signal names</a> <li><a href="#org803cadc">3. Simulink files and signal names</a>
<ul> <ul>
<li><a href="#orga9d8af9">3.1. List of Simscape files</a></li> <li><a href="#orgb5c767a">3.1. List of Simscape files</a></li>
<li><a href="#org2a9705c">3.2. List of Inputs</a> <li><a href="#org64dbd0d">3.2. List of Inputs</a>
<ul> <ul>
<li><a href="#org7fd5d25">3.2.1. Perturbations</a></li> <li><a href="#orgeecef4d">3.2.1. Perturbations</a></li>
<li><a href="#org0337003">3.2.2. Measurement Noise</a></li> <li><a href="#org4e58ed6">3.2.2. Measurement Noise</a></li>
<li><a href="#org250f869">3.2.3. Control Inputs</a></li> <li><a href="#orgc9006ba">3.2.3. Control Inputs</a></li>
</ul> </ul>
</li> </li>
<li><a href="#orge7abaa0">3.3. List of Outputs</a></li> <li><a href="#orgeb985b0">3.3. List of Outputs</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org0fbeac7">4. Simulink Library</a> <li><a href="#orgfd5289f">4. Simulink Library</a>
<ul> <ul>
<li><a href="#org3ec0c6f">4.1. <code>inputs</code></a></li> <li><a href="#orgd1ebe0f">4.1. <code>inputs</code></a></li>
<li><a href="#org0abb61d">4.2. <code>nass_library</code></a></li> <li><a href="#org1ae1d37">4.2. <code>nass_library</code></a></li>
<li><a href="#org04c0eb4">4.3. <code>pos_error_wrt_nass_base</code></a></li> <li><a href="#org54e3d07">4.3. <code>pos_error_wrt_nass_base</code></a></li>
<li><a href="#orgef1709b">4.4. <code>QuaternionToAngles</code></a></li> <li><a href="#orgc0f0f02">4.4. <code>QuaternionToAngles</code></a></li>
<li><a href="#org8a40a80">4.5. <code>RotationMatrixToAngle</code></a></li> <li><a href="#orgbda19ed">4.5. <code>RotationMatrixToAngle</code></a></li>
</ul> </ul>
</li> </li>
<li><a href="#org81a7962">5. Functions</a> <li><a href="#org54193c9">5. Functions</a>
<ul> <ul>
<li><a href="#orge3706e5">5.1. computePsdDispl</a></li> <li><a href="#orgc1f5840">5.1. computePsdDispl</a></li>
<li><a href="#org14a492d">5.2. computeSetpoint</a></li> <li><a href="#org0d8e9e6">5.2. computeSetpoint</a></li>
<li><a href="#org3e9a86f">5.3. converErrorBasis</a></li> <li><a href="#org9a191e3">5.3. converErrorBasis</a></li>
<li><a href="#org14d5b4c">5.4. generateDiagPidControl</a></li> <li><a href="#org2d44efa">5.4. generateDiagPidControl</a></li>
<li><a href="#org5876af4">5.5. identifyPlant</a></li> <li><a href="#org6f7736a">5.5. identifyPlant</a></li>
<li><a href="#orgd01681b">5.6. runSimulation</a></li> <li><a href="#org32dd782">5.6. runSimulation</a></li>
</ul> </ul>
</li> </li>
<li><a href="#orgc88bb4f">6. Initialize Elements</a> <li><a href="#org5cc2276">6. Initialize Elements</a>
<ul> <ul>
<li><a href="#org53e8eea">6.1. Experiment</a></li> <li><a href="#org664a7e3">6.1. Experiment</a></li>
<li><a href="#orgd101ec3">6.2. Generate Reference Signals</a></li> <li><a href="#org89c58f8">6.2. Generate Reference Signals</a></li>
<li><a href="#org15d2680">6.3. <span class="todo TODO">TODO</span> Inputs</a></li> <li><a href="#org77d160f">6.3. <span class="todo TODO">TODO</span> Inputs</a></li>
<li><a href="#org2b6b02d">6.4. Ground</a></li> <li><a href="#org962187e">6.4. Ground</a></li>
<li><a href="#orge2875ed">6.5. Granite</a></li> <li><a href="#org198c366">6.5. Granite</a></li>
<li><a href="#org61ee0ba">6.6. Translation Stage</a></li> <li><a href="#org6e967d2">6.6. Translation Stage</a></li>
<li><a href="#orgdba0ed0">6.7. Tilt Stage</a></li> <li><a href="#org6c95b53">6.7. Tilt Stage</a></li>
<li><a href="#org6d74dd3">6.8. Spindle</a></li> <li><a href="#org13271e7">6.8. Spindle</a></li>
<li><a href="#orgde1ec20">6.9. Micro Hexapod</a></li> <li><a href="#org3c482fe">6.9. Initialize Hexapod legs' length</a></li>
<li><a href="#orgbfe4e10">6.10. Center of gravity compensation</a></li> <li><a href="#org7c295aa">6.10. Micro Hexapod</a></li>
<li><a href="#org1c8d312">6.11. Mirror</a></li> <li><a href="#org72492f3">6.11. Center of gravity compensation</a></li>
<li><a href="#orgf75cc87">6.12. Nano Hexapod</a></li> <li><a href="#org68d031b">6.12. Mirror</a></li>
<li><a href="#orgbb8d5dd">6.13. Cedrat Actuator</a></li> <li><a href="#org74149ff">6.13. Nano Hexapod</a></li>
<li><a href="#orgbb20f65">6.14. Sample</a></li> <li><a href="#org94d9a68">6.14. Cedrat Actuator</a></li>
<li><a href="#org8ceaacc">6.15. Sample</a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
@ -326,19 +327,19 @@ for the JavaScript code in this tag.
This file is used to explain how this Simscape Model works. This file is used to explain how this Simscape Model works.
</p> </p>
<ul class="org-ul"> <ul class="org-ul">
<li>In section <a href="#org041c0e9">1</a>, the simulink project with the associated scripts are presented</li> <li>In section <a href="#orgfd7237b">1</a>, the simulink project with the associated scripts are presented</li>
<li>In section <a href="#orgd5bef84">2</a>, an introduction to Simscape Multibody is done</li> <li>In section <a href="#orgb6586ca">2</a>, an introduction to Simscape Multibody is done</li>
<li>In section <a href="#orgfff3010">3</a>, each simscape files are presented with the associated signal names and joint architectures</li> <li>In section <a href="#org3e8132a">3</a>, each simscape files are presented with the associated signal names and joint architectures</li>
<li>In section <a href="#org3c886b5">4</a>, the list of the Simulink library elements are described</li> <li>In section <a href="#orgecb3837">4</a>, the list of the Simulink library elements are described</li>
<li>In section <a href="#org21528cf">5</a>, a list of Matlab function that will be used are defined here</li> <li>In section <a href="#org94ca66a">5</a>, a list of Matlab function that will be used are defined here</li>
<li>In section <a href="#org882f3b1">6</a>, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.</li> <li>In section <a href="#org18b2c56">6</a>, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.</li>
</ul> </ul>
<div id="outline-container-org5c21e47" class="outline-2"> <div id="outline-container-org88d59ba" class="outline-2">
<h2 id="org5c21e47"><span class="section-number-2">1</span> Simulink Project - Startup and Shutdown scripts</h2> <h2 id="org88d59ba"><span class="section-number-2">1</span> Simulink Project - Startup and Shutdown scripts</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p> <p>
<a id="org041c0e9"></a> <a id="orgfd7237b"></a>
</p> </p>
<p> <p>
@ -400,11 +401,11 @@ The project also permits to automatically add defined folder to the path when th
</div> </div>
</div> </div>
<div id="outline-container-org5ca7221" class="outline-2"> <div id="outline-container-orga2206d5" class="outline-2">
<h2 id="org5ca7221"><span class="section-number-2">2</span> Simscape Multibody - Presentation</h2> <h2 id="orga2206d5"><span class="section-number-2">2</span> Simscape Multibody - Presentation</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
<p> <p>
<a id="orgd5bef84"></a> <a id="orgb6586ca"></a>
</p> </p>
<p> <p>
@ -416,8 +417,8 @@ A <a href="https://.mathworks.com/products/simscape.html">simscape</a> model per
</p> </p>
</div> </div>
<div id="outline-container-orga5fbe1d" class="outline-3"> <div id="outline-container-org6b4540f" class="outline-3">
<h3 id="orga5fbe1d"><span class="section-number-3">2.1</span> Solid bodies</h3> <h3 id="org6b4540f"><span class="section-number-3">2.1</span> Solid bodies</h3>
<div class="outline-text-3" id="text-2-1"> <div class="outline-text-3" id="text-2-1">
<p> <p>
Each solid body is represented by a <a href="https://mathworks.com/help/physmod/sm/ref/solid.html">solid block</a>. Each solid body is represented by a <a href="https://mathworks.com/help/physmod/sm/ref/solid.html">solid block</a>.
@ -426,8 +427,8 @@ The geometry of the solid body can be imported using a <code>step</code> file. T
</div> </div>
</div> </div>
<div id="outline-container-org8017e71" class="outline-3"> <div id="outline-container-orge470a34" class="outline-3">
<h3 id="org8017e71"><span class="section-number-3">2.2</span> Frames</h3> <h3 id="orge470a34"><span class="section-number-3">2.2</span> Frames</h3>
<div class="outline-text-3" id="text-2-2"> <div class="outline-text-3" id="text-2-2">
<p> <p>
Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made. Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.
@ -439,8 +440,8 @@ They can be defined from the solid body geometry, or using the <a href="https://
</div> </div>
</div> </div>
<div id="outline-container-org486d9bf" class="outline-3"> <div id="outline-container-org474edd3" class="outline-3">
<h3 id="org486d9bf"><span class="section-number-3">2.3</span> Joints</h3> <h3 id="org474edd3"><span class="section-number-3">2.3</span> Joints</h3>
<div class="outline-text-3" id="text-2-3"> <div class="outline-text-3" id="text-2-3">
<p> <p>
Solid Bodies are connected with joints (between frames of the two solid bodies). Solid Bodies are connected with joints (between frames of the two solid bodies).
@ -450,7 +451,7 @@ Solid Bodies are connected with joints (between frames of the two solid bodies).
There are various types of joints that are all described <a href="https://mathworks.com/help/physmod/sm/ug/joints.html">here</a>. There are various types of joints that are all described <a href="https://mathworks.com/help/physmod/sm/ug/joints.html">here</a>.
</p> </p>
<table id="org14596b7" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org57b7cc8" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 1:</span> Degrees of freedom associated with each joint</caption> <caption class="t-above"><span class="table-number">Table 1:</span> Degrees of freedom associated with each joint</caption>
<colgroup> <colgroup>
@ -583,7 +584,7 @@ Joint blocks are assortments of joint primitives:
<li><b>Constant Velocity</b>: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: <code>CV</code></li> <li><b>Constant Velocity</b>: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: <code>CV</code></li>
</ul> </ul>
<table id="orgc0ad582" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org75d3b05" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 2:</span> Joint primitives for each joint type</caption> <caption class="t-above"><span class="table-number">Table 2:</span> Joint primitives for each joint type</caption>
<colgroup> <colgroup>
@ -875,8 +876,8 @@ Composite Force/Torque sensing:
</div> </div>
</div> </div>
<div id="outline-container-org5a1378a" class="outline-3"> <div id="outline-container-org9171b40" class="outline-3">
<h3 id="org5a1378a"><span class="section-number-3">2.4</span> Measurements</h3> <h3 id="org9171b40"><span class="section-number-3">2.4</span> Measurements</h3>
<div class="outline-text-3" id="text-2-4"> <div class="outline-text-3" id="text-2-4">
<p> <p>
A transform sensor block measures the spatial relationship between two frames: the base <code>B</code> and the follower <code>F</code>. A transform sensor block measures the spatial relationship between two frames: the base <code>B</code> and the follower <code>F</code>.
@ -900,8 +901,8 @@ If we want to simulate an <b>inertial sensor</b>, we just have to choose <code>B
</div> </div>
</div> </div>
<div id="outline-container-orga435910" class="outline-3"> <div id="outline-container-org7df2aef" class="outline-3">
<h3 id="orga435910"><span class="section-number-3">2.5</span> Excitation</h3> <h3 id="org7df2aef"><span class="section-number-3">2.5</span> Excitation</h3>
<div class="outline-text-3" id="text-2-5"> <div class="outline-text-3" id="text-2-5">
<p> <p>
We can apply <b>external forces</b> to the model by using an <a href="https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html">external force and torque block</a>. We can apply <b>external forces</b> to the model by using an <a href="https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html">external force and torque block</a>.
@ -914,11 +915,11 @@ Internal force, acting reciprocally between base and following origins is implem
</div> </div>
</div> </div>
<div id="outline-container-org289000e" class="outline-2"> <div id="outline-container-org803cadc" class="outline-2">
<h2 id="org289000e"><span class="section-number-2">3</span> Simulink files and signal names</h2> <h2 id="org803cadc"><span class="section-number-2">3</span> Simulink files and signal names</h2>
<div class="outline-text-2" id="text-3"> <div class="outline-text-2" id="text-3">
<p> <p>
<a id="orgfff3010"></a> <a id="org3e8132a"></a>
</p> </p>
<p> <p>
@ -926,8 +927,8 @@ In order to "normalize" things, the names of all the signal are listed here.
</p> </p>
</div> </div>
<div id="outline-container-orga9d8af9" class="outline-3"> <div id="outline-container-orgb5c767a" class="outline-3">
<h3 id="orga9d8af9"><span class="section-number-3">3.1</span> List of Simscape files</h3> <h3 id="orgb5c767a"><span class="section-number-3">3.1</span> List of Simscape files</h3>
<div class="outline-text-3" id="text-3-1"> <div class="outline-text-3" id="text-3-1">
<p> <p>
Few different Simulink files are used: Few different Simulink files are used:
@ -939,7 +940,7 @@ Few different Simulink files are used:
<li>control</li> <li>control</li>
</ul> </ul>
<table id="org768a47d" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org6eeaf61" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 3:</span> List of simscape files</caption> <caption class="t-above"><span class="table-number">Table 3:</span> List of simscape files</caption>
<colgroup> <colgroup>
@ -1006,14 +1007,14 @@ Few different Simulink files are used:
</div> </div>
</div> </div>
<div id="outline-container-org2a9705c" class="outline-3"> <div id="outline-container-org64dbd0d" class="outline-3">
<h3 id="org2a9705c"><span class="section-number-3">3.2</span> List of Inputs</h3> <h3 id="org64dbd0d"><span class="section-number-3">3.2</span> List of Inputs</h3>
<div class="outline-text-3" id="text-3-2"> <div class="outline-text-3" id="text-3-2">
</div> </div>
<div id="outline-container-org7fd5d25" class="outline-4"> <div id="outline-container-orgeecef4d" class="outline-4">
<h4 id="org7fd5d25"><span class="section-number-4">3.2.1</span> Perturbations</h4> <h4 id="orgeecef4d"><span class="section-number-4">3.2.1</span> Perturbations</h4>
<div class="outline-text-4" id="text-3-2-1"> <div class="outline-text-4" id="text-3-2-1">
<table id="orgcc1cb27" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org5c60a9c" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 4:</span> List of Disturbances</caption> <caption class="t-above"><span class="table-number">Table 4:</span> List of Disturbances</caption>
<colgroup> <colgroup>
@ -1059,10 +1060,10 @@ Few different Simulink files are used:
</div> </div>
</div> </div>
<div id="outline-container-org0337003" class="outline-4"> <div id="outline-container-org4e58ed6" class="outline-4">
<h4 id="org0337003"><span class="section-number-4">3.2.2</span> Measurement Noise</h4> <h4 id="org4e58ed6"><span class="section-number-4">3.2.2</span> Measurement Noise</h4>
<div class="outline-text-4" id="text-3-2-2"> <div class="outline-text-4" id="text-3-2-2">
<table id="orgb37a76c" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="orgdadd8d8" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 5:</span> List of Measurement Noise</caption> <caption class="t-above"><span class="table-number">Table 5:</span> List of Measurement Noise</caption>
<colgroup> <colgroup>
@ -1094,10 +1095,10 @@ Few different Simulink files are used:
</div> </div>
</div> </div>
<div id="outline-container-org250f869" class="outline-4"> <div id="outline-container-orgc9006ba" class="outline-4">
<h4 id="org250f869"><span class="section-number-4">3.2.3</span> Control Inputs</h4> <h4 id="orgc9006ba"><span class="section-number-4">3.2.3</span> Control Inputs</h4>
<div class="outline-text-4" id="text-3-2-3"> <div class="outline-text-4" id="text-3-2-3">
<table id="orgb342550" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="orgfc73f52" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 6:</span> List of Control Inputs</caption> <caption class="t-above"><span class="table-number">Table 6:</span> List of Control Inputs</caption>
<colgroup> <colgroup>
@ -1219,10 +1220,10 @@ Few different Simulink files are used:
</div> </div>
</div> </div>
<div id="outline-container-orge7abaa0" class="outline-3"> <div id="outline-container-orgeb985b0" class="outline-3">
<h3 id="orge7abaa0"><span class="section-number-3">3.3</span> List of Outputs</h3> <h3 id="orgeb985b0"><span class="section-number-3">3.3</span> List of Outputs</h3>
<div class="outline-text-3" id="text-3-3"> <div class="outline-text-3" id="text-3-3">
<table id="orgbcd4e2c" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides"> <table id="org59a5204" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 7:</span> List of Outputs</caption> <caption class="t-above"><span class="table-number">Table 7:</span> List of Outputs</caption>
<colgroup> <colgroup>
@ -1338,11 +1339,11 @@ Few different Simulink files are used:
</div> </div>
</div> </div>
<div id="outline-container-org0fbeac7" class="outline-2"> <div id="outline-container-orgfd5289f" class="outline-2">
<h2 id="org0fbeac7"><span class="section-number-2">4</span> Simulink Library</h2> <h2 id="orgfd5289f"><span class="section-number-2">4</span> Simulink Library</h2>
<div class="outline-text-2" id="text-4"> <div class="outline-text-2" id="text-4">
<p> <p>
<a id="org3c886b5"></a> <a id="orgecb3837"></a>
</p> </p>
<p> <p>
@ -1350,8 +1351,8 @@ A simulink library is developed in order to share elements between the different
</p> </p>
</div> </div>
<div id="outline-container-org3ec0c6f" class="outline-3"> <div id="outline-container-orgd1ebe0f" class="outline-3">
<h3 id="org3ec0c6f"><span class="section-number-3">4.1</span> <code>inputs</code></h3> <h3 id="orgd1ebe0f"><span class="section-number-3">4.1</span> <code>inputs</code></h3>
<div class="outline-text-3" id="text-4-1"> <div class="outline-text-3" id="text-4-1">
<p> <p>
<a href="../nass_library/inputs.slx">inputs.slx</a> <a href="../nass_library/inputs.slx">inputs.slx</a>
@ -1359,8 +1360,8 @@ A simulink library is developed in order to share elements between the different
</div> </div>
</div> </div>
<div id="outline-container-org0abb61d" class="outline-3"> <div id="outline-container-org1ae1d37" class="outline-3">
<h3 id="org0abb61d"><span class="section-number-3">4.2</span> <code>nass_library</code></h3> <h3 id="org1ae1d37"><span class="section-number-3">4.2</span> <code>nass_library</code></h3>
<div class="outline-text-3" id="text-4-2"> <div class="outline-text-3" id="text-4-2">
<p> <p>
<a href="../nass_library/nass_library.slx">nass<sub>library.slx</sub></a> <a href="../nass_library/nass_library.slx">nass<sub>library.slx</sub></a>
@ -1368,8 +1369,8 @@ A simulink library is developed in order to share elements between the different
</div> </div>
</div> </div>
<div id="outline-container-org04c0eb4" class="outline-3"> <div id="outline-container-org54e3d07" class="outline-3">
<h3 id="org04c0eb4"><span class="section-number-3">4.3</span> <code>pos_error_wrt_nass_base</code></h3> <h3 id="org54e3d07"><span class="section-number-3">4.3</span> <code>pos_error_wrt_nass_base</code></h3>
<div class="outline-text-3" id="text-4-3"> <div class="outline-text-3" id="text-4-3">
<p> <p>
<a href="../nass_library/pos_error_wrt_nass_base.slx">pos<sub>error</sub><sub>wrt</sub><sub>nass</sub><sub>base.slx</sub></a> <a href="../nass_library/pos_error_wrt_nass_base.slx">pos<sub>error</sub><sub>wrt</sub><sub>nass</sub><sub>base.slx</sub></a>
@ -1377,8 +1378,8 @@ A simulink library is developed in order to share elements between the different
</div> </div>
</div> </div>
<div id="outline-container-orgef1709b" class="outline-3"> <div id="outline-container-orgc0f0f02" class="outline-3">
<h3 id="orgef1709b"><span class="section-number-3">4.4</span> <code>QuaternionToAngles</code></h3> <h3 id="orgc0f0f02"><span class="section-number-3">4.4</span> <code>QuaternionToAngles</code></h3>
<div class="outline-text-3" id="text-4-4"> <div class="outline-text-3" id="text-4-4">
<p> <p>
<a href="../nass_library/QuaternionToAngles.slx">QuaternionToAngles.slx</a> <a href="../nass_library/QuaternionToAngles.slx">QuaternionToAngles.slx</a>
@ -1386,8 +1387,8 @@ A simulink library is developed in order to share elements between the different
</div> </div>
</div> </div>
<div id="outline-container-org8a40a80" class="outline-3"> <div id="outline-container-orgbda19ed" class="outline-3">
<h3 id="org8a40a80"><span class="section-number-3">4.5</span> <code>RotationMatrixToAngle</code></h3> <h3 id="orgbda19ed"><span class="section-number-3">4.5</span> <code>RotationMatrixToAngle</code></h3>
<div class="outline-text-3" id="text-4-5"> <div class="outline-text-3" id="text-4-5">
<p> <p>
<a href="../nass_library/RotationMatrixToAngle.slx">RotationMatrixToAngle.slx</a> <a href="../nass_library/RotationMatrixToAngle.slx">RotationMatrixToAngle.slx</a>
@ -1396,18 +1397,18 @@ A simulink library is developed in order to share elements between the different
</div> </div>
</div> </div>
<div id="outline-container-org81a7962" class="outline-2"> <div id="outline-container-org54193c9" class="outline-2">
<h2 id="org81a7962"><span class="section-number-2">5</span> Functions</h2> <h2 id="org54193c9"><span class="section-number-2">5</span> Functions</h2>
<div class="outline-text-2" id="text-5"> <div class="outline-text-2" id="text-5">
<p> <p>
<a id="org21528cf"></a> <a id="org94ca66a"></a>
</p> </p>
</div> </div>
<div id="outline-container-orge3706e5" class="outline-3"> <div id="outline-container-orgc1f5840" class="outline-3">
<h3 id="orge3706e5"><span class="section-number-3">5.1</span> computePsdDispl</h3> <h3 id="orgc1f5840"><span class="section-number-3">5.1</span> computePsdDispl</h3>
<div class="outline-text-3" id="text-5-1"> <div class="outline-text-3" id="text-5-1">
<p> <p>
<a id="org63dd43c"></a> <a id="org46da1ed"></a>
</p> </p>
<p> <p>
@ -1443,11 +1444,11 @@ This Matlab function is accessible <a href="../src/computePsdDispl.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org14a492d" class="outline-3"> <div id="outline-container-org0d8e9e6" class="outline-3">
<h3 id="org14a492d"><span class="section-number-3">5.2</span> computeSetpoint</h3> <h3 id="org0d8e9e6"><span class="section-number-3">5.2</span> computeSetpoint</h3>
<div class="outline-text-3" id="text-5-2"> <div class="outline-text-3" id="text-5-2">
<p> <p>
<a id="org0dcd84b"></a> <a id="orga7c431b"></a>
</p> </p>
<p> <p>
@ -1519,11 +1520,11 @@ setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-hi
</div> </div>
</div> </div>
<div id="outline-container-org3e9a86f" class="outline-3"> <div id="outline-container-org9a191e3" class="outline-3">
<h3 id="org3e9a86f"><span class="section-number-3">5.3</span> converErrorBasis</h3> <h3 id="org9a191e3"><span class="section-number-3">5.3</span> converErrorBasis</h3>
<div class="outline-text-3" id="text-5-3"> <div class="outline-text-3" id="text-5-3">
<p> <p>
<a id="org273e422"></a> <a id="org2e6fbe8"></a>
</p> </p>
<p> <p>
@ -1661,11 +1662,11 @@ error_nass = <span class="org-rainbow-delimiters-depth-1">[</span>dx; dy; dz; th
</div> </div>
</div> </div>
<div id="outline-container-org14d5b4c" class="outline-3"> <div id="outline-container-org2d44efa" class="outline-3">
<h3 id="org14d5b4c"><span class="section-number-3">5.4</span> generateDiagPidControl</h3> <h3 id="org2d44efa"><span class="section-number-3">5.4</span> generateDiagPidControl</h3>
<div class="outline-text-3" id="text-5-4"> <div class="outline-text-3" id="text-5-4">
<p> <p>
<a id="org4a60577"></a> <a id="orgd1ce773"></a>
</p> </p>
<p> <p>
@ -1695,11 +1696,11 @@ This Matlab function is accessible <a href="../src/generateDiagPidControl.m">her
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org5876af4" class="outline-3"> <div id="outline-container-org6f7736a" class="outline-3">
<h3 id="org5876af4"><span class="section-number-3">5.5</span> identifyPlant</h3> <h3 id="org6f7736a"><span class="section-number-3">5.5</span> identifyPlant</h3>
<div class="outline-text-3" id="text-5-5"> <div class="outline-text-3" id="text-5-5">
<p> <p>
<a id="orgff9faac"></a> <a id="orgd69607b"></a>
</p> </p>
<p> <p>
@ -1791,11 +1792,11 @@ This Matlab function is accessible <a href="../src/identifyPlant.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orgd01681b" class="outline-3"> <div id="outline-container-org32dd782" class="outline-3">
<h3 id="orgd01681b"><span class="section-number-3">5.6</span> runSimulation</h3> <h3 id="org32dd782"><span class="section-number-3">5.6</span> runSimulation</h3>
<div class="outline-text-3" id="text-5-6"> <div class="outline-text-3" id="text-5-6">
<p> <p>
<a id="org1f4a257"></a> <a id="org95edf73"></a>
</p> </p>
<p> <p>
@ -1864,18 +1865,18 @@ This Matlab function is accessible <a href="../src/runSimulation.m">here</a>.
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orgc88bb4f" class="outline-2"> <div id="outline-container-org5cc2276" class="outline-2">
<h2 id="orgc88bb4f"><span class="section-number-2">6</span> Initialize Elements</h2> <h2 id="org5cc2276"><span class="section-number-2">6</span> Initialize Elements</h2>
<div class="outline-text-2" id="text-6"> <div class="outline-text-2" id="text-6">
<p> <p>
<a id="org882f3b1"></a> <a id="org18b2c56"></a>
</p> </p>
</div> </div>
<div id="outline-container-org53e8eea" class="outline-3"> <div id="outline-container-org664a7e3" class="outline-3">
<h3 id="org53e8eea"><span class="section-number-3">6.1</span> Experiment</h3> <h3 id="org664a7e3"><span class="section-number-3">6.1</span> Experiment</h3>
<div class="outline-text-3" id="text-6-1"> <div class="outline-text-3" id="text-6-1">
<p> <p>
<a id="org7855770"></a> <a id="org395aa48"></a>
</p> </p>
<p> <p>
@ -1909,11 +1910,11 @@ This Matlab function is accessible <a href="../src/initializeExperiment.m">here<
</div> </div>
</div> </div>
<div id="outline-container-orgd101ec3" class="outline-3"> <div id="outline-container-org89c58f8" class="outline-3">
<h3 id="orgd101ec3"><span class="section-number-3">6.2</span> Generate Reference Signals</h3> <h3 id="org89c58f8"><span class="section-number-3">6.2</span> Generate Reference Signals</h3>
<div class="outline-text-3" id="text-6-2"> <div class="outline-text-3" id="text-6-2">
<p> <p>
<a id="org4821745"></a> <a id="orgdda511e"></a>
</p> </p>
<p> <p>
@ -1929,10 +1930,10 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
<span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span> <span class="org-string">'Dy_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude of the displacement [m]</span>
<span class="org-string">'Dy_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the displacement [s]</span> <span class="org-string">'Dy_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
<span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span> <span class="org-string">'Ry_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "triangular" / "sinusoidal"</span>
<span class="org-string">'Ry_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude [deg]</span> <span class="org-string">'Ry_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Amplitude [rad]</span>
<span class="org-string">'Ry_period'</span>, <span class="org-highlight-numbers-number">10</span>, ...<span class="org-comment"> % Period of the displacement [s]</span> <span class="org-string">'Ry_period'</span>, <span class="org-highlight-numbers-number">10</span>, ...<span class="org-comment"> % Period of the displacement [s]</span>
<span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span> <span class="org-string">'Rz_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % Either "constant" / "rotating"</span>
<span class="org-string">'Rz_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Initial angle [deg]</span> <span class="org-string">'Rz_amplitude'</span>, <span class="org-highlight-numbers-number">0</span>, ...<span class="org-comment"> % Initial angle [rad]</span>
<span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the rotating [s]</span> <span class="org-string">'Rz_period'</span>, <span class="org-highlight-numbers-number">1</span>, ...<span class="org-comment"> % Period of the rotating [s]</span>
<span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span> <span class="org-string">'Dh_type'</span>, <span class="org-string">'constant'</span>, ...<span class="org-comment"> % For now, only constant is implemented</span>
<span class="org-string">'Dh_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span> <span class="org-string">'Dh_pos'</span>, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span>; <span class="org-highlight-numbers-number">0</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Initial position [m,m,m,rad,rad,rad] of the top platform</span>
@ -1976,13 +1977,13 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
<span class="org-keyword">switch</span> <span class="org-constant">opts.Ry_type</span> <span class="org-keyword">switch</span> <span class="org-constant">opts.Ry_type</span>
<span class="org-keyword">case</span> <span class="org-string">'constant'</span> <span class="org-keyword">case</span> <span class="org-string">'constant'</span>
Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Ry_amplitude; Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Ry_amplitude;
<span class="org-keyword">case</span> <span class="org-string">'triangular'</span> <span class="org-keyword">case</span> <span class="org-string">'triangular'</span>
Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">4</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">+</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t; Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-type">-</span><span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude <span class="org-type">+</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t;
Ry<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span>; Ry<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>opts.Ry_amplitude <span class="org-type">-</span> <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">75</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span>;
Ry<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span>; Ry<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-highlight-numbers-number">4</span><span class="org-type">*</span>opts.Ry_amplitude<span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">(</span>t<span class="org-type">&lt;</span><span class="org-highlight-numbers-number">0</span>.<span class="org-highlight-numbers-number">25</span><span class="org-type">*</span>opts.Ry_period<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span> <span class="org-keyword">case</span> <span class="org-string">'sinusoidal'</span>
Ry<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Ry_amplitude<span class="org-type">*</span>sin<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>opts.Ry_period<span class="org-type">*</span>t<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">otherwise</span> <span class="org-keyword">otherwise</span>
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Ry_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>; warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Ry_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
@ -1994,9 +1995,9 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
<span class="org-keyword">switch</span> <span class="org-constant">opts.Rz_type</span> <span class="org-keyword">switch</span> <span class="org-constant">opts.Rz_type</span>
<span class="org-keyword">case</span> <span class="org-string">'constant'</span> <span class="org-keyword">case</span> <span class="org-string">'constant'</span>
Rz<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Rz_amplitude; Rz<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Rz_amplitude;
<span class="org-keyword">case</span> <span class="org-string">'rotating'</span> <span class="org-keyword">case</span> <span class="org-string">'rotating'</span>
Rz<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Rz_amplitude<span class="org-type">+</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>opts.Rz_period<span class="org-type">*</span>t; Rz<span class="org-type"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-type">:</span><span class="org-type"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-type"> </span>= opts.Rz_amplitude<span class="org-type">+</span><span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">/</span>opts.Rz_period<span class="org-type">*</span>t;
<span class="org-keyword">otherwise</span> <span class="org-keyword">otherwise</span>
warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Rz_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>; warning<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Rz_type is not set correctly'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
@ -2006,8 +2007,6 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>; t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>;
Dh = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>; Dh = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
opts.Dh_pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Dh_pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% convert from [deg] to [rad]</span>
<span class="org-keyword">switch</span> <span class="org-constant">opts.Dh_type</span> <span class="org-keyword">switch</span> <span class="org-constant">opts.Dh_type</span>
<span class="org-keyword">case</span> <span class="org-string">'constant'</span> <span class="org-keyword">case</span> <span class="org-string">'constant'</span>
Dh = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Dh_pos, opts.Dh_pos<span class="org-rainbow-delimiters-depth-1">]</span>; Dh = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Dh_pos, opts.Dh_pos<span class="org-rainbow-delimiters-depth-1">]</span>;
@ -2025,8 +2024,6 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>; t = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span>, Ts<span class="org-rainbow-delimiters-depth-1">]</span>;
Dn = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>; Dn = zeros<span class="org-rainbow-delimiters-depth-1">(</span>length<span class="org-rainbow-delimiters-depth-2">(</span>t<span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>;
opts.Dn_pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-constant">pi</span><span class="org-type">/</span><span class="org-highlight-numbers-number">180</span><span class="org-type">*</span>opts.Dn_pos<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% convert from [deg] to [rad]</span>
<span class="org-keyword">switch</span> <span class="org-constant">opts.Dn_type</span> <span class="org-keyword">switch</span> <span class="org-constant">opts.Dn_type</span>
<span class="org-keyword">case</span> <span class="org-string">'constant'</span> <span class="org-keyword">case</span> <span class="org-string">'constant'</span>
Dn = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Dn_pos, opts.Dn_pos<span class="org-rainbow-delimiters-depth-1">]</span>; Dn = <span class="org-rainbow-delimiters-depth-1">[</span>opts.Dn_pos, opts.Dn_pos<span class="org-rainbow-delimiters-depth-1">]</span>;
@ -2043,11 +2040,11 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org15d2680" class="outline-3"> <div id="outline-container-org77d160f" class="outline-3">
<h3 id="org15d2680"><span class="section-number-3">6.3</span> <span class="todo TODO">TODO</span> Inputs</h3> <h3 id="org77d160f"><span class="section-number-3">6.3</span> <span class="todo TODO">TODO</span> Inputs</h3>
<div class="outline-text-3" id="text-6-3"> <div class="outline-text-3" id="text-6-3">
<p> <p>
<a id="org2728bfe"></a> <a id="org7501b33"></a>
</p> </p>
<ul class="org-ul"> <ul class="org-ul">
@ -2239,11 +2236,11 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org2b6b02d" class="outline-3"> <div id="outline-container-org962187e" class="outline-3">
<h3 id="org2b6b02d"><span class="section-number-3">6.4</span> Ground</h3> <h3 id="org962187e"><span class="section-number-3">6.4</span> Ground</h3>
<div class="outline-text-3" id="text-6-4"> <div class="outline-text-3" id="text-6-4">
<p> <p>
<a id="orgc97869c"></a> <a id="org763b0ff"></a>
</p> </p>
<p> <p>
@ -2267,11 +2264,11 @@ This Matlab function is accessible <a href="../src/initializeGround.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orge2875ed" class="outline-3"> <div id="outline-container-org198c366" class="outline-3">
<h3 id="orge2875ed"><span class="section-number-3">6.5</span> Granite</h3> <h3 id="org198c366"><span class="section-number-3">6.5</span> Granite</h3>
<div class="outline-text-3" id="text-6-5"> <div class="outline-text-3" id="text-6-5">
<p> <p>
<a id="org706d529"></a> <a id="orgbdb5477"></a>
</p> </p>
<p> <p>
@ -2337,11 +2334,11 @@ This Matlab function is accessible <a href="../src/initializeGranite.m">here</a>
</div> </div>
</div> </div>
<div id="outline-container-org61ee0ba" class="outline-3"> <div id="outline-container-org6e967d2" class="outline-3">
<h3 id="org61ee0ba"><span class="section-number-3">6.6</span> Translation Stage</h3> <h3 id="org6e967d2"><span class="section-number-3">6.6</span> Translation Stage</h3>
<div class="outline-text-3" id="text-6-6"> <div class="outline-text-3" id="text-6-6">
<p> <p>
<a id="org5407512"></a> <a id="org2597c18"></a>
</p> </p>
<p> <p>
@ -2423,11 +2420,11 @@ This Matlab function is accessible <a href="../src/initializeTy.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orgdba0ed0" class="outline-3"> <div id="outline-container-org6c95b53" class="outline-3">
<h3 id="orgdba0ed0"><span class="section-number-3">6.7</span> Tilt Stage</h3> <h3 id="org6c95b53"><span class="section-number-3">6.7</span> Tilt Stage</h3>
<div class="outline-text-3" id="text-6-7"> <div class="outline-text-3" id="text-6-7">
<p> <p>
<a id="org22557f6"></a> <a id="org29b1fab"></a>
</p> </p>
<p> <p>
@ -2498,11 +2495,11 @@ This Matlab function is accessible <a href="../src/initializeRy.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org6d74dd3" class="outline-3"> <div id="outline-container-org13271e7" class="outline-3">
<h3 id="org6d74dd3"><span class="section-number-3">6.8</span> Spindle</h3> <h3 id="org13271e7"><span class="section-number-3">6.8</span> Spindle</h3>
<div class="outline-text-3" id="text-6-8"> <div class="outline-text-3" id="text-6-8">
<p> <p>
<a id="orgc1728c6"></a> <a id="org50164cd"></a>
</p> </p>
<p> <p>
@ -2569,11 +2566,49 @@ This Matlab function is accessible <a href="../src/initializeRz.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orgde1ec20" class="outline-3"> <div id="outline-container-org3c482fe" class="outline-3">
<h3 id="orgde1ec20"><span class="section-number-3">6.9</span> Micro Hexapod</h3> <h3 id="org3c482fe"><span class="section-number-3">6.9</span> Initialize Hexapod legs' length</h3>
<div class="outline-text-3" id="text-6-9"> <div class="outline-text-3" id="text-6-9">
<p> <p>
<a id="org3e9a594"></a> <a id="orgee2252f"></a>
</p>
<p>
This Matlab function is accessible <a href="../src/initializeHexapodPosition.m">here</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">hexapod</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeHexapodPosition</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">hexapod</span>, <span class="org-variable-name">AP</span>, <span class="org-variable-name">ARB</span><span class="org-rainbow-delimiters-depth-1">)</span>
<span class="org-comment">% initializeHexapodPosition -</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: initializeHexapodPosition(hexapod, AP, ARB)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - hexapod - Hexapod object containing the geometry of the hexapod</span>
<span class="org-comment">% - AP - Position vector of point OB expressed in frame {A}</span>
<span class="org-comment">% - ARB - Rotation Matrix expressed in frame {A}</span>
L0 = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:length</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">L</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span>
Bbi = hexapod.pos_top_tranform<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'</span>;
Aai = hexapod.pos_base<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'</span>;
L0<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = sqrt<span class="org-rainbow-delimiters-depth-1">(</span>AP<span class="org-type">'*</span>AP <span class="org-type">+</span> Bbi<span class="org-type">'*</span>Bbi <span class="org-type">+</span> Aai<span class="org-type">'*</span>Aai <span class="org-type">-</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span>Aai <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>Aai<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
hexapod.L0 = L0;
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org7c295aa" class="outline-3">
<h3 id="org7c295aa"><span class="section-number-3">6.10</span> Micro Hexapod</h3>
<div class="outline-text-3" id="text-6-10">
<p>
<a id="org9e25f5d"></a>
</p> </p>
<p> <p>
@ -2583,7 +2618,11 @@ This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">her
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">micro_hexapod</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeMicroHexapod</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span> <pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">micro_hexapod</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeMicroHexapod</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Default values for opts</span></span>
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'rigid'</span>, <span class="org-constant">false</span><span class="org-rainbow-delimiters-depth-1">)</span>; opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
<span class="org-string">'rigid'</span>, <span class="org-constant">false</span>, ...
<span class="org-string">'AP'</span>, zeros<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, ...<span class="org-comment"> % Wanted position in [m] of OB with respect to frame {A}</span>
<span class="org-string">'ARB'</span>, eye<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span> ...<span class="org-comment"> % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}</span>
<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Populate opts with input parameters</span></span>
<span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-keyword">if</span> exist<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span><span class="org-rainbow-delimiters-depth-1">)</span>
@ -2595,7 +2634,7 @@ This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">her
<span class="org-matlab-cellbreak"><span class="org-comment">%% Stewart Object</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Stewart Object</span></span>
micro_hexapod = struct<span class="org-rainbow-delimiters-depth-1">()</span>; micro_hexapod = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
micro_hexapod.h = <span class="org-highlight-numbers-number">350</span>; <span class="org-comment">% Total height of the platform [mm]</span> micro_hexapod.h = <span class="org-highlight-numbers-number">350</span>; <span class="org-comment">% Total height of the platform [mm]</span>
micro_hexapod.jacobian = <span class="org-highlight-numbers-number">270</span>; <span class="org-comment">% Distance from the top platform to the Jacobian point [mm]</span> micro_hexapod.jacobian = <span class="org-highlight-numbers-number">270</span>; <span class="org-comment">% Distance from the top of the mobile platform to the Jacobian point [mm]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Bottom Plate - Mechanical Design</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Bottom Plate - Mechanical Design</span></span>
BP = struct<span class="org-rainbow-delimiters-depth-1">()</span>; BP = struct<span class="org-rainbow-delimiters-depth-1">()</span>;
@ -2676,6 +2715,9 @@ This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">her
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
micro_hexapod = initializeParameters<span class="org-rainbow-delimiters-depth-1">(</span>micro_hexapod<span class="org-rainbow-delimiters-depth-1">)</span>; micro_hexapod = initializeParameters<span class="org-rainbow-delimiters-depth-1">(</span>micro_hexapod<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Setup equilibrium position of each leg</span></span>
micro_hexapod.L0 = initializeHexapodPosition<span class="org-rainbow-delimiters-depth-1">(</span>micro_hexapod, opts.AP, opts.ARB<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Save</span></span>
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'micro_hexapod'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>; save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stages.mat'</span>, <span class="org-string">'micro_hexapod'</span>, <span class="org-string">'-append'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
@ -2769,6 +2811,7 @@ This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">her
stewart.J = getJacobianMatrix<span class="org-rainbow-delimiters-depth-1">(</span>leg_vectors<span class="org-type">'</span>, aa<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>; stewart.J = getJacobianMatrix<span class="org-rainbow-delimiters-depth-1">(</span>leg_vectors<span class="org-type">'</span>, aa<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
<span class="org-keyword">function</span> <span class="org-variable-name">J</span> = <span class="org-function-name">getJacobianMatrix</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">RM</span>, <span class="org-variable-name">M_pos_base</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-keyword">function</span> <span class="org-variable-name">J</span> = <span class="org-function-name">getJacobianMatrix</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">RM</span>, <span class="org-variable-name">M_pos_base</span><span class="org-rainbow-delimiters-depth-1">)</span>
% RM<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] unit vector of each leg in the fixed frame % RM<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] unit vector of each leg in the fixed frame
% M_pos_base<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] vector of the leg connection at the top platform location in the fixed frame % M_pos_base<span class="org-type">:</span> [<span class="org-highlight-numbers-number">3x6</span>] vector of the leg connection at the top platform location in the fixed frame
@ -2776,17 +2819,39 @@ This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">her
J<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = RM<span class="org-type">'</span>; J<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = RM<span class="org-type">'</span>;
J<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = cross<span class="org-rainbow-delimiters-depth-1">(</span>M_pos_base, RM<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'</span>; J<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-highlight-numbers-number">4</span><span class="org-type">:</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</span> = cross<span class="org-rainbow-delimiters-depth-1">(</span>M_pos_base, RM<span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'</span>;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%%</span></span>
<span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">L</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeHexapodPosition</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">hexapod</span>, <span class="org-variable-name">AP</span>, <span class="org-variable-name">ARB</span><span class="org-rainbow-delimiters-depth-1">)</span>
<span class="org-comment">% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: initializeHexapodPosition(hexapod, AP, ARB)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - hexapod - Hexapod object containing the geometry of the hexapod</span>
<span class="org-comment">% - AP - Position vector of point OB expressed in frame {A} in [m]</span>
<span class="org-comment">% - ARB - Rotation Matrix expressed in frame {A}</span>
<span class="org-comment">% Wanted Length of the hexapod's legs [m]</span>
L = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant">:length</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">L</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span>
Bbi = hexapod.pos_top_tranform<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">0</span> ; hexapod.TP.thickness<span class="org-type">+</span>hexapod.Leg.sphere.top<span class="org-type">+</span>hexapod.SP.thickness.top<span class="org-type">+</span>hexapod.jacobian<span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
Aai = hexapod.pos_base<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">'</span> <span class="org-type">+</span> <span class="org-highlight-numbers-number">1e</span><span class="org-type">-</span><span class="org-highlight-numbers-number">3</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">0</span> ; <span class="org-highlight-numbers-number">0</span> ; hexapod.BP.thickness<span class="org-type">+</span>hexapod.Leg.sphere.bottom<span class="org-type">+</span>hexapod.SP.thickness.bottom<span class="org-type">-</span>micro_hexapod.h<span class="org-type">-</span>hexapod.jacobian<span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
L<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = sqrt<span class="org-rainbow-delimiters-depth-1">(</span>AP<span class="org-type">'*</span>AP <span class="org-type">+</span> Bbi<span class="org-type">'*</span>Bbi <span class="org-type">+</span> Aai<span class="org-type">'*</span>Aai <span class="org-type">-</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span>Aai <span class="org-type">+</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>AP<span class="org-type">'*</span><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span> <span class="org-highlight-numbers-number">2</span><span class="org-type">*</span><span class="org-rainbow-delimiters-depth-2">(</span>ARB<span class="org-type">*</span>Bbi<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-type">'*</span>Aai<span class="org-rainbow-delimiters-depth-1">)</span>;
<span class="org-keyword">end</span>
<span class="org-keyword">end</span>
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orgbfe4e10" class="outline-3"> <div id="outline-container-org72492f3" class="outline-3">
<h3 id="orgbfe4e10"><span class="section-number-3">6.10</span> Center of gravity compensation</h3> <h3 id="org72492f3"><span class="section-number-3">6.11</span> Center of gravity compensation</h3>
<div class="outline-text-3" id="text-6-10"> <div class="outline-text-3" id="text-6-11">
<p> <p>
<a id="org3db12eb"></a> <a id="org991aec0"></a>
</p> </p>
<p> <p>
@ -2830,11 +2895,11 @@ This Matlab function is accessible <a href="../src/initializeAxisc.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org1c8d312" class="outline-3"> <div id="outline-container-org68d031b" class="outline-3">
<h3 id="org1c8d312"><span class="section-number-3">6.11</span> Mirror</h3> <h3 id="org68d031b"><span class="section-number-3">6.12</span> Mirror</h3>
<div class="outline-text-3" id="text-6-11"> <div class="outline-text-3" id="text-6-12">
<p> <p>
<a id="org1354fa1"></a> <a id="org43cadec"></a>
</p> </p>
<p> <p>
@ -2900,11 +2965,11 @@ This Matlab function is accessible <a href="../src/initializeMirror.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-orgf75cc87" class="outline-3"> <div id="outline-container-org74149ff" class="outline-3">
<h3 id="orgf75cc87"><span class="section-number-3">6.12</span> Nano Hexapod</h3> <h3 id="org74149ff"><span class="section-number-3">6.13</span> Nano Hexapod</h3>
<div class="outline-text-3" id="text-6-12"> <div class="outline-text-3" id="text-6-13">
<p> <p>
<a id="orgbb91911"></a> <a id="orgcb27cac"></a>
</p> </p>
<p> <p>
@ -3121,11 +3186,11 @@ This Matlab function is accessible <a href="../src/initializeNanoHexapod.m">here
</div> </div>
</div> </div>
<div id="outline-container-orgbb8d5dd" class="outline-3"> <div id="outline-container-org94d9a68" class="outline-3">
<h3 id="orgbb8d5dd"><span class="section-number-3">6.13</span> Cedrat Actuator</h3> <h3 id="org94d9a68"><span class="section-number-3">6.14</span> Cedrat Actuator</h3>
<div class="outline-text-3" id="text-6-13"> <div class="outline-text-3" id="text-6-14">
<p> <p>
<a id="org3955a2d"></a> <a id="orgdb4500f"></a>
</p> </p>
<p> <p>
@ -3165,11 +3230,11 @@ This Matlab function is accessible <a href="../src/initializeCedratPiezo.m">here
</div> </div>
</div> </div>
<div id="outline-container-orgbb20f65" class="outline-3"> <div id="outline-container-org8ceaacc" class="outline-3">
<h3 id="orgbb20f65"><span class="section-number-3">6.14</span> Sample</h3> <h3 id="org8ceaacc"><span class="section-number-3">6.15</span> Sample</h3>
<div class="outline-text-3" id="text-6-14"> <div class="outline-text-3" id="text-6-15">
<p> <p>
<a id="org92d0630"></a> <a id="org96164b7"></a>
</p> </p>
<p> <p>
@ -3214,7 +3279,7 @@ This Matlab function is accessible <a href="../src/initializeSample.m">here</a>.
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-12-06 ven. 12:04</p> <p class="date">Created: 2019-12-10 mar. 18:05</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p> <p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
</div> </div>
</body> </body>

91
simscape/index.org
View File

@ -797,6 +797,7 @@ This Matlab function is accessible [[file:../src/initializeExperiment.m][here]].
:header-args:matlab+: :comments org :mkdirp yes :header-args:matlab+: :comments org :mkdirp yes
:header-args:matlab+: :eval no :results none :header-args:matlab+: :eval no :results none
:END: :END:
<<sec:initializeReferences>> <<sec:initializeReferences>>
This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
@ -810,10 +811,10 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
'Dy_amplitude', 0, ... % Amplitude of the displacement [m] 'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
'Dy_period', 1, ... % Period of the displacement [s] 'Dy_period', 1, ... % Period of the displacement [s]
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" 'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
'Ry_amplitude', 0, ... % Amplitude [deg] 'Ry_amplitude', 0, ... % Amplitude [rad]
'Ry_period', 10, ... % Period of the displacement [s] 'Ry_period', 10, ... % Period of the displacement [s]
'Rz_type', 'constant', ... % Either "constant" / "rotating" 'Rz_type', 'constant', ... % Either "constant" / "rotating"
'Rz_amplitude', 0, ... % Initial angle [deg] 'Rz_amplitude', 0, ... % Initial angle [rad]
'Rz_period', 1, ... % Period of the rotating [s] 'Rz_period', 1, ... % Period of the rotating [s]
'Dh_type', 'constant', ... % For now, only constant is implemented 'Dh_type', 'constant', ... % For now, only constant is implemented
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform 'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
@ -857,13 +858,13 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
switch opts.Ry_type switch opts.Ry_type
case 'constant' case 'constant'
Ry(:) = pi/180*opts.Ry_amplitude; Ry(:) = opts.Ry_amplitude;
case 'triangular' case 'triangular'
Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t; Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period); Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period); Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
case 'sinusoidal' case 'sinusoidal'
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
otherwise otherwise
warning('Ry_type is not set correctly'); warning('Ry_type is not set correctly');
end end
@ -875,9 +876,9 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
switch opts.Rz_type switch opts.Rz_type
case 'constant' case 'constant'
Rz(:) = pi/180*opts.Rz_amplitude; Rz(:) = opts.Rz_amplitude;
case 'rotating' case 'rotating'
Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t; Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
otherwise otherwise
warning('Rz_type is not set correctly'); warning('Rz_type is not set correctly');
end end
@ -887,8 +888,6 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
t = [0, Ts]; t = [0, Ts];
Dh = zeros(length(t), 6); Dh = zeros(length(t), 6);
opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad]
switch opts.Dh_type switch opts.Dh_type
case 'constant' case 'constant'
Dh = [opts.Dh_pos, opts.Dh_pos]; Dh = [opts.Dh_pos, opts.Dh_pos];
@ -906,8 +905,6 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
t = [0, Ts]; t = [0, Ts];
Dn = zeros(length(t), 6); Dn = zeros(length(t), 6);
opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad]
switch opts.Dn_type switch opts.Dn_type
case 'constant' case 'constant'
Dn = [opts.Dn_pos, opts.Dn_pos]; Dn = [opts.Dn_pos, opts.Dn_pos];
@ -1421,6 +1418,40 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]].
end end
#+end_src #+end_src
** Initialize Hexapod legs' length
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeHexapodPosition.m
:header-args:matlab+: :comments org :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
<<sec:initializeHexapodPosition>>
This Matlab function is accessible [[file:../src/initializeHexapodPosition.m][here]].
#+begin_src matlab
function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition -
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A}
% - ARB - Rotation Matrix expressed in frame {A}
L0 = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)';
Aai = hexapod.pos_base(i, :)';
L0(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
hexapod.L0 = L0;
end
#+end_src
** Micro Hexapod ** Micro Hexapod
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m :header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
@ -1434,7 +1465,11 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
#+begin_src matlab #+begin_src matlab
function [micro_hexapod] = initializeMicroHexapod(opts_param) function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Default values for opts %% Default values for opts
opts = struct('rigid', false); opts = struct(...
'rigid', false, ...
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters %% Populate opts with input parameters
if exist('opts_param','var') if exist('opts_param','var')
@ -1446,7 +1481,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
%% Stewart Object %% Stewart Object
micro_hexapod = struct(); micro_hexapod = struct();
micro_hexapod.h = 350; % Total height of the platform [mm] micro_hexapod.h = 350; % Total height of the platform [mm]
micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm] micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
%% Bottom Plate - Mechanical Design %% Bottom Plate - Mechanical Design
BP = struct(); BP = struct();
@ -1527,6 +1562,9 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
%% %%
micro_hexapod = initializeParameters(micro_hexapod); micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
%% Save %% Save
save('./mat/stages.mat', 'micro_hexapod', '-append'); save('./mat/stages.mat', 'micro_hexapod', '-append');
@ -1620,6 +1658,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
stewart.J = getJacobianMatrix(leg_vectors', aa'); stewart.J = getJacobianMatrix(leg_vectors', aa');
end end
%%
function J = getJacobianMatrix(RM, M_pos_base) function J = getJacobianMatrix(RM, M_pos_base)
% RM: [3x6] unit vector of each leg in the fixed frame % RM: [3x6] unit vector of each leg in the fixed frame
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
@ -1627,6 +1666,28 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
J(:, 1:3) = RM'; J(:, 1:3) = RM';
J(:, 4:6) = cross(M_pos_base, RM)'; J(:, 4:6) = cross(M_pos_base, RM)';
end end
%%
function [L] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
end end
#+end_src #+end_src

BIN
simscape_subsystems/hexapod_leg.slx Normal file
View File

Binary file not shown.

34
src/initializeMicroHexapod.m
View File

@ -11,7 +11,11 @@
function [micro_hexapod] = initializeMicroHexapod(opts_param) function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Default values for opts %% Default values for opts
opts = struct('rigid', false); opts = struct(...
'rigid', false, ...
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters %% Populate opts with input parameters
if exist('opts_param','var') if exist('opts_param','var')
@ -23,7 +27,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Stewart Object %% Stewart Object
micro_hexapod = struct(); micro_hexapod = struct();
micro_hexapod.h = 350; % Total height of the platform [mm] micro_hexapod.h = 350; % Total height of the platform [mm]
micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm] micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
%% Bottom Plate - Mechanical Design %% Bottom Plate - Mechanical Design
BP = struct(); BP = struct();
@ -104,6 +108,9 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% %%
micro_hexapod = initializeParameters(micro_hexapod); micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
%% Save %% Save
save('./mat/stages.mat', 'micro_hexapod', '-append'); save('./mat/stages.mat', 'micro_hexapod', '-append');
@ -197,6 +204,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
stewart.J = getJacobianMatrix(leg_vectors', aa'); stewart.J = getJacobianMatrix(leg_vectors', aa');
end end
%%
function J = getJacobianMatrix(RM, M_pos_base) function J = getJacobianMatrix(RM, M_pos_base)
% RM: [3x6] unit vector of each leg in the fixed frame % RM: [3x6] unit vector of each leg in the fixed frame
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
@ -204,4 +212,26 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
J(:, 1:3) = RM'; J(:, 1:3) = RM';
J(:, 4:6) = cross(M_pos_base, RM)'; J(:, 4:6) = cross(M_pos_base, RM)';
end end
%%
function [L] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
end end