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
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">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<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 name="viewport" content="width=device-width, initial-scale=1" />
<title>Kinematics of the station</title>
@@ -246,6 +246,31 @@ for the JavaScript code in this tag.
}
/*]]>*///-->
</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>
<body>
<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>
</div><div id="content">
<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>
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>
<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 id="postamble" class="status">
<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>
</div>
</body>

156
kinematics/index.org
View File

@@ -42,23 +42,91 @@
:END:
* 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:
#+begin_src bash :exports none :results none
if [ matlab/kinematics.m -nt data/kinematics.zip ]; then
cp matlab/kinematics.m kinematics.m;
zip data/kinematics \
mat/data.mat \
kinematics.m
rm kinematics.m;
fi
#+end_src
* Micro Hexapod
** How the Symetrie Hexapod is controlled on the micro station
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):
#+begin_note
All the files (data and Matlab scripts) are accessible [[file:data/kinematics.zip][here]].
#+end_note
#+begin_quote
The *Euler type II convention* is used to express the rotation vector.
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)
<<matlab-dir>>
#+end_src
@@ -66,3 +134,63 @@
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+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 |