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

Reworked index.org: better filenames

Browse Source 
Removed few unused functions
This commit is contained in:
Thomas Dehaeze 2020-01-27 17:42:09 +01:00
parent 3e4816929f
commit 950302e5d6
24 changed files with 4041 additions and 3618 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

530
cubic-configuration.html
View File

@ -1,14 +1,15 @@
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN" <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2019-12-12 jeu. 20:10 --> <!-- 2020-01-27 lun. 17:41 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Cubic configuration for the Stewart Platform</title> <title>Cubic configuration for the Stewart Platform</title>
<meta name="generator" content="Org mode" /> <meta name="generator" content="Org mode" />
<meta name="author" content="Thomas Dehaeze" /> <meta name="author" content="Dehaeze Thomas" />
<style type="text/css"> <style type="text/css">
<!--/*--><![CDATA[/*><!--*/ <!--/*--><![CDATA[/*><!--*/
.title { text-align: center; .title { text-align: center;
@ -193,14 +194,18 @@
.org-svg { width: 90%; } .org-svg { width: 90%; }
/*]]>*/--> /*]]>*/-->
</style> </style>
<link rel="stylesheet" type="text/css" href="https://gongzhitaao.org/orgcss/org.css"/>
<link rel="stylesheet" type="text/css" href="./css/htmlize.css"/> <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
<script src="./js/jquery.min.js"></script>
<script src="./js/bootstrap.min.js"></script>
<script src="./js/jquery.stickytableheaders.min.js"></script>
<script src="./js/readtheorg.js"></script>
<script type="text/javascript"> <script type="text/javascript">
/* /*
@licstart The following is the entire license notice for the @licstart The following is the entire license notice for the
JavaScript code in this tag. JavaScript code in this tag.
Copyright (C) 2012-2019 Free Software Foundation, Inc. Copyright (C) 2012-2020 Free Software Foundation, Inc.
The JavaScript code in this tag is free software: you can The JavaScript code in this tag is free software: you can
redistribute it and/or modify it under the terms of the GNU redistribute it and/or modify it under the terms of the GNU
@ -278,33 +283,33 @@ for the JavaScript code in this tag.
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#org4a16be2">1. Questions we wish to answer with this analysis</a></li> <li><a href="#org86c83bf">1. Questions we wish to answer with this analysis</a></li>
<li><a href="#org289931f">2. Configuration Analysis - Stiffness Matrix</a> <li><a href="#org0b05973">2. <span class="todo TODO">TODO</span> Configuration Analysis - Stiffness Matrix</a>
<ul> <ul>
<li><a href="#orgc378f8a">2.1. Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center</a></li> <li><a href="#org3f035e8">2.1. Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center</a></li>
<li><a href="#org608174e">2.2. Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center</a></li> <li><a href="#org77ecb36">2.2. Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center</a></li>
<li><a href="#orgbd736ef">2.3. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center</a></li> <li><a href="#org42ea8ad">2.3. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center</a></li>
<li><a href="#org6fbeda1">2.4. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center</a></li> <li><a href="#org38870ce">2.4. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center</a></li>
<li><a href="#org18633d3">2.5. Conclusion</a></li> <li><a href="#org08c7461">2.5. Conclusion</a></li>
</ul> </ul>
</li> </li>
<li><a href="#orgf0ba2d0">3. Cubic size analysis</a></li> <li><a href="#orgc4c2abd">3. <span class="todo TODO">TODO</span> Cubic size analysis</a></li>
<li><a href="#org97dffbc">4. initializeCubicConfiguration</a> <li><a href="#org36a27e6">4. <span class="todo TODO">TODO</span> initializeCubicConfiguration</a>
<ul> <ul>
<li><a href="#org4eb8b23">4.1. Function description</a></li> <li><a href="#orgf299c5c">4.1. Function description</a></li>
<li><a href="#orga42cb17">4.2. Optional Parameters</a></li> <li><a href="#org46c8589">4.2. Optional Parameters</a></li>
<li><a href="#orgc281f60">4.3. Cube Creation</a></li> <li><a href="#orgd8d9b14">4.3. Cube Creation</a></li>
<li><a href="#orgfed01f0">4.4. Vectors of each leg</a></li> <li><a href="#org181d1d8">4.4. Vectors of each leg</a></li>
<li><a href="#org21db1ef">4.5. Verification of Height of the Stewart Platform</a></li> <li><a href="#orgb396e98">4.5. Verification of Height of the Stewart Platform</a></li>
<li><a href="#org9578c3c">4.6. Determinate the location of the joints</a></li> <li><a href="#orgf38af83">4.6. Determinate the location of the joints</a></li>
<li><a href="#org71c9d4e">4.7. Returns Stewart Structure</a></li> <li><a href="#orgdf9e3cf">4.7. Returns Stewart Structure</a></li>
</ul> </ul>
</li> </li>
<li><a href="#orgb2d1742">5. Tests</a> <li><a href="#orgf8fb731">5. <span class="todo TODO">TODO</span> Tests</a>
<ul> <ul>
<li><a href="#org6e933c9">5.1. First attempt to parametrisation</a></li> <li><a href="#org4434fe5">5.1. First attempt to parametrisation</a></li>
<li><a href="#org60486ce">5.2. Second attempt</a></li> <li><a href="#org723e6eb">5.2. Second attempt</a></li>
<li><a href="#orge571873">5.3. Generate the Stewart platform for a Cubic configuration</a></li> <li><a href="#orgcc173ac">5.3. Generate the Stewart platform for a Cubic configuration</a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
@ -326,15 +331,15 @@ The specificity of the Cubic configuration is that each actuator is orthogonal w
</p> </p>
<p> <p>
To generate and study the Cubic configuration, <code>initializeCubicConfiguration</code> is used (description in section <a href="#org38614bc">4</a>). To generate and study the Cubic configuration, <code>initializeCubicConfiguration</code> is used (description in section <a href="#org8b1f609">4</a>).
</p> </p>
<p> <p>
According to <a class='org-ref-reference' href="#preumont07_six_axis_singl_stage_activ">preumont07_six_axis_singl_stage_activ</a>, the cubic configuration provides a uniform stiffness in all directions and <b>minimizes the crosscoupling</b> from actuator to sensor of different legs (being orthogonal to each other). According to <a class='org-ref-reference' href="#preumont07_six_axis_singl_stage_activ">preumont07_six_axis_singl_stage_activ</a>, the cubic configuration provides a uniform stiffness in all directions and <b>minimizes the crosscoupling</b> from actuator to sensor of different legs (being orthogonal to each other).
</p> </p>
<div id="outline-container-org4a16be2" class="outline-2"> <div id="outline-container-org86c83bf" class="outline-2">
<h2 id="org4a16be2"><span class="section-number-2">1</span> Questions we wish to answer with this analysis</h2> <h2 id="org86c83bf"><span class="section-number-2">1</span> Questions we wish to answer with this analysis</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p> <p>
The goal is to study the benefits of using a cubic configuration: The goal is to study the benefits of using a cubic configuration:
@ -347,40 +352,40 @@ The goal is to study the benefits of using a cubic configuration:
</div> </div>
</div> </div>
<div id="outline-container-org289931f" class="outline-2"> <div id="outline-container-org0b05973" class="outline-2">
<h2 id="org289931f"><span class="section-number-2">2</span> Configuration Analysis - Stiffness Matrix</h2> <h2 id="org0b05973"><span class="section-number-2">2</span> <span class="todo TODO">TODO</span> Configuration Analysis - Stiffness Matrix</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
</div> </div>
<div id="outline-container-orgc378f8a" class="outline-3"> <div id="outline-container-org3f035e8" class="outline-3">
<h3 id="orgc378f8a"><span class="section-number-3">2.1</span> Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center</h3> <h3 id="org3f035e8"><span class="section-number-3">2.1</span> Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center</h3>
<div class="outline-text-3" id="text-2-1"> <div class="outline-text-3" id="text-2-1">
<p> <p>
We create a cubic Stewart platform (figure <a href="#org8e23773">1</a>) in such a way that the center of the cube (black dot) is located at the center of the Stewart platform (blue dot). We create a cubic Stewart platform (figure <a href="#org1effc0f">1</a>) in such a way that the center of the cube (black dot) is located at the center of the Stewart platform (blue dot).
The Jacobian matrix is estimated at the location of the center of the cube. The Jacobian matrix is estimated at the location of the center of the cube.
</p> </p>
<div id="org8e23773" class="figure"> <div id="org1effc0f" class="figure">
<p><img src="./figs/3d-cubic-stewart-aligned.png" alt="3d-cubic-stewart-aligned.png" /> <p><img src="./figs/3d-cubic-stewart-aligned.png" alt="3d-cubic-stewart-aligned.png" />
</p> </p>
<p><span class="figure-number">Figure 1: </span>Centered cubic configuration</p> <p><span class="figure-number">Figure 1: </span>Centered cubic configuration</p>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... <pre class="src src-matlab">opts = struct(...
<span class="org-string">'H_tot'</span>, <span class="org-highlight-numbers-number">100</span>, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span> <span class="org-string">'H_tot'</span>, 100, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span>
<span class="org-string">'L'</span>, <span class="org-highlight-numbers-number">200</span><span class="org-type">/</span>sqrt<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"> % Size of the Cube [mm]</span> <span class="org-string">'L'</span>, 200<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [mm]</span>
<span class="org-string">'H'</span>, <span class="org-highlight-numbers-number">60</span>, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span> <span class="org-string">'H'</span>, 60, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span>
<span class="org-string">'H0'</span>, <span class="org-highlight-numbers-number">200</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">-</span><span class="org-highlight-numbers-number">60</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span> ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span> <span class="org-string">'H0'</span>, 200<span class="org-type">/</span>2<span class="org-type">-</span>60<span class="org-type">/</span>2 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = initializeCubicConfiguration(opts);
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... opts = struct(...
<span class="org-string">'Jd_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-type">-</span><span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span> <span class="org-string">'Jd_pos'</span>, [0, 0, <span class="org-type">-</span>50], ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span>
<span class="org-string">'Jf_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-type">-</span><span class="org-highlight-numbers-number">50</span><span class="org-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span> <span class="org-string">'Jf_pos'</span>, [0, 0, <span class="org-type">-</span>50] ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-1">(</span>stewart, opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = computeGeometricalProperties(stewart, opts);
save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'./mat/stewart.mat'</span>, <span class="org-string">'stewart'</span><span class="org-rainbow-delimiters-depth-1">)</span>; save(<span class="org-string">'./mat/stewart.mat'</span>, <span class="org-string">'stewart'</span>);
</pre> </pre>
</div> </div>
@ -464,27 +469,27 @@ save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string
</div> </div>
</div> </div>
<div id="outline-container-org608174e" class="outline-3"> <div id="outline-container-org77ecb36" class="outline-3">
<h3 id="org608174e"><span class="section-number-3">2.2</span> Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center</h3> <h3 id="org77ecb36"><span class="section-number-3">2.2</span> Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center</h3>
<div class="outline-text-3" id="text-2-2"> <div class="outline-text-3" id="text-2-2">
<p> <p>
We create a cubic Stewart platform with center of the cube located at the center of the Stewart platform (figure <a href="#org8e23773">1</a>). We create a cubic Stewart platform with center of the cube located at the center of the Stewart platform (figure <a href="#org1effc0f">1</a>).
The Jacobian matrix is not estimated at the location of the center of the cube. The Jacobian matrix is not estimated at the location of the center of the cube.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... <pre class="src src-matlab">opts = struct(...
<span class="org-string">'H_tot'</span>, <span class="org-highlight-numbers-number">100</span>, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span> <span class="org-string">'H_tot'</span>, 100, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span>
<span class="org-string">'L'</span>, <span class="org-highlight-numbers-number">200</span><span class="org-type">/</span>sqrt<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"> % Size of the Cube [mm]</span> <span class="org-string">'L'</span>, 200<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [mm]</span>
<span class="org-string">'H'</span>, <span class="org-highlight-numbers-number">60</span>, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span> <span class="org-string">'H'</span>, 60, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span>
<span class="org-string">'H0'</span>, <span class="org-highlight-numbers-number">200</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">-</span><span class="org-highlight-numbers-number">60</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span> ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span> <span class="org-string">'H0'</span>, 200<span class="org-type">/</span>2<span class="org-type">-</span>60<span class="org-type">/</span>2 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = initializeCubicConfiguration(opts);
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... opts = struct(...
<span class="org-string">'Jd_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-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span> <span class="org-string">'Jd_pos'</span>, [0, 0, 0], ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span>
<span class="org-string">'Jf_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-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span> <span class="org-string">'Jf_pos'</span>, [0, 0, 0] ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-1">(</span>stewart, opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = computeGeometricalProperties(stewart, opts);
</pre> </pre>
</div> </div>
@ -568,16 +573,16 @@ stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-
</div> </div>
</div> </div>
<div id="outline-container-orgbd736ef" class="outline-3"> <div id="outline-container-org42ea8ad" class="outline-3">
<h3 id="orgbd736ef"><span class="section-number-3">2.3</span> Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center</h3> <h3 id="org42ea8ad"><span class="section-number-3">2.3</span> Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center</h3>
<div class="outline-text-3" id="text-2-3"> <div class="outline-text-3" id="text-2-3">
<p> <p>
Here, the "center" of the Stewart platform is not at the cube center (figure <a href="#org3982eac">2</a>). Here, the &ldquo;center&rdquo; of the Stewart platform is not at the cube center (figure <a href="#org3f10bc2">2</a>).
The Jacobian is estimated at the cube center. The Jacobian is estimated at the cube center.
</p> </p>
<div id="org3982eac" class="figure"> <div id="org3f10bc2" class="figure">
<p><img src="./figs/3d-cubic-stewart-misaligned.png" alt="3d-cubic-stewart-misaligned.png" /> <p><img src="./figs/3d-cubic-stewart-misaligned.png" alt="3d-cubic-stewart-misaligned.png" />
</p> </p>
<p><span class="figure-number">Figure 2: </span>Not centered cubic configuration</p> <p><span class="figure-number">Figure 2: </span>Not centered cubic configuration</p>
@ -591,18 +596,18 @@ The center of the cube from the top platform is at \(z = 110 - 175 = -65\).
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... <pre class="src src-matlab">opts = struct(...
<span class="org-string">'H_tot'</span>, <span class="org-highlight-numbers-number">100</span>, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span> <span class="org-string">'H_tot'</span>, 100, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span>
<span class="org-string">'L'</span>, <span class="org-highlight-numbers-number">220</span><span class="org-type">/</span>sqrt<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"> % Size of the Cube [mm]</span> <span class="org-string">'L'</span>, 220<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [mm]</span>
<span class="org-string">'H'</span>, <span class="org-highlight-numbers-number">60</span>, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span> <span class="org-string">'H'</span>, 60, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span>
<span class="org-string">'H0'</span>, <span class="org-highlight-numbers-number">75</span> ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span> <span class="org-string">'H0'</span>, 75 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = initializeCubicConfiguration(opts);
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... opts = struct(...
<span class="org-string">'Jd_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-type">-</span><span class="org-highlight-numbers-number">65</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span> <span class="org-string">'Jd_pos'</span>, [0, 0, <span class="org-type">-</span>65], ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span>
<span class="org-string">'Jf_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-type">-</span><span class="org-highlight-numbers-number">65</span><span class="org-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span> <span class="org-string">'Jf_pos'</span>, [0, 0, <span class="org-type">-</span>65] ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-1">(</span>stewart, opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = computeGeometricalProperties(stewart, opts);
</pre> </pre>
</div> </div>
@ -690,11 +695,11 @@ We obtain \(k_x = k_y = k_z\) and \(k_{\theta_x} = k_{\theta_y}\), but the Stiff
</div> </div>
</div> </div>
<div id="outline-container-org6fbeda1" class="outline-3"> <div id="outline-container-org38870ce" class="outline-3">
<h3 id="org6fbeda1"><span class="section-number-3">2.4</span> Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center</h3> <h3 id="org38870ce"><span class="section-number-3">2.4</span> Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center</h3>
<div class="outline-text-3" id="text-2-4"> <div class="outline-text-3" id="text-2-4">
<p> <p>
Here, the "center" of the Stewart platform is not at the cube center. Here, the &ldquo;center&rdquo; of the Stewart platform is not at the cube center.
The Jacobian is estimated at the center of the Stewart platform. The Jacobian is estimated at the center of the Stewart platform.
</p> </p>
@ -706,18 +711,18 @@ The center of the cube from the top platform is at \(z = 110 - 175 = -65\).
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... <pre class="src src-matlab">opts = struct(...
<span class="org-string">'H_tot'</span>, <span class="org-highlight-numbers-number">100</span>, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span> <span class="org-string">'H_tot'</span>, 100, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span>
<span class="org-string">'L'</span>, <span class="org-highlight-numbers-number">220</span><span class="org-type">/</span>sqrt<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"> % Size of the Cube [mm]</span> <span class="org-string">'L'</span>, 220<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [mm]</span>
<span class="org-string">'H'</span>, <span class="org-highlight-numbers-number">60</span>, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span> <span class="org-string">'H'</span>, 60, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span>
<span class="org-string">'H0'</span>, <span class="org-highlight-numbers-number">75</span> ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span> <span class="org-string">'H0'</span>, 75 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = initializeCubicConfiguration(opts);
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... opts = struct(...
<span class="org-string">'Jd_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-type">-</span><span class="org-highlight-numbers-number">60</span><span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span> <span class="org-string">'Jd_pos'</span>, [0, 0, <span class="org-type">-</span>60], ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span>
<span class="org-string">'Jf_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-type">-</span><span class="org-highlight-numbers-number">60</span><span class="org-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span> <span class="org-string">'Jf_pos'</span>, [0, 0, <span class="org-type">-</span>60] ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-1">(</span>stewart, opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = computeGeometricalProperties(stewart, opts);
</pre> </pre>
</div> </div>
@ -805,8 +810,8 @@ We obtain \(k_x = k_y = k_z\) and \(k_{\theta_x} = k_{\theta_y}\), but the Stiff
</div> </div>
</div> </div>
<div id="outline-container-org18633d3" class="outline-3"> <div id="outline-container-org08c7461" class="outline-3">
<h3 id="org18633d3"><span class="section-number-3">2.5</span> Conclusion</h3> <h3 id="org08c7461"><span class="section-number-3">2.5</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-5"> <div class="outline-text-3" id="text-2-5">
<div class="important"> <div class="important">
<ul class="org-ul"> <ul class="org-ul">
@ -819,8 +824,8 @@ We obtain \(k_x = k_y = k_z\) and \(k_{\theta_x} = k_{\theta_y}\), but the Stiff
</div> </div>
</div> </div>
<div id="outline-container-orgf0ba2d0" class="outline-2"> <div id="outline-container-orgc4c2abd" class="outline-2">
<h2 id="orgf0ba2d0"><span class="section-number-2">3</span> Cubic size analysis</h2> <h2 id="orgc4c2abd"><span class="section-number-2">3</span> <span class="todo TODO">TODO</span> Cubic size analysis</h2>
<div class="outline-text-2" id="text-3"> <div class="outline-text-2" id="text-3">
<p> <p>
We here study the effect of the size of the cube used for the Stewart configuration. We here study the effect of the size of the cube used for the Stewart configuration.
@ -835,31 +840,31 @@ We only vary the size of the cube.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">H_cubes = <span class="org-highlight-numbers-number">250</span><span class="org-type">:</span><span class="org-highlight-numbers-number">20</span><span class="org-type">:</span><span class="org-highlight-numbers-number">350</span>; <pre class="src src-matlab">H_cubes = 250<span class="org-type">:</span>20<span class="org-type">:</span>350;
stewarts = <span class="org-rainbow-delimiters-depth-1">{</span>zeros<span class="org-rainbow-delimiters-depth-2">(</span>length<span class="org-rainbow-delimiters-depth-3">(</span>H_cubes<span class="org-rainbow-delimiters-depth-3">)</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">}</span>; stewarts = {zeros(length(H_cubes), 1)};
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><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">H_cubes</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span> <pre class="src src-matlab"><span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(H_cubes)</span>
H_cube = H_cubes<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span>; H_cube = H_cubes(<span class="org-constant">i</span>);
H_tot = <span class="org-highlight-numbers-number">100</span>; H_tot = 100;
H = <span class="org-highlight-numbers-number">80</span>; H = 80;
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... opts = struct(...
<span class="org-string">'H_tot'</span>, H_tot, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span> <span class="org-string">'H_tot'</span>, H_tot, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span>
<span class="org-string">'L'</span>, H_cube<span class="org-type">/</span>sqrt<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"> % Size of the Cube [mm]</span> <span class="org-string">'L'</span>, H_cube<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [mm]</span>
<span class="org-string">'H'</span>, H, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span> <span class="org-string">'H'</span>, H, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span>
<span class="org-string">'H0'</span>, H_cube<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">-</span>H<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span> ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span> <span class="org-string">'H0'</span>, H_cube<span class="org-type">/</span>2<span class="org-type">-</span>H<span class="org-type">/</span>2 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = initializeCubicConfiguration(opts);
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... opts = struct(...
<span class="org-string">'Jd_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>, H_cube<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">-</span>opts.H0<span class="org-type">-</span>opts.H_tot<span class="org-rainbow-delimiters-depth-2">]</span>, ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span> <span class="org-string">'Jd_pos'</span>, [0, 0, H_cube<span class="org-type">/</span>2<span class="org-type">-</span>opts.H0<span class="org-type">-</span>opts.H_tot], ...<span class="org-comment"> % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]</span>
<span class="org-string">'Jf_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>, H_cube<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-type">-</span>opts.H0<span class="org-type">-</span>opts.H_tot<span class="org-rainbow-delimiters-depth-2">]</span> ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span> <span class="org-string">'Jf_pos'</span>, [0, 0, H_cube<span class="org-type">/</span>2<span class="org-type">-</span>opts.H0<span class="org-type">-</span>opts.H_tot] ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-1">(</span>stewart, opts<span class="org-rainbow-delimiters-depth-1">)</span>; stewart = computeGeometricalProperties(stewart, opts);
stewarts<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = <span class="org-rainbow-delimiters-depth-1">{</span>stewart<span class="org-rainbow-delimiters-depth-1">}</span>; stewarts(<span class="org-constant">i</span>) = {stewart};
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -869,9 +874,9 @@ stewarts = <span class="org-rainbow-delimiters-depth-1">{</span>zeros<span class
The Stiffness matrix is computed for all generated Stewart platforms. The Stiffness matrix is computed for all generated Stewart platforms.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Ks = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">6</span>, length<span class="org-rainbow-delimiters-depth-2">(</span>H_cube<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Ks = zeros(6, 6, length(H_cube));
<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">H_cubes</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(H_cubes)</span>
Ks<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">)</span> = stewarts<span class="org-rainbow-delimiters-depth-1">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">}</span>.Jd<span class="org-type">'*</span>stewarts<span class="org-rainbow-delimiters-depth-1">{</span><span class="org-constant">i</span><span class="org-rainbow-delimiters-depth-1">}</span>.Jd; Ks(<span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span>) = stewarts{<span class="org-constant">i</span>}.Jd<span class="org-type">'*</span>stewarts{<span class="org-constant">i</span>}.Jd;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -886,16 +891,16 @@ Finally, we plot \(k_{\theta_x} = k_{\theta_y}\) and \(k_{\theta_z}\)
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>; <pre class="src src-matlab"><span class="org-type">figure</span>;
hold on; hold on;
plot<span class="org-rainbow-delimiters-depth-1">(</span>H_cubes, squeeze<span class="org-rainbow-delimiters-depth-2">(</span>Ks<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">4</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-string">'DisplayName'</span>, <span class="org-string">'$k_</span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">{</span></span><span class="org-string">\theta_x</span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">}</span></span><span class="org-string">$'</span><span class="org-rainbow-delimiters-depth-1">)</span>; plot(H_cubes, squeeze(Ks(4, 4, <span class="org-type">:</span>)), <span class="org-string">'DisplayName'</span>, <span class="org-string">'$k_{\theta_x}$'</span>);
plot<span class="org-rainbow-delimiters-depth-1">(</span>H_cubes, squeeze<span class="org-rainbow-delimiters-depth-2">(</span>Ks<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">6</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-string">'DisplayName'</span>, <span class="org-string">'$k_</span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">{</span></span><span class="org-string">\theta_z</span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">}</span></span><span class="org-string">$'</span><span class="org-rainbow-delimiters-depth-1">)</span>; plot(H_cubes, squeeze(Ks(6, 6, <span class="org-type">:</span>)), <span class="org-string">'DisplayName'</span>, <span class="org-string">'$k_{\theta_z}$'</span>);
hold off; hold off;
legend<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'location'</span>, <span class="org-string">'northwest'</span><span class="org-rainbow-delimiters-depth-1">)</span>; legend(<span class="org-string">'location'</span>, <span class="org-string">'northwest'</span>);
xlabel<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Cube Size </span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">[</span></span><span class="org-string">mm</span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">]</span></span><span class="org-string">'</span><span class="org-rainbow-delimiters-depth-1">)</span>; ylabel<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string">'Rotational stiffnes </span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">[</span></span><span class="org-string">normalized</span><span class="org-string"><span class="org-rainbow-delimiters-depth-2">]</span></span><span class="org-string">'</span><span class="org-rainbow-delimiters-depth-1">)</span>; xlabel(<span class="org-string">'Cube Size [mm]'</span>); ylabel(<span class="org-string">'Rotational stiffnes [normalized]'</span>);
</pre> </pre>
</div> </div>
<div id="org7d4f005" class="figure"> <div id="org659a01f" class="figure">
<p><img src="figs/stiffness_cube_size.png" alt="stiffness_cube_size.png" /> <p><img src="figs/stiffness_cube_size.png" alt="stiffness_cube_size.png" />
</p> </p>
<p><span class="figure-number">Figure 3: </span>\(k_{\theta_x} = k_{\theta_y}\) and \(k_{\theta_z}\) function of the size of the cube</p> <p><span class="figure-number">Figure 3: </span>\(k_{\theta_x} = k_{\theta_y}\) and \(k_{\theta_z}\) function of the size of the cube</p>
@ -916,37 +921,37 @@ In that case, the legs will the further separated. Size of the cube is then limi
</div> </div>
</div> </div>
<div id="outline-container-org97dffbc" class="outline-2"> <div id="outline-container-org36a27e6" class="outline-2">
<h2 id="org97dffbc"><span class="section-number-2">4</span> initializeCubicConfiguration</h2> <h2 id="org36a27e6"><span class="section-number-2">4</span> <span class="todo TODO">TODO</span> initializeCubicConfiguration</h2>
<div class="outline-text-2" id="text-4"> <div class="outline-text-2" id="text-4">
<p> <p>
<a id="org38614bc"></a> <a id="org8b1f609"></a>
</p> </p>
</div> </div>
<div id="outline-container-org4eb8b23" class="outline-3"> <div id="outline-container-orgf299c5c" class="outline-3">
<h3 id="org4eb8b23"><span class="section-number-3">4.1</span> Function description</h3> <h3 id="orgf299c5c"><span class="section-number-3">4.1</span> Function description</h3>
<div class="outline-text-3" id="text-4-1"> <div class="outline-text-3" id="text-4-1">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">[</span></span><span class="org-variable-name">stewart</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">initializeCubicConfiguration</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">opts_param</span><span class="org-rainbow-delimiters-depth-1">)</span> <pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">initializeCubicConfiguration</span>(<span class="org-variable-name">opts_param</span>)
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orga42cb17" class="outline-3"> <div id="outline-container-org46c8589" class="outline-3">
<h3 id="orga42cb17"><span class="section-number-3">4.2</span> Optional Parameters</h3> <h3 id="org46c8589"><span class="section-number-3">4.2</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-4-2"> <div class="outline-text-3" id="text-4-2">
<p> <p>
Default values for opts. Default values for opts.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>... <pre class="src src-matlab">opts = struct(...
<span class="org-string">'H_tot'</span>, <span class="org-highlight-numbers-number">90</span>, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span> <span class="org-string">'H_tot'</span>, 90, ...<span class="org-comment"> % Total height of the Hexapod [mm]</span>
<span class="org-string">'L'</span>, <span class="org-highlight-numbers-number">110</span>, ...<span class="org-comment"> % Size of the Cube [mm]</span> <span class="org-string">'L'</span>, 110, ...<span class="org-comment"> % Size of the Cube [mm]</span>
<span class="org-string">'H'</span>, <span class="org-highlight-numbers-number">40</span>, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span> <span class="org-string">'H'</span>, 40, ...<span class="org-comment"> % Height between base joints and platform joints [mm]</span>
<span class="org-string">'H0'</span>, <span class="org-highlight-numbers-number">75</span> ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span> <span class="org-string">'H0'</span>, 75 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
<span class="org-rainbow-delimiters-depth-1">)</span>; );
</pre> </pre>
</div> </div>
@ -954,9 +959,9 @@ Default values for opts.
Populate opts with input parameters Populate opts with input parameters
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><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> <pre class="src src-matlab"><span class="org-keyword">if</span> exist(<span class="org-string">'opts_param'</span>,<span class="org-string">'var'</span>)
<span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">opts_param</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span><span class="org-constant">'</span> <span class="org-keyword">for</span> <span class="org-variable-name">opt</span> = <span class="org-constant">fieldnames(opts_param)'</span>
opts.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span> = opts_param.<span class="org-rainbow-delimiters-depth-1">(</span>opt<span class="org-rainbow-delimiters-depth-2">{</span><span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">}</span><span class="org-rainbow-delimiters-depth-1">)</span>; opts.(opt{1}) = opts_param.(opt{1});
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
@ -964,18 +969,18 @@ Populate opts with input parameters
</div> </div>
</div> </div>
<div id="outline-container-orgc281f60" class="outline-3"> <div id="outline-container-orgd8d9b14" class="outline-3">
<h3 id="orgc281f60"><span class="section-number-3">4.3</span> Cube Creation</h3> <h3 id="orgd8d9b14"><span class="section-number-3">4.3</span> Cube Creation</h3>
<div class="outline-text-3" id="text-4-3"> <div class="outline-text-3" id="text-4-3">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">points = <span class="org-rainbow-delimiters-depth-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>; ... <pre class="src src-matlab">points = [0, 0, 0; ...
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>; ... 0, 0, 1; ...
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>; ... 0, 1, 0; ...
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>; ... 0, 1, 1; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">0</span>; ... 1, 0, 0; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>; ... 1, 0, 1; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>; ... 1, 1, 0; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; 1, 1, 1];
points = opts.L<span class="org-type">*</span>points; points = opts.L<span class="org-type">*</span>points;
</pre> </pre>
</div> </div>
@ -984,16 +989,16 @@ points = opts.L<span class="org-type">*</span>points;
We create the rotation matrix to rotate the cube We create the rotation matrix to rotate the cube
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">sx = cross<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-rainbow-delimiters-depth-2">[</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-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">sx = cross([1, 1, 1], [1 0 0]);
sx = sx<span class="org-type">/</span>norm<span class="org-rainbow-delimiters-depth-1">(</span>sx<span class="org-rainbow-delimiters-depth-1">)</span>; sx = sx<span class="org-type">/</span>norm(sx);
sy = <span class="org-type">-</span>cross<span class="org-rainbow-delimiters-depth-1">(</span>sx, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; sy = <span class="org-type">-</span>cross(sx, [1, 1, 1]);
sy = sy<span class="org-type">/</span>norm<span class="org-rainbow-delimiters-depth-1">(</span>sy<span class="org-rainbow-delimiters-depth-1">)</span>; sy = sy<span class="org-type">/</span>norm(sy);
sz = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; sz = [1, 1, 1];
sz = sz<span class="org-type">/</span>norm<span class="org-rainbow-delimiters-depth-1">(</span>sz<span class="org-rainbow-delimiters-depth-1">)</span>; sz = sz<span class="org-type">/</span>norm(sz);
R = <span class="org-rainbow-delimiters-depth-1">[</span>sx<span class="org-type">'</span>, sy<span class="org-type">'</span>, sz<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span><span class="org-type">'</span>; R = [sx<span class="org-type">'</span>, sy<span class="org-type">'</span>, sz<span class="org-type">'</span>]<span class="org-type">'</span>;
</pre> </pre>
</div> </div>
@ -1001,25 +1006,25 @@ R = <span class="org-rainbow-delimiters-depth-1">[</span>sx<span class="org-type
We use to rotation matrix to rotate the cube We use to rotation matrix to rotate the cube
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">cube = zeros<span class="org-rainbow-delimiters-depth-1">(</span>size<span class="org-rainbow-delimiters-depth-2">(</span>points<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">cube = zeros(size(points));
<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">:size</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">points, </span><span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:size(points, 1)</span>
cube<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> = R <span class="org-type">*</span> points<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>; cube(<span class="org-constant">i</span>, <span class="org-type">:</span>) = R <span class="org-type">*</span> points(<span class="org-constant">i</span>, <span class="org-type">:</span>)<span class="org-type">'</span>;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orgfed01f0" class="outline-3"> <div id="outline-container-org181d1d8" class="outline-3">
<h3 id="orgfed01f0"><span class="section-number-3">4.4</span> Vectors of each leg</h3> <h3 id="org181d1d8"><span class="section-number-3">4.4</span> Vectors of each leg</h3>
<div class="outline-text-3" id="text-4-4"> <div class="outline-text-3" id="text-4-4">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">leg_indices = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">4</span>; ... <pre class="src src-matlab">leg_indices = [3, 4; ...
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">4</span>; ... 2, 4; ...
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">6</span>; ... 2, 6; ...
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">6</span>; ... 5, 6; ...
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">7</span>; ... 5, 7; ...
<span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">7</span><span class="org-rainbow-delimiters-depth-1">]</span>; 3, 7];
</pre> </pre>
</div> </div>
@ -1027,50 +1032,50 @@ We use to rotation matrix to rotate the cube
Vectors are: Vectors are:
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">legs = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">legs = zeros(6, 3);
legs_start = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; legs_start = zeros(6, 3);
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
legs<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> = cube<span class="org-rainbow-delimiters-depth-1">(</span>leg_indices<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> cube<span class="org-rainbow-delimiters-depth-1">(</span>leg_indices<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span>; legs(<span class="org-constant">i</span>, <span class="org-type">:</span>) = cube(leg_indices(<span class="org-constant">i</span>, 2), <span class="org-type">:</span>) <span class="org-type">-</span> cube(leg_indices(<span class="org-constant">i</span>, 1), <span class="org-type">:</span>);
legs_start<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> = cube<span class="org-rainbow-delimiters-depth-1">(</span>leg_indices<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span>; legs_start(<span class="org-constant">i</span>, <span class="org-type">:</span>) = cube(leg_indices(<span class="org-constant">i</span>, 1), <span class="org-type">:</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org21db1ef" class="outline-3"> <div id="outline-container-orgb396e98" class="outline-3">
<h3 id="org21db1ef"><span class="section-number-3">4.5</span> Verification of Height of the Stewart Platform</h3> <h3 id="orgb396e98"><span class="section-number-3">4.5</span> Verification of Height of the Stewart Platform</h3>
<div class="outline-text-3" id="text-4-5"> <div class="outline-text-3" id="text-4-5">
<p> <p>
If the Stewart platform is not contained in the cube, throw an error. If the Stewart platform is not contained in the cube, throw an error.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Hmax = cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Hmax = cube(4, 3) <span class="org-type">-</span> cube(2, 3);
<span class="org-keyword">if</span> opts.H0 <span class="org-type">&lt;</span> cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-keyword">if</span> opts.H0 <span class="org-type">&lt;</span> cube(2, 3)
error<span class="org-rainbow-delimiters-depth-1">(</span>sprintf<span class="org-rainbow-delimiters-depth-2">(</span>'H0 is not high enought. Minimum H0 = %.<span class="org-highlight-numbers-number">1f</span><span class="org-type">'</span>, cube(<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">3</span>))); error(sprintf(<span class="org-string">'H0 is not high enought. Minimum H0 = %.1f'</span>, cube(2, 3)));
<span class="org-keyword">else</span> <span class="org-keyword">if</span> opts.H0 <span class="org-type">+</span> opts.H <span class="org-type">&gt;</span> cube<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-3">)</span> <span class="org-keyword">else</span> <span class="org-keyword">if</span> opts.H0 <span class="org-type">+</span> opts.H <span class="org-type">&gt;</span> cube(4, 3)
error<span class="org-rainbow-delimiters-depth-3">(</span>sprintf<span class="org-rainbow-delimiters-depth-4">(</span>'H0<span class="org-type">+</span>H is too high. Maximum H0<span class="org-type">+</span>H = %.<span class="org-highlight-numbers-number">1f</span><span class="org-type">'</span>, cube(<span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">3</span>))); error(sprintf(<span class="org-string">'H0+H is too high. Maximum H0+H = %.1f'</span>, cube(4, 3)));
error<span class="org-rainbow-delimiters-depth-5">(</span><span class="org-string">'H0+H is too high'</span><span class="org-rainbow-delimiters-depth-5">)</span>; error(<span class="org-string">'H0+H is too high'</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org9578c3c" class="outline-3"> <div id="outline-container-orgf38af83" class="outline-3">
<h3 id="org9578c3c"><span class="section-number-3">4.6</span> Determinate the location of the joints</h3> <h3 id="orgf38af83"><span class="section-number-3">4.6</span> Determinate the location of the joints</h3>
<div class="outline-text-3" id="text-4-6"> <div class="outline-text-3" id="text-4-6">
<p> <p>
We now determine the location of the joints on the fixed platform w.r.t the fixed frame \(\{A\}\). We now determine the location of the joints on the fixed platform w.r.t the fixed frame \(\{A\}\).
\(\{A\}\) is fixed to the bottom of the base. \(\{A\}\) is fixed to the bottom of the base.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Aa = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Aa = zeros(6, 3);
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
t = <span class="org-rainbow-delimiters-depth-1">(</span>opts.H0<span class="org-type">-</span>legs_start<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>legs<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; t = (opts.H0<span class="org-type">-</span>legs_start(<span class="org-constant">i</span>, 3))<span class="org-type">/</span>(legs(<span class="org-constant">i</span>, 3));
Aa<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> = legs_start<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> t<span class="org-type">*</span>legs<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>; Aa(<span class="org-constant">i</span>, <span class="org-type">:</span>) = legs_start(<span class="org-constant">i</span>, <span class="org-type">:</span>) <span class="org-type">+</span> t<span class="org-type">*</span>legs(<span class="org-constant">i</span>, <span class="org-type">:</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -1079,10 +1084,10 @@ We now determine the location of the joints on the fixed platform w.r.t the fixe
And the location of the joints on the mobile platform with respect to \(\{A\}\). And the location of the joints on the mobile platform with respect to \(\{A\}\).
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Ab = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Ab = zeros(6, 3);
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
t = <span class="org-rainbow-delimiters-depth-1">(</span>opts.H0<span class="org-type">+</span>opts.H<span class="org-type">-</span>legs_start<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>legs<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; t = (opts.H0<span class="org-type">+</span>opts.H<span class="org-type">-</span>legs_start(<span class="org-constant">i</span>, 3))<span class="org-type">/</span>(legs(<span class="org-constant">i</span>, 3));
Ab<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> = legs_start<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> t<span class="org-type">*</span>legs<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>; Ab(<span class="org-constant">i</span>, <span class="org-type">:</span>) = legs_start(<span class="org-constant">i</span>, <span class="org-type">:</span>) <span class="org-type">+</span> t<span class="org-type">*</span>legs(<span class="org-constant">i</span>, <span class="org-type">:</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -1091,25 +1096,25 @@ And the location of the joints on the mobile platform with respect to \(\{A\}\).
And the location of the joints on the mobile platform with respect to \(\{B\}\). And the location of the joints on the mobile platform with respect to \(\{B\}\).
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Bb = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Bb = zeros(6, 3);
Bb = Ab <span class="org-type">-</span> <span class="org-rainbow-delimiters-depth-1">(</span>opts.H0 <span class="org-type">+</span> opts.H_tot<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span> <span class="org-type">+</span> opts.H<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-1">)</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>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; Bb = Ab <span class="org-type">-</span> (opts.H0 <span class="org-type">+</span> opts.H_tot<span class="org-type">/</span>2 <span class="org-type">+</span> opts.H<span class="org-type">/</span>2)<span class="org-type">*</span>[0, 0, 1];
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">h = opts.H0 <span class="org-type">+</span> opts.H<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span> <span class="org-type">-</span> opts.H_tot<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>; <pre class="src src-matlab">h = opts.H0 <span class="org-type">+</span> opts.H<span class="org-type">/</span>2 <span class="org-type">-</span> opts.H_tot<span class="org-type">/</span>2;
Aa = Aa <span class="org-type">-</span> h<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>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; Aa = Aa <span class="org-type">-</span> h<span class="org-type">*</span>[0, 0, 1];
Ab = Ab <span class="org-type">-</span> h<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>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; Ab = Ab <span class="org-type">-</span> h<span class="org-type">*</span>[0, 0, 1];
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org71c9d4e" class="outline-3"> <div id="outline-container-orgdf9e3cf" class="outline-3">
<h3 id="org71c9d4e"><span class="section-number-3">4.7</span> Returns Stewart Structure</h3> <h3 id="orgdf9e3cf"><span class="section-number-3">4.7</span> Returns Stewart Structure</h3>
<div class="outline-text-3" id="text-4-7"> <div class="outline-text-3" id="text-4-7">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"> stewart = struct<span class="org-rainbow-delimiters-depth-1">()</span>; <pre class="src src-matlab"> stewart = struct();
stewart.Aa = Aa; stewart.Aa = Aa;
stewart.Ab = Ab; stewart.Ab = Ab;
stewart.Bb = Bb; stewart.Bb = Bb;
@ -1121,15 +1126,15 @@ Ab = Ab <span class="org-type">-</span> h<span class="org-type">*</span><span cl
</div> </div>
</div> </div>
<div id="outline-container-orgb2d1742" class="outline-2"> <div id="outline-container-orgf8fb731" class="outline-2">
<h2 id="orgb2d1742"><span class="section-number-2">5</span> Tests</h2> <h2 id="orgf8fb731"><span class="section-number-2">5</span> <span class="todo TODO">TODO</span> Tests</h2>
<div class="outline-text-2" id="text-5"> <div class="outline-text-2" id="text-5">
</div> </div>
<div id="outline-container-org6e933c9" class="outline-3"> <div id="outline-container-org4434fe5" class="outline-3">
<h3 id="org6e933c9"><span class="section-number-3">5.1</span> First attempt to parametrisation</h3> <h3 id="org4434fe5"><span class="section-number-3">5.1</span> First attempt to parametrisation</h3>
<div class="outline-text-3" id="text-5-1"> <div class="outline-text-3" id="text-5-1">
<div id="org94bcd9c" class="figure"> <div id="org8dfcb96" class="figure">
<p><img src="./figs/stewart_bottom_plate.png" alt="stewart_bottom_plate.png" /> <p><img src="./figs/stewart_bottom_plate.png" alt="stewart_bottom_plate.png" />
</p> </p>
<p><span class="figure-number">Figure 4: </span>Schematic of the bottom plates with all the parameters</p> <p><span class="figure-number">Figure 4: </span>Schematic of the bottom plates with all the parameters</p>
@ -1164,8 +1169,8 @@ Lets express \(a_i\), \(b_i\) and \(a_j\):
</div> </div>
</div> </div>
<div id="outline-container-org60486ce" class="outline-3"> <div id="outline-container-org723e6eb" class="outline-3">
<h3 id="org60486ce"><span class="section-number-3">5.2</span> Second attempt</h3> <h3 id="org723e6eb"><span class="section-number-3">5.2</span> Second attempt</h3>
<div class="outline-text-3" id="text-5-2"> <div class="outline-text-3" id="text-5-2">
<p> <p>
We start with the point of a cube in space: We start with the point of a cube in space:
@ -1184,41 +1189,41 @@ Then we have the direction of all the vectors expressed in the frame of the hexa
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">points = <span class="org-rainbow-delimiters-depth-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>; ... <pre class="src src-matlab">points = [0, 0, 0; ...
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>; ... 0, 0, 1; ...
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>; ... 0, 1, 0; ...
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>; ... 0, 1, 1; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">0</span>; ... 1, 0, 0; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>; ... 1, 0, 1; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>; ... 1, 1, 0; ...
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; 1, 1, 1];
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>; <pre class="src src-matlab"><span class="org-type">figure</span>;
plot3<span class="org-rainbow-delimiters-depth-1">(</span>points<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, points<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, points<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-string">'ko'</span><span class="org-rainbow-delimiters-depth-1">)</span> plot3(points(<span class="org-type">:</span>,1), points(<span class="org-type">:</span>,2), points(<span class="org-type">:</span>,3), <span class="org-string">'ko'</span>)
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">sx = cross<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-rainbow-delimiters-depth-2">[</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-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">sx = cross([1, 1, 1], [1 0 0]);
sx = sx<span class="org-type">/</span>norm<span class="org-rainbow-delimiters-depth-1">(</span>sx<span class="org-rainbow-delimiters-depth-1">)</span>; sx = sx<span class="org-type">/</span>norm(sx);
sy = <span class="org-type">-</span>cross<span class="org-rainbow-delimiters-depth-1">(</span>sx, <span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; sy = <span class="org-type">-</span>cross(sx, [1, 1, 1]);
sy = sy<span class="org-type">/</span>norm<span class="org-rainbow-delimiters-depth-1">(</span>sy<span class="org-rainbow-delimiters-depth-1">)</span>; sy = sy<span class="org-type">/</span>norm(sy);
sz = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-1">]</span>; sz = [1, 1, 1];
sz = sz<span class="org-type">/</span>norm<span class="org-rainbow-delimiters-depth-1">(</span>sz<span class="org-rainbow-delimiters-depth-1">)</span>; sz = sz<span class="org-type">/</span>norm(sz);
R = <span class="org-rainbow-delimiters-depth-1">[</span>sx<span class="org-type">'</span>, sy<span class="org-type">'</span>, sz<span class="org-type">'</span><span class="org-rainbow-delimiters-depth-1">]</span><span class="org-type">'</span>; R = [sx<span class="org-type">'</span>, sy<span class="org-type">'</span>, sz<span class="org-type">'</span>]<span class="org-type">'</span>;
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">cube = zeros<span class="org-rainbow-delimiters-depth-1">(</span>size<span class="org-rainbow-delimiters-depth-2">(</span>points<span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">cube = zeros(size(points));
<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">:size</span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">(</span></span><span class="org-constant">points, </span><span class="org-constant"><span class="org-highlight-numbers-number">1</span></span><span class="org-constant"><span class="org-rainbow-delimiters-depth-1">)</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:size(points, 1)</span>
cube<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> = R <span class="org-type">*</span> points<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>; cube(<span class="org-constant">i</span>, <span class="org-type">:</span>) = R <span class="org-type">*</span> points(<span class="org-constant">i</span>, <span class="org-type">:</span>)<span class="org-type">'</span>;
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -1226,8 +1231,8 @@ R = <span class="org-rainbow-delimiters-depth-1">[</span>sx<span class="org-type
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>; <pre class="src src-matlab"><span class="org-type">figure</span>;
hold on; hold on;
plot3<span class="org-rainbow-delimiters-depth-1">(</span>points<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, points<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, points<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-string">'ko'</span><span class="org-rainbow-delimiters-depth-1">)</span>; plot3(points(<span class="org-type">:</span>,1), points(<span class="org-type">:</span>,2), points(<span class="org-type">:</span>,3), <span class="org-string">'ko'</span>);
plot3<span class="org-rainbow-delimiters-depth-1">(</span>cube<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, cube<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, cube<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-type">:</span>,<span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-string">'ro'</span><span class="org-rainbow-delimiters-depth-1">)</span>; plot3(cube(<span class="org-type">:</span>,1), cube(<span class="org-type">:</span>,2), cube(<span class="org-type">:</span>,3), <span class="org-string">'ro'</span>);
hold off; hold off;
</pre> </pre>
</div> </div>
@ -1236,17 +1241,17 @@ hold off;
Now we plot the legs of the hexapod. Now we plot the legs of the hexapod.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">leg_indices = <span class="org-rainbow-delimiters-depth-1">[</span><span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">4</span>; ... <pre class="src src-matlab">leg_indices = [3, 4; ...
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">4</span>; ... 2, 4; ...
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">6</span>; ... 2, 6; ...
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">6</span>; ... 5, 6; ...
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">7</span>; ... 5, 7; ...
<span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">7</span><span class="org-rainbow-delimiters-depth-1">]</span> 3, 7]
<span class="org-type">figure</span>; <span class="org-type">figure</span>;
hold on; hold on;
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
plot3<span class="org-rainbow-delimiters-depth-1">(</span>cube<span class="org-rainbow-delimiters-depth-2">(</span>leg_indices<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-3">)</span>,<span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, cube<span class="org-rainbow-delimiters-depth-2">(</span>leg_indices<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-3">)</span>,<span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, cube<span class="org-rainbow-delimiters-depth-2">(</span>leg_indices<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-3">)</span>,<span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-string">'-'</span><span class="org-rainbow-delimiters-depth-1">)</span>; plot3(cube(leg_indices(<span class="org-constant">i</span>, <span class="org-type">:</span>),1), cube(leg_indices(<span class="org-constant">i</span>, <span class="org-type">:</span>),2), cube(leg_indices(<span class="org-constant">i</span>, <span class="org-type">:</span>),3), <span class="org-string">'-'</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
hold off; hold off;
</pre> </pre>
@ -1256,12 +1261,12 @@ hold off;
Vectors are: Vectors are:
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">legs = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">legs = zeros(6, 3);
legs_start = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; legs_start = zeros(6, 3);
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
legs<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> = cube<span class="org-rainbow-delimiters-depth-1">(</span>leg_indices<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> cube<span class="org-rainbow-delimiters-depth-1">(</span>leg_indices<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span>; legs(<span class="org-constant">i</span>, <span class="org-type">:</span>) = cube(leg_indices(<span class="org-constant">i</span>, 2), <span class="org-type">:</span>) <span class="org-type">-</span> cube(leg_indices(<span class="org-constant">i</span>, 1), <span class="org-type">:</span>);
legs_start<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> = cube<span class="org-rainbow-delimiters-depth-1">(</span>leg_indices<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">)</span>, <span class="org-type">:</span><span class="org-rainbow-delimiters-depth-1">)</span> legs_start(<span class="org-constant">i</span>, <span class="org-type">:</span>) = cube(leg_indices(<span class="org-constant">i</span>, 1), <span class="org-type">:</span>)
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -1271,40 +1276,40 @@ We now have the orientation of each leg.
</p> </p>
<p> <p>
We here want to see if the position of the "slice" changes something. We here want to see if the position of the &ldquo;slice&rdquo; changes something.
</p> </p>
<p> <p>
Let's first estimate the maximum height of the Stewart platform. Let&rsquo;s first estimate the maximum height of the Stewart platform.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Hmax = cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">4</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> <span class="org-type">-</span> cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Hmax = cube(4, 3) <span class="org-type">-</span> cube(2, 3);
</pre> </pre>
</div> </div>
<p> <p>
Let's then estimate the middle position of the platform Let&rsquo;s then estimate the middle position of the platform
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Hmid = cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">8</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>; <pre class="src src-matlab">Hmid = cube(8, 3)<span class="org-type">/</span>2;
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orge571873" class="outline-3"> <div id="outline-container-orgcc173ac" class="outline-3">
<h3 id="orge571873"><span class="section-number-3">5.3</span> Generate the Stewart platform for a Cubic configuration</h3> <h3 id="orgcc173ac"><span class="section-number-3">5.3</span> Generate the Stewart platform for a Cubic configuration</h3>
<div class="outline-text-3" id="text-5-3"> <div class="outline-text-3" id="text-5-3">
<p> <p>
First we defined the height of the Hexapod. First we defined the height of the Hexapod.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">H = Hmax<span class="org-type">/</span><span class="org-highlight-numbers-number">2</span>; <pre class="src src-matlab">H = Hmax<span class="org-type">/</span>2;
</pre> </pre>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Zs = <span class="org-highlight-numbers-number">1</span>.<span class="org-highlight-numbers-number">2</span><span class="org-type">*</span>cube<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <span class="org-comment">% Height of the fixed platform</span> <pre class="src src-matlab">Zs = 1.2<span class="org-type">*</span>cube(2, 3); <span class="org-comment">% Height of the fixed platform</span>
Ze = Zs <span class="org-type">+</span> H; <span class="org-comment">% Height of the mobile platform</span> Ze = Zs <span class="org-type">+</span> H; <span class="org-comment">% Height of the mobile platform</span>
</pre> </pre>
</div> </div>
@ -1313,10 +1318,10 @@ Ze = Zs <span class="org-type">+</span> H; <span class="org-comment">% Height of
We now determine the location of the joints on the fixed platform. We now determine the location of the joints on the fixed platform.
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Aa = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Aa = zeros(6, 3);
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
t = <span class="org-rainbow-delimiters-depth-1">(</span>Zs<span class="org-type">-</span>legs_start<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>legs<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; t = (Zs<span class="org-type">-</span>legs_start(<span class="org-constant">i</span>, 3))<span class="org-type">/</span>(legs(<span class="org-constant">i</span>, 3));
Aa<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> = legs_start<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> t<span class="org-type">*</span>legs<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>; Aa(<span class="org-constant">i</span>, <span class="org-type">:</span>) = legs_start(<span class="org-constant">i</span>, <span class="org-type">:</span>) <span class="org-type">+</span> t<span class="org-type">*</span>legs(<span class="org-constant">i</span>, <span class="org-type">:</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -1325,10 +1330,10 @@ We now determine the location of the joints on the fixed platform.
And the location of the joints on the mobile platform And the location of the joints on the mobile platform
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Ab = zeros<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-highlight-numbers-number">6</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span>; <pre class="src src-matlab">Ab = zeros(6, 3);
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
t = <span class="org-rainbow-delimiters-depth-1">(</span>Ze<span class="org-type">-</span>legs_start<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span><span class="org-type">/</span><span class="org-rainbow-delimiters-depth-1">(</span>legs<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">)</span>; t = (Ze<span class="org-type">-</span>legs_start(<span class="org-constant">i</span>, 3))<span class="org-type">/</span>(legs(<span class="org-constant">i</span>, 3));
Ab<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> = legs_start<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> t<span class="org-type">*</span>legs<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>; Ab(<span class="org-constant">i</span>, <span class="org-type">:</span>) = legs_start(<span class="org-constant">i</span>, <span class="org-type">:</span>) <span class="org-type">+</span> t<span class="org-type">*</span>legs(<span class="org-constant">i</span>, <span class="org-type">:</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
</pre> </pre>
</div> </div>
@ -1339,13 +1344,13 @@ And we plot the legs.
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>; <pre class="src src-matlab"><span class="org-type">figure</span>;
hold on; hold on;
<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">:</span><span class="org-constant"><span class="org-highlight-numbers-number">6</span></span> <span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
plot3<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span>Ab<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span>,Aa<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-rainbow-delimiters-depth-2">[</span>Ab<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-3">)</span>,Aa<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-rainbow-delimiters-depth-2">[</span>Ab<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-3">)</span>,Aa<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-constant">i</span>, <span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">]</span>, <span class="org-string">'k-'</span><span class="org-rainbow-delimiters-depth-1">)</span>; plot3([Ab(<span class="org-constant">i</span>, 1),Aa(<span class="org-constant">i</span>, 1)], [Ab(<span class="org-constant">i</span>, 2),Aa(<span class="org-constant">i</span>, 2)], [Ab(<span class="org-constant">i</span>, 3),Aa(<span class="org-constant">i</span>, 3)], <span class="org-string">'k-'</span>);
<span class="org-keyword">end</span> <span class="org-keyword">end</span>
hold off; hold off;
xlim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; xlim([<span class="org-type">-</span>1, 1]);
ylim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-type">-</span><span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; ylim([<span class="org-type">-</span>1, 1]);
zlim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbow-delimiters-depth-2">[</span><span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">2</span><span class="org-rainbow-delimiters-depth-2">]</span><span class="org-rainbow-delimiters-depth-1">)</span>; zlim([0, 2]);
</pre> </pre>
</div> </div>
</div> </div>
@ -1362,9 +1367,8 @@ zlim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbo
</p> </p>
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Thomas Dehaeze</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2019-12-12 jeu. 20:10</p> <p class="date">Created: 2020-01-27 lun. 17:41</p>
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
</div> </div>
</body> </body>
</html> </html>

468
identification.org
View File

@ -21,6 +21,32 @@
#+PROPERTY: header-args:matlab+ :output-dir figs #+PROPERTY: header-args:matlab+ :output-dir figs
:END: :END:
* Introduction :ignore:
We would like to extract a state space model of the Stewart Platform from the Simscape model.
The inputs are:
| Symbol | Meaning |
|------------------------+--------------------------------------------------|
| $\bm{\mathcal{F}}_{d}$ | External forces applied in {B} |
| $\bm{\tau}$ | Joint forces |
| $\bm{\mathcal{F}}$ | Cartesian forces applied by the Joints |
| $\bm{D}_{w}$ | Fixed Based translation and rotations around {A} |
The outputs are:
| Symbol | Meaning |
|--------------------+---------------------------------------------------------------------------|
| $\bm{\mathcal{X}}$ | Relative Motion of {B} with respect to {A} |
| $\bm{\mathcal{L}}$ | Joint Displacement |
| $\bm{F}_{m}$ | Force Sensors in each strut |
| $\bm{v}_{m}$ | Inertial Sensors located at $b_i$ measuring in the direction of the strut |
#+begin_quote
An important difference from basic Simulink models is that the states in a physical network are not independent in general, because some states have dependencies on other states through constraints.
#+end_quote
* Identification * Identification
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
@ -35,27 +61,358 @@
simulinkproject('./'); simulinkproject('./');
#+end_src #+end_src
** Script ** Simscape Model
** Initialize the Stewart Platform
#+begin_src matlab
stewart = initializeFramesPositions();
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
#+end_src
** Identification
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_identification';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Taum'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Lm'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6', ...
'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', ...
'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6', ...
'taum1', 'taum2', 'taum3', 'taum4', 'taum5', 'taum6', ...
'Lm1', 'Lm2', 'Lm3', 'Lm4', 'Lm5', 'Lm6'};
#+end_src
* States as the motion of the mobile platform
** Initialize the Stewart Platform
#+begin_src matlab
stewart = initializeFramesPositions();
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
#+end_src
** Identification
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_identification_simple';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Xdot'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io);
% G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
% G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
#+end_src
Let's check the size of =G=:
#+begin_src matlab :results replace output
size(G)
#+end_src
#+RESULTS:
: size(G)
: State-space model with 12 outputs, 6 inputs, and 18 states.
We expect to have only 12 states (corresponding to the 6dof of the mobile platform).
#+begin_src matlab :results replace output
Gm = minreal(G);
#+end_src
#+RESULTS:
: Gm = minreal(G);
: 6 states removed.
And indeed, we obtain 12 states.
** Coordinate transformation
We can perform the following transformation using the =ss2ss= command.
#+begin_src matlab
Gt = ss2ss(Gm, Gm.C);
#+end_src
Then, the =C= matrix of =Gt= is the unity matrix which means that the states of the state space model are equal to the measurements $\bm{Y}$.
The measurements are the 6 displacement and 6 velocities of mobile platform with respect to $\{B\}$.
We could perform the transformation by hand:
#+begin_src matlab
At = Gm.C*Gm.A*pinv(Gm.C);
Bt = Gm.C*Gm.B;
Ct = eye(12);
Dt = zeros(12, 6);
Gt = ss(At, Bt, Ct, Dt);
#+end_src
** Analysis
#+begin_src matlab
[V,D] = eig(Gt.A);
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
ws = imag(diag(D))/2/pi;
[ws,I] = sort(ws)
xi = 100*real(diag(D))./imag(diag(D));
xi = xi(I);
data2orgtable([[1:length(ws(ws>0))]', ws(ws>0), xi(xi>0)], {}, {'Mode Number', 'Resonance Frequency [Hz]', 'Damping Ratio [%]'}, ' %.1f ');
#+end_src
#+RESULTS:
| Mode Number | Resonance Frequency [Hz] | Damping Ratio [%] |
|-------------+--------------------------+-------------------|
| 1.0 | 174.5 | 0.9 |
| 2.0 | 174.5 | 0.7 |
| 3.0 | 202.1 | 0.7 |
| 4.0 | 237.3 | 0.6 |
| 5.0 | 237.3 | 0.5 |
| 6.0 | 283.8 | 0.5 |
** Visualizing the modes
To visualize the i'th mode, we may excite the system using the inputs $U_i$ such that $B U_i$ is co-linear to $\xi_i$ (the mode we want to excite).
\[ U(t) = e^{\alpha t} ( ) \]
Let's first sort the modes and just take the modes corresponding to a eigenvalue with a positive imaginary part.
#+begin_src matlab
ws = imag(diag(D));
[ws,I] = sort(ws)
ws = ws(7:end); I = I(7:end);
#+end_src
#+begin_src matlab
for i = 1:length(ws)
#+end_src
#+begin_src matlab
i_mode = I(i); % the argument is the i'th mode
#+end_src
#+begin_src matlab
lambda_i = D(i_mode, i_mode);
xi_i = V(:,i_mode);
a_i = real(lambda_i);
b_i = imag(lambda_i);
#+end_src
Let do 10 periods of the mode.
#+begin_src matlab
t = linspace(0, 10/(imag(lambda_i)/2/pi), 1000);
U_i = pinv(Gt.B) * real(xi_i * lambda_i * (cos(b_i * t) + 1i*sin(b_i * t)));
#+end_src
#+begin_src matlab
U = timeseries(U_i, t);
#+end_src
Simulation:
#+begin_src matlab
load('mat/conf_simscape.mat');
set_param(conf_simscape, 'StopTime', num2str(t(end)));
sim(mdl);
#+end_src
Save the movie of the mode shape.
#+begin_src matlab
smwritevideo(mdl, sprintf('figs/mode%i', i), ...
'PlaybackSpeedRatio', 1/(b_i/2/pi), ...
'FrameRate', 30, ...
'FrameSize', [800, 400]);
#+end_src
#+begin_src matlab
end
#+end_src
** Identification
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_identification';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Lm'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
% G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
% G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
#+end_src
#+begin_src matlab
size(G)
#+end_src
** Change of states
#+begin_src matlab
At = G.C*G.A*pinv(G.C);
Bt = G.C*G.B;
Ct = eye(12);
Dt = zeros(12, 6);
#+end_src
#+begin_src matlab
Gt = ss(At, Bt, Ct, Dt);
#+end_src
#+begin_src matlab
size(Gt)
#+end_src
* Simple Model without any sensor
** 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
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :results none :exports none #+begin_src matlab :results none :exports none
open stewart simulinkproject('./');
#+end_src #+end_src
The hexapod structure and Sample structure are initialized. ** Simscape Model
#+begin_src matlab :results none #+begin_src matlab
stewart = initializeGeneralConfiguration(); open 'stewart_identification_simple.slx'
stewart = computeGeometricalProperties(stewart);
stewart = initializeMechanicalElements(stewart);
save('./mat/stewart.mat', 'stewart');
initializeSample();
#+end_src #+end_src
#+begin_src matlab :results none
G = identifyPlant(); ** Initialize the Stewart Platform
#+begin_src matlab
stewart = initializeFramesPositions();
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
#+end_src #+end_src
#+begin_src matlab :results none ** Identification
freqs = logspace(2, 4, 1000); #+begin_src matlab
stateorder = {...
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_1_1_1',...
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_2_1_1',...
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_3_1_1',...
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_4_1_1',...
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_5_1_1',...
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_6_1_1',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Rz.q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Rz.q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Rz.q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Rz.q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Rz.q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Rz.q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Pz.p',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Pz.p',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Pz.p',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Pz.p',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Pz.p',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Pz.p',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Rz.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Rz.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Rz.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Rz.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Rz.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Rz.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Pz.v',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Pz.v',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Pz.v',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Pz.v',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Pz.v',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Pz.v',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_F.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.spherical_joint_F.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.spherical_joint_F.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.spherical_joint_F.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.spherical_joint_F.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.spherical_joint_F.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.spherical_joint_F.S.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.spherical_joint_F.S.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.spherical_joint_F.S.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.spherical_joint_F.S.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.spherical_joint_F.S.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_F.S.w',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_M.S.Q',...
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_M.S.w'};
#+end_src
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_identification_simple';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Xdot'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
#+end_src
#+begin_src matlab
size(G)
#+end_src
#+begin_src matlab
G.StateName
#+end_src #+end_src
* Cartesian Plot * Cartesian Plot
@ -189,86 +546,3 @@ From a force applied on the Cartesian frame to the absolute displacement of the
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
#+end_src #+end_src
* identifyPlant
:PROPERTIES:
:HEADER-ARGS:matlab+: :exports code
:HEADER-ARGS:matlab+: :comments yes
:HEADER-ARGS:matlab+: :eval no
:HEADER-ARGS:matlab+: :tangle src/identifyPlant.m
:END:
#+begin_src matlab
function [sys] = identifyPlant(opts_param)
#+end_src
We use this code block to pass optional parameters.
#+begin_src matlab
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
#+end_src
We defined the options for the =linearize= command.
Here, we just identify the system at time $t = 0$.
#+begin_src matlab
options = linearizeOptions;
options.SampleTime = 0;
#+end_src
We define the name of the Simulink File used to identification.
#+begin_src matlab
mdl = 'stewart';
#+end_src
Then we defined the input/output of the transfer function we want to identify.
#+begin_src matlab
%% Inputs
io(1) = linio([mdl, '/F'], 1, 'input'); % Cartesian forces
io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces
io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces
io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion
%% Outputs
io(5) = linio([mdl, '/Dm'], 1, 'output'); % Relative Motion
io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg
io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg
io(8) = linio([mdl, '/Xm'], 1, 'output'); % Absolute motion of platform
#+end_src
The linearization is run.
#+begin_src matlab
G = linearize(mdl, io, 0);
#+end_src
We defined all the Input/Output names of the identified transfer function.
#+begin_src matlab
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ...
'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'};
G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ...
'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ...
'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ...
'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'};
#+end_src
We split the transfer function into sub transfer functions and we compute their minimum realization.
#+begin_src matlab
sys.G_cart = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}));
sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
sys.G_legs = minreal(G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
sys.G_tran = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}));
sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
% sys.G_all = minreal(G);
#+end_src
#+begin_src matlab
end
#+end_src

212
index.html
View File

@ -1,14 +1,15 @@
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN" <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2019-12-19 jeu. 15:14 --> <!-- 2020-01-27 lun. 17:40 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Stewart Platforms</title> <title>Stewart Platforms</title>
<meta name="generator" content="Org mode" /> <meta name="generator" content="Org mode" />
<meta name="author" content="Thomas Dehaeze" /> <meta name="author" content="Dehaeze Thomas" />
<style type="text/css"> <style type="text/css">
<!--/*--><![CDATA[/*><!--*/ <!--/*--><![CDATA[/*><!--*/
.title { text-align: center; .title { text-align: center;
@ -204,7 +205,7 @@
@licstart The following is the entire license notice for the @licstart The following is the entire license notice for the
JavaScript code in this tag. JavaScript code in this tag.
Copyright (C) 2012-2019 Free Software Foundation, Inc. Copyright (C) 2012-2020 Free Software Foundation, Inc.
The JavaScript code in this tag is free software: you can The JavaScript code in this tag is free software: you can
redistribute it and/or modify it under the terms of the GNU redistribute it and/or modify it under the terms of the GNU
@ -245,212 +246,77 @@ for the JavaScript code in this tag.
} }
/*]]>*///--> /*]]>*///-->
</script> </script>
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
displayAlign: "center",
displayIndent: "0em",
"HTML-CSS": { scale: 100,
linebreaks: { automatic: "false" },
webFont: "TeX"
},
SVG: {scale: 100,
linebreaks: { automatic: "false" },
font: "TeX"},
NativeMML: {scale: 100},
TeX: { equationNumbers: {autoNumber: "AMS"},
MultLineWidth: "85%",
TagSide: "right",
TagIndent: ".8em",
Macros: {
bm: ["{\\boldsymbol #1}",1],
}
}
});
</script>
<script type="text/javascript"
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
</head> </head>
<body> <body>
<div id="content"> <div id="content">
<h1 class="title">Stewart Platforms</h1> <h1 class="title">Stewart Platforms</h1>
<p> <p>
<a class='org-ref-reference' href="#preumont07_six_axis_singl_stage_activ">preumont07_six_axis_singl_stage_activ</a> The goal here is to provide a Matlab/Simscape Toolbox to study Stewart platforms.
</p> </p>
<div id="outline-container-org9cd44e0" class="outline-2"> <div id="outline-container-orgf137e1a" class="outline-2">
<h2 id="org9cd44e0"><span class="section-number-2">1</span> Simscape Model</h2> <h2 id="orgf137e1a"><span class="section-number-2">1</span> Simulink Project (<a href="simulink-project.html">link</a>)</h2>
<div class="outline-text-2" id="text-1">
<ul class="org-ul">
<li><a href="simscape-model.html">Model of the Stewart Platform</a></li>
<li><a href="identification.html">Identification of the Simscape Model</a></li>
</ul>
</div>
</div> </div>
<div id="outline-container-org7a44762" class="outline-2"> <div id="outline-container-org67d7b56" class="outline-2">
<h2 id="org7a44762"><span class="section-number-2">2</span> Architecture Study</h2> <h2 id="org67d7b56"><span class="section-number-2">2</span> Stewart Platform Architecture Definition (<a href="stewart-architecture.html">link</a>)</h2>
<div class="outline-text-2" id="text-2">
<ul class="org-ul">
<li><a href="kinematic-study.html">Kinematic Study</a></li>
<li><a href="stiffness-study.html">Stiffness Matrix Study</a></li>
<li>Jacobian Study</li>
<li><a href="cubic-configuration.html">Cubic Architecture</a></li>
</ul>
</div>
</div> </div>
<div id="outline-container-org77767cc" class="outline-2">
<h2 id="org77767cc"><span class="section-number-2">3</span> Motion Control</h2> <div id="outline-container-org740c1af" class="outline-2">
<div class="outline-text-2" id="text-3"> <h2 id="org740c1af"><span class="section-number-2">3</span> Simscape Model of the Stewart Platform (<a href="simscape-model.html">link</a>)</h2>
<ul class="org-ul">
<li>Active Damping</li>
<li>Inertial Control</li>
<li>Decentralized Control</li>
</ul>
</div> </div>
</div>
<div id="outline-container-org9d06c58" class="outline-2">
<h2 id="org9d06c58"><span class="section-number-2">4</span> Notes about Stewart platforms</h2> <div id="outline-container-orgde95161" class="outline-2">
<h2 id="orgde95161"><span class="section-number-2">4</span> Kinematic Analysis (<a href="kinematic-study.html">link</a>)</h2>
<div class="outline-text-2" id="text-4"> <div class="outline-text-2" id="text-4">
</div>
<div id="outline-container-orgffe6651" class="outline-3">
<h3 id="orgffe6651"><span class="section-number-3">4.1</span> Jacobian</h3>
<div class="outline-text-3" id="text-4-1">
</div>
<div id="outline-container-org6b92660" class="outline-4">
<h4 id="org6b92660"><span class="section-number-4">4.1.1</span> Relation to platform parameters</h4>
<div class="outline-text-4" id="text-4-1-1">
<p>
A Jacobian is defined by:
</p>
<ul class="org-ul"> <ul class="org-ul">
<li>the orientations of the struts \(\hat{s}_i\) expressed in a frame \(\{A\}\) linked to the fixed platform.</li> <li>Jacobian Analysis</li>
<li>the vectors from \(O_B\) to \(b_i\) expressed in the frame \(\{A\}\)</li> <li>Stiffness Analysis</li>
<li>Static Forces</li>
</ul> </ul>
<p>
Then, the choice of \(O_B\) changes the Jacobian.
</p>
</div> </div>
</div> </div>
<div id="outline-container-orgcec2e05" class="outline-4"> <div id="outline-container-orgeae2d7c" class="outline-2">
<h4 id="orgcec2e05"><span class="section-number-4">4.1.2</span> Jacobian for displacement</h4> <h2 id="orgeae2d7c"><span class="section-number-2">5</span> Identification of the Stewart Dynamics (<a href="identification.html">link</a>)</h2>
<div class="outline-text-4" id="text-4-1-2"> <div class="outline-text-2" id="text-5">
<p>
\[ \dot{q} = J \dot{X} \]
With:
</p>
<ul class="org-ul"> <ul class="org-ul">
<li>\(q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]\) vector of linear displacement of actuated joints</li> <li>Extraction of State Space models</li>
<li>\(X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]\) position and orientation of \(O_B\) expressed in the frame \(\{A\}\)</li> <li>Resonant Frequencies and Modal Damping</li>
<li>Mode Shapes</li>
</ul> </ul>
<p>
For very small displacements \(\delta q\) and \(\delta X\), we have \(\delta q = J \delta X\).
</p>
</div> </div>
</div> </div>
<div id="outline-container-orgbf33a4e" class="outline-4"> <div id="outline-container-org748e065" class="outline-2">
<h4 id="orgbf33a4e"><span class="section-number-4">4.1.3</span> Jacobian for forces</h4> <h2 id="org748e065"><span class="section-number-2">6</span> Active Damping (<a href="active-damping.html">link</a>)</h2>
<div class="outline-text-4" id="text-4-1-3"> <div class="outline-text-2" id="text-6">
<p>
\[ F = J^T \tau \]
With:
</p>
<ul class="org-ul"> <ul class="org-ul">
<li>\(\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]\) vector of actuator forces</li> <li>Inertial Sensor</li>
<li>\(F = [f_x\ f_y\ f_z\ n_x\ n_y\ n_z]\) force and torque acting on point \(O_B\)</li> <li>Force Sensor</li>
<li>Relative Motion Sensor</li>
</ul> </ul>
</div> </div>
</div> </div>
<div id="outline-container-org905e69f" class="outline-2">
<h2 id="org905e69f"><span class="section-number-2">7</span> Control of the Stewart Platform (<a href="control-study.html">link</a>)</h2>
</div> </div>
<div id="outline-container-org3710914" class="outline-3"> <div id="outline-container-org57e9202" class="outline-2">
<h3 id="org3710914"><span class="section-number-3">4.2</span> Stiffness matrix \(K\)</h3> <h2 id="org57e9202"><span class="section-number-2">8</span> Cubic Configuration (<a href="cubic-configuration.html">link</a>)</h2>
<div class="outline-text-3" id="text-4-2">
<p>
\[ K = J^T \text{diag}(k_i) J \]
</p>
<p>
If all the struts have the same stiffness \(k\), then \(K = k J^T J\)
</p>
<p>
\(K\) only depends of the geometry of the stewart platform: it depends on the Jacobian, that is on the orientations of the struts, position of the joints and choice of frame \(\{B\}\).
</p>
<p>
\[ F = K X \]
</p>
<p>
With \(F\) forces and torques applied to the moving platform at the origin of \(\{B\}\) and \(X\) the translations and rotations of \(\{B\}\) with respect to \(\{A\}\).
</p>
<p>
\[ C = K^{-1} \]
</p>
<p>
The compliance element \(C_{ij}\) is then the stiffness
\[ X_i = C_{ij} F_j \]
</p>
</div>
</div> </div>
<div id="outline-container-orgd5d2c08" class="outline-3"> <div id="outline-container-orgb6982d7" class="outline-2">
<h3 id="orgd5d2c08"><span class="section-number-3">4.3</span> Coupling</h3> <h2 id="orgb6982d7"><span class="section-number-2">9</span> Architecture Optimization</h2>
<div class="outline-text-3" id="text-4-3">
<p>
What causes the coupling from \(F_i\) to \(X_i\) ?
</p>
<div class="org-src-container">
<pre class="src src-latex"><span class="org-font-latex-sedate"><span class="org-keyword">\begin</span></span>{<span class="org-function-name">tikzpicture</span>}
<span class="org-font-latex-sedate">\node</span>[block] (Jt) at (0, 0) {<span class="org-font-latex-math">$J</span><span class="org-font-latex-math"><span class="org-font-latex-script-char">^{-T}</span></span><span class="org-font-latex-math">$</span>};
<span class="org-font-latex-sedate">\node</span>[block, right= of Jt] (G) {<span class="org-font-latex-math">$G$</span>};
<span class="org-font-latex-sedate">\node</span>[block, right= of G] (J) {<span class="org-font-latex-math">$J</span><span class="org-font-latex-math"><span class="org-font-latex-script-char">^{-1}</span></span><span class="org-font-latex-math">$</span>};
<span class="org-font-latex-sedate">\draw</span>[-&gt;] (<span class="org-font-latex-math">$(Jt.west)+(-0.8, 0)$</span>) -- (Jt.west) node[above left]{<span class="org-font-latex-math">$F</span><span class="org-font-latex-math"><span class="org-font-latex-script-char">_i</span></span><span class="org-font-latex-math">$</span>};
<span class="org-font-latex-sedate">\draw</span>[-&gt;] (Jt.east) -- (G.west) node[above left]{<span class="org-font-latex-math">$</span><span class="org-font-latex-sedate"><span class="org-font-latex-math">\tau</span></span><span class="org-font-latex-math"><span class="org-font-latex-script-char">_i</span></span><span class="org-font-latex-math">$</span>};
<span class="org-font-latex-sedate">\draw</span>[-&gt;] (G.east) -- (J.west) node[above left]{<span class="org-font-latex-math">$q</span><span class="org-font-latex-math"><span class="org-font-latex-script-char">_i</span></span><span class="org-font-latex-math">$</span>};
<span class="org-font-latex-sedate">\draw</span>[-&gt;] (J.east) -- ++(0.8, 0) node[above left]{<span class="org-font-latex-math">$X</span><span class="org-font-latex-math"><span class="org-font-latex-script-char">_i</span></span><span class="org-font-latex-math">$</span>};
<span class="org-font-latex-sedate"><span class="org-keyword">\end</span></span>{<span class="org-function-name">tikzpicture</span>}
</pre>
</div>
<div id="orgc118680" class="figure">
<p><img src="figs/coupling.png" alt="coupling.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Block diagram to control an hexapod</p>
</div>
<p>
There is no coupling from \(F_i\) to \(X_j\) if \(J^{-1} G J^{-T}\) is diagonal.
</p>
<p>
If \(G\) is diagonal (cubic configuration), then \(J^{-1} G J^{-T} = G J^{-1} J^{-T} = G (J^{T} J)^{-1} = G K^{-1}\)
</p>
<p>
Thus, the system is uncoupled if \(G\) and \(K\) are diagonal.
</p>
</div>
</div>
</div> </div>
<p> <p>
<h1 class='org-ref-bib-h1'>Bibliography</h1> <a href="ref.bib">ref.bib</a>
<ul class='org-ref-bib'><li><a id="preumont07_six_axis_singl_stage_activ">[preumont07_six_axis_singl_stage_activ]</a> <a name="preumont07_six_axis_singl_stage_activ"></a>Preumont, Horodinca, Romanescu, de, Marneffe, Avraam, Deraemaeker, Bossens, & Abu Hanieh, A Six-Axis Single-Stage Active Vibration Isolator Based on Stewart Platform, <i>Journal of Sound and Vibration</i>, <b>300(3-5)</b>, 644-661 (2007). <a href="https://doi.org/10.1016/j.jsv.2006.07.050">link</a>. <a href="http://dx.doi.org/10.1016/j.jsv.2006.07.050">doi</a>.</li>
</ul>
</p> </p>
</div> </div>
</body> </body>

91
index.org
View File

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

12
readme.org Normal file
View File

@ -0,0 +1,12 @@
#+TITLE: Stewart Platforms Toolbox
:DRAWER:
#+OPTIONS: toc:nil
#+OPTIONS: html-postamble:nil
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
:END:

962
simscape-model.html
View File

@ -4,7 +4,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2020-01-22 mer. 11:35 --> <!-- 2020-01-27 lun. 17:41 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Stewart Platform - Simscape Model</title> <title>Stewart Platform - Simscape Model</title>
@ -246,31 +246,6 @@ for the JavaScript code in this tag.
} }
/*]]>*///--> /*]]>*///-->
</script> </script>
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
displayAlign: "center",
displayIndent: "0em",
"HTML-CSS": { scale: 100,
linebreaks: { automatic: "false" },
webFont: "TeX"
},
SVG: {scale: 100,
linebreaks: { automatic: "false" },
font: "TeX"},
NativeMML: {scale: 100},
TeX: { equationNumbers: {autoNumber: "AMS"},
MultLineWidth: "85%",
TagSide: "right",
TagIndent: ".8em",
Macros: {
bm: ["{\\boldsymbol #1}",1],
}
}
});
</script>
<script type="text/javascript"
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
</head> </head>
<body> <body>
<div id="org-div-home-and-up"> <div id="org-div-home-and-up">
@ -279,944 +254,11 @@ for the JavaScript code in this tag.
<a accesskey="H" href="./index.html"> HOME </a> <a accesskey="H" href="./index.html"> HOME </a>
</div><div id="content"> </div><div id="content">
<h1 class="title">Stewart Platform - Simscape Model</h1> <h1 class="title">Stewart Platform - Simscape Model</h1>
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#org6f92f51">1. Procedure</a></li>
<li><a href="#orgf7df3dd">2. Matlab Code</a>
<ul>
<li><a href="#org36ddd65">2.1. Simscape Model</a></li>
<li><a href="#orgfdd5b30">2.2. Test the functions</a></li>
</ul>
</li>
<li><a href="#org9b04ea6">3. <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</a>
<ul>
<li><a href="#org12408b9">3.1. Function description</a></li>
<li><a href="#org65e1007">3.2. Documentation</a></li>
<li><a href="#org94be80d">3.3. Optional Parameters</a></li>
<li><a href="#org87eaafa">3.4. Initialize the Stewart structure</a></li>
<li><a href="#org23ee353">3.5. Compute the position of each frame</a></li>
</ul>
</li>
<li><a href="#org087790f">4. <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</a>
<ul>
<li><a href="#org4227245">4.1. Function description</a></li>
<li><a href="#org0a67b9a">4.2. Documentation</a></li>
<li><a href="#orgedf8c0c">4.3. Optional Parameters</a></li>
<li><a href="#org512c9d4">4.4. Position of the Cube</a></li>
<li><a href="#orgcb8a030">4.5. Compute the pose</a></li>
</ul>
</li>
<li><a href="#org1a639eb">5. <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</a>
<ul>
<li><a href="#orgaf38049">5.1. Function description</a></li>
<li><a href="#org99d670a">5.2. Documentation</a></li>
<li><a href="#orgb94dd5e">5.3. Optional Parameters</a></li>
<li><a href="#org217593d">5.4. Compute the pose</a></li>
</ul>
</li>
<li><a href="#org027ac62">6. <code>computeJointsPose</code>: Compute the Pose of the Joints</a>
<ul>
<li><a href="#org9851a88">6.1. Function description</a></li>
<li><a href="#org38475a0">6.2. Documentation</a></li>
<li><a href="#orgcb68548">6.3. Compute the position of the Joints</a></li>
<li><a href="#org17b24ef">6.4. Compute the strut length and orientation</a></li>
<li><a href="#orgdf76376">6.5. Compute the orientation of the Joints</a></li>
</ul>
</li>
<li><a href="#org18a1d1b">7. <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</a>
<ul>
<li><a href="#orgfdf3d88">7.1. Function description</a></li>
<li><a href="#orge5e71a3">7.2. Optional Parameters</a></li>
<li><a href="#org85adb8d">7.3. Add Stiffness and Damping properties of each strut</a></li>
</ul>
</li>
<li><a href="#orgbaa0753">8. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
<ul>
<li><a href="#org7f7fdc1">8.1. Function description</a></li>
<li><a href="#orgc824a02">8.2. Compute Jacobian Matrix</a></li>
<li><a href="#org2806583">8.3. Compute Stiffness Matrix</a></li>
<li><a href="#orgb5560fc">8.4. Compute Compliance Matrix</a></li>
</ul>
</li>
<li><a href="#orgb6aa2e4">9. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
<ul>
<li><a href="#org1abf793">9.1. Function description</a></li>
<li><a href="#orgae295b6">9.2. Optional Parameters</a></li>
<li><a href="#orgfd5d40a">9.3. Theory</a></li>
<li><a href="#orgc7dd5e8">9.4. Compute</a></li>
</ul>
</li>
<li><a href="#org689b179">10. <code>forwardKinematicsApprox</code>: Compute the Forward Kinematics</a>
<ul>
<li><a href="#orgba48270">10.1. Function description</a></li>
<li><a href="#org22e2134">10.2. Optional Parameters</a></li>
<li><a href="#orgfa57f93">10.3. Computation</a></li>
</ul>
</li>
</ul>
</div>
</div>
<p>
Stewart platforms are generated in multiple steps.
</p>
<p>
We define 4 important <b>frames</b>:
</p>
<ul class="org-ul">
<li>\(\{F\}\): Frame fixed to the <b>Fixed</b> base and located at the center of its bottom surface.
This is used to fix the Stewart platform to some support.</li>
<li>\(\{M\}\): Frame fixed to the <b>Moving</b> platform and located at the center of its top surface.
This is used to place things on top of the Stewart platform.</li>
<li>\(\{A\}\): Frame fixed to the fixed base.
It defined the center of rotation of the moving platform.</li>
<li>\(\{B\}\): Frame fixed to the moving platform.
The motion of the moving platforms and forces applied to it are defined with respect to this frame \(\{B\}\).</li>
</ul>
<p>
Then, we define the <b>location of the spherical joints</b>:
</p>
<ul class="org-ul">
<li>\(\bm{a}_{i}\) are the position of the spherical joints fixed to the fixed base</li>
<li>\(\bm{b}_{i}\) are the position of the spherical joints fixed to the moving platform</li>
</ul>
<p>
We define the <b>rest position</b> of the Stewart platform:
</p>
<ul class="org-ul">
<li>For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.</li>
<li>Thus, to define the rest position of the Stewart platform, we just have to defined its total height \(H\).
\(H\) corresponds to the distance from the bottom of the fixed base to the top of the moving platform.</li>
</ul>
<p>
From \(\bm{a}_{i}\) and \(\bm{b}_{i}\), we can determine the <b>length and orientation of each strut</b>:
</p>
<ul class="org-ul">
<li>\(l_{i}\) is the length of the strut</li>
<li>\({}^{A}\hat{\bm{s}}_{i}\) is the unit vector align with the strut</li>
</ul>
<p>
The position of the Spherical joints can be computed using various methods:
</p>
<ul class="org-ul">
<li>Cubic configuration</li>
<li>Circular configuration</li>
<li>Arbitrary position</li>
<li>These methods should be easily scriptable and corresponds to specific functions that returns \({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\).
The input of these functions are the parameters corresponding to the wanted geometry.</li>
</ul>
<p>
For Simscape, we need:
</p>
<ul class="org-ul">
<li>The position and orientation of each spherical joint fixed to the fixed base: \({}^{F}\bm{a}_{i}\) and \({}^{F}\bm{R}_{a_{i}}\)</li>
<li>The position and orientation of each spherical joint fixed to the moving platform: \({}^{M}\bm{b}_{i}\) and \({}^{M}\bm{R}_{b_{i}}\)</li>
<li>The rest length of each strut: \(l_{i}\)</li>
<li>The stiffness and damping of each actuator: \(k_{i}\) and \(c_{i}\)</li>
<li>The position of the frame \(\{A\}\) with respect to the frame \(\{F\}\): \({}^{F}\bm{O}_{A}\)</li>
<li>The position of the frame \(\{B\}\) with respect to the frame \(\{M\}\): \({}^{M}\bm{O}_{B}\)</li>
</ul>
<div id="outline-container-org6f92f51" class="outline-2">
<h2 id="org6f92f51"><span class="section-number-2">1</span> Procedure</h2>
<div class="outline-text-2" id="text-1">
<p>
The procedure to define the Stewart platform is the following:
</p>
<ol class="org-ol">
<li>Define the initial position of frames {A}, {B}, {F} and {M}.
We do that using the <code>initializeFramesPositions</code> function.
We have to specify the total height of the Stewart platform \(H\) and the position \({}^{M}O_{B}\) of {B} with respect to {M}.</li>
<li>Compute the positions of joints \({}^{F}a_{i}\) and \({}^{M}b_{i}\).
We can do that using various methods depending on the wanted architecture:
<ul class="org-ul">
<li><code>generateCubicConfiguration</code> permits to generate a cubic configuration</li>
</ul></li>
<li>Compute the position and orientation of the joints with respect to the fixed base and the moving platform.
This is done with the <code>computeJointsPose</code> function.</li>
<li>Define the dynamical properties of the Stewart platform.
The output are the stiffness and damping of each strut \(k_{i}\) and \(c_{i}\).
This can be done we simply choosing directly the stiffness and damping of each strut.
The stiffness and damping of each actuator can also be determine from the wanted stiffness of the Stewart platform for instance.</li>
<li>Define the mass and inertia of each element of the Stewart platform.</li>
</ol>
<p>
By following this procedure, we obtain a Matlab structure <code>stewart</code> that contains all the information for the Simscape model and for further analysis.
</p>
</div>
</div>
<div id="outline-container-orgf7df3dd" class="outline-2">
<h2 id="orgf7df3dd"><span class="section-number-2">2</span> Matlab Code</h2>
<div class="outline-text-2" id="text-2">
</div>
<div id="outline-container-org36ddd65" class="outline-3">
<h3 id="org36ddd65"><span class="section-number-3">2.1</span> Simscape Model</h3>
<div class="outline-text-3" id="text-2-1">
<div class="org-src-container">
<pre class="src src-matlab">open(<span class="org-string">'stewart_platform.slx'</span>)
</pre>
</div>
</div>
</div>
<div id="outline-container-orgfdd5b30" class="outline-3">
<h3 id="orgfdd5b30"><span class="section-number-3">2.2</span> Test the functions</h3>
<div class="outline-text-3" id="text-2-2">
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeFramesPositions(<span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateCubicConfiguration(stewart, <span class="org-string">'Hc'</span>, 60e<span class="org-type">-</span>3, <span class="org-string">'FOc'</span>, 45e<span class="org-type">-</span>3, <span class="org-string">'FHa'</span>, 5e<span class="org-type">-</span>3, <span class="org-string">'MHb'</span>, 5e<span class="org-type">-</span>3);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, <span class="org-string">'Ki'</span>, 1e6<span class="org-type">*</span>ones(6,1), <span class="org-string">'Ci'</span>, 1e2<span class="org-type">*</span>ones(6,1));
stewart = computeJacobian(stewart);
[Li, dLi] = inverseKinematics(stewart, <span class="org-string">'AP'</span>, [0;0;0.00001], <span class="org-string">'ARB'</span>, eye(3));
[P, R] = forwardKinematicsApprox(stewart, <span class="org-string">'dL'</span>, dLi)
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org9b04ea6" class="outline-2">
<h2 id="org9b04ea6"><span class="section-number-2">3</span> <code>initializeFramesPositions</code>: Initialize the positions of frames {A}, {B}, {F} and {M}</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="org88d4785"></a>
</p>
<p>
This Matlab function is accessible <a href="src/initializeFramesPositions.m">here</a>.
</p>
</div>
<div id="outline-container-org12408b9" class="outline-3">
<h3 id="org12408b9"><span class="section-number-3">3.1</span> Function description</h3>
<div class="outline-text-3" id="text-3-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">initializeFramesPositions</span>(<span class="org-variable-name">args</span>)
<span class="org-comment">% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = initializeFramesPositions(args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - args - Can have the following fields:</span>
<span class="org-comment">% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]</span>
<span class="org-comment">% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - stewart - A structure with the following fields:</span>
<span class="org-comment">% - H [1x1] - Total Height of the Stewart Platform [m]</span>
<span class="org-comment">% - FO_M [3x1] - Position of {M} with respect to {F} [m]</span>
<span class="org-comment">% - MO_B [3x1] - Position of {B} with respect to {M} [m]</span>
<span class="org-comment">% - FO_A [3x1] - Position of {A} with respect to {F} [m]</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org65e1007" class="outline-3">
<h3 id="org65e1007"><span class="section-number-3">3.2</span> Documentation</h3>
<div class="outline-text-3" id="text-3-2">
<div id="org4b4d91b" class="figure">
<p><img src="figs/stewart-frames-position.png" alt="stewart-frames-position.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Definition of the position of the frames</p>
</div>
</div>
</div>
<div id="outline-container-org94be80d" class="outline-3">
<h3 id="org94be80d"><span class="section-number-3">3.3</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-3-3">
<div class="org-src-container">
<pre class="src src-matlab">arguments
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3
args.MO_B (1,1) double {mustBeNumeric} = 50e<span class="org-type">-</span>3
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org87eaafa" class="outline-3">
<h3 id="org87eaafa"><span class="section-number-3">3.4</span> Initialize the Stewart structure</h3>
<div class="outline-text-3" id="text-3-4">
<div class="org-src-container">
<pre class="src src-matlab">stewart = struct();
</pre>
</div>
</div>
</div>
<div id="outline-container-org23ee353" class="outline-3">
<h3 id="org23ee353"><span class="section-number-3">3.5</span> Compute the position of each frame</h3>
<div class="outline-text-3" id="text-3-5">
<div class="org-src-container">
<pre class="src src-matlab">stewart.H = args.H; <span class="org-comment">% Total Height of the Stewart Platform [m]</span>
stewart.FO_M = [0; 0; stewart.H]; <span class="org-comment">% Position of {M} with respect to {F} [m]</span>
stewart.MO_B = [0; 0; args.MO_B]; <span class="org-comment">% Position of {B} with respect to {M} [m]</span>
stewart.FO_A = stewart.MO_B <span class="org-type">+</span> stewart.FO_M; <span class="org-comment">% Position of {A} with respect to {F} [m]</span>
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org087790f" class="outline-2">
<h2 id="org087790f"><span class="section-number-2">4</span> <code>generateCubicConfiguration</code>: Generate a Cubic Configuration</h2>
<div class="outline-text-2" id="text-4">
<p>
<a id="org9bd21cb"></a>
</p>
<p>
This Matlab function is accessible <a href="src/generateCubicConfiguration.m">here</a>.
</p>
</div>
<div id="outline-container-org4227245" class="outline-3">
<h3 id="org4227245"><span class="section-number-3">4.1</span> Function description</h3>
<div class="outline-text-3" id="text-4-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">generateCubicConfiguration</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% generateCubicConfiguration - Generate a Cubic Configuration</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = generateCubicConfiguration(stewart, args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - stewart - A structure with the following fields</span>
<span class="org-comment">% - H [1x1] - Total height of the platform [m]</span>
<span class="org-comment">% - args - Can have the following fields:</span>
<span class="org-comment">% - Hc [1x1] - Height of the "useful" part of the cube [m]</span>
<span class="org-comment">% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]</span>
<span class="org-comment">% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]</span>
<span class="org-comment">% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
<span class="org-comment">% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}</span>
<span class="org-comment">% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org0a67b9a" class="outline-3">
<h3 id="org0a67b9a"><span class="section-number-3">4.2</span> Documentation</h3>
<div class="outline-text-3" id="text-4-2">
<div id="org77ddaf9" class="figure">
<p><img src="figs/cubic-configuration-definition.png" alt="cubic-configuration-definition.png" />
</p>
<p><span class="figure-number">Figure 2: </span>Cubic Configuration</p>
</div>
</div>
</div>
<div id="outline-container-orgedf8c0c" class="outline-3">
<h3 id="orgedf8c0c"><span class="section-number-3">4.3</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-4-3">
<div class="org-src-container">
<pre class="src src-matlab">arguments
stewart
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e<span class="org-type">-</span>3
args.FOc (1,1) double {mustBeNumeric} = 50e<span class="org-type">-</span>3
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org512c9d4" class="outline-3">
<h3 id="org512c9d4"><span class="section-number-3">4.4</span> Position of the Cube</h3>
<div class="outline-text-3" id="text-4-4">
<p>
We define the useful points of the cube with respect to the Cube&rsquo;s center.
\({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is
located at the center of the cube and aligned with {F} and {M}.
</p>
<div class="org-src-container">
<pre class="src src-matlab">sx = [ 2; <span class="org-type">-</span>1; <span class="org-type">-</span>1];
sy = [ 0; 1; <span class="org-type">-</span>1];
sz = [ 1; 1; 1];
R = [sx, sy, sz]<span class="org-type">./</span>vecnorm([sx, sy, sz]);
L = args.Hc<span class="org-type">*</span>sqrt(3);
Cc = R<span class="org-type">'*</span>[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] <span class="org-type">-</span> [0;0;1.5<span class="org-type">*</span>args.Hc];
CCf = [Cc(<span class="org-type">:</span>,1), Cc(<span class="org-type">:</span>,3), Cc(<span class="org-type">:</span>,3), Cc(<span class="org-type">:</span>,5), Cc(<span class="org-type">:</span>,5), Cc(<span class="org-type">:</span>,1)]; <span class="org-comment">% CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg</span>
CCm = [Cc(<span class="org-type">:</span>,2), Cc(<span class="org-type">:</span>,2), Cc(<span class="org-type">:</span>,4), Cc(<span class="org-type">:</span>,4), Cc(<span class="org-type">:</span>,6), Cc(<span class="org-type">:</span>,6)]; <span class="org-comment">% CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-orgcb8a030" class="outline-3">
<h3 id="orgcb8a030"><span class="section-number-3">4.5</span> Compute the pose</h3>
<div class="outline-text-3" id="text-4-5">
<p>
We can compute the vector of each leg \({}^{C}\hat{\bm{s}}_{i}\) (unit vector from \({}^{C}C_{f}\) to \({}^{C}C_{m}\)).
</p>
<div class="org-src-container">
<pre class="src src-matlab">CSi = (CCm <span class="org-type">-</span> CCf)<span class="org-type">./</span>vecnorm(CCm <span class="org-type">-</span> CCf);
</pre>
</div>
<p>
We now which to compute the position of the joints \(a_{i}\) and \(b_{i}\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart.Fa = CCf <span class="org-type">+</span> [0; 0; args.FOc] <span class="org-type">+</span> ((args.FHa<span class="org-type">-</span>(args.FOc<span class="org-type">-</span>args.Hc<span class="org-type">/</span>2))<span class="org-type">./</span>CSi(3,<span class="org-type">:</span>))<span class="org-type">.*</span>CSi;
stewart.Mb = CCf <span class="org-type">+</span> [0; 0; args.FOc<span class="org-type">-</span>stewart.H] <span class="org-type">+</span> ((stewart.H<span class="org-type">-</span>args.MHb<span class="org-type">-</span>(args.FOc<span class="org-type">-</span>args.Hc<span class="org-type">/</span>2))<span class="org-type">./</span>CSi(3,<span class="org-type">:</span>))<span class="org-type">.*</span>CSi;
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org1a639eb" class="outline-2">
<h2 id="org1a639eb"><span class="section-number-2">5</span> <code>generateGeneralConfiguration</code>: Generate a Very General Configuration</h2>
<div class="outline-text-2" id="text-5">
<p>
<a id="org4135659"></a>
</p>
<p>
This Matlab function is accessible <a href="src/generateGeneralConfiguration.m">here</a>.
</p>
</div>
<div id="outline-container-orgaf38049" class="outline-3">
<h3 id="orgaf38049"><span class="section-number-3">5.1</span> Function description</h3>
<div class="outline-text-3" id="text-5-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">generateGeneralConfiguration</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% generateGeneralConfiguration - Generate a Very General Configuration</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - stewart - A structure with the following fields</span>
<span class="org-comment">% - H [1x1] - Total height of the platform [m]</span>
<span class="org-comment">% - args - Can have the following fields:</span>
<span class="org-comment">% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]</span>
<span class="org-comment">% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]</span>
<span class="org-comment">% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]</span>
<span class="org-comment">% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]</span>
<span class="org-comment">% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]</span>
<span class="org-comment">% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
<span class="org-comment">% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}</span>
<span class="org-comment">% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org99d670a" class="outline-3">
<h3 id="org99d670a"><span class="section-number-3">5.2</span> Documentation</h3>
<div class="outline-text-3" id="text-5-2">
<p>
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
The radius of the circles can be chosen as well as the angles where the joints are located.
</p>
</div>
</div>
<div id="outline-container-orgb94dd5e" class="outline-3">
<h3 id="orgb94dd5e"><span class="section-number-3">5.3</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-5-3">
<div class="org-src-container">
<pre class="src src-matlab">arguments
stewart
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e<span class="org-type">-</span>3;
args.FTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>10, 10, 120<span class="org-type">-</span>10, 120<span class="org-type">+</span>10, 240<span class="org-type">-</span>10, 240<span class="org-type">+</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180);
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e<span class="org-type">-</span>3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e<span class="org-type">-</span>3;
args.MTh (6,1) double {mustBeNumeric} = [<span class="org-type">-</span>60<span class="org-type">+</span>10, 60<span class="org-type">-</span>10, 60<span class="org-type">+</span>10, 180<span class="org-type">-</span>10, 180<span class="org-type">+</span>10, <span class="org-type">-</span>60<span class="org-type">-</span>10]<span class="org-type">*</span>(<span class="org-constant">pi</span><span class="org-type">/</span>180);
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org217593d" class="outline-3">
<h3 id="org217593d"><span class="section-number-3">5.4</span> Compute the pose</h3>
<div class="outline-text-3" id="text-5-4">
<div class="org-src-container">
<pre class="src src-matlab">stewart.Fa = zeros(3,6);
stewart.Mb = zeros(3,6);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
stewart.Fa(<span class="org-type">:</span>,<span class="org-constant">i</span>) = [args.FR<span class="org-type">*</span>cos(args.FTh(<span class="org-constant">i</span>)); args.FR<span class="org-type">*</span>sin(args.FTh(<span class="org-constant">i</span>)); args.FH];
stewart.Mb(<span class="org-type">:</span>,<span class="org-constant">i</span>) = [args.MR<span class="org-type">*</span>cos(args.MTh(<span class="org-constant">i</span>)); args.MR<span class="org-type">*</span>sin(args.MTh(<span class="org-constant">i</span>)); <span class="org-type">-</span>args.MH];
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org027ac62" class="outline-2">
<h2 id="org027ac62"><span class="section-number-2">6</span> <code>computeJointsPose</code>: Compute the Pose of the Joints</h2>
<div class="outline-text-2" id="text-6">
<p>
<a id="org86d3c8d"></a>
</p>
<p>
This Matlab function is accessible <a href="src/computeJointsPose.m">here</a>.
</p>
</div>
<div id="outline-container-org9851a88" class="outline-3">
<h3 id="org9851a88"><span class="section-number-3">6.1</span> Function description</h3>
<div class="outline-text-3" id="text-6-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">computeJointsPose</span>(<span class="org-variable-name">stewart</span>)
<span class="org-comment">% computeJointsPose -</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = computeJointsPose(stewart)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - stewart - A structure with the following fields</span>
<span class="org-comment">% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}</span>
<span class="org-comment">% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}</span>
<span class="org-comment">% - FO_A [3x1] - Position of {A} with respect to {F}</span>
<span class="org-comment">% - MO_B [3x1] - Position of {B} with respect to {M}</span>
<span class="org-comment">% - FO_M [3x1] - Position of {M} with respect to {F}</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - stewart - A structure with the following added fields</span>
<span class="org-comment">% - Aa [3x6] - The i'th column is the position of ai with respect to {A}</span>
<span class="org-comment">% - Ab [3x6] - The i'th column is the position of bi with respect to {A}</span>
<span class="org-comment">% - Ba [3x6] - The i'th column is the position of ai with respect to {B}</span>
<span class="org-comment">% - Bb [3x6] - The i'th column is the position of bi with respect to {B}</span>
<span class="org-comment">% - l [6x1] - The i'th element is the initial length of strut i</span>
<span class="org-comment">% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}</span>
<span class="org-comment">% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}</span>
<span class="org-comment">% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}</span>
<span class="org-comment">% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org38475a0" class="outline-3">
<h3 id="org38475a0"><span class="section-number-3">6.2</span> Documentation</h3>
<div class="outline-text-3" id="text-6-2">
<div id="orge85a023" class="figure">
<p><img src="figs/stewart-struts.png" alt="stewart-struts.png" />
</p>
<p><span class="figure-number">Figure 3: </span>Position and orientation of the struts</p>
</div>
</div>
</div>
<div id="outline-container-orgcb68548" class="outline-3">
<h3 id="orgcb68548"><span class="section-number-3">6.3</span> Compute the position of the Joints</h3>
<div class="outline-text-3" id="text-6-3">
<div class="org-src-container">
<pre class="src src-matlab">stewart.Aa = stewart.Fa <span class="org-type">-</span> repmat(stewart.FO_A, [1, 6]);
stewart.Bb = stewart.Mb <span class="org-type">-</span> repmat(stewart.MO_B, [1, 6]);
stewart.Ab = stewart.Bb <span class="org-type">-</span> repmat(<span class="org-type">-</span>stewart.MO_B<span class="org-type">-</span>stewart.FO_M<span class="org-type">+</span>stewart.FO_A, [1, 6]);
stewart.Ba = stewart.Aa <span class="org-type">-</span> repmat( stewart.MO_B<span class="org-type">+</span>stewart.FO_M<span class="org-type">-</span>stewart.FO_A, [1, 6]);
</pre>
</div>
</div>
</div>
<div id="outline-container-org17b24ef" class="outline-3">
<h3 id="org17b24ef"><span class="section-number-3">6.4</span> Compute the strut length and orientation</h3>
<div class="outline-text-3" id="text-6-4">
<div class="org-src-container">
<pre class="src src-matlab">stewart.As = (stewart.Ab <span class="org-type">-</span> stewart.Aa)<span class="org-type">./</span>vecnorm(stewart.Ab <span class="org-type">-</span> stewart.Aa); <span class="org-comment">% As_i is the i'th vector of As</span>
stewart.l = vecnorm(stewart.Ab <span class="org-type">-</span> stewart.Aa)<span class="org-type">'</span>;
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">stewart.Bs = (stewart.Bb <span class="org-type">-</span> stewart.Ba)<span class="org-type">./</span>vecnorm(stewart.Bb <span class="org-type">-</span> stewart.Ba);
</pre>
</div>
</div>
</div>
<div id="outline-container-orgdf76376" class="outline-3">
<h3 id="orgdf76376"><span class="section-number-3">6.5</span> Compute the orientation of the Joints</h3>
<div class="outline-text-3" id="text-6-5">
<div class="org-src-container">
<pre class="src src-matlab">stewart.FRa = zeros(3,3,6);
stewart.MRb = zeros(3,3,6);
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
stewart.FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [cross([0;1;0], stewart.As(<span class="org-type">:</span>,<span class="org-constant">i</span>)) , cross(stewart.As(<span class="org-type">:</span>,<span class="org-constant">i</span>), cross([0;1;0], stewart.As(<span class="org-type">:</span>,<span class="org-constant">i</span>))) , stewart.As(<span class="org-type">:</span>,<span class="org-constant">i</span>)];
stewart.FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = stewart.FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>)<span class="org-type">./</span>vecnorm(stewart.FRa(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>));
stewart.MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [cross([0;1;0], stewart.Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>)) , cross(stewart.Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>), cross([0;1;0], stewart.Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>))) , stewart.Bs(<span class="org-type">:</span>,<span class="org-constant">i</span>)];
stewart.MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = stewart.MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>)<span class="org-type">./</span>vecnorm(stewart.MRb(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>));
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org18a1d1b" class="outline-2">
<h2 id="org18a1d1b"><span class="section-number-2">7</span> <code>initializeStrutDynamics</code>: Add Stiffness and Damping properties of each strut</h2>
<div class="outline-text-2" id="text-7">
<p>
<a id="org41cab5e"></a>
</p>
<p>
This Matlab function is accessible <a href="src/initializeStrutDynamics.m">here</a>.
</p>
</div>
<div id="outline-container-orgfdf3d88" class="outline-3">
<h3 id="orgfdf3d88"><span class="section-number-3">7.1</span> Function description</h3>
<div class="outline-text-3" id="text-7-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">initializeStrutDynamics</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% initializeStrutDynamics - Add Stiffness and Damping properties of each strut</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = initializeStrutDynamics(args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - args - Structure with the following fields:</span>
<span class="org-comment">% - Ki [6x1] - Stiffness of each strut [N/m]</span>
<span class="org-comment">% - Ci [6x1] - Damping of each strut [N/(m/s)]</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - stewart - updated Stewart structure with the added fields:</span>
<span class="org-comment">% - Ki [6x1] - Stiffness of each strut [N/m]</span>
<span class="org-comment">% - Ci [6x1] - Damping of each strut [N/(m/s)]</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-orge5e71a3" class="outline-3">
<h3 id="orge5e71a3"><span class="section-number-3">7.2</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-7-2">
<div class="org-src-container">
<pre class="src src-matlab">arguments
stewart
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6<span class="org-type">*</span>ones(6,1)
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3<span class="org-type">*</span>ones(6,1)
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org85adb8d" class="outline-3">
<h3 id="org85adb8d"><span class="section-number-3">7.3</span> Add Stiffness and Damping properties of each strut</h3>
<div class="outline-text-3" id="text-7-3">
<div class="org-src-container">
<pre class="src src-matlab">stewart.Ki = args.Ki;
stewart.Ci = args.Ci;
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-orgbaa0753" class="outline-2">
<h2 id="orgbaa0753"><span class="section-number-2">8</span> <code>computeJacobian</code>: Compute the Jacobian Matrix</h2>
<div class="outline-text-2" id="text-8">
<p>
<a id="orgfa470f8"></a>
</p>
<p>
This Matlab function is accessible <a href="src/computeJacobian.m">here</a>.
</p>
</div>
<div id="outline-container-org7f7fdc1" class="outline-3">
<h3 id="org7f7fdc1"><span class="section-number-3">8.1</span> Function description</h3>
<div class="outline-text-3" id="text-8-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[stewart]</span> = <span class="org-function-name">computeJacobian</span>(<span class="org-variable-name">stewart</span>)
<span class="org-comment">% computeJacobian -</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = computeJacobian(stewart)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - stewart - With at least the following fields:</span>
<span class="org-comment">% - As [3x6] - The 6 unit vectors for each strut expressed in {A}</span>
<span class="org-comment">% - Ab [3x6] - The 6 position of the joints bi expressed in {A}</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - stewart - With the 3 added field:</span>
<span class="org-comment">% - J [6x6] - The Jacobian Matrix</span>
<span class="org-comment">% - K [6x6] - The Stiffness Matrix</span>
<span class="org-comment">% - C [6x6] - The Compliance Matrix</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-orgc824a02" class="outline-3">
<h3 id="orgc824a02"><span class="section-number-3">8.2</span> Compute Jacobian Matrix</h3>
<div class="outline-text-3" id="text-8-2">
<div class="org-src-container">
<pre class="src src-matlab">stewart.J = [stewart.As<span class="org-type">'</span> , cross(stewart.Ab, stewart.As)<span class="org-type">'</span>];
</pre>
</div>
</div>
</div>
<div id="outline-container-org2806583" class="outline-3">
<h3 id="org2806583"><span class="section-number-3">8.3</span> Compute Stiffness Matrix</h3>
<div class="outline-text-3" id="text-8-3">
<div class="org-src-container">
<pre class="src src-matlab">stewart.K = stewart.J<span class="org-type">'*</span>diag(stewart.Ki)<span class="org-type">*</span>stewart.J;
</pre>
</div>
</div>
</div>
<div id="outline-container-orgb5560fc" class="outline-3">
<h3 id="orgb5560fc"><span class="section-number-3">8.4</span> Compute Compliance Matrix</h3>
<div class="outline-text-3" id="text-8-4">
<div class="org-src-container">
<pre class="src src-matlab">stewart.C = inv(stewart.K);
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-orgb6aa2e4" class="outline-2">
<h2 id="orgb6aa2e4"><span class="section-number-2">9</span> <code>inverseKinematics</code>: Compute Inverse Kinematics</h2>
<div class="outline-text-2" id="text-9">
<p>
<a id="org85d5414"></a>
</p>
<p>
This Matlab function is accessible <a href="src/inverseKinematics.m">here</a>.
</p>
</div>
<div id="outline-container-org1abf793" class="outline-3">
<h3 id="org1abf793"><span class="section-number-3">9.1</span> Function description</h3>
<div class="outline-text-3" id="text-9-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[Li, dLi]</span> = <span class="org-function-name">inverseKinematics</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [stewart] = inverseKinematics(stewart)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - stewart - A structure with the following fields</span>
<span class="org-comment">% - Aa [3x6] - The positions ai expressed in {A}</span>
<span class="org-comment">% - Bb [3x6] - The positions bi expressed in {B}</span>
<span class="org-comment">% - args - Can have the following fields:</span>
<span class="org-comment">% - AP [3x1] - The wanted position of {B} with respect to {A}</span>
<span class="org-comment">% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}</span>
<span class="org-comment">% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-orgae295b6" class="outline-3">
<h3 id="orgae295b6"><span class="section-number-3">9.2</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-9-2">
<div class="org-src-container">
<pre class="src src-matlab">arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-orgfd5d40a" class="outline-3">
<h3 id="orgfd5d40a"><span class="section-number-3">9.3</span> Theory</h3>
<div class="outline-text-3" id="text-9-3">
<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&rsquo; lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
</p>
</div>
</div>
<div id="outline-container-orgc7dd5e8" class="outline-3">
<h3 id="orgc7dd5e8"><span class="section-number-3">9.4</span> Compute</h3>
<div class="outline-text-3" id="text-9-4">
<div class="org-src-container">
<pre class="src src-matlab">Li = sqrt(args.AP<span class="org-type">'*</span>args.AP <span class="org-type">+</span> diag(stewart.Bb<span class="org-type">'*</span>stewart.Bb) <span class="org-type">+</span> diag(stewart.Aa<span class="org-type">'*</span>stewart.Aa) <span class="org-type">-</span> (2<span class="org-type">*</span>args.AP<span class="org-type">'*</span>stewart.Aa)<span class="org-type">'</span> <span class="org-type">+</span> (2<span class="org-type">*</span>args.AP<span class="org-type">'*</span>(args.ARB<span class="org-type">*</span>stewart.Bb))<span class="org-type">'</span> <span class="org-type">-</span> diag(2<span class="org-type">*</span>(args.ARB<span class="org-type">*</span>stewart.Bb)<span class="org-type">'*</span>stewart.Aa));
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">dLi = Li<span class="org-type">-</span>stewart.l;
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org689b179" class="outline-2">
<h2 id="org689b179"><span class="section-number-2">10</span> <code>forwardKinematicsApprox</code>: Compute the Forward Kinematics</h2>
<div class="outline-text-2" id="text-10">
<p>
<a id="org887d5a9"></a>
</p>
<p>
This Matlab function is accessible <a href="src/forwardKinematicsApprox.m">here</a>.
</p>
</div>
<div id="outline-container-orgba48270" class="outline-3">
<h3 id="orgba48270"><span class="section-number-3">10.1</span> Function description</h3>
<div class="outline-text-3" id="text-10-1">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[P, R]</span> = <span class="org-function-name">forwardKinematicsApprox</span>(<span class="org-variable-name">stewart</span>, <span class="org-variable-name">args</span>)
<span class="org-comment">% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using</span>
<span class="org-comment">% the Jacobian Matrix</span>
<span class="org-comment">%</span>
<span class="org-comment">% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)</span>
<span class="org-comment">%</span>
<span class="org-comment">% Inputs:</span>
<span class="org-comment">% - stewart - A structure with the following fields</span>
<span class="org-comment">% - J [6x6] - The Jacobian Matrix</span>
<span class="org-comment">% - args - Can have the following fields:</span>
<span class="org-comment">% - dL [6x1] - Displacement of each strut [m]</span>
<span class="org-comment">%</span>
<span class="org-comment">% Outputs:</span>
<span class="org-comment">% - P [3x1] - The estimated position of {B} with respect to {A}</span>
<span class="org-comment">% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org22e2134" class="outline-3">
<h3 id="org22e2134"><span class="section-number-3">10.2</span> Optional Parameters</h3>
<div class="outline-text-3" id="text-10-2">
<div class="org-src-container">
<pre class="src src-matlab">arguments
stewart
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
<span class="org-keyword">end</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-orgfa57f93" class="outline-3">
<h3 id="orgfa57f93"><span class="section-number-3">10.3</span> Computation</h3>
<div class="outline-text-3" id="text-10-3">
<p>
From a small displacement of each strut \(d\bm{\mathcal{L}}\), we can compute the
position and orientation of {B} with respect to {A} using the following formula:
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
</p>
<div class="org-src-container">
<pre class="src src-matlab">X = stewart.J<span class="org-type">\</span>args.dL;
</pre>
</div>
<p>
The position vector corresponds to the first 3 elements.
</p>
<div class="org-src-container">
<pre class="src src-matlab">P = X(1<span class="org-type">:</span>3);
</pre>
</div>
<p>
The next 3 elements are the orientation of {B} with respect to {A} expressed
using the screw axis.
</p>
<div class="org-src-container">
<pre class="src src-matlab">theta = norm(X(4<span class="org-type">:</span>6));
s = X(4<span class="org-type">:</span>6)<span class="org-type">/</span>theta;
</pre>
</div>
<p>
We then compute the corresponding rotation matrix.
</p>
<div class="org-src-container">
<pre class="src src-matlab">R = [s(1)<span class="org-type">^</span>2<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">+</span> cos(theta) , s(1)<span class="org-type">*</span>s(2)<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">-</span> s(3)<span class="org-type">*</span>sin(theta), s(1)<span class="org-type">*</span>s(3)<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">+</span> s(2)<span class="org-type">*</span>sin(theta);
s<span class="org-type">(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);</span>
s<span class="org-type">(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];</span>
</pre>
</div>
</div>
</div>
</div>
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2020-01-22 mer. 11:35</p> <p class="date">Created: 2020-01-27 lun. 17:41</p>
</div> </div>
</body> </body>
</html> </html>

1593
simscape-model.org
View File

File diff suppressed because it is too large Load Diff

BIN
simscape_subsystems/stewart_strut_no_sensors.slx
View File

Binary file not shown.

BIN
simulink/stewart_platform_identification.slx Normal file
View File

Binary file not shown.

BIN
simulink/stewart_platform_identification_simple.slx Normal file
View File

Binary file not shown.

4
src/computeJointsPose.m
View File

@ -1,10 +1,12 @@
function [stewart] = computeJointsPose(stewart) function [stewart] = computeJointsPose(stewart)
% computeJointsPose - % computeJointsPose -
% %
% Syntax: [stewart] = computeJointsPose(stewart, opts_param) % Syntax: [stewart] = computeJointsPose(stewart)
% %
% Inputs: % Inputs:
% - stewart - A structure with the following fields % - stewart - A structure with the following fields
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - FO_A [3x1] - Position of {A} with respect to {F} % - FO_A [3x1] - Position of {A} with respect to {F}
% - MO_B [3x1] - Position of {B} with respect to {M} % - MO_B [3x1] - Position of {B} with respect to {M}
% - FO_M [3x1] - Position of {M} with respect to {F} % - FO_M [3x1] - Position of {M} with respect to {F}

30
src/forwardKinematics.m
View File

@ -1,30 +0,0 @@
function [P, R] = forwardKinematics(stewart, args)
% forwardKinematics - Computed the pose of {B} with respect to {A} from the length of each strut
%
% Syntax: [in_data] = forwardKinematics(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - J [6x6] - The Jacobian Matrix
% - args - Can have the following fields:
% - L [6x1] - Length of each strut [m]
%
% Outputs:
% - P [3x1] - The estimated position of {B} with respect to {A}
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
arguments
stewart
args.L (6,1) double {mustBeNumeric} = zeros(6,1)
end
X = stewart.J\args.L;
P = X(1:3);
theta = norm(X(4:6));
s = X(4:6)/theta;
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];

6
src/generateGeneralConfiguration.m
View File

@ -4,8 +4,6 @@ function [stewart] = generateGeneralConfiguration(stewart, args)
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args) % Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
% %
% Inputs: % Inputs:
% - stewart - A structure with the following fields
% - H [1x1] - Total height of the platform [m]
% - args - Can have the following fields: % - args - Can have the following fields:
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] % - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] % - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
@ -22,10 +20,10 @@ function [stewart] = generateGeneralConfiguration(stewart, args)
arguments arguments
stewart stewart
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e-3; args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
end end

18
src/getMaxPositions.m
View File

@ -1,18 +0,0 @@
function [X, Y, Z] = getMaxPositions(stewart)
Leg = stewart.Leg;
J = stewart.Jd;
theta = linspace(0, 2*pi, 100);
phi = linspace(-pi/2 , pi/2, 100);
dmax = zeros(length(theta), length(phi));
for i = 1:length(theta)
for j = 1:length(phi)
L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
dmax(i, j) = Leg.stroke/max(abs(L));
end
end
X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
Z = dmax.*sin(repmat(phi,length(theta),1));
end

9
src/getMaxPureDisplacement.m
View File

@ -1,9 +0,0 @@
function [max_disp] = getMaxPureDisplacement(Leg, J)
max_disp = zeros(6, 1);
max_disp(1) = Leg.stroke/max(abs(J*[1 0 0 0 0 0]'));
max_disp(2) = Leg.stroke/max(abs(J*[0 1 0 0 0 0]'));
max_disp(3) = Leg.stroke/max(abs(J*[0 0 1 0 0 0]'));
max_disp(4) = Leg.stroke/max(abs(J*[0 0 0 1 0 0]'));
max_disp(5) = Leg.stroke/max(abs(J*[0 0 0 0 1 0]'));
max_disp(6) = Leg.stroke/max(abs(J*[0 0 0 0 0 1]'));
end

5
src/getStiffnessMatrix.m
View File

@ -1,5 +0,0 @@
function [K] = getStiffnessMatrix(k, J)
% k - leg stiffness
% J - Jacobian matrix
K = k*(J'*J);
end

67
src/identifyPlant.m
View File

@ -1,67 +0,0 @@
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:1]]
function [sys] = identifyPlant(opts_param)
% identifyPlant:1 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:2]]
%% Default values for opts
opts = struct();
%% Populate opts with input parameters
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
% identifyPlant:2 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:3]]
options = linearizeOptions;
options.SampleTime = 0;
% identifyPlant:3 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:4]]
mdl = 'stewart';
% identifyPlant:4 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:5]]
%% Inputs
io(1) = linio([mdl, '/F'], 1, 'input'); % Cartesian forces
io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces
io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces
io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion
%% Outputs
io(5) = linio([mdl, '/Dm'], 1, 'output'); % Relative Motion
io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg
io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg
io(8) = linio([mdl, '/Xm'], 1, 'output'); % Absolute motion of platform
% identifyPlant:5 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:6]]
G = linearize(mdl, io, 0);
% identifyPlant:6 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:7]]
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ...
'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'};
G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ...
'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ...
'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ...
'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'};
% identifyPlant:7 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:8]]
sys.G_cart = G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'});
sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
sys.G_legs = minreal(G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
sys.G_tran = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}));
sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
% sys.G_all = minreal(G);
% identifyPlant:8 ends here
% [[file:~/MEGA/These/Matlab/Simscape/stewart-simscape/identification.org::*identifyPlant][identifyPlant:9]]
end
% identifyPlant:9 ends here

171
src/initializeHexapod.m
View File

@ -1,171 +0,0 @@
function [stewart] = initializeHexapod(opts_param)
opts = struct(...
'height', 90, ... % Height of the platform [mm]
'density', 10, ... % Density of the material used for the hexapod [kg/m3]
'k_ax', 1e8, ... % Stiffness of each actuator [N/m]
'c_ax', 1000, ... % Damping of each actuator [N/(m/s)]
'stroke', 50e-6, ... % Maximum stroke of each actuator [m]
'name', 'stewart' ... % Name of the file
);
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
stewart = struct();
stewart.H = opts.height; % [mm]
BP = struct();
BP.Rint = 0; % Internal Radius [mm]
BP.Rext = 150; % External Radius [mm]
BP.H = 10; % Thickness of the Bottom Plate [mm]
BP.Rleg = 100; % Radius where the legs articulations are positionned [mm]
BP.alpha = 30; % Angle Offset [deg]
BP.density = opts.density; % Density of the material [kg/m3]
BP.color = [0.7 0.7 0.7]; % Color [RGB]
BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm]
stewart.BP = BP;
TP = struct();
TP.Rint = 0; % [mm]
TP.Rext = 100; % [mm]
TP.H = 10; % [mm]
TP.Rleg = 80; % Radius where the legs articulations are positionned [mm]
TP.alpha = 10; % Angle [deg]
TP.dalpha = 0; % Angle Offset from 0 position [deg]
TP.density = opts.density; % Density of the material [kg/m3]
TP.color = [0.7 0.7 0.7]; % Color [RGB]
TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H];
stewart.TP = TP;
Leg = struct();
Leg.stroke = opts.stroke; % [m]
Leg.k_ax = opts.k_ax; % Stiffness of each leg [N/m]
Leg.c_ax = opts.c_ax; % Damping of each leg [N/(m/s)]
Leg.Rtop = 10; % Radius of the cylinder of the top part of the leg[mm]
Leg.Rbot = 12; % Radius of the cylinder of the bottom part of the leg [mm]
Leg.density = 0.01*opts.density; % Density of the material used for the legs [kg/m3]
Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB]
Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm]
stewart.Leg = Leg;
SP = struct();
SP.k = 0; % [N*m/deg]
SP.c = 0; % [N*m/deg]
SP.H = 15; % [mm]
SP.R = Leg.R; % [mm]
SP.section = [0 SP.H-SP.R;
0 0;
SP.R 0;
SP.R SP.H];
SP.density = opts.density; % [kg/m^3]
SP.color = [0.7 0.7 0.7]; % [RGB]
stewart.SP = SP;
stewart = initializeParameters(stewart);
save('./mat/stewart.mat', 'stewart')
function [stewart] = initializeParameters(stewart)
stewart.Aa = zeros(6, 3); % [mm]
stewart.Ab = zeros(6, 3); % [mm]
stewart.Bb = zeros(6, 3); % [mm]
for i = 1:3
stewart.Aa(2*i-1,:) = [stewart.BP.Rleg*cos( pi/180*(120*(i-1) - stewart.BP.alpha) ), ...
stewart.BP.Rleg*sin( pi/180*(120*(i-1) - stewart.BP.alpha) ), ...
stewart.BP.H+stewart.SP.H];
stewart.Aa(2*i,:) = [stewart.BP.Rleg*cos( pi/180*(120*(i-1) + stewart.BP.alpha) ), ...
stewart.BP.Rleg*sin( pi/180*(120*(i-1) + stewart.BP.alpha) ), ...
stewart.BP.H+stewart.SP.H];
stewart.Ab(2*i-1,:) = [stewart.TP.Rleg*cos( pi/180*(120*(i-1) + stewart.TP.dalpha - stewart.TP.alpha) ), ...
stewart.TP.Rleg*sin( pi/180*(120*(i-1) + stewart.TP.dalpha - stewart.TP.alpha) ), ...
stewart.H - stewart.TP.H - stewart.SP.H];
stewart.Ab(2*i,:) = [stewart.TP.Rleg*cos( pi/180*(120*(i-1) + stewart.TP.dalpha + stewart.TP.alpha) ), ...
stewart.TP.Rleg*sin( pi/180*(120*(i-1) + stewart.TP.dalpha + stewart.TP.alpha) ), ...
stewart.H - stewart.TP.H - stewart.SP.H];
end
stewart.Bb = stewart.Ab - stewart.H*[0,0,1];
leg_length = zeros(6, 1); % [mm]
leg_vectors = zeros(6, 3);
legs = stewart.Ab - stewart.Aa;
for i = 1:6
leg_length(i) = norm(legs(i,:));
leg_vectors(i,:) = legs(i,:) / leg_length(i);
end
stewart.Leg.lenght = leg_length(1)/1.5;
stewart.Leg.shape.bot = ...
[0 0; ...
stewart.Leg.Rbot 0; ...
stewart.Leg.Rbot stewart.Leg.lenght; ...
stewart.Leg.Rtop stewart.Leg.lenght; ...
stewart.Leg.Rtop 0.2*stewart.Leg.lenght; ...
0 0.2*stewart.Leg.lenght];
stewart.Rm = struct('R', eye(3));
for i = 1:6
sx = cross(leg_vectors(i,:), [1 0 0]);
sx = sx/norm(sx);
sy = -cross(sx, leg_vectors(i,:));
sy = sy/norm(sy);
sz = leg_vectors(i,:);
sz = sz/norm(sz);
stewart.Rm(i).R = [sx', sy', sz'];
end
J = zeros(6);
for i = 1:6
J(i, 1:3) = leg_vectors(i, :);
J(i, 4:6) = cross(0.001*(stewart.Ab(i, :)- stewart.H*[0,0,1]), leg_vectors(i, :));
end
stewart.J = J;
stewart.Jinv = inv(J);
stewart.K = stewart.Leg.k_ax*stewart.J'*stewart.J;
end
end

59
src/initializeSimscapeData.m
View File

@ -1,59 +0,0 @@
function [stewart] = initializeSimscapeData(stewart, opts_param)
opts = struct(...
'Jd_pos', [0, 0, 30], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]
'Jf_pos', [0, 0, 30] ... % Position of the Jacobian for force location from the top of the mobile platform [mm]
);
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
leg_length = zeros(6, 1); % [mm]
leg_vectors = zeros(6, 3);
legs = stewart.Ab - stewart.Aa;
for i = 1:6
leg_length(i) = norm(legs(i,:));
leg_vectors(i,:) = legs(i,:) / leg_length(i);
end
stewart.Rm = struct('R', eye(3));
for i = 1:6
sx = cross(leg_vectors(i,:), [1 0 0]);
sx = sx/norm(sx);
sy = -cross(sx, leg_vectors(i,:));
sy = sy/norm(sy);
sz = leg_vectors(i,:);
sz = sz/norm(sz);
stewart.Rm(i).R = [sx', sy', sz'];
end
Jd = zeros(6);
for i = 1:6
Jd(i, 1:3) = leg_vectors(i, :);
Jd(i, 4:6) = cross(0.001*(stewart.Bb(i, :) - opts.Jd_pos), leg_vectors(i, :));
end
stewart.Jd = Jd;
stewart.Jd_inv = inv(Jd);
Jf = zeros(6);
for i = 1:6
Jf(i, 1:3) = leg_vectors(i, :);
Jf(i, 4:6) = cross(0.001*(stewart.Bb(i, :) - opts.Jf_pos), leg_vectors(i, :));
end
stewart.Jf = Jf;
stewart.Jf_inv = inv(Jf);
end

94
src/initializeStewartPlatform.m
View File

@ -1,94 +0,0 @@
function [stewart] = initializeStewartPlatform(stewart, opts_param)
opts = struct(...
'thickness', 10, ... % Thickness of the base and platform [mm]
'density', 1000, ... % Density of the material used for the hexapod [kg/m3]
'k_ax', 1e8, ... % Stiffness of each actuator [N/m]
'c_ax', 1000, ... % Damping of each actuator [N/(m/s)]
'stroke', 50e-6 ... % Maximum stroke of each actuator [m]
);
if exist('opts_param','var')
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
end
BP = struct();
BP.Rint = 0; % Internal Radius [mm]
BP.Rext = 150; % External Radius [mm]
BP.H = opts.thickness; % Thickness of the Bottom Plate [mm]
BP.density = opts.density; % Density of the material [kg/m3]
BP.color = [0.7 0.7 0.7]; % Color [RGB]
BP.shape = [BP.Rint BP.H; BP.Rint 0; BP.Rext 0; BP.Rext BP.H]; % [mm]
stewart.BP = BP;
TP = struct();
TP.Rint = 0; % [mm]
TP.Rext = 100; % [mm]
TP.H = 10; % [mm]
TP.density = opts.density; % Density of the material [kg/m3]
TP.color = [0.7 0.7 0.7]; % Color [RGB]
TP.shape = [TP.Rint TP.H; TP.Rint 0; TP.Rext 0; TP.Rext TP.H];
stewart.TP = TP;
Leg = struct();
Leg.stroke = opts.stroke; % [m]
Leg.k_ax = opts.k_ax; % Stiffness of each leg [N/m]
Leg.c_ax = opts.c_ax; % Damping of each leg [N/(m/s)]
Leg.Rtop = 10; % Radius of the cylinder of the top part of the leg[mm]
Leg.Rbot = 12; % Radius of the cylinder of the bottom part of the leg [mm]
Leg.density = opts.density; % Density of the material used for the legs [kg/m3]
Leg.color = [0.5 0.5 0.5]; % Color of the top part of the leg [RGB]
Leg.R = 1.3*Leg.Rbot; % Size of the sphere at the extremity of the leg [mm]
legs = stewart.Ab - stewart.Aa;
Leg.lenght = norm(legs(1,:))/1.5;
Leg.shape.bot = ...
[0 0; ...
Leg.Rbot 0; ...
Leg.Rbot Leg.lenght; ...
Leg.Rtop Leg.lenght; ...
Leg.Rtop 0.2*Leg.lenght; ...
0 0.2*Leg.lenght];
stewart.Leg = Leg;
SP = struct();
SP.k = 0; % [N*m/deg]
SP.c = 0; % [N*m/deg]
SP.H = stewart.Aa(1, 3) - BP.H; % [mm]
SP.R = Leg.R; % [mm]
SP.section = [0 SP.H-SP.R;
0 0;
SP.R 0;
SP.R SP.H];
SP.density = opts.density; % [kg/m^3]
SP.color = [0.7 0.7 0.7]; % [RGB]
stewart.SP = SP;

2
src/initializeStrutDynamics.m
View File

@ -16,7 +16,7 @@ function [stewart] = initializeStrutDynamics(stewart, args)
arguments arguments
stewart stewart
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1) args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1) args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1)
end end
stewart.Ki = args.Ki; stewart.Ki = args.Ki;

87
static-analysis.org Normal file
View File

@ -0,0 +1,87 @@
#+TITLE: Stewart Platform - Static Analysis
:DRAWER:
#+HTML_LINK_HOME: ./index.html
#+HTML_LINK_UP: ./index.html
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
#+PROPERTY: header-args:matlab :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs
:END:
* Jacobian
** Relation to platform parameters
A Jacobian is defined by:
- the orientations of the struts $\hat{s}_i$ expressed in a frame $\{A\}$ linked to the fixed platform.
- the vectors from $O_B$ to $b_i$ expressed in the frame $\{A\}$
Then, the choice of $O_B$ changes the Jacobian.
** Jacobian for displacement
\[ \dot{q} = J \dot{X} \]
With:
- $q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]$ vector of linear displacement of actuated joints
- $X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]$ position and orientation of $O_B$ expressed in the frame $\{A\}$
For very small displacements $\delta q$ and $\delta X$, we have $\delta q = J \delta X$.
** Jacobian for forces
\[ F = J^T \tau \]
With:
- $\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]$ vector of actuator forces
- $F = [f_x\ f_y\ f_z\ n_x\ n_y\ n_z]$ force and torque acting on point $O_B$
* Stiffness matrix $K$
\[ K = J^T \text{diag}(k_i) J \]
If all the struts have the same stiffness $k$, then $K = k J^T J$
$K$ only depends of the geometry of the stewart platform: it depends on the Jacobian, that is on the orientations of the struts, position of the joints and choice of frame $\{B\}$.
\[ F = K X \]
With $F$ forces and torques applied to the moving platform at the origin of $\{B\}$ and $X$ the translations and rotations of $\{B\}$ with respect to $\{A\}$.
\[ C = K^{-1} \]
The compliance element $C_{ij}$ is then the stiffness
\[ X_i = C_{ij} F_j \]
* Coupling
What causes the coupling from $F_i$ to $X_i$ ?
#+begin_src latex :file coupling.pdf :post pdf2svg(file=*this*, ext="png") :exports both
\begin{tikzpicture}
\node[block] (Jt) at (0, 0) {$J^{-T}$};
\node[block, right= of Jt] (G) {$G$};
\node[block, right= of G] (J) {$J^{-1}$};
\draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$};
\draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$};
\draw[->] (G.east) -- (J.west) node[above left]{$q_i$};
\draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$};
\end{tikzpicture}
#+end_src
#+name: fig:block_diag_coupling
#+caption: Block diagram to control an hexapod
#+RESULTS:
[[file:figs/coupling.png]]
There is no coupling from $F_i$ to $X_j$ if $J^{-1} G J^{-T}$ is diagonal.
If $G$ is diagonal (cubic configuration), then $J^{-1} G J^{-T} = G J^{-1} J^{-T} = G (J^{T} J)^{-1} = G K^{-1}$
Thus, the system is uncoupled if $G$ and $K$ are diagonal.

1622
stewart-architecture.html Normal file
View File

File diff suppressed because it is too large Load Diff

1615
stewart-architecture.org Normal file
View File

File diff suppressed because it is too large Load Diff