Test of position/orientation of an Hexapod
This commit is contained in:
parent
b14de47379
commit
9fb86964ef
@ -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>
|
||||
|
||||
@ -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 |
|
||||
|
||||
BIN
simscape/hexapod_tests.slx
Normal file
BIN
simscape/hexapod_tests.slx
Normal file
Binary file not shown.
@ -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-12-06 ven. 12:04 -->
|
||||
<!-- 2019-12-10 mar. 18:05 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Simscape Model</title>
|
||||
@ -258,64 +258,65 @@ for the JavaScript code in this tag.
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#org5c21e47">1. Simulink Project - Startup and Shutdown scripts</a></li>
|
||||
<li><a href="#org5ca7221">2. Simscape Multibody - Presentation</a>
|
||||
<li><a href="#org88d59ba">1. Simulink Project - Startup and Shutdown scripts</a></li>
|
||||
<li><a href="#orga2206d5">2. Simscape Multibody - Presentation</a>
|
||||
<ul>
|
||||
<li><a href="#orga5fbe1d">2.1. Solid bodies</a></li>
|
||||
<li><a href="#org8017e71">2.2. Frames</a></li>
|
||||
<li><a href="#org486d9bf">2.3. Joints</a></li>
|
||||
<li><a href="#org5a1378a">2.4. Measurements</a></li>
|
||||
<li><a href="#orga435910">2.5. Excitation</a></li>
|
||||
<li><a href="#org6b4540f">2.1. Solid bodies</a></li>
|
||||
<li><a href="#orge470a34">2.2. Frames</a></li>
|
||||
<li><a href="#org474edd3">2.3. Joints</a></li>
|
||||
<li><a href="#org9171b40">2.4. Measurements</a></li>
|
||||
<li><a href="#org7df2aef">2.5. Excitation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org289000e">3. Simulink files and signal names</a>
|
||||
<li><a href="#org803cadc">3. Simulink files and signal names</a>
|
||||
<ul>
|
||||
<li><a href="#orga9d8af9">3.1. List of Simscape files</a></li>
|
||||
<li><a href="#org2a9705c">3.2. List of Inputs</a>
|
||||
<li><a href="#orgb5c767a">3.1. List of Simscape files</a></li>
|
||||
<li><a href="#org64dbd0d">3.2. List of Inputs</a>
|
||||
<ul>
|
||||
<li><a href="#org7fd5d25">3.2.1. Perturbations</a></li>
|
||||
<li><a href="#org0337003">3.2.2. Measurement Noise</a></li>
|
||||
<li><a href="#org250f869">3.2.3. Control Inputs</a></li>
|
||||
<li><a href="#orgeecef4d">3.2.1. Perturbations</a></li>
|
||||
<li><a href="#org4e58ed6">3.2.2. Measurement Noise</a></li>
|
||||
<li><a href="#orgc9006ba">3.2.3. Control Inputs</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orge7abaa0">3.3. List of Outputs</a></li>
|
||||
<li><a href="#orgeb985b0">3.3. List of Outputs</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org0fbeac7">4. Simulink Library</a>
|
||||
<li><a href="#orgfd5289f">4. Simulink Library</a>
|
||||
<ul>
|
||||
<li><a href="#org3ec0c6f">4.1. <code>inputs</code></a></li>
|
||||
<li><a href="#org0abb61d">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="#orgef1709b">4.4. <code>QuaternionToAngles</code></a></li>
|
||||
<li><a href="#org8a40a80">4.5. <code>RotationMatrixToAngle</code></a></li>
|
||||
<li><a href="#orgd1ebe0f">4.1. <code>inputs</code></a></li>
|
||||
<li><a href="#org1ae1d37">4.2. <code>nass_library</code></a></li>
|
||||
<li><a href="#org54e3d07">4.3. <code>pos_error_wrt_nass_base</code></a></li>
|
||||
<li><a href="#orgc0f0f02">4.4. <code>QuaternionToAngles</code></a></li>
|
||||
<li><a href="#orgbda19ed">4.5. <code>RotationMatrixToAngle</code></a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org81a7962">5. Functions</a>
|
||||
<li><a href="#org54193c9">5. Functions</a>
|
||||
<ul>
|
||||
<li><a href="#orge3706e5">5.1. computePsdDispl</a></li>
|
||||
<li><a href="#org14a492d">5.2. computeSetpoint</a></li>
|
||||
<li><a href="#org3e9a86f">5.3. converErrorBasis</a></li>
|
||||
<li><a href="#org14d5b4c">5.4. generateDiagPidControl</a></li>
|
||||
<li><a href="#org5876af4">5.5. identifyPlant</a></li>
|
||||
<li><a href="#orgd01681b">5.6. runSimulation</a></li>
|
||||
<li><a href="#orgc1f5840">5.1. computePsdDispl</a></li>
|
||||
<li><a href="#org0d8e9e6">5.2. computeSetpoint</a></li>
|
||||
<li><a href="#org9a191e3">5.3. converErrorBasis</a></li>
|
||||
<li><a href="#org2d44efa">5.4. generateDiagPidControl</a></li>
|
||||
<li><a href="#org6f7736a">5.5. identifyPlant</a></li>
|
||||
<li><a href="#org32dd782">5.6. runSimulation</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgc88bb4f">6. Initialize Elements</a>
|
||||
<li><a href="#org5cc2276">6. Initialize Elements</a>
|
||||
<ul>
|
||||
<li><a href="#org53e8eea">6.1. Experiment</a></li>
|
||||
<li><a href="#orgd101ec3">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="#org2b6b02d">6.4. Ground</a></li>
|
||||
<li><a href="#orge2875ed">6.5. Granite</a></li>
|
||||
<li><a href="#org61ee0ba">6.6. Translation Stage</a></li>
|
||||
<li><a href="#orgdba0ed0">6.7. Tilt Stage</a></li>
|
||||
<li><a href="#org6d74dd3">6.8. Spindle</a></li>
|
||||
<li><a href="#orgde1ec20">6.9. Micro Hexapod</a></li>
|
||||
<li><a href="#orgbfe4e10">6.10. Center of gravity compensation</a></li>
|
||||
<li><a href="#org1c8d312">6.11. Mirror</a></li>
|
||||
<li><a href="#orgf75cc87">6.12. Nano Hexapod</a></li>
|
||||
<li><a href="#orgbb8d5dd">6.13. Cedrat Actuator</a></li>
|
||||
<li><a href="#orgbb20f65">6.14. Sample</a></li>
|
||||
<li><a href="#org664a7e3">6.1. Experiment</a></li>
|
||||
<li><a href="#org89c58f8">6.2. Generate Reference Signals</a></li>
|
||||
<li><a href="#org77d160f">6.3. <span class="todo TODO">TODO</span> Inputs</a></li>
|
||||
<li><a href="#org962187e">6.4. Ground</a></li>
|
||||
<li><a href="#org198c366">6.5. Granite</a></li>
|
||||
<li><a href="#org6e967d2">6.6. Translation Stage</a></li>
|
||||
<li><a href="#org6c95b53">6.7. Tilt Stage</a></li>
|
||||
<li><a href="#org13271e7">6.8. Spindle</a></li>
|
||||
<li><a href="#org3c482fe">6.9. Initialize Hexapod legs' length</a></li>
|
||||
<li><a href="#org7c295aa">6.10. Micro Hexapod</a></li>
|
||||
<li><a href="#org72492f3">6.11. Center of gravity compensation</a></li>
|
||||
<li><a href="#org68d031b">6.12. Mirror</a></li>
|
||||
<li><a href="#org74149ff">6.13. Nano Hexapod</a></li>
|
||||
<li><a href="#org94d9a68">6.14. Cedrat Actuator</a></li>
|
||||
<li><a href="#org8ceaacc">6.15. Sample</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
@ -326,19 +327,19 @@ for the JavaScript code in this tag.
|
||||
This file is used to explain how this Simscape Model works.
|
||||
</p>
|
||||
<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="#orgd5bef84">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="#org3c886b5">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="#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="#orgfd7237b">1</a>, the simulink project with the associated scripts are presented</li>
|
||||
<li>In section <a href="#orgb6586ca">2</a>, an introduction to Simscape Multibody is done</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="#orgecb3837">4</a>, the list of the Simulink library elements are described</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="#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>
|
||||
|
||||
<div id="outline-container-org5c21e47" class="outline-2">
|
||||
<h2 id="org5c21e47"><span class="section-number-2">1</span> Simulink Project - Startup and Shutdown scripts</h2>
|
||||
<div id="outline-container-org88d59ba" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
<a id="org041c0e9"></a>
|
||||
<a id="orgfd7237b"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -400,11 +401,11 @@ The project also permits to automatically add defined folder to the path when th
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org5ca7221" class="outline-2">
|
||||
<h2 id="org5ca7221"><span class="section-number-2">2</span> Simscape Multibody - Presentation</h2>
|
||||
<div id="outline-container-orga2206d5" class="outline-2">
|
||||
<h2 id="orga2206d5"><span class="section-number-2">2</span> Simscape Multibody - Presentation</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
<a id="orgd5bef84"></a>
|
||||
<a id="orgb6586ca"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -416,8 +417,8 @@ A <a href="https://.mathworks.com/products/simscape.html">simscape</a> model per
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orga5fbe1d" class="outline-3">
|
||||
<h3 id="orga5fbe1d"><span class="section-number-3">2.1</span> Solid bodies</h3>
|
||||
<div id="outline-container-org6b4540f" class="outline-3">
|
||||
<h3 id="org6b4540f"><span class="section-number-3">2.1</span> Solid bodies</h3>
|
||||
<div class="outline-text-3" id="text-2-1">
|
||||
<p>
|
||||
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 id="outline-container-org8017e71" class="outline-3">
|
||||
<h3 id="org8017e71"><span class="section-number-3">2.2</span> Frames</h3>
|
||||
<div id="outline-container-orge470a34" class="outline-3">
|
||||
<h3 id="orge470a34"><span class="section-number-3">2.2</span> Frames</h3>
|
||||
<div class="outline-text-3" id="text-2-2">
|
||||
<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.
|
||||
@ -439,8 +440,8 @@ They can be defined from the solid body geometry, or using the <a href="https://
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org486d9bf" class="outline-3">
|
||||
<h3 id="org486d9bf"><span class="section-number-3">2.3</span> Joints</h3>
|
||||
<div id="outline-container-org474edd3" class="outline-3">
|
||||
<h3 id="org474edd3"><span class="section-number-3">2.3</span> Joints</h3>
|
||||
<div class="outline-text-3" id="text-2-3">
|
||||
<p>
|
||||
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>.
|
||||
</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>
|
||||
|
||||
<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>
|
||||
</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>
|
||||
|
||||
<colgroup>
|
||||
@ -875,8 +876,8 @@ Composite Force/Torque sensing:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org5a1378a" class="outline-3">
|
||||
<h3 id="org5a1378a"><span class="section-number-3">2.4</span> Measurements</h3>
|
||||
<div id="outline-container-org9171b40" class="outline-3">
|
||||
<h3 id="org9171b40"><span class="section-number-3">2.4</span> Measurements</h3>
|
||||
<div class="outline-text-3" id="text-2-4">
|
||||
<p>
|
||||
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 id="outline-container-orga435910" class="outline-3">
|
||||
<h3 id="orga435910"><span class="section-number-3">2.5</span> Excitation</h3>
|
||||
<div id="outline-container-org7df2aef" class="outline-3">
|
||||
<h3 id="org7df2aef"><span class="section-number-3">2.5</span> Excitation</h3>
|
||||
<div class="outline-text-3" id="text-2-5">
|
||||
<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>.
|
||||
@ -914,11 +915,11 @@ Internal force, acting reciprocally between base and following origins is implem
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org289000e" class="outline-2">
|
||||
<h2 id="org289000e"><span class="section-number-2">3</span> Simulink files and signal names</h2>
|
||||
<div id="outline-container-org803cadc" class="outline-2">
|
||||
<h2 id="org803cadc"><span class="section-number-2">3</span> Simulink files and signal names</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<p>
|
||||
<a id="orgfff3010"></a>
|
||||
<a id="org3e8132a"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -926,8 +927,8 @@ In order to "normalize" things, the names of all the signal are listed here.
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orga9d8af9" class="outline-3">
|
||||
<h3 id="orga9d8af9"><span class="section-number-3">3.1</span> List of Simscape files</h3>
|
||||
<div id="outline-container-orgb5c767a" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
Few different Simulink files are used:
|
||||
@ -939,7 +940,7 @@ Few different Simulink files are used:
|
||||
<li>control</li>
|
||||
</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>
|
||||
|
||||
<colgroup>
|
||||
@ -1006,14 +1007,14 @@ Few different Simulink files are used:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org2a9705c" class="outline-3">
|
||||
<h3 id="org2a9705c"><span class="section-number-3">3.2</span> List of Inputs</h3>
|
||||
<div id="outline-container-org64dbd0d" class="outline-3">
|
||||
<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>
|
||||
<div id="outline-container-org7fd5d25" class="outline-4">
|
||||
<h4 id="org7fd5d25"><span class="section-number-4">3.2.1</span> Perturbations</h4>
|
||||
<div id="outline-container-orgeecef4d" class="outline-4">
|
||||
<h4 id="orgeecef4d"><span class="section-number-4">3.2.1</span> Perturbations</h4>
|
||||
<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>
|
||||
|
||||
<colgroup>
|
||||
@ -1059,10 +1060,10 @@ Few different Simulink files are used:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org0337003" class="outline-4">
|
||||
<h4 id="org0337003"><span class="section-number-4">3.2.2</span> Measurement Noise</h4>
|
||||
<div id="outline-container-org4e58ed6" class="outline-4">
|
||||
<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">
|
||||
<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>
|
||||
|
||||
<colgroup>
|
||||
@ -1094,10 +1095,10 @@ Few different Simulink files are used:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org250f869" class="outline-4">
|
||||
<h4 id="org250f869"><span class="section-number-4">3.2.3</span> Control Inputs</h4>
|
||||
<div id="outline-container-orgc9006ba" class="outline-4">
|
||||
<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">
|
||||
<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>
|
||||
|
||||
<colgroup>
|
||||
@ -1219,10 +1220,10 @@ Few different Simulink files are used:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orge7abaa0" class="outline-3">
|
||||
<h3 id="orge7abaa0"><span class="section-number-3">3.3</span> List of Outputs</h3>
|
||||
<div id="outline-container-orgeb985b0" class="outline-3">
|
||||
<h3 id="orgeb985b0"><span class="section-number-3">3.3</span> List of Outputs</h3>
|
||||
<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>
|
||||
|
||||
<colgroup>
|
||||
@ -1338,11 +1339,11 @@ Few different Simulink files are used:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org0fbeac7" class="outline-2">
|
||||
<h2 id="org0fbeac7"><span class="section-number-2">4</span> Simulink Library</h2>
|
||||
<div id="outline-container-orgfd5289f" class="outline-2">
|
||||
<h2 id="orgfd5289f"><span class="section-number-2">4</span> Simulink Library</h2>
|
||||
<div class="outline-text-2" id="text-4">
|
||||
<p>
|
||||
<a id="org3c886b5"></a>
|
||||
<a id="orgecb3837"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1350,8 +1351,8 @@ A simulink library is developed in order to share elements between the different
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org3ec0c6f" class="outline-3">
|
||||
<h3 id="org3ec0c6f"><span class="section-number-3">4.1</span> <code>inputs</code></h3>
|
||||
<div id="outline-container-orgd1ebe0f" class="outline-3">
|
||||
<h3 id="orgd1ebe0f"><span class="section-number-3">4.1</span> <code>inputs</code></h3>
|
||||
<div class="outline-text-3" id="text-4-1">
|
||||
<p>
|
||||
<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 id="outline-container-org0abb61d" class="outline-3">
|
||||
<h3 id="org0abb61d"><span class="section-number-3">4.2</span> <code>nass_library</code></h3>
|
||||
<div id="outline-container-org1ae1d37" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
<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 id="outline-container-org04c0eb4" class="outline-3">
|
||||
<h3 id="org04c0eb4"><span class="section-number-3">4.3</span> <code>pos_error_wrt_nass_base</code></h3>
|
||||
<div id="outline-container-org54e3d07" class="outline-3">
|
||||
<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">
|
||||
<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>
|
||||
@ -1377,8 +1378,8 @@ A simulink library is developed in order to share elements between the different
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgef1709b" class="outline-3">
|
||||
<h3 id="orgef1709b"><span class="section-number-3">4.4</span> <code>QuaternionToAngles</code></h3>
|
||||
<div id="outline-container-orgc0f0f02" class="outline-3">
|
||||
<h3 id="orgc0f0f02"><span class="section-number-3">4.4</span> <code>QuaternionToAngles</code></h3>
|
||||
<div class="outline-text-3" id="text-4-4">
|
||||
<p>
|
||||
<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 id="outline-container-org8a40a80" class="outline-3">
|
||||
<h3 id="org8a40a80"><span class="section-number-3">4.5</span> <code>RotationMatrixToAngle</code></h3>
|
||||
<div id="outline-container-orgbda19ed" class="outline-3">
|
||||
<h3 id="orgbda19ed"><span class="section-number-3">4.5</span> <code>RotationMatrixToAngle</code></h3>
|
||||
<div class="outline-text-3" id="text-4-5">
|
||||
<p>
|
||||
<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 id="outline-container-org81a7962" class="outline-2">
|
||||
<h2 id="org81a7962"><span class="section-number-2">5</span> Functions</h2>
|
||||
<div id="outline-container-org54193c9" class="outline-2">
|
||||
<h2 id="org54193c9"><span class="section-number-2">5</span> Functions</h2>
|
||||
<div class="outline-text-2" id="text-5">
|
||||
<p>
|
||||
<a id="org21528cf"></a>
|
||||
<a id="org94ca66a"></a>
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-orge3706e5" class="outline-3">
|
||||
<h3 id="orge3706e5"><span class="section-number-3">5.1</span> computePsdDispl</h3>
|
||||
<div id="outline-container-orgc1f5840" class="outline-3">
|
||||
<h3 id="orgc1f5840"><span class="section-number-3">5.1</span> computePsdDispl</h3>
|
||||
<div class="outline-text-3" id="text-5-1">
|
||||
<p>
|
||||
<a id="org63dd43c"></a>
|
||||
<a id="org46da1ed"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1443,11 +1444,11 @@ This Matlab function is accessible <a href="../src/computePsdDispl.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org14a492d" class="outline-3">
|
||||
<h3 id="org14a492d"><span class="section-number-3">5.2</span> computeSetpoint</h3>
|
||||
<div id="outline-container-org0d8e9e6" class="outline-3">
|
||||
<h3 id="org0d8e9e6"><span class="section-number-3">5.2</span> computeSetpoint</h3>
|
||||
<div class="outline-text-3" id="text-5-2">
|
||||
<p>
|
||||
<a id="org0dcd84b"></a>
|
||||
<a id="orga7c431b"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1519,11 +1520,11 @@ setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-hi
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org3e9a86f" class="outline-3">
|
||||
<h3 id="org3e9a86f"><span class="section-number-3">5.3</span> converErrorBasis</h3>
|
||||
<div id="outline-container-org9a191e3" class="outline-3">
|
||||
<h3 id="org9a191e3"><span class="section-number-3">5.3</span> converErrorBasis</h3>
|
||||
<div class="outline-text-3" id="text-5-3">
|
||||
<p>
|
||||
<a id="org273e422"></a>
|
||||
<a id="org2e6fbe8"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1661,11 +1662,11 @@ error_nass = <span class="org-rainbow-delimiters-depth-1">[</span>dx; dy; dz; th
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org14d5b4c" class="outline-3">
|
||||
<h3 id="org14d5b4c"><span class="section-number-3">5.4</span> generateDiagPidControl</h3>
|
||||
<div id="outline-container-org2d44efa" class="outline-3">
|
||||
<h3 id="org2d44efa"><span class="section-number-3">5.4</span> generateDiagPidControl</h3>
|
||||
<div class="outline-text-3" id="text-5-4">
|
||||
<p>
|
||||
<a id="org4a60577"></a>
|
||||
<a id="orgd1ce773"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1695,11 +1696,11 @@ This Matlab function is accessible <a href="../src/generateDiagPidControl.m">her
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="outline-container-org5876af4" class="outline-3">
|
||||
<h3 id="org5876af4"><span class="section-number-3">5.5</span> identifyPlant</h3>
|
||||
<div id="outline-container-org6f7736a" class="outline-3">
|
||||
<h3 id="org6f7736a"><span class="section-number-3">5.5</span> identifyPlant</h3>
|
||||
<div class="outline-text-3" id="text-5-5">
|
||||
<p>
|
||||
<a id="orgff9faac"></a>
|
||||
<a id="orgd69607b"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1791,11 +1792,11 @@ This Matlab function is accessible <a href="../src/identifyPlant.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgd01681b" class="outline-3">
|
||||
<h3 id="orgd01681b"><span class="section-number-3">5.6</span> runSimulation</h3>
|
||||
<div id="outline-container-org32dd782" class="outline-3">
|
||||
<h3 id="org32dd782"><span class="section-number-3">5.6</span> runSimulation</h3>
|
||||
<div class="outline-text-3" id="text-5-6">
|
||||
<p>
|
||||
<a id="org1f4a257"></a>
|
||||
<a id="org95edf73"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1864,18 +1865,18 @@ This Matlab function is accessible <a href="../src/runSimulation.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="outline-container-orgc88bb4f" class="outline-2">
|
||||
<h2 id="orgc88bb4f"><span class="section-number-2">6</span> Initialize Elements</h2>
|
||||
<div id="outline-container-org5cc2276" class="outline-2">
|
||||
<h2 id="org5cc2276"><span class="section-number-2">6</span> Initialize Elements</h2>
|
||||
<div class="outline-text-2" id="text-6">
|
||||
<p>
|
||||
<a id="org882f3b1"></a>
|
||||
<a id="org18b2c56"></a>
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-org53e8eea" class="outline-3">
|
||||
<h3 id="org53e8eea"><span class="section-number-3">6.1</span> Experiment</h3>
|
||||
<div id="outline-container-org664a7e3" class="outline-3">
|
||||
<h3 id="org664a7e3"><span class="section-number-3">6.1</span> Experiment</h3>
|
||||
<div class="outline-text-3" id="text-6-1">
|
||||
<p>
|
||||
<a id="org7855770"></a>
|
||||
<a id="org395aa48"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1909,11 +1910,11 @@ This Matlab function is accessible <a href="../src/initializeExperiment.m">here<
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgd101ec3" class="outline-3">
|
||||
<h3 id="orgd101ec3"><span class="section-number-3">6.2</span> Generate Reference Signals</h3>
|
||||
<div id="outline-container-org89c58f8" class="outline-3">
|
||||
<h3 id="org89c58f8"><span class="section-number-3">6.2</span> Generate Reference Signals</h3>
|
||||
<div class="outline-text-3" id="text-6-2">
|
||||
<p>
|
||||
<a id="org4821745"></a>
|
||||
<a id="orgdda511e"></a>
|
||||
</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_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_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">'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">'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>
|
||||
@ -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">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>
|
||||
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-rainbow-delimiters-depth-1">(</span>t<span class="org-type"><</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"><</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"><</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"><</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-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"><</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"><</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"><</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"><</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>
|
||||
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>
|
||||
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>
|
||||
@ -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">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>
|
||||
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>
|
||||
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>
|
||||
@ -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>;
|
||||
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">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>;
|
||||
@ -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>;
|
||||
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">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>;
|
||||
@ -2043,11 +2040,11 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org15d2680" class="outline-3">
|
||||
<h3 id="org15d2680"><span class="section-number-3">6.3</span> <span class="todo TODO">TODO</span> Inputs</h3>
|
||||
<div id="outline-container-org77d160f" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
<a id="org2728bfe"></a>
|
||||
<a id="org7501b33"></a>
|
||||
</p>
|
||||
|
||||
<ul class="org-ul">
|
||||
@ -2239,11 +2236,11 @@ This Matlab function is accessible <a href="../src/initializeInputs.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org2b6b02d" class="outline-3">
|
||||
<h3 id="org2b6b02d"><span class="section-number-3">6.4</span> Ground</h3>
|
||||
<div id="outline-container-org962187e" class="outline-3">
|
||||
<h3 id="org962187e"><span class="section-number-3">6.4</span> Ground</h3>
|
||||
<div class="outline-text-3" id="text-6-4">
|
||||
<p>
|
||||
<a id="orgc97869c"></a>
|
||||
<a id="org763b0ff"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2267,11 +2264,11 @@ This Matlab function is accessible <a href="../src/initializeGround.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orge2875ed" class="outline-3">
|
||||
<h3 id="orge2875ed"><span class="section-number-3">6.5</span> Granite</h3>
|
||||
<div id="outline-container-org198c366" class="outline-3">
|
||||
<h3 id="org198c366"><span class="section-number-3">6.5</span> Granite</h3>
|
||||
<div class="outline-text-3" id="text-6-5">
|
||||
<p>
|
||||
<a id="org706d529"></a>
|
||||
<a id="orgbdb5477"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2337,11 +2334,11 @@ This Matlab function is accessible <a href="../src/initializeGranite.m">here</a>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org61ee0ba" class="outline-3">
|
||||
<h3 id="org61ee0ba"><span class="section-number-3">6.6</span> Translation Stage</h3>
|
||||
<div id="outline-container-org6e967d2" class="outline-3">
|
||||
<h3 id="org6e967d2"><span class="section-number-3">6.6</span> Translation Stage</h3>
|
||||
<div class="outline-text-3" id="text-6-6">
|
||||
<p>
|
||||
<a id="org5407512"></a>
|
||||
<a id="org2597c18"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2423,11 +2420,11 @@ This Matlab function is accessible <a href="../src/initializeTy.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgdba0ed0" class="outline-3">
|
||||
<h3 id="orgdba0ed0"><span class="section-number-3">6.7</span> Tilt Stage</h3>
|
||||
<div id="outline-container-org6c95b53" class="outline-3">
|
||||
<h3 id="org6c95b53"><span class="section-number-3">6.7</span> Tilt Stage</h3>
|
||||
<div class="outline-text-3" id="text-6-7">
|
||||
<p>
|
||||
<a id="org22557f6"></a>
|
||||
<a id="org29b1fab"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2498,11 +2495,11 @@ This Matlab function is accessible <a href="../src/initializeRy.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6d74dd3" class="outline-3">
|
||||
<h3 id="org6d74dd3"><span class="section-number-3">6.8</span> Spindle</h3>
|
||||
<div id="outline-container-org13271e7" class="outline-3">
|
||||
<h3 id="org13271e7"><span class="section-number-3">6.8</span> Spindle</h3>
|
||||
<div class="outline-text-3" id="text-6-8">
|
||||
<p>
|
||||
<a id="orgc1728c6"></a>
|
||||
<a id="org50164cd"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2569,11 +2566,49 @@ This Matlab function is accessible <a href="../src/initializeRz.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgde1ec20" class="outline-3">
|
||||
<h3 id="orgde1ec20"><span class="section-number-3">6.9</span> Micro Hexapod</h3>
|
||||
<div id="outline-container-org3c482fe" class="outline-3">
|
||||
<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">
|
||||
<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>
|
||||
@ -2583,7 +2618,11 @@ This Matlab function is accessible <a href="../src/initializeMicroHexapod.m">her
|
||||
<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>
|
||||
<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-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>
|
||||
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.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>
|
||||
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>
|
||||
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>
|
||||
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,24 +2811,47 @@ 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>;
|
||||
<span class="org-keyword">end</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-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>
|
||||
% 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
|
||||
J = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-1">)</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>;
|
||||
<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>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbfe4e10" class="outline-3">
|
||||
<h3 id="orgbfe4e10"><span class="section-number-3">6.10</span> Center of gravity compensation</h3>
|
||||
<div class="outline-text-3" id="text-6-10">
|
||||
<div id="outline-container-org72492f3" class="outline-3">
|
||||
<h3 id="org72492f3"><span class="section-number-3">6.11</span> Center of gravity compensation</h3>
|
||||
<div class="outline-text-3" id="text-6-11">
|
||||
<p>
|
||||
<a id="org3db12eb"></a>
|
||||
<a id="org991aec0"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2830,11 +2895,11 @@ This Matlab function is accessible <a href="../src/initializeAxisc.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org1c8d312" class="outline-3">
|
||||
<h3 id="org1c8d312"><span class="section-number-3">6.11</span> Mirror</h3>
|
||||
<div class="outline-text-3" id="text-6-11">
|
||||
<div id="outline-container-org68d031b" class="outline-3">
|
||||
<h3 id="org68d031b"><span class="section-number-3">6.12</span> Mirror</h3>
|
||||
<div class="outline-text-3" id="text-6-12">
|
||||
<p>
|
||||
<a id="org1354fa1"></a>
|
||||
<a id="org43cadec"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -2900,11 +2965,11 @@ This Matlab function is accessible <a href="../src/initializeMirror.m">here</a>.
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgf75cc87" class="outline-3">
|
||||
<h3 id="orgf75cc87"><span class="section-number-3">6.12</span> Nano Hexapod</h3>
|
||||
<div class="outline-text-3" id="text-6-12">
|
||||
<div id="outline-container-org74149ff" class="outline-3">
|
||||
<h3 id="org74149ff"><span class="section-number-3">6.13</span> Nano Hexapod</h3>
|
||||
<div class="outline-text-3" id="text-6-13">
|
||||
<p>
|
||||
<a id="orgbb91911"></a>
|
||||
<a id="orgcb27cac"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -3121,11 +3186,11 @@ This Matlab function is accessible <a href="../src/initializeNanoHexapod.m">here
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbb8d5dd" class="outline-3">
|
||||
<h3 id="orgbb8d5dd"><span class="section-number-3">6.13</span> Cedrat Actuator</h3>
|
||||
<div class="outline-text-3" id="text-6-13">
|
||||
<div id="outline-container-org94d9a68" class="outline-3">
|
||||
<h3 id="org94d9a68"><span class="section-number-3">6.14</span> Cedrat Actuator</h3>
|
||||
<div class="outline-text-3" id="text-6-14">
|
||||
<p>
|
||||
<a id="org3955a2d"></a>
|
||||
<a id="orgdb4500f"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -3165,11 +3230,11 @@ This Matlab function is accessible <a href="../src/initializeCedratPiezo.m">here
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbb20f65" class="outline-3">
|
||||
<h3 id="orgbb20f65"><span class="section-number-3">6.14</span> Sample</h3>
|
||||
<div class="outline-text-3" id="text-6-14">
|
||||
<div id="outline-container-org8ceaacc" class="outline-3">
|
||||
<h3 id="org8ceaacc"><span class="section-number-3">6.15</span> Sample</h3>
|
||||
<div class="outline-text-3" id="text-6-15">
|
||||
<p>
|
||||
<a id="org92d0630"></a>
|
||||
<a id="org96164b7"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -3214,7 +3279,7 @@ This Matlab function is accessible <a href="../src/initializeSample.m">here</a>.
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<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>
|
||||
</div>
|
||||
</body>
|
||||
|
||||
@ -797,6 +797,7 @@ This Matlab function is accessible [[file:../src/initializeExperiment.m][here]].
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:header-args:matlab+: :eval no :results none
|
||||
:END:
|
||||
|
||||
<<sec:initializeReferences>>
|
||||
|
||||
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_period', 1, ... % Period of the displacement [s]
|
||||
'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]
|
||||
'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]
|
||||
'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
|
||||
@ -857,13 +858,13 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
|
||||
switch opts.Ry_type
|
||||
case 'constant'
|
||||
Ry(:) = pi/180*opts.Ry_amplitude;
|
||||
Ry(:) = opts.Ry_amplitude;
|
||||
case 'triangular'
|
||||
Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*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.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period);
|
||||
Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
|
||||
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*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
|
||||
case 'sinusoidal'
|
||||
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
|
||||
|
||||
otherwise
|
||||
warning('Ry_type is not set correctly');
|
||||
end
|
||||
@ -875,9 +876,9 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
|
||||
switch opts.Rz_type
|
||||
case 'constant'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude;
|
||||
Rz(:) = opts.Rz_amplitude;
|
||||
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
|
||||
warning('Rz_type is not set correctly');
|
||||
end
|
||||
@ -887,8 +888,6 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
t = [0, Ts];
|
||||
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
|
||||
case 'constant'
|
||||
Dh = [opts.Dh_pos, opts.Dh_pos];
|
||||
@ -906,8 +905,6 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
||||
t = [0, Ts];
|
||||
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
|
||||
case 'constant'
|
||||
Dn = [opts.Dn_pos, opts.Dn_pos];
|
||||
@ -1421,6 +1418,40 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]].
|
||||
end
|
||||
#+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
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
|
||||
@ -1434,7 +1465,11 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
|
||||
#+begin_src matlab
|
||||
function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
%% 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
|
||||
if exist('opts_param','var')
|
||||
@ -1446,7 +1481,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
|
||||
%% Stewart Object
|
||||
micro_hexapod = struct();
|
||||
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
|
||||
BP = struct();
|
||||
@ -1527,6 +1562,9 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
|
||||
%%
|
||||
micro_hexapod = initializeParameters(micro_hexapod);
|
||||
|
||||
%% Setup equilibrium position of each leg
|
||||
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
|
||||
|
||||
%% Save
|
||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||
|
||||
@ -1620,13 +1658,36 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
|
||||
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
||||
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
|
||||
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
|
||||
J = zeros(6);
|
||||
J(:, 1:3) = RM';
|
||||
J(:, 4:6) = cross(M_pos_base, RM)';
|
||||
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_src
|
||||
|
||||
|
||||
BIN
simscape_subsystems/hexapod_leg.slx
Normal file
BIN
simscape_subsystems/hexapod_leg.slx
Normal file
Binary file not shown.
@ -11,7 +11,11 @@
|
||||
|
||||
function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
%% 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
|
||||
if exist('opts_param','var')
|
||||
@ -23,7 +27,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
%% Stewart Object
|
||||
micro_hexapod = struct();
|
||||
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
|
||||
BP = struct();
|
||||
@ -104,6 +108,9 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
%%
|
||||
micro_hexapod = initializeParameters(micro_hexapod);
|
||||
|
||||
%% Setup equilibrium position of each leg
|
||||
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
|
||||
|
||||
%% Save
|
||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||
|
||||
@ -197,11 +204,34 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
||||
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
|
||||
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
|
||||
J = zeros(6);
|
||||
J(:, 1:3) = RM';
|
||||
J(:, 4:6) = cross(M_pos_base, RM)';
|
||||
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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user