stewart-simscape/kinematic-study.html

759 lines
32 KiB
HTML

<?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>
<!-- 2020-01-29 mer. 13:29 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Kinematic Study of the Stewart Platform</title>
<meta name="generator" content="Org mode" />
<meta name="author" content="Dehaeze Thomas" />
<style type="text/css">
<!--/*--><![CDATA[/*><!--*/
.title { text-align: center;
margin-bottom: .2em; }
.subtitle { text-align: center;
font-size: medium;
font-weight: bold;
margin-top:0; }
.todo { font-family: monospace; color: red; }
.done { font-family: monospace; color: green; }
.priority { font-family: monospace; color: orange; }
.tag { background-color: #eee; font-family: monospace;
padding: 2px; font-size: 80%; font-weight: normal; }
.timestamp { color: #bebebe; }
.timestamp-kwd { color: #5f9ea0; }
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
.underline { text-decoration: underline; }
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
p.verse { margin-left: 3%; }
pre {
border: 1px solid #ccc;
box-shadow: 3px 3px 3px #eee;
padding: 8pt;
font-family: monospace;
overflow: auto;
margin: 1.2em;
}
pre.src {
position: relative;
overflow: visible;
padding-top: 1.2em;
}
pre.src:before {
display: none;
position: absolute;
background-color: white;
top: -10px;
right: 10px;
padding: 3px;
border: 1px solid black;
}
pre.src:hover:before { display: inline;}
/* Languages per Org manual */
pre.src-asymptote:before { content: 'Asymptote'; }
pre.src-awk:before { content: 'Awk'; }
pre.src-C:before { content: 'C'; }
/* pre.src-C++ doesn't work in CSS */
pre.src-clojure:before { content: 'Clojure'; }
pre.src-css:before { content: 'CSS'; }
pre.src-D:before { content: 'D'; }
pre.src-ditaa:before { content: 'ditaa'; }
pre.src-dot:before { content: 'Graphviz'; }
pre.src-calc:before { content: 'Emacs Calc'; }
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
pre.src-fortran:before { content: 'Fortran'; }
pre.src-gnuplot:before { content: 'gnuplot'; }
pre.src-haskell:before { content: 'Haskell'; }
pre.src-hledger:before { content: 'hledger'; }
pre.src-java:before { content: 'Java'; }
pre.src-js:before { content: 'Javascript'; }
pre.src-latex:before { content: 'LaTeX'; }
pre.src-ledger:before { content: 'Ledger'; }
pre.src-lisp:before { content: 'Lisp'; }
pre.src-lilypond:before { content: 'Lilypond'; }
pre.src-lua:before { content: 'Lua'; }
pre.src-matlab:before { content: 'MATLAB'; }
pre.src-mscgen:before { content: 'Mscgen'; }
pre.src-ocaml:before { content: 'Objective Caml'; }
pre.src-octave:before { content: 'Octave'; }
pre.src-org:before { content: 'Org mode'; }
pre.src-oz:before { content: 'OZ'; }
pre.src-plantuml:before { content: 'Plantuml'; }
pre.src-processing:before { content: 'Processing.js'; }
pre.src-python:before { content: 'Python'; }
pre.src-R:before { content: 'R'; }
pre.src-ruby:before { content: 'Ruby'; }
pre.src-sass:before { content: 'Sass'; }
pre.src-scheme:before { content: 'Scheme'; }
pre.src-screen:before { content: 'Gnu Screen'; }
pre.src-sed:before { content: 'Sed'; }
pre.src-sh:before { content: 'shell'; }
pre.src-sql:before { content: 'SQL'; }
pre.src-sqlite:before { content: 'SQLite'; }
/* additional languages in org.el's org-babel-load-languages alist */
pre.src-forth:before { content: 'Forth'; }
pre.src-io:before { content: 'IO'; }
pre.src-J:before { content: 'J'; }
pre.src-makefile:before { content: 'Makefile'; }
pre.src-maxima:before { content: 'Maxima'; }
pre.src-perl:before { content: 'Perl'; }
pre.src-picolisp:before { content: 'Pico Lisp'; }
pre.src-scala:before { content: 'Scala'; }
pre.src-shell:before { content: 'Shell Script'; }
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
/* additional language identifiers per "defun org-babel-execute"
in ob-*.el */
pre.src-cpp:before { content: 'C++'; }
pre.src-abc:before { content: 'ABC'; }
pre.src-coq:before { content: 'Coq'; }
pre.src-groovy:before { content: 'Groovy'; }
/* additional language identifiers from org-babel-shell-names in
ob-shell.el: ob-shell is the only babel language using a lambda to put
the execution function name together. */
pre.src-bash:before { content: 'bash'; }
pre.src-csh:before { content: 'csh'; }
pre.src-ash:before { content: 'ash'; }
pre.src-dash:before { content: 'dash'; }
pre.src-ksh:before { content: 'ksh'; }
pre.src-mksh:before { content: 'mksh'; }
pre.src-posh:before { content: 'posh'; }
/* Additional Emacs modes also supported by the LaTeX listings package */
pre.src-ada:before { content: 'Ada'; }
pre.src-asm:before { content: 'Assembler'; }
pre.src-caml:before { content: 'Caml'; }
pre.src-delphi:before { content: 'Delphi'; }
pre.src-html:before { content: 'HTML'; }
pre.src-idl:before { content: 'IDL'; }
pre.src-mercury:before { content: 'Mercury'; }
pre.src-metapost:before { content: 'MetaPost'; }
pre.src-modula-2:before { content: 'Modula-2'; }
pre.src-pascal:before { content: 'Pascal'; }
pre.src-ps:before { content: 'PostScript'; }
pre.src-prolog:before { content: 'Prolog'; }
pre.src-simula:before { content: 'Simula'; }
pre.src-tcl:before { content: 'tcl'; }
pre.src-tex:before { content: 'TeX'; }
pre.src-plain-tex:before { content: 'Plain TeX'; }
pre.src-verilog:before { content: 'Verilog'; }
pre.src-vhdl:before { content: 'VHDL'; }
pre.src-xml:before { content: 'XML'; }
pre.src-nxml:before { content: 'XML'; }
/* add a generic configuration mode; LaTeX export needs an additional
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
pre.src-conf:before { content: 'Configuration File'; }
table { border-collapse:collapse; }
caption.t-above { caption-side: top; }
caption.t-bottom { caption-side: bottom; }
td, th { vertical-align:top; }
th.org-right { text-align: center; }
th.org-left { text-align: center; }
th.org-center { text-align: center; }
td.org-right { text-align: right; }
td.org-left { text-align: left; }
td.org-center { text-align: center; }
dt { font-weight: bold; }
.footpara { display: inline; }
.footdef { margin-bottom: 1em; }
.figure { padding: 1em; }
.figure p { text-align: center; }
.equation-container {
display: table;
text-align: center;
width: 100%;
}
.equation {
vertical-align: middle;
}
.equation-label {
display: table-cell;
text-align: right;
vertical-align: middle;
}
.inlinetask {
padding: 10px;
border: 2px solid gray;
margin: 10px;
background: #ffffcc;
}
#org-div-home-and-up
{ text-align: right; font-size: 70%; white-space: nowrap; }
textarea { overflow-x: auto; }
.linenr { font-size: smaller }
.code-highlighted { background-color: #ffff00; }
.org-info-js_info-navigation { border-style: none; }
#org-info-js_console-label
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
.org-info-js_search-highlight
{ background-color: #ffff00; color: #000000; font-weight: bold; }
.org-svg { width: 90%; }
/*]]>*/-->
</style>
<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-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
General Public License (GNU GPL) as published by the Free Software
Foundation, either version 3 of the License, or (at your option)
any later version. The code is distributed WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
As additional permission under GNU GPL version 3 section 7, you
may distribute non-source (e.g., minimized or compacted) forms of
that code without the copy of the GNU GPL normally required by
section 4, provided you include this license notice and a URL
through which recipients can access the Corresponding Source.
@licend The above is the entire license notice
for the JavaScript code in this tag.
*/
<!--/*--><![CDATA[/*><!--*/
function CodeHighlightOn(elem, id)
{
var target = document.getElementById(id);
if(null != target) {
elem.cacheClassElem = elem.className;
elem.cacheClassTarget = target.className;
target.className = "code-highlighted";
elem.className = "code-highlighted";
}
}
function CodeHighlightOff(elem, id)
{
var target = document.getElementById(id);
if(elem.cacheClassElem)
elem.className = elem.cacheClassElem;
if(elem.cacheClassTarget)
target.className = elem.cacheClassTarget;
}
/*]]>*///-->
</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">
<a accesskey="h" href="./index.html"> UP </a>
|
<a accesskey="H" href="./index.html"> HOME </a>
</div><div id="content">
<h1 class="title">Kinematic Study of the Stewart Platform</h1>
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#orge42fba6">1. Jacobian Analysis</a>
<ul>
<li><a href="#org8938d37">1.1. Jacobian Computation</a></li>
<li><a href="#orgbcdebed">1.2. Velocity of the struts to the velocity of the mobile platform</a></li>
<li><a href="#org9482fe8">1.3. Displacement of the struts to the displacement of the mobile platform</a></li>
<li><a href="#orgc7c6052">1.4. Force Transformation</a></li>
</ul>
</li>
<li><a href="#orgda7fde9">2. Forward and Inverse Kinematics</a>
<ul>
<li><a href="#org26477b8">2.1. Inverse Kinematics</a></li>
<li><a href="#org01066c5">2.2. Forward Kinematics</a></li>
<li><a href="#org37b3180">2.3. Approximate Forward Kinematics</a></li>
</ul>
</li>
<li><a href="#org5304e0f">3. Stiffness Analysis</a>
<ul>
<li><a href="#orgdc3ef4e">3.1. Computation of the Stiffness and Compliance Matrix</a></li>
</ul>
</li>
<li><a href="#org03cb27a">4. Estimated required actuator stroke for specified platform mobility</a>
<ul>
<li><a href="#orgfa74621">4.1. Needed Actuator Stroke</a>
<ul>
<li><a href="#org79abcb3">4.1.1. Stewart architecture definition</a></li>
<li><a href="#org5bf59b4">4.1.2. Wanted translations and rotations</a></li>
<li><a href="#org1dce5e1">4.1.3. Needed stroke for &ldquo;pure&rdquo; rotations or translations</a></li>
</ul>
</li>
</ul>
</li>
<li><a href="#org7319607">5. Estimated platform mobility from specified actuator stroke</a></li>
<li><a href="#org951a228">6. Functions</a>
<ul>
<li><a href="#org2387af5">6.1. <code>computeJacobian</code>: Compute the Jacobian Matrix</a>
<ul>
<li><a href="#org0734fbe">6.1.1. Function description</a></li>
<li><a href="#orge2bf995">6.1.2. Compute Jacobian Matrix</a></li>
<li><a href="#orgc3abc35">6.1.3. Compute Stiffness Matrix</a></li>
<li><a href="#org5be4e51">6.1.4. Compute Compliance Matrix</a></li>
</ul>
</li>
<li><a href="#org2510ad8">6.2. <code>inverseKinematics</code>: Compute Inverse Kinematics</a>
<ul>
<li><a href="#org15abed6">6.2.1. Function description</a></li>
<li><a href="#orgfa724fa">6.2.2. Optional Parameters</a></li>
<li><a href="#org9510865">6.2.3. Theory</a></li>
<li><a href="#orgad46e51">6.2.4. Compute</a></li>
</ul>
</li>
<li><a href="#orgc42ae4c">6.3. <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</a>
<ul>
<li><a href="#orgba5a90f">6.3.1. Function description</a></li>
<li><a href="#org0a3069a">6.3.2. Optional Parameters</a></li>
<li><a href="#orgf878173">6.3.3. Computation</a></li>
</ul>
</li>
</ul>
</li>
</ul>
</div>
</div>
<div id="outline-container-orge42fba6" class="outline-2">
<h2 id="orge42fba6"><span class="section-number-2">1</span> Jacobian Analysis</h2>
<div class="outline-text-2" id="text-1">
</div>
<div id="outline-container-org8938d37" class="outline-3">
<h3 id="org8938d37"><span class="section-number-3">1.1</span> Jacobian Computation</h3>
</div>
<div id="outline-container-orgbcdebed" class="outline-3">
<h3 id="orgbcdebed"><span class="section-number-3">1.2</span> Velocity of the struts to the velocity of the mobile platform</h3>
</div>
<div id="outline-container-org9482fe8" class="outline-3">
<h3 id="org9482fe8"><span class="section-number-3">1.3</span> Displacement of the struts to the displacement of the mobile platform</h3>
</div>
<div id="outline-container-orgc7c6052" class="outline-3">
<h3 id="orgc7c6052"><span class="section-number-3">1.4</span> Force Transformation</h3>
</div>
</div>
<div id="outline-container-orgda7fde9" class="outline-2">
<h2 id="orgda7fde9"><span class="section-number-2">2</span> Forward and Inverse Kinematics</h2>
<div class="outline-text-2" id="text-2">
</div>
<div id="outline-container-org26477b8" class="outline-3">
<h3 id="org26477b8"><span class="section-number-3">2.1</span> Inverse Kinematics</h3>
</div>
<div id="outline-container-org01066c5" class="outline-3">
<h3 id="org01066c5"><span class="section-number-3">2.2</span> Forward Kinematics</h3>
</div>
<div id="outline-container-org37b3180" class="outline-3">
<h3 id="org37b3180"><span class="section-number-3">2.3</span> Approximate Forward Kinematics</h3>
</div>
</div>
<div id="outline-container-org5304e0f" class="outline-2">
<h2 id="org5304e0f"><span class="section-number-2">3</span> Stiffness Analysis</h2>
<div class="outline-text-2" id="text-3">
</div>
<div id="outline-container-orgdc3ef4e" class="outline-3">
<h3 id="orgdc3ef4e"><span class="section-number-3">3.1</span> Computation of the Stiffness and Compliance Matrix</h3>
</div>
</div>
<div id="outline-container-org03cb27a" class="outline-2">
<h2 id="org03cb27a"><span class="section-number-2">4</span> Estimated required actuator stroke for specified platform mobility</h2>
<div class="outline-text-2" id="text-4">
</div>
<div id="outline-container-orgfa74621" class="outline-3">
<h3 id="orgfa74621"><span class="section-number-3">4.1</span> Needed Actuator Stroke</h3>
<div class="outline-text-3" id="text-4-1">
<p>
The goal is to determine the needed stroke of the actuators to obtain wanted translations and rotations.
</p>
</div>
<div id="outline-container-org79abcb3" class="outline-4">
<h4 id="org79abcb3"><span class="section-number-4">4.1.1</span> Stewart architecture definition</h4>
<div class="outline-text-4" id="text-4-1-1">
<p>
We use a cubic architecture.
</p>
<div class="org-src-container">
<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>, 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, 100], ...<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);
opts = struct(...
<span class="org-string">'stroke'</span>, 50e<span class="org-type">-</span>6 ...<span class="org-comment"> % Maximum stroke of each actuator [m]</span>
);
stewart = initializeMechanicalElements(stewart, opts);
save(<span class="org-string">'./mat/stewart.mat'</span>, <span class="org-string">'stewart'</span>);
</pre>
</div>
</div>
</div>
<div id="outline-container-org5bf59b4" class="outline-4">
<h4 id="org5bf59b4"><span class="section-number-4">4.1.2</span> Wanted translations and rotations</h4>
<div class="outline-text-4" id="text-4-1-2">
<p>
We define wanted translations and rotations
</p>
<div class="org-src-container">
<pre class="src src-matlab">Tx_max = 15e<span class="org-type">-</span>6; <span class="org-comment">% Translation [m]</span>
Ty_max = 15e<span class="org-type">-</span>6; <span class="org-comment">% Translation [m]</span>
Tz_max = 15e<span class="org-type">-</span>6; <span class="org-comment">% Translation [m]</span>
Rx_max = 30e<span class="org-type">-</span>6; <span class="org-comment">% Rotation [rad]</span>
Ry_max = 30e<span class="org-type">-</span>6; <span class="org-comment">% Rotation [rad]</span>
</pre>
</div>
</div>
</div>
<div id="outline-container-org1dce5e1" class="outline-4">
<h4 id="org1dce5e1"><span class="section-number-4">4.1.3</span> Needed stroke for &ldquo;pure&rdquo; rotations or translations</h4>
<div class="outline-text-4" id="text-4-1-3">
<p>
First, we estimate the needed actuator stroke for &ldquo;pure&rdquo; rotations and translation.
</p>
<div class="org-src-container">
<pre class="src src-matlab">LTx = stewart.Jd<span class="org-type">*</span>[Tx_max 0 0 0 0 0]<span class="org-type">'</span>;
LTy = stewart.Jd<span class="org-type">*</span>[0 Ty_max 0 0 0 0]<span class="org-type">'</span>;
LTz = stewart.Jd<span class="org-type">*</span>[0 0 Tz_max 0 0 0]<span class="org-type">'</span>;
LRx = stewart.Jd<span class="org-type">*</span>[0 0 0 Rx_max 0 0]<span class="org-type">'</span>;
LRy = stewart.Jd<span class="org-type">*</span>[0 0 0 0 Ry_max 0]<span class="org-type">'</span>;
</pre>
</div>
<pre class="example">
From -1.2e-05[m] to 1.1e-05[m]: Total stroke = 22.9[um]
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org7319607" class="outline-2">
<h2 id="org7319607"><span class="section-number-2">5</span> Estimated platform mobility from specified actuator stroke</h2>
</div>
<div id="outline-container-org951a228" class="outline-2">
<h2 id="org951a228"><span class="section-number-2">6</span> Functions</h2>
<div class="outline-text-2" id="text-6">
</div>
<div id="outline-container-org2387af5" class="outline-3">
<h3 id="org2387af5"><span class="section-number-3">6.1</span> <code>computeJacobian</code>: Compute the Jacobian Matrix</h3>
<div class="outline-text-3" id="text-6-1">
<p>
<a id="org2ea0e42"></a>
</p>
<p>
This Matlab function is accessible <a href="src/computeJacobian.m">here</a>.
</p>
</div>
<div id="outline-container-org0734fbe" class="outline-4">
<h4 id="org0734fbe"><span class="section-number-4">6.1.1</span> Function description</h4>
<div class="outline-text-4" id="text-6-1-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-orge2bf995" class="outline-4">
<h4 id="orge2bf995"><span class="section-number-4">6.1.2</span> Compute Jacobian Matrix</h4>
<div class="outline-text-4" id="text-6-1-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-orgc3abc35" class="outline-4">
<h4 id="orgc3abc35"><span class="section-number-4">6.1.3</span> Compute Stiffness Matrix</h4>
<div class="outline-text-4" id="text-6-1-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-org5be4e51" class="outline-4">
<h4 id="org5be4e51"><span class="section-number-4">6.1.4</span> Compute Compliance Matrix</h4>
<div class="outline-text-4" id="text-6-1-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-org2510ad8" class="outline-3">
<h3 id="org2510ad8"><span class="section-number-3">6.2</span> <code>inverseKinematics</code>: Compute Inverse Kinematics</h3>
<div class="outline-text-3" id="text-6-2">
<p>
<a id="orgd507362"></a>
</p>
<p>
This Matlab function is accessible <a href="src/inverseKinematics.m">here</a>.
</p>
</div>
<div id="outline-container-org15abed6" class="outline-4">
<h4 id="org15abed6"><span class="section-number-4">6.2.1</span> Function description</h4>
<div class="outline-text-4" id="text-6-2-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-orgfa724fa" class="outline-4">
<h4 id="orgfa724fa"><span class="section-number-4">6.2.2</span> Optional Parameters</h4>
<div class="outline-text-4" id="text-6-2-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-org9510865" class="outline-4">
<h4 id="org9510865"><span class="section-number-4">6.2.3</span> Theory</h4>
<div class="outline-text-4" id="text-6-2-3">
<p>
For inverse kinematic analysis, it is assumed that the position \({}^A\bm{P}\) and orientation of the moving platform \({}^A\bm{R}_B\) are given and the problem is to obtain the joint variables, namely, \(\bm{L} = [l_1, l_2, \dots, l_6]^T\).
</p>
<p>
From the geometry of the manipulator, the loop closure for each limb, \(i = 1, 2, \dots, 6\) can be written as
</p>
\begin{align*}
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
\end{align*}
<p>
To obtain the length of each actuator and eliminate \(\hat{\bm{s}}_i\), it is sufficient to dot multiply each side by itself:
</p>
\begin{equation}
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
\end{equation}
<p>
Hence, for \(i = 1, 2, \dots, 6\), each limb length can be uniquely determined by:
</p>
\begin{equation}
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
\end{equation}
<p>
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
Otherwise, when the limbs&rsquo; lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
</p>
</div>
</div>
<div id="outline-container-orgad46e51" class="outline-4">
<h4 id="orgad46e51"><span class="section-number-4">6.2.4</span> Compute</h4>
<div class="outline-text-4" id="text-6-2-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-orgc42ae4c" class="outline-3">
<h3 id="orgc42ae4c"><span class="section-number-3">6.3</span> <code>forwardKinematicsApprox</code>: Compute the Approximate Forward Kinematics</h3>
<div class="outline-text-3" id="text-6-3">
<p>
<a id="org6e7838d"></a>
</p>
<p>
This Matlab function is accessible <a href="src/forwardKinematicsApprox.m">here</a>.
</p>
</div>
<div id="outline-container-orgba5a90f" class="outline-4">
<h4 id="orgba5a90f"><span class="section-number-4">6.3.1</span> Function description</h4>
<div class="outline-text-4" id="text-6-3-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-org0a3069a" class="outline-4">
<h4 id="org0a3069a"><span class="section-number-4">6.3.2</span> Optional Parameters</h4>
<div class="outline-text-4" id="text-6-3-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-orgf878173" class="outline-4">
<h4 id="orgf878173"><span class="section-number-4">6.3.3</span> Computation</h4>
<div class="outline-text-4" id="text-6-3-3">
<p>
From a small displacement of each strut \(d\bm{\mathcal{L}}\), we can compute the
position and orientation of {B} with respect to {A} using the following formula:
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
</p>
<div class="org-src-container">
<pre class="src src-matlab">X = stewart.J<span class="org-type">\</span>args.dL;
</pre>
</div>
<p>
The position vector corresponds to the first 3 elements.
</p>
<div class="org-src-container">
<pre class="src src-matlab">P = X(1<span class="org-type">:</span>3);
</pre>
</div>
<p>
The next 3 elements are the orientation of {B} with respect to {A} expressed
using the screw axis.
</p>
<div class="org-src-container">
<pre class="src src-matlab">theta = norm(X(4<span class="org-type">:</span>6));
s = X(4<span class="org-type">:</span>6)<span class="org-type">/</span>theta;
</pre>
</div>
<p>
We then compute the corresponding rotation matrix.
</p>
<div class="org-src-container">
<pre class="src src-matlab">R = [s(1)<span class="org-type">^</span>2<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">+</span> cos(theta) , s(1)<span class="org-type">*</span>s(2)<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">-</span> s(3)<span class="org-type">*</span>sin(theta), s(1)<span class="org-type">*</span>s(3)<span class="org-type">*</span>(1<span class="org-type">-</span>cos(theta)) <span class="org-type">+</span> s(2)<span class="org-type">*</span>sin(theta);
s<span class="org-type">(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);</span>
s<span class="org-type">(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];</span>
</pre>
</div>
</div>
</div>
</div>
</div>
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2020-01-29 mer. 13:29</p>
</div>
</body>
</html>