Reworked index.org: better filenames
Removed few unused functions
This commit is contained in:
parent
3e4816929f
commit
950302e5d6
@ -1,14 +1,15 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2019-12-12 jeu. 20:10 -->
|
||||
<!-- 2020-01-27 lun. 17:41 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Cubic configuration for the Stewart Platform</title>
|
||||
<meta name="generator" content="Org mode" />
|
||||
<meta name="author" content="Thomas Dehaeze" />
|
||||
<meta name="author" content="Dehaeze Thomas" />
|
||||
<style type="text/css">
|
||||
<!--/*--><![CDATA[/*><!--*/
|
||||
.title { text-align: center;
|
||||
@ -193,14 +194,18 @@
|
||||
.org-svg { width: 90%; }
|
||||
/*]]>*/-->
|
||||
</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/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">
|
||||
/*
|
||||
@licstart The following is the entire license notice for the
|
||||
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
|
||||
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>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#org4a16be2">1. Questions we wish to answer with this analysis</a></li>
|
||||
<li><a href="#org289931f">2. Configuration Analysis - Stiffness Matrix</a>
|
||||
<li><a href="#org86c83bf">1. Questions we wish to answer with this analysis</a></li>
|
||||
<li><a href="#org0b05973">2. <span class="todo TODO">TODO</span> Configuration Analysis - Stiffness Matrix</a>
|
||||
<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="#org608174e">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="#org6fbeda1">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="#org3f035e8">2.1. Cubic Stewart platform centered with the cube center - Jacobian 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="#org42ea8ad">2.3. Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube 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="#org08c7461">2.5. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgf0ba2d0">3. Cubic size analysis</a></li>
|
||||
<li><a href="#org97dffbc">4. initializeCubicConfiguration</a>
|
||||
<li><a href="#orgc4c2abd">3. <span class="todo TODO">TODO</span> Cubic size analysis</a></li>
|
||||
<li><a href="#org36a27e6">4. <span class="todo TODO">TODO</span> initializeCubicConfiguration</a>
|
||||
<ul>
|
||||
<li><a href="#org4eb8b23">4.1. Function description</a></li>
|
||||
<li><a href="#orga42cb17">4.2. Optional Parameters</a></li>
|
||||
<li><a href="#orgc281f60">4.3. Cube Creation</a></li>
|
||||
<li><a href="#orgfed01f0">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="#org9578c3c">4.6. Determinate the location of the joints</a></li>
|
||||
<li><a href="#org71c9d4e">4.7. Returns Stewart Structure</a></li>
|
||||
<li><a href="#orgf299c5c">4.1. Function description</a></li>
|
||||
<li><a href="#org46c8589">4.2. Optional Parameters</a></li>
|
||||
<li><a href="#orgd8d9b14">4.3. Cube Creation</a></li>
|
||||
<li><a href="#org181d1d8">4.4. Vectors of each leg</a></li>
|
||||
<li><a href="#orgb396e98">4.5. Verification of Height of the Stewart Platform</a></li>
|
||||
<li><a href="#orgf38af83">4.6. Determinate the location of the joints</a></li>
|
||||
<li><a href="#orgdf9e3cf">4.7. Returns Stewart Structure</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgb2d1742">5. Tests</a>
|
||||
<li><a href="#orgf8fb731">5. <span class="todo TODO">TODO</span> Tests</a>
|
||||
<ul>
|
||||
<li><a href="#org6e933c9">5.1. First attempt to parametrisation</a></li>
|
||||
<li><a href="#org60486ce">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="#org4434fe5">5.1. First attempt to parametrisation</a></li>
|
||||
<li><a href="#org723e6eb">5.2. Second attempt</a></li>
|
||||
<li><a href="#orgcc173ac">5.3. Generate the Stewart platform for a Cubic configuration</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
@ -326,15 +331,15 @@ The specificity of the Cubic configuration is that each actuator is orthogonal w
|
||||
</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>
|
||||
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>
|
||||
|
||||
<div id="outline-container-org4a16be2" class="outline-2">
|
||||
<h2 id="org4a16be2"><span class="section-number-2">1</span> Questions we wish to answer with this analysis</h2>
|
||||
<div id="outline-container-org86c83bf" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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 id="outline-container-org289931f" class="outline-2">
|
||||
<h2 id="org289931f"><span class="section-number-2">2</span> Configuration Analysis - Stiffness Matrix</h2>
|
||||
<div id="outline-container-org0b05973" class="outline-2">
|
||||
<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>
|
||||
<div id="outline-container-orgc378f8a" 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>
|
||||
<div id="outline-container-org3f035e8" class="outline-3">
|
||||
<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">
|
||||
<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.
|
||||
</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>
|
||||
<p><span class="figure-number">Figure 1: </span>Centered cubic configuration</p>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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">'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">'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-rainbow-delimiters-depth-1">)</span>;
|
||||
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||||
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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-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>;
|
||||
<pre class="src src-matlab">opts = struct(...
|
||||
<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>, 200<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [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>, 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>
|
||||
);
|
||||
stewart = initializeCubicConfiguration(opts);
|
||||
opts = struct(...
|
||||
<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>, [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>
|
||||
);
|
||||
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>
|
||||
</div>
|
||||
|
||||
@ -464,27 +469,27 @@ save<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-string
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org608174e" 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>
|
||||
<div id="outline-container-org77ecb36" class="outline-3">
|
||||
<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">
|
||||
<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.
|
||||
</p>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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">'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">'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-rainbow-delimiters-depth-1">)</span>;
|
||||
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||||
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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-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>;
|
||||
<pre class="src src-matlab">opts = struct(...
|
||||
<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>, 200<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [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>, 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>
|
||||
);
|
||||
stewart = initializeCubicConfiguration(opts);
|
||||
opts = struct(...
|
||||
<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>, [0, 0, 0] ...<span class="org-comment"> % Position of the Jacobian for force location from the top of the mobile platform [mm]</span>
|
||||
);
|
||||
stewart = computeGeometricalProperties(stewart, opts);
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
@ -568,16 +573,16 @@ stewart = computeGeometricalProperties<span class="org-rainbow-delimiters-depth-
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbd736ef" 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>
|
||||
<div id="outline-container-org42ea8ad" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
Here, the "center" of the Stewart platform is not at the cube center (figure <a href="#org3982eac">2</a>).
|
||||
Here, the “center” of the Stewart platform is not at the cube center (figure <a href="#org3f10bc2">2</a>).
|
||||
The Jacobian is estimated at the cube center.
|
||||
</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>
|
||||
<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>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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">'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">'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-rainbow-delimiters-depth-1">)</span>;
|
||||
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||||
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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-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>;
|
||||
<pre class="src src-matlab">opts = struct(...
|
||||
<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>, 220<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [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>, 75 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
|
||||
);
|
||||
stewart = initializeCubicConfiguration(opts);
|
||||
opts = struct(...
|
||||
<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>, [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>
|
||||
);
|
||||
stewart = computeGeometricalProperties(stewart, opts);
|
||||
</pre>
|
||||
</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 id="outline-container-org6fbeda1" 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>
|
||||
<div id="outline-container-org38870ce" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
Here, the "center" of the Stewart platform is not at the cube center.
|
||||
Here, the “center” of the Stewart platform is not at the cube center.
|
||||
The Jacobian is estimated at the center of the Stewart platform.
|
||||
</p>
|
||||
|
||||
@ -706,18 +711,18 @@ The center of the cube from the top platform is at \(z = 110 - 175 = -65\).
|
||||
</p>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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">'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">'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-rainbow-delimiters-depth-1">)</span>;
|
||||
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||||
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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-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>;
|
||||
<pre class="src src-matlab">opts = struct(...
|
||||
<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>, 220<span class="org-type">/</span>sqrt(3), ...<span class="org-comment"> % Size of the Cube [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>, 75 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
|
||||
);
|
||||
stewart = initializeCubicConfiguration(opts);
|
||||
opts = struct(...
|
||||
<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>, [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>
|
||||
);
|
||||
stewart = computeGeometricalProperties(stewart, opts);
|
||||
</pre>
|
||||
</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 id="outline-container-org18633d3" class="outline-3">
|
||||
<h3 id="org18633d3"><span class="section-number-3">2.5</span> Conclusion</h3>
|
||||
<div id="outline-container-org08c7461" class="outline-3">
|
||||
<h3 id="org08c7461"><span class="section-number-3">2.5</span> Conclusion</h3>
|
||||
<div class="outline-text-3" id="text-2-5">
|
||||
<div class="important">
|
||||
<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 id="outline-container-orgf0ba2d0" class="outline-2">
|
||||
<h2 id="orgf0ba2d0"><span class="section-number-2">3</span> Cubic size analysis</h2>
|
||||
<div id="outline-container-orgc4c2abd" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
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>
|
||||
|
||||
<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>;
|
||||
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>;
|
||||
<pre class="src src-matlab">H_cubes = 250<span class="org-type">:</span>20<span class="org-type">:</span>350;
|
||||
stewarts = {zeros(length(H_cubes), 1)};
|
||||
</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"><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>
|
||||
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_tot = <span class="org-highlight-numbers-number">100</span>;
|
||||
H = <span class="org-highlight-numbers-number">80</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-constant">i</span>);
|
||||
H_tot = 100;
|
||||
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">'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">'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-rainbow-delimiters-depth-1">)</span>;
|
||||
stewart = initializeCubicConfiguration<span class="org-rainbow-delimiters-depth-1">(</span>opts<span class="org-rainbow-delimiters-depth-1">)</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>
|
||||
);
|
||||
stewart = initializeCubicConfiguration(opts);
|
||||
|
||||
opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'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-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>;
|
||||
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>;
|
||||
opts = struct(...
|
||||
<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>, [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>
|
||||
);
|
||||
stewart = computeGeometricalProperties(stewart, opts);
|
||||
stewarts(<span class="org-constant">i</span>) = {stewart};
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</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.
|
||||
</p>
|
||||
<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>;
|
||||
<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>
|
||||
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;
|
||||
<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">1:length(H_cubes)</span>
|
||||
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>
|
||||
</pre>
|
||||
</div>
|
||||
@ -886,16 +891,16 @@ Finally, we plot \(k_{\theta_x} = k_{\theta_y}\) and \(k_{\theta_z}\)
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
||||
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<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(4, 4, <span class="org-type">:</span>)), <span class="org-string">'DisplayName'</span>, <span class="org-string">'$k_{\theta_x}$'</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;
|
||||
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>;
|
||||
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>;
|
||||
legend(<span class="org-string">'location'</span>, <span class="org-string">'northwest'</span>);
|
||||
xlabel(<span class="org-string">'Cube Size [mm]'</span>); ylabel(<span class="org-string">'Rotational stiffnes [normalized]'</span>);
|
||||
</pre>
|
||||
</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>
|
||||
<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 id="outline-container-org97dffbc" class="outline-2">
|
||||
<h2 id="org97dffbc"><span class="section-number-2">4</span> initializeCubicConfiguration</h2>
|
||||
<div id="outline-container-org36a27e6" class="outline-2">
|
||||
<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">
|
||||
<p>
|
||||
<a id="org38614bc"></a>
|
||||
<a id="org8b1f609"></a>
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4eb8b23" class="outline-3">
|
||||
<h3 id="org4eb8b23"><span class="section-number-3">4.1</span> Function description</h3>
|
||||
<div id="outline-container-orgf299c5c" class="outline-3">
|
||||
<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="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>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orga42cb17" class="outline-3">
|
||||
<h3 id="orga42cb17"><span class="section-number-3">4.2</span> Optional Parameters</h3>
|
||||
<div id="outline-container-org46c8589" class="outline-3">
|
||||
<h3 id="org46c8589"><span class="section-number-3">4.2</span> Optional Parameters</h3>
|
||||
<div class="outline-text-3" id="text-4-2">
|
||||
<p>
|
||||
Default values for opts.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">opts = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
<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">'L'</span>, <span class="org-highlight-numbers-number">110</span>, ...<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">'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-rainbow-delimiters-depth-1">)</span>;
|
||||
<pre class="src src-matlab">opts = struct(...
|
||||
<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>, 110, ...<span class="org-comment"> % Size of the Cube [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>, 75 ...<span class="org-comment"> % Height between the corner of the cube and the plane containing the base joints [mm]</span>
|
||||
);
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
@ -954,9 +959,9 @@ Default values for opts.
|
||||
Populate opts with input parameters
|
||||
</p>
|
||||
<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>
|
||||
<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>
|
||||
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>;
|
||||
<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(opts_param)'</span>
|
||||
opts.(opt{1}) = opts_param.(opt{1});
|
||||
<span class="org-keyword">end</span>
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
@ -964,18 +969,18 @@ Populate opts with input parameters
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc281f60" class="outline-3">
|
||||
<h3 id="orgc281f60"><span class="section-number-3">4.3</span> Cube Creation</h3>
|
||||
<div id="outline-container-orgd8d9b14" class="outline-3">
|
||||
<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="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>; ...
|
||||
<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-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>; ...
|
||||
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>; ...
|
||||
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">0</span>; ...
|
||||
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</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-highlight-numbers-number">0</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>;
|
||||
<pre class="src src-matlab">points = [0, 0, 0; ...
|
||||
0, 0, 1; ...
|
||||
0, 1, 0; ...
|
||||
0, 1, 1; ...
|
||||
1, 0, 0; ...
|
||||
1, 0, 1; ...
|
||||
1, 1, 0; ...
|
||||
1, 1, 1];
|
||||
points = opts.L<span class="org-type">*</span>points;
|
||||
</pre>
|
||||
</div>
|
||||
@ -984,16 +989,16 @@ points = opts.L<span class="org-type">*</span>points;
|
||||
We create the rotation matrix to rotate the cube
|
||||
</p>
|
||||
<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>;
|
||||
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>;
|
||||
<pre class="src src-matlab">sx = cross([1, 1, 1], [1 0 0]);
|
||||
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 = 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 = <span class="org-type">-</span>cross(sx, [1, 1, 1]);
|
||||
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 = 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 = [1, 1, 1];
|
||||
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>
|
||||
</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
|
||||
</p>
|
||||
<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>;
|
||||
<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>
|
||||
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>;
|
||||
<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">1:size(points, 1)</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>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgfed01f0" class="outline-3">
|
||||
<h3 id="orgfed01f0"><span class="section-number-3">4.4</span> Vectors of each leg</h3>
|
||||
<div id="outline-container-org181d1d8" class="outline-3">
|
||||
<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="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>; ...
|
||||
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">4</span>; ...
|
||||
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">6</span>; ...
|
||||
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">6</span>; ...
|
||||
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">7</span>; ...
|
||||
<span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">7</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
<pre class="src src-matlab">leg_indices = [3, 4; ...
|
||||
2, 4; ...
|
||||
2, 6; ...
|
||||
5, 6; ...
|
||||
5, 7; ...
|
||||
3, 7];
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
@ -1027,50 +1032,50 @@ We use to rotation matrix to rotate the cube
|
||||
Vectors are:
|
||||
</p>
|
||||
<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>;
|
||||
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>;
|
||||
<pre class="src src-matlab">legs = zeros(6, 3);
|
||||
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>
|
||||
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_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>;
|
||||
<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-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-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>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org21db1ef" class="outline-3">
|
||||
<h3 id="org21db1ef"><span class="section-number-3">4.5</span> Verification of Height of the Stewart Platform</h3>
|
||||
<div id="outline-container-orgb396e98" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
If the Stewart platform is not contained in the cube, throw an error.
|
||||
</p>
|
||||
|
||||
<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>;
|
||||
<span class="org-keyword">if</span> opts.H0 <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>
|
||||
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>)));
|
||||
<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">></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>
|
||||
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<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>;
|
||||
<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"><</span> cube(2, 3)
|
||||
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">></span> cube(4, 3)
|
||||
error(sprintf(<span class="org-string">'H0+H is too high. Maximum H0+H = %.1f'</span>, cube(4, 3)));
|
||||
error(<span class="org-string">'H0+H is too high'</span>);
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org9578c3c" class="outline-3">
|
||||
<h3 id="org9578c3c"><span class="section-number-3">4.6</span> Determinate the location of the joints</h3>
|
||||
<div id="outline-container-orgf38af83" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
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.
|
||||
</p>
|
||||
<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>;
|
||||
<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>
|
||||
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>;
|
||||
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>;
|
||||
<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">1:6</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-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>
|
||||
</pre>
|
||||
</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\}\).
|
||||
</p>
|
||||
<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>;
|
||||
<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>
|
||||
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>;
|
||||
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>;
|
||||
<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">1:6</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-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>
|
||||
</pre>
|
||||
</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\}\).
|
||||
</p>
|
||||
<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>;
|
||||
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>;
|
||||
<pre class="src src-matlab">Bb = zeros(6, 3);
|
||||
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>
|
||||
</div>
|
||||
|
||||
<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>;
|
||||
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>;
|
||||
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>;
|
||||
<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>[0, 0, 1];
|
||||
Ab = Ab <span class="org-type">-</span> h<span class="org-type">*</span>[0, 0, 1];
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org71c9d4e" class="outline-3">
|
||||
<h3 id="org71c9d4e"><span class="section-number-3">4.7</span> Returns Stewart Structure</h3>
|
||||
<div id="outline-container-orgdf9e3cf" class="outline-3">
|
||||
<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="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.Ab = Ab;
|
||||
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 id="outline-container-orgb2d1742" class="outline-2">
|
||||
<h2 id="orgb2d1742"><span class="section-number-2">5</span> Tests</h2>
|
||||
<div id="outline-container-orgf8fb731" class="outline-2">
|
||||
<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>
|
||||
<div id="outline-container-org6e933c9" class="outline-3">
|
||||
<h3 id="org6e933c9"><span class="section-number-3">5.1</span> First attempt to parametrisation</h3>
|
||||
<div id="outline-container-org4434fe5" class="outline-3">
|
||||
<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 id="org94bcd9c" class="figure">
|
||||
<div id="org8dfcb96" class="figure">
|
||||
<p><img src="./figs/stewart_bottom_plate.png" alt="stewart_bottom_plate.png" />
|
||||
</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 id="outline-container-org60486ce" class="outline-3">
|
||||
<h3 id="org60486ce"><span class="section-number-3">5.2</span> Second attempt</h3>
|
||||
<div id="outline-container-org723e6eb" class="outline-3">
|
||||
<h3 id="org723e6eb"><span class="section-number-3">5.2</span> Second attempt</h3>
|
||||
<div class="outline-text-3" id="text-5-2">
|
||||
<p>
|
||||
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>
|
||||
|
||||
<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>; ...
|
||||
<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-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>; ...
|
||||
<span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">1</span>; ...
|
||||
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</span>, <span class="org-highlight-numbers-number">0</span>; ...
|
||||
<span class="org-highlight-numbers-number">1</span>, <span class="org-highlight-numbers-number">0</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-highlight-numbers-number">0</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>;
|
||||
<pre class="src src-matlab">points = [0, 0, 0; ...
|
||||
0, 0, 1; ...
|
||||
0, 1, 0; ...
|
||||
0, 1, 1; ...
|
||||
1, 0, 0; ...
|
||||
1, 0, 1; ...
|
||||
1, 1, 0; ...
|
||||
1, 1, 1];
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<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>
|
||||
</div>
|
||||
|
||||
<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>;
|
||||
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>;
|
||||
<pre class="src src-matlab">sx = cross([1, 1, 1], [1 0 0]);
|
||||
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 = 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 = <span class="org-type">-</span>cross(sx, [1, 1, 1]);
|
||||
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 = 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 = [1, 1, 1];
|
||||
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>
|
||||
</div>
|
||||
|
||||
<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>;
|
||||
<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>
|
||||
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>;
|
||||
<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">1:size(points, 1)</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>
|
||||
</pre>
|
||||
</div>
|
||||
@ -1226,8 +1231,8 @@ R = <span class="org-rainbow-delimiters-depth-1">[</span>sx<span class="org-type
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
||||
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<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(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(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;
|
||||
</pre>
|
||||
</div>
|
||||
@ -1236,17 +1241,17 @@ hold off;
|
||||
Now we plot the legs of the hexapod.
|
||||
</p>
|
||||
<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>; ...
|
||||
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">4</span>; ...
|
||||
<span class="org-highlight-numbers-number">2</span>, <span class="org-highlight-numbers-number">6</span>; ...
|
||||
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">6</span>; ...
|
||||
<span class="org-highlight-numbers-number">5</span>, <span class="org-highlight-numbers-number">7</span>; ...
|
||||
<span class="org-highlight-numbers-number">3</span>, <span class="org-highlight-numbers-number">7</span><span class="org-rainbow-delimiters-depth-1">]</span>
|
||||
<pre class="src src-matlab">leg_indices = [3, 4; ...
|
||||
2, 4; ...
|
||||
2, 6; ...
|
||||
5, 6; ...
|
||||
5, 7; ...
|
||||
3, 7]
|
||||
|
||||
<span class="org-type">figure</span>;
|
||||
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>
|
||||
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>;
|
||||
<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(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>
|
||||
hold off;
|
||||
</pre>
|
||||
@ -1256,12 +1261,12 @@ hold off;
|
||||
Vectors are:
|
||||
</p>
|
||||
<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>;
|
||||
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>;
|
||||
<pre class="src src-matlab">legs = zeros(6, 3);
|
||||
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>
|
||||
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_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>
|
||||
<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-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-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>
|
||||
</pre>
|
||||
</div>
|
||||
@ -1271,40 +1276,40 @@ We now have the orientation of each leg.
|
||||
</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 “slice” changes something.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
Let's first estimate the maximum height of the Stewart platform.
|
||||
Let’s first estimate the maximum height of the Stewart platform.
|
||||
</p>
|
||||
<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>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Let's then estimate the middle position of the platform
|
||||
Let’s then estimate the middle position of the platform
|
||||
</p>
|
||||
<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>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orge571873" class="outline-3">
|
||||
<h3 id="orge571873"><span class="section-number-3">5.3</span> Generate the Stewart platform for a Cubic configuration</h3>
|
||||
<div id="outline-container-orgcc173ac" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
First we defined the height of the Hexapod.
|
||||
</p>
|
||||
<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>
|
||||
</div>
|
||||
|
||||
<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>
|
||||
</pre>
|
||||
</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.
|
||||
</p>
|
||||
<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>;
|
||||
<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>
|
||||
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>;
|
||||
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>;
|
||||
<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">1:6</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-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>
|
||||
</pre>
|
||||
</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
|
||||
</p>
|
||||
<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>;
|
||||
<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>
|
||||
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>;
|
||||
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>;
|
||||
<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">1:6</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-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>
|
||||
</pre>
|
||||
</div>
|
||||
@ -1339,13 +1344,13 @@ And we plot the legs.
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
||||
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>
|
||||
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>;
|
||||
<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([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>
|
||||
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>;
|
||||
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>;
|
||||
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>;
|
||||
xlim([<span class="org-type">-</span>1, 1]);
|
||||
ylim([<span class="org-type">-</span>1, 1]);
|
||||
zlim([0, 2]);
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
@ -1362,9 +1367,8 @@ zlim<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-rainbo
|
||||
</p>
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Thomas Dehaeze</p>
|
||||
<p class="date">Created: 2019-12-12 jeu. 20:10</p>
|
||||
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2020-01-27 lun. 17:41</p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@ -21,6 +21,32 @@
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
: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
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+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('./');
|
||||
#+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
|
||||
open stewart
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
The hexapod structure and Sample structure are initialized.
|
||||
#+begin_src matlab :results none
|
||||
stewart = initializeGeneralConfiguration();
|
||||
stewart = computeGeometricalProperties(stewart);
|
||||
stewart = initializeMechanicalElements(stewart);
|
||||
save('./mat/stewart.mat', 'stewart');
|
||||
|
||||
initializeSample();
|
||||
** Simscape Model
|
||||
#+begin_src matlab
|
||||
open 'stewart_identification_simple.slx'
|
||||
#+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
|
||||
|
||||
#+begin_src matlab :results none
|
||||
freqs = logspace(2, 4, 1000);
|
||||
** Identification
|
||||
#+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
|
||||
|
||||
* 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]');
|
||||
#+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
212
index.html
@ -1,14 +1,15 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2019-12-19 jeu. 15:14 -->
|
||||
<!-- 2020-01-27 lun. 17:40 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Stewart Platforms</title>
|
||||
<meta name="generator" content="Org mode" />
|
||||
<meta name="author" content="Thomas Dehaeze" />
|
||||
<meta name="author" content="Dehaeze Thomas" />
|
||||
<style type="text/css">
|
||||
<!--/*--><![CDATA[/*><!--*/
|
||||
.title { text-align: center;
|
||||
@ -204,7 +205,7 @@
|
||||
@licstart The following is the entire license notice for the
|
||||
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
|
||||
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 type="text/x-mathjax-config">
|
||||
MathJax.Hub.Config({
|
||||
displayAlign: "center",
|
||||
displayIndent: "0em",
|
||||
|
||||
"HTML-CSS": { scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
webFont: "TeX"
|
||||
},
|
||||
SVG: {scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
font: "TeX"},
|
||||
NativeMML: {scale: 100},
|
||||
TeX: { equationNumbers: {autoNumber: "AMS"},
|
||||
MultLineWidth: "85%",
|
||||
TagSide: "right",
|
||||
TagIndent: ".8em",
|
||||
Macros: {
|
||||
bm: ["{\\boldsymbol #1}",1],
|
||||
}
|
||||
}
|
||||
});
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="content">
|
||||
<h1 class="title">Stewart Platforms</h1>
|
||||
|
||||
<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>
|
||||
|
||||
<div id="outline-container-org9cd44e0" class="outline-2">
|
||||
<h2 id="org9cd44e0"><span class="section-number-2">1</span> Simscape Model</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 id="outline-container-orgf137e1a" class="outline-2">
|
||||
<h2 id="orgf137e1a"><span class="section-number-2">1</span> Simulink Project (<a href="simulink-project.html">link</a>)</h2>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org7a44762" class="outline-2">
|
||||
<h2 id="org7a44762"><span class="section-number-2">2</span> Architecture Study</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 id="outline-container-org67d7b56" class="outline-2">
|
||||
<h2 id="org67d7b56"><span class="section-number-2">2</span> Stewart Platform Architecture Definition (<a href="stewart-architecture.html">link</a>)</h2>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org77767cc" class="outline-2">
|
||||
<h2 id="org77767cc"><span class="section-number-2">3</span> Motion Control</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<ul class="org-ul">
|
||||
<li>Active Damping</li>
|
||||
<li>Inertial Control</li>
|
||||
<li>Decentralized Control</li>
|
||||
</ul>
|
||||
|
||||
<div id="outline-container-org740c1af" class="outline-2">
|
||||
<h2 id="org740c1af"><span class="section-number-2">3</span> Simscape Model of the Stewart Platform (<a href="simscape-model.html">link</a>)</h2>
|
||||
</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>
|
||||
<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">
|
||||
<li>the orientations of the struts \(\hat{s}_i\) expressed in a frame \(\{A\}\) linked to the fixed platform.</li>
|
||||
<li>the vectors from \(O_B\) to \(b_i\) expressed in the frame \(\{A\}\)</li>
|
||||
<li>Jacobian Analysis</li>
|
||||
<li>Stiffness Analysis</li>
|
||||
<li>Static Forces</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
Then, the choice of \(O_B\) changes the Jacobian.
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgcec2e05" class="outline-4">
|
||||
<h4 id="orgcec2e05"><span class="section-number-4">4.1.2</span> Jacobian for displacement</h4>
|
||||
<div class="outline-text-4" id="text-4-1-2">
|
||||
<p>
|
||||
\[ \dot{q} = J \dot{X} \]
|
||||
With:
|
||||
</p>
|
||||
<div id="outline-container-orgeae2d7c" class="outline-2">
|
||||
<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-2" id="text-5">
|
||||
<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>\(X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]\) position and orientation of \(O_B\) expressed in the frame \(\{A\}\)</li>
|
||||
<li>Extraction of State Space models</li>
|
||||
<li>Resonant Frequencies and Modal Damping</li>
|
||||
<li>Mode Shapes</li>
|
||||
</ul>
|
||||
|
||||
<p>
|
||||
For very small displacements \(\delta q\) and \(\delta X\), we have \(\delta q = J \delta X\).
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgbf33a4e" class="outline-4">
|
||||
<h4 id="orgbf33a4e"><span class="section-number-4">4.1.3</span> Jacobian for forces</h4>
|
||||
<div class="outline-text-4" id="text-4-1-3">
|
||||
<p>
|
||||
\[ F = J^T \tau \]
|
||||
With:
|
||||
</p>
|
||||
<div id="outline-container-org748e065" class="outline-2">
|
||||
<h2 id="org748e065"><span class="section-number-2">6</span> Active Damping (<a href="active-damping.html">link</a>)</h2>
|
||||
<div class="outline-text-2" id="text-6">
|
||||
<ul class="org-ul">
|
||||
<li>\(\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]\) vector of actuator forces</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>Inertial Sensor</li>
|
||||
<li>Force Sensor</li>
|
||||
<li>Relative Motion Sensor</li>
|
||||
</ul>
|
||||
</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 id="outline-container-org3710914" class="outline-3">
|
||||
<h3 id="org3710914"><span class="section-number-3">4.2</span> Stiffness matrix \(K\)</h3>
|
||||
<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 id="outline-container-org57e9202" class="outline-2">
|
||||
<h2 id="org57e9202"><span class="section-number-2">8</span> Cubic Configuration (<a href="cubic-configuration.html">link</a>)</h2>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgd5d2c08" class="outline-3">
|
||||
<h3 id="orgd5d2c08"><span class="section-number-3">4.3</span> Coupling</h3>
|
||||
<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>[->] (<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>[->] (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>[->] (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>[->] (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 id="outline-container-orgb6982d7" class="outline-2">
|
||||
<h2 id="orgb6982d7"><span class="section-number-2">9</span> Architecture Optimization</h2>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
|
||||
<h1 class='org-ref-bib-h1'>Bibliography</h1>
|
||||
<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>
|
||||
<a href="ref.bib">ref.bib</a>
|
||||
</p>
|
||||
</div>
|
||||
</body>
|
||||
|
||||
91
index.org
91
index.org
@ -22,89 +22,36 @@
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The goal here is to
|
||||
The goal here is to provide a Matlab/Simscape Toolbox to study Stewart platforms.
|
||||
|
||||
* Simscape Model of the Stewart Platform
|
||||
- [[file:simscape-model.org][Model of the Stewart Platform]]
|
||||
- [[file:identification.org][Identification of the Simscape Model]]
|
||||
* Simulink Project ([[file:simulink-project.org][link]])
|
||||
|
||||
* Architecture Study
|
||||
- [[file:kinematic-study.org][Kinematic Study]]
|
||||
- [[file:stiffness-study.org][Stiffness Matrix Study]]
|
||||
- Jacobian Study
|
||||
- [[file:cubic-configuration.org][Cubic Architecture]]
|
||||
* Stewart Platform Architecture Definition ([[file:stewart-architecture.org][link]])
|
||||
|
||||
* Motion Control
|
||||
- Active Damping
|
||||
- Inertial Control
|
||||
- Decentralized Control
|
||||
|
||||
* Notes about Stewart platforms :noexport:
|
||||
** Jacobian
|
||||
*** Relation to platform parameters
|
||||
A Jacobian is defined by:
|
||||
- the orientations of the struts $\hat{s}_i$ expressed in a frame $\{A\}$ linked to the fixed platform.
|
||||
- the vectors from $O_B$ to $b_i$ expressed in the frame $\{A\}$
|
||||
* Simscape Model of the Stewart Platform ([[file:simscape-model.org][link]])
|
||||
|
||||
Then, the choice of $O_B$ changes the Jacobian.
|
||||
|
||||
*** Jacobian for displacement
|
||||
\[ \dot{q} = J \dot{X} \]
|
||||
With:
|
||||
- $q = [q_1\ q_2\ q_3\ q_4\ q_5\ q_6]$ vector of linear displacement of actuated joints
|
||||
- $X = [x\ y\ z\ \theta_x\ \theta_y\ \theta_z]$ position and orientation of $O_B$ expressed in the frame $\{A\}$
|
||||
* Kinematic Analysis ([[file:kinematic-study.org][link]])
|
||||
- Jacobian Analysis
|
||||
- Stiffness Analysis
|
||||
- Static Forces
|
||||
|
||||
For very small displacements $\delta q$ and $\delta X$, we have $\delta q = J \delta X$.
|
||||
* Identification of the Stewart Dynamics ([[file:identification.org][link]])
|
||||
- Extraction of State Space models
|
||||
- Resonant Frequencies and Modal Damping
|
||||
- Mode Shapes
|
||||
|
||||
*** Jacobian for forces
|
||||
\[ F = J^T \tau \]
|
||||
With:
|
||||
- $\tau = [\tau_1\ \tau_2\ \tau_3\ \tau_4\ \tau_5\ \tau_6]$ vector of actuator forces
|
||||
- $F = [f_x\ f_y\ f_z\ n_x\ n_y\ n_z]$ force and torque acting on point $O_B$
|
||||
* Active Damping ([[file:active-damping.org][link]])
|
||||
- Inertial Sensor
|
||||
- Force Sensor
|
||||
- Relative Motion Sensor
|
||||
|
||||
** Stiffness matrix $K$
|
||||
* Control of the Stewart Platform ([[file:control-study.org][link]])
|
||||
|
||||
\[ K = J^T \text{diag}(k_i) J \]
|
||||
* Cubic Configuration ([[file:cubic-configuration.org][link]])
|
||||
|
||||
If all the struts have the same stiffness $k$, then $K = k J^T J$
|
||||
|
||||
$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.
|
||||
* Architecture Optimization
|
||||
|
||||
* Bibliography :ignore:
|
||||
bibliographystyle:unsrt
|
||||
|
||||
12
readme.org
Normal file
12
readme.org
Normal 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:
|
||||
@ -4,7 +4,7 @@
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2020-01-22 mer. 11:35 -->
|
||||
<!-- 2020-01-27 lun. 17:41 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Stewart Platform - Simscape Model</title>
|
||||
@ -246,31 +246,6 @@ for the JavaScript code in this tag.
|
||||
}
|
||||
/*]]>*///-->
|
||||
</script>
|
||||
<script type="text/x-mathjax-config">
|
||||
MathJax.Hub.Config({
|
||||
displayAlign: "center",
|
||||
displayIndent: "0em",
|
||||
|
||||
"HTML-CSS": { scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
webFont: "TeX"
|
||||
},
|
||||
SVG: {scale: 100,
|
||||
linebreaks: { automatic: "false" },
|
||||
font: "TeX"},
|
||||
NativeMML: {scale: 100},
|
||||
TeX: { equationNumbers: {autoNumber: "AMS"},
|
||||
MultLineWidth: "85%",
|
||||
TagSide: "right",
|
||||
TagIndent: ".8em",
|
||||
Macros: {
|
||||
bm: ["{\\boldsymbol #1}",1],
|
||||
}
|
||||
}
|
||||
});
|
||||
</script>
|
||||
<script type="text/javascript"
|
||||
src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js?config=TeX-AMS_HTML"></script>
|
||||
</head>
|
||||
<body>
|
||||
<div id="org-div-home-and-up">
|
||||
@ -279,944 +254,11 @@ for the JavaScript code in this tag.
|
||||
<a accesskey="H" href="./index.html"> HOME </a>
|
||||
</div><div id="content">
|
||||
<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’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’ 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 id="postamble" class="status">
|
||||
<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>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
1593
simscape-model.org
1593
simscape-model.org
File diff suppressed because it is too large
Load Diff
Binary file not shown.
BIN
simulink/stewart_platform_identification.slx
Normal file
BIN
simulink/stewart_platform_identification.slx
Normal file
Binary file not shown.
BIN
simulink/stewart_platform_identification_simple.slx
Normal file
BIN
simulink/stewart_platform_identification_simple.slx
Normal file
Binary file not shown.
@ -1,10 +1,12 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart, opts_param)
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
||||
|
||||
@ -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)];
|
||||
@ -4,8 +4,6 @@ function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - H [1x1] - Total height of the platform [m]
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
@ -22,10 +20,10 @@ function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e-3;
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
|
||||
@ -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
|
||||
@ -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
|
||||
@ -1,5 +0,0 @@
|
||||
function [K] = getStiffnessMatrix(k, J)
|
||||
% k - leg stiffness
|
||||
% J - Jacobian matrix
|
||||
K = k*(J'*J);
|
||||
end
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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;
|
||||
@ -16,7 +16,7 @@ function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
arguments
|
||||
stewart
|
||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1)
|
||||
end
|
||||
|
||||
stewart.Ki = args.Ki;
|
||||
|
||||
87
static-analysis.org
Normal file
87
static-analysis.org
Normal 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
1622
stewart-architecture.html
Normal file
File diff suppressed because it is too large
Load Diff
1615
stewart-architecture.org
Normal file
1615
stewart-architecture.org
Normal file
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user