2019-10-16 11:18:06 +02:00
<?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-10 18:06:33 +01:00
<!-- 2019 - 12 - 10 mar. 18:05 -->
2019-10-16 11:18:06 +02:00
< meta http-equiv = "Content-Type" content = "text/html;charset=utf-8" / >
< meta name = "viewport" content = "width=device-width, initial-scale=1" / >
< title > Simscape Model< / title >
< 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" / >
< link rel = "stylesheet" type = "text/css" href = "../css/zenburn.css" / >
< script type = "text/javascript" src = "../js/jquery.min.js" > < / script >
< script type = "text/javascript" src = "../js/bootstrap.min.js" > < / script >
< script type = "text/javascript" src = "../js/jquery.stickytableheaders.min.js" > < / script >
< script type = "text/javascript" 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.
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 >
< / 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" > Simscape Model< / h1 >
< div id = "table-of-contents" >
< h2 > Table of Contents< / h2 >
< div id = "text-table-of-contents" >
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#org88d59ba" > 1. Simulink Project - Startup and Shutdown scripts< / a > < / li >
< li > < a href = "#orga2206d5" > 2. Simscape Multibody - Presentation< / a >
2019-10-16 11:18:06 +02:00
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#org6b4540f" > 2.1. Solid bodies< / a > < / li >
< li > < a href = "#orge470a34" > 2.2. Frames< / a > < / li >
< li > < a href = "#org474edd3" > 2.3. Joints< / a > < / li >
< li > < a href = "#org9171b40" > 2.4. Measurements< / a > < / li >
< li > < a href = "#org7df2aef" > 2.5. Excitation< / a > < / li >
2019-10-16 11:18:06 +02:00
< / ul >
< / li >
2019-12-10 18:06:33 +01:00
< li > < a href = "#org803cadc" > 3. Simulink files and signal names< / a >
2019-10-16 11:18:06 +02:00
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#orgb5c767a" > 3.1. List of Simscape files< / a > < / li >
< li > < a href = "#org64dbd0d" > 3.2. List of Inputs< / a >
2019-10-16 11:18:06 +02:00
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#orgeecef4d" > 3.2.1. Perturbations< / a > < / li >
< li > < a href = "#org4e58ed6" > 3.2.2. Measurement Noise< / a > < / li >
< li > < a href = "#orgc9006ba" > 3.2.3. Control Inputs< / a > < / li >
2019-10-16 11:18:06 +02:00
< / ul >
< / li >
2019-12-10 18:06:33 +01:00
< li > < a href = "#orgeb985b0" > 3.3. List of Outputs< / a > < / li >
2019-10-16 11:18:06 +02:00
< / ul >
< / li >
2019-12-10 18:06:33 +01:00
< li > < a href = "#orgfd5289f" > 4. Simulink Library< / a >
2019-10-16 11:18:06 +02:00
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#orgd1ebe0f" > 4.1. < code > inputs< / code > < / a > < / li >
< li > < a href = "#org1ae1d37" > 4.2. < code > nass_library< / code > < / a > < / li >
< li > < a href = "#org54e3d07" > 4.3. < code > pos_error_wrt_nass_base< / code > < / a > < / li >
< li > < a href = "#orgc0f0f02" > 4.4. < code > QuaternionToAngles< / code > < / a > < / li >
< li > < a href = "#orgbda19ed" > 4.5. < code > RotationMatrixToAngle< / code > < / a > < / li >
2019-10-16 11:18:06 +02:00
< / ul >
< / li >
2019-12-10 18:06:33 +01:00
< li > < a href = "#org54193c9" > 5. Functions< / a >
2019-10-16 11:18:06 +02:00
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#orgc1f5840" > 5.1. computePsdDispl< / a > < / li >
< li > < a href = "#org0d8e9e6" > 5.2. computeSetpoint< / a > < / li >
< li > < a href = "#org9a191e3" > 5.3. converErrorBasis< / a > < / li >
< li > < a href = "#org2d44efa" > 5.4. generateDiagPidControl< / a > < / li >
< li > < a href = "#org6f7736a" > 5.5. identifyPlant< / a > < / li >
< li > < a href = "#org32dd782" > 5.6. runSimulation< / a > < / li >
2019-10-16 11:18:06 +02:00
< / ul >
< / li >
2019-12-10 18:06:33 +01:00
< li > < a href = "#org5cc2276" > 6. Initialize Elements< / a >
2019-10-16 11:18:06 +02:00
< ul >
2019-12-10 18:06:33 +01:00
< li > < a href = "#org664a7e3" > 6.1. Experiment< / a > < / li >
< li > < a href = "#org89c58f8" > 6.2. Generate Reference Signals< / a > < / li >
< li > < a href = "#org77d160f" > 6.3. < span class = "todo TODO" > TODO< / span > Inputs< / a > < / li >
< li > < a href = "#org962187e" > 6.4. Ground< / a > < / li >
< li > < a href = "#org198c366" > 6.5. Granite< / a > < / li >
< li > < a href = "#org6e967d2" > 6.6. Translation Stage< / a > < / li >
< li > < a href = "#org6c95b53" > 6.7. Tilt Stage< / a > < / li >
< li > < a href = "#org13271e7" > 6.8. Spindle< / a > < / li >
< li > < a href = "#org3c482fe" > 6.9. Initialize Hexapod legs' length< / a > < / li >
< li > < a href = "#org7c295aa" > 6.10. Micro Hexapod< / a > < / li >
< li > < a href = "#org72492f3" > 6.11. Center of gravity compensation< / a > < / li >
< li > < a href = "#org68d031b" > 6.12. Mirror< / a > < / li >
< li > < a href = "#org74149ff" > 6.13. Nano Hexapod< / a > < / li >
< li > < a href = "#org94d9a68" > 6.14. Cedrat Actuator< / a > < / li >
< li > < a href = "#org8ceaacc" > 6.15. Sample< / a > < / li >
2019-10-16 11:18:06 +02:00
< / ul >
< / li >
< / ul >
< / div >
< / div >
< p >
This file is used to explain how this Simscape Model works.
< / p >
< ul class = "org-ul" >
2019-12-10 18:06:33 +01:00
< li > In section < a href = "#orgfd7237b" > 1< / a > , the simulink project with the associated scripts are presented< / li >
< li > In section < a href = "#orgb6586ca" > 2< / a > , an introduction to Simscape Multibody is done< / li >
< li > In section < a href = "#org3e8132a" > 3< / a > , each simscape files are presented with the associated signal names and joint architectures< / li >
< li > In section < a href = "#orgecb3837" > 4< / a > , the list of the Simulink library elements are described< / li >
< li > In section < a href = "#org94ca66a" > 5< / a > , a list of Matlab function that will be used are defined here< / li >
< li > In section < a href = "#org18b2c56" > 6< / a > , all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.< / li >
2019-10-16 11:18:06 +02:00
< / ul >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org88d59ba" class = "outline-2" >
< h2 id = "org88d59ba" > < span class = "section-number-2" > 1< / span > Simulink Project - Startup and Shutdown scripts< / h2 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-2" id = "text-1" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgfd7237b" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
From the < a href = "https://mathworks.com/products/simulink/projects.html" > Simulink project< / a > mathworks page:
< / p >
< blockquote >
< p >
Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place.
< / p >
< p >
With Simulink Projects, you can:
< / p >
< ul class = "org-ul" >
< li > < b > Collaborate< / b > : Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.< / li >
< li > < b > Automate< / b > : Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.< / li >
< li > < b > Integrate with source control< / b > : Enable easy integration with source control and configuration management tools.< / li >
< / ul >
< / blockquote >
< p >
The project can be opened using the < code > simulinkproject< / code > function:
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > simulinkproject< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< p >
When the project opens, a startup script is ran.
The startup script is defined below and is exported to the < code > project_startup.m< / code > script.
< / p >
< div class = "org-src-container" >
2019-12-06 12:04:11 +01:00
< pre class = "src src-matlab" > project = simulinkproject;
2019-10-16 11:18:06 +02:00
projectRoot = project.RootFolder;
myCacheFolder = fullfile< span class = "org-rainbow-delimiters-depth-1" > (< / span > projectRoot, < span class = "org-string" > '.SimulinkCache'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
myCodeFolder = fullfile< span class = "org-rainbow-delimiters-depth-1" > (< / span > projectRoot, < span class = "org-string" > '.SimulinkCode'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Simulink.fileGenControl< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'set'< / span > ,...
< span class = "org-string" > 'CacheFolder'< / span > , myCacheFolder,...
< span class = "org-string" > 'CodeGenFolder'< / span > , myCodeFolder,...
< span class = "org-string" > 'createDir'< / span > , < span class = "org-constant" > true< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< p >
When the project closes, it runs the < code > project_shutdown.m< / code > script defined below.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Simulink.fileGenControl< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'reset'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< / pre >
< / div >
< p >
The project also permits to automatically add defined folder to the path when the project is opened.
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orga2206d5" class = "outline-2" >
< h2 id = "orga2206d5" > < span class = "section-number-2" > 2< / span > Simscape Multibody - Presentation< / h2 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-2" id = "text-2" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgb6586ca" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
A < a href = "https://.mathworks.com/products/simscape.html" > simscape< / a > model permits to model multi-physics systems.
< / p >
< p >
< a href = "https://mathworks.com/products/simmechanics.html" > Simscape Multibody< / a > permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors.
< / p >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org6b4540f" class = "outline-3" >
< h3 id = "org6b4540f" > < span class = "section-number-3" > 2.1< / span > Solid bodies< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-2-1" >
< p >
Each solid body is represented by a < a href = "https://mathworks.com/help/physmod/sm/ref/solid.html" > solid block< / a > .
The geometry of the solid body can be imported using a < code > step< / code > file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the < code > step< / code > file. They also can be set by hand.
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orge470a34" class = "outline-3" >
< h3 id = "orge470a34" > < span class = "section-number-3" > 2.2< / span > Frames< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-2-2" >
< p >
Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.
< / p >
< p >
They can be defined from the solid body geometry, or using the < a href = "https://mathworks.com/help/physmod/sm/ref/rigidtransform.html" > rigid transform block< / a > .
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org474edd3" class = "outline-3" >
< h3 id = "org474edd3" > < span class = "section-number-3" > 2.3< / span > Joints< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-2-3" >
< p >
Solid Bodies are connected with joints (between frames of the two solid bodies).
< / p >
< p >
There are various types of joints that are all described < a href = "https://mathworks.com/help/physmod/sm/ug/joints.html" > here< / a > .
< / p >
2019-12-10 18:06:33 +01:00
< table id = "org57b7cc8" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 1:< / span > Degrees of freedom associated with each joint< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-right" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Joint Block< / th >
< th scope = "col" class = "org-right" > Translational DOFs< / th >
< th scope = "col" class = "org-right" > Rotational DOFs< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > 6-DOF< / td >
< td class = "org-right" > 3< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Bearing< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Bushing< / td >
< td class = "org-right" > 3< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Cartesian< / td >
< td class = "org-right" > 3< / td >
< td class = "org-right" > 0< / td >
< / tr >
< tr >
< td class = "org-left" > Constant Velocity< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 2< / td >
< / tr >
< tr >
< td class = "org-left" > Cylindrical< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Gimbal< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Leadscrew< / td >
< td class = "org-right" > 1 (coupled)< / td >
< td class = "org-right" > 1 (coupled)< / td >
< / tr >
< tr >
< td class = "org-left" > Pin Slot< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Planar< / td >
< td class = "org-right" > 2< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Prismatic< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 0< / td >
< / tr >
< tr >
< td class = "org-left" > Rectangular< / td >
< td class = "org-right" > 2< / td >
< td class = "org-right" > 0< / td >
< / tr >
< tr >
< td class = "org-left" > Revolute< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 1< / td >
< / tr >
< tr >
< td class = "org-left" > Spherical< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Telescoping< / td >
< td class = "org-right" > 1< / td >
< td class = "org-right" > 3< / td >
< / tr >
< tr >
< td class = "org-left" > Universal< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 2< / td >
< / tr >
< tr >
< td class = "org-left" > Weld< / td >
< td class = "org-right" > 0< / td >
< td class = "org-right" > 0< / td >
< / tr >
< / tbody >
< / table >
< p >
Joint blocks are assortments of joint primitives:
< / p >
< ul class = "org-ul" >
< li > < b > Prismatic< / b > : allows translation along a single standard axis: < code > Px< / code > , < code > Py< / code > , < code > Pz< / code > < / li >
< li > < b > Revolute< / b > : allows rotation about a single standard axis: < code > Rx< / code > , < code > Ry< / code > , < code > Rz< / code > < / li >
< li > < b > Spherical< / b > : allow rotation about any 3D axis: < code > S< / code > < / li >
< li > < b > Lead Screw< / b > : allows coupled rotation and translation on a standard axis: < code > LSz< / code > < / li >
< li > < b > Constant Velocity< / b > : Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: < code > CV< / code > < / li >
< / ul >
2019-12-10 18:06:33 +01:00
< table id = "org75d3b05" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 2:< / span > Joint primitives for each joint type< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Joint Block< / th >
< th scope = "col" class = "org-left" > Px< / th >
< th scope = "col" class = "org-left" > Py< / th >
< th scope = "col" class = "org-left" > Pz< / th >
< th scope = "col" class = "org-left" > Rx< / th >
< th scope = "col" class = "org-left" > Ry< / th >
< th scope = "col" class = "org-left" > Rz< / th >
< th scope = "col" class = "org-left" > S< / th >
< th scope = "col" class = "org-left" > CV< / th >
< th scope = "col" class = "org-left" > LSz< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > 6-DOF< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Bearing< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Bushing< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Cartesian< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Constant Velocity< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Cylindrical< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Gimbal< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Leadscrew< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< / tr >
< tr >
< td class = "org-left" > Pin Slot< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Planar< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Prismatic< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Rectangular< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Revolute< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Spherical< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Telescoping< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Universal< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" > x< / td >
< td class = "org-left" > x< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > Weld< / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< / tbody >
< / table >
< p >
For each of the joint primitive, we can specify the dynamical properties:
< / p >
< ul class = "org-ul" >
< li > The < b > spring stiffness< / b > : either linear or rotational one< / li >
< li > The < b > damping coefficient< / b > < / li >
< / ul >
< p >
For the actuation, we can either specify the motion or the force:
< / p >
< ul class = "org-ul" >
< li > the force applied in the corresponding DOF is provided by the input< / li >
< li > the motion is provided by the input< / li >
< / ul >
< p >
A sensor can be added to measure either the position, velocity or acceleration of the joint DOF.
< / p >
< p >
Composite Force/Torque sensing:
< / p >
< ul class = "org-ul" >
< li > Constraint force< / li >
< li > Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers.< / li >
< / ul >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org9171b40" class = "outline-3" >
< h3 id = "org9171b40" > < span class = "section-number-3" > 2.4< / span > Measurements< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-2-4" >
< p >
A transform sensor block measures the spatial relationship between two frames: the base < code > B< / code > and the follower < code > F< / code > .
< / p >
< p >
It can give the rotational and translational position, velocity and acceleration.
< / p >
< p >
The measurement frame should be specified: it corresponds to the frame in which to resolve the selected spatial measurements. The default is < code > world< / code > .
< / p >
< p >
If we want to simulate an < b > inertial sensor< / b > , we just have to choose < code > B< / code > to be the < code > world< / code > frame.
< / p >
< p >
< b > Force sensors< / b > are included in the joints blocks.
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org7df2aef" class = "outline-3" >
< h3 id = "org7df2aef" > < span class = "section-number-3" > 2.5< / span > Excitation< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-2-5" >
< p >
We can apply < b > external forces< / b > to the model by using an < a href = "https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html" > external force and torque block< / a > .
< / p >
< p >
Internal force, acting reciprocally between base and following origins is implemented using the < a href = "https://mathworks.com/help/physmod/sm/ref/internalforce.html" > internal force block< / a > even though it is usually included in one joint block.
< / p >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org803cadc" class = "outline-2" >
< h2 id = "org803cadc" > < span class = "section-number-2" > 3< / span > Simulink files and signal names< / h2 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-2" id = "text-3" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "org3e8132a" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
In order to "normalize" things, the names of all the signal are listed here.
< / p >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgb5c767a" class = "outline-3" >
< h3 id = "orgb5c767a" > < span class = "section-number-3" > 3.1< / span > List of Simscape files< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-3-1" >
< p >
Few different Simulink files are used:
< / p >
< ul class = "org-ul" >
< li > kinematics< / li >
< li > identification - micro station< / li >
< li > identification - nano station< / li >
< li > control< / li >
< / ul >
2019-12-10 18:06:33 +01:00
< table id = "org6eeaf61" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 3:< / span > List of simscape files< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Simscape Name< / th >
< th scope = "col" class = "org-left" > Ty< / th >
< th scope = "col" class = "org-left" > Ry< / th >
< th scope = "col" class = "org-left" > Rz< / th >
< th scope = "col" class = "org-left" > Hexa< / th >
< th scope = "col" class = "org-left" > NASS< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > id micro station< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > id nano station stages< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< td class = "org-left" > F< / td >
< / tr >
< tr >
< td class = "org-left" > id nano station config< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > F< / td >
< / tr >
< tr >
< td class = "org-left" > control nano station< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > D< / td >
< td class = "org-left" > F< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org64dbd0d" class = "outline-3" >
< h3 id = "org64dbd0d" > < span class = "section-number-3" > 3.2< / span > List of Inputs< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-3-2" >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgeecef4d" class = "outline-4" >
< h4 id = "orgeecef4d" > < span class = "section-number-4" > 3.2.1< / span > Perturbations< / h4 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-4" id = "text-3-2-1" >
2019-12-10 18:06:33 +01:00
< table id = "org5c60a9c" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 4:< / span > List of Disturbances< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-right" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < code > Dw< / code > < / td >
< td class = "org-left" > Ground motion< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fg< / code > < / td >
< td class = "org-left" > External force applied on granite< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fs< / code > < / td >
< td class = "org-left" > External force applied on the Sample< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org4e58ed6" class = "outline-4" >
< h4 id = "org4e58ed6" > < span class = "section-number-4" > 3.2.2< / span > Measurement Noise< / h4 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-4" id = "text-3-2-2" >
2019-12-10 18:06:33 +01:00
< table id = "orgdadd8d8" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 5:< / span > List of Measurement Noise< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-left" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< td class = "org-left" >   < / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgc9006ba" class = "outline-4" >
< h4 id = "orgc9006ba" > < span class = "section-number-4" > 3.2.3< / span > Control Inputs< / h4 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-4" id = "text-3-2-3" >
2019-12-10 18:06:33 +01:00
< table id = "orgfc73f52" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 6:< / span > List of Control Inputs< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-right" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < code > Fy< / code > < / td >
< td class = "org-left" > Actuation force for Ty< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dy< / code > < / td >
< td class = "org-left" > Imposed displacement for Ty< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > My< / code > < / td >
< td class = "org-left" > Actuation torque for Ry< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Ry< / code > < / td >
< td class = "org-left" > Imposed rotation for Ry< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Mz< / code > < / td >
< td class = "org-left" > Actuation torque for Rz< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Rz< / code > < / td >
< td class = "org-left" > Imposed rotation for Rz< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Fh< / code > < / td >
< td class = "org-left" > Actuation force/torque for hexapod (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N, N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fhl< / code > < / td >
< td class = "org-left" > Actuation force/torque for hexapod (legs)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dh< / code > < / td >
< td class = "org-left" > Imposed position for hexapod (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Rm< / code > < / td >
< td class = "org-left" > Position of the two masses< / td >
< td class = "org-right" > 2< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Fn< / code > < / td >
< td class = "org-left" > Actuation force for the NASS (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N, N.m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Fnl< / code > < / td >
< td class = "org-left" > Actuation force for the NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dn< / code > < / td >
< td class = "org-left" > Imposed position for the NASS (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgeb985b0" class = "outline-3" >
< h3 id = "orgeb985b0" > < span class = "section-number-3" > 3.3< / span > List of Outputs< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-3-3" >
2019-12-10 18:06:33 +01:00
< table id = "org59a5204" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
2019-10-16 11:18:06 +02:00
< caption class = "t-above" > < span class = "table-number" > Table 7:< / span > List of Outputs< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-right" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" > Variable< / th >
< th scope = "col" class = "org-left" > Meaning< / th >
< th scope = "col" class = "org-right" > Size< / th >
< th scope = "col" class = "org-left" > Unit< / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < code > Dgm< / code > < / td >
< td class = "org-left" > Absolute displacement of the granite< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Vgm< / code > < / td >
< td class = "org-left" > Absolute Velocity of the granite< / td >
< td class = "org-right" > 3< / td >
< td class = "org-left" > [m/s]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Dym< / code > < / td >
< td class = "org-left" > Measured displacement of Ty< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Rym< / code > < / td >
< td class = "org-left" > Measured rotation of Ry< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Rzm< / code > < / td >
< td class = "org-left" > Measured rotation of Rz< / td >
< td class = "org-right" > 1< / td >
< td class = "org-left" > [rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Dhm< / code > < / td >
< td class = "org-left" > Measured position of hexapod (cart)< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Fnlm< / code > < / td >
< td class = "org-left" > Measured force of NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [N]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dnlm< / code > < / td >
< td class = "org-left" > Measured elongation of NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Dnm< / code > < / td >
< td class = "org-left" > Measured position of NASS w.r.t NASS's base< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Vnm< / code > < / td >
< td class = "org-left" > Measured absolute velocity of NASS platform< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m/s, rad/s]< / td >
< / tr >
< tr >
< td class = "org-left" > < code > Vnlm< / code > < / td >
< td class = "org-left" > Measured absolute velocity of NASS's legs< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m/s]< / td >
< / tr >
< / tbody >
< tbody >
< tr >
< td class = "org-left" > < code > Dsm< / code > < / td >
< td class = "org-left" > Position of Sample w.r.t. granite frame< / td >
< td class = "org-right" > 6< / td >
< td class = "org-left" > [m, rad]< / td >
< / tr >
< / tbody >
< / table >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgfd5289f" class = "outline-2" >
< h2 id = "orgfd5289f" > < span class = "section-number-2" > 4< / span > Simulink Library< / h2 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-2" id = "text-4" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgecb3837" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
A simulink library is developed in order to share elements between the different simulink files.
< / p >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgd1ebe0f" class = "outline-3" >
< h3 id = "orgd1ebe0f" > < span class = "section-number-3" > 4.1< / span > < code > inputs< / code > < / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-4-1" >
< p >
< a href = "../nass_library/inputs.slx" > inputs.slx< / a >
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org1ae1d37" class = "outline-3" >
< h3 id = "org1ae1d37" > < span class = "section-number-3" > 4.2< / span > < code > nass_library< / code > < / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-4-2" >
< p >
< a href = "../nass_library/nass_library.slx" > nass< sub > library.slx< / sub > < / a >
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org54e3d07" class = "outline-3" >
< h3 id = "org54e3d07" > < span class = "section-number-3" > 4.3< / span > < code > pos_error_wrt_nass_base< / code > < / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-4-3" >
< p >
< a href = "../nass_library/pos_error_wrt_nass_base.slx" > pos< sub > error< / sub > < sub > wrt< / sub > < sub > nass< / sub > < sub > base.slx< / sub > < / a >
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgc0f0f02" class = "outline-3" >
< h3 id = "orgc0f0f02" > < span class = "section-number-3" > 4.4< / span > < code > QuaternionToAngles< / code > < / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-4-4" >
< p >
< a href = "../nass_library/QuaternionToAngles.slx" > QuaternionToAngles.slx< / a >
< / p >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgbda19ed" class = "outline-3" >
< h3 id = "orgbda19ed" > < span class = "section-number-3" > 4.5< / span > < code > RotationMatrixToAngle< / code > < / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-4-5" >
< p >
< a href = "../nass_library/RotationMatrixToAngle.slx" > RotationMatrixToAngle.slx< / a >
< / p >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org54193c9" class = "outline-2" >
< h2 id = "org54193c9" > < span class = "section-number-2" > 5< / span > Functions< / h2 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-2" id = "text-5" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "org94ca66a" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-orgc1f5840" class = "outline-3" >
< h3 id = "orgc1f5840" > < span class = "section-number-3" > 5.1< / span > computePsdDispl< / h3 >
2019-10-16 11:18:06 +02:00
< div class = "outline-text-3" id = "text-5-1" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "org46da1ed" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/computePsdDispl.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > psd_object< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > computePsdDispl< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > sys_data< / span > , < span class = "org-variable-name" > t_init< / span > , < span class = "org-variable-name" > n_av< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
i_init = find< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.time < span class = "org-type" > > < / span > t_init, < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
han_win = hanning< span class = "org-rainbow-delimiters-depth-1" > (< / span > ceil< span class = "org-rainbow-delimiters-depth-2" > (< / span > length< span class = "org-rainbow-delimiters-depth-3" > (< / span > sys_data.Dx< span class = "org-rainbow-delimiters-depth-4" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-4" > )< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > /< / span > n_av< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Fs = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > sys_data.time< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pdx, f< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Dx< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pdy, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Dy< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pdz, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Dz< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > prx, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Rx< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > pry, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Ry< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > prz, < span class = "org-type" > ~< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > = pwelch< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_data.Rz< span class = "org-rainbow-delimiters-depth-2" > (< / span > i_init< span class = "org-type" > :< / span > end, < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , han_win, < span class = "org-rainbow-delimiters-depth-2" > []< / span > , < span class = "org-rainbow-delimiters-depth-2" > []< / span > , Fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
psd_object = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'f'< / span > , f, ...
< span class = "org-string" > 'dx'< / span > , pdx, ...
< span class = "org-string" > 'dy'< / span > , pdy, ...
< span class = "org-string" > 'dz'< / span > , pdz, ...
< span class = "org-string" > 'rx'< / span > , prx, ...
< span class = "org-string" > 'ry'< / span > , pry, ...
< span class = "org-string" > 'rz'< / span > , prz< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org0d8e9e6" class = "outline-3" >
< h3 id = "org0d8e9e6" > < span class = "section-number-3" > 5.2< / span > computeSetpoint< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-5-2" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orga7c431b" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/computeSetpoint.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > setpoint< / span > = < span class = "org-function-name" > computeSetpoint< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > ty< / span > , < span class = "org-variable-name" > ry< / span > , < span class = "org-variable-name" > rz< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
setpoint = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ty< / span > < / span >
Ty = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ty ;
< 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" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
% Tyinv = [< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
% < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > -< / span > ty ;
% < 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" > 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-matlab-cellbreak" > < span class = "org-comment" > %% Ry< / span > < / span >
Ry = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< 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" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % TMry = Ty*Ry*Tyinv;< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Rz< / span > < / span >
Rz = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 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" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % TMrz = Ty*TMry*Rz*TMry'*Tyinv;< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% All stages< / span > < / span >
< span class = "org-comment" > % < / span > < span class = "org-comment" > < span class = "org-constant" > TM < / span > < / span > < span class = "org-comment" > = TMrz*TMry*Ty;< / span >
TM = Ty< span class = "org-type" > *< / span > Ry< span class = "org-type" > *< / span > Rz;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = RM2angle< span class = "org-rainbow-delimiters-depth-1" > (< / span > TM< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / 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-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = TM< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > thetax, thetay, thetaz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > RM2angle< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > R< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > abs< span class = "org-rainbow-delimiters-depth-2" > (< / span > R< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-highlight-numbers-number" > 3< / 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-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > > < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 < span class = "org-type" > !< / span > = < span class = "org-highlight-numbers-number" > 1< / span > and R31 < span class = "org-type" > !< / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-type" > -< / span > asin< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetax = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetaz = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< 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-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetaz = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / 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-keyword" > else< / span >
thetay = < span class = "org-type" > -< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = < span class = "org-type" > -< / span > thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / 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-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org9a191e3" class = "outline-3" >
< h3 id = "org9a191e3" > < span class = "section-number-3" > 5.3< / span > converErrorBasis< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-5-3" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org2e6fbe8" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/converErrorBasis.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > error_nass< / span > = < span class = "org-function-name" > convertErrorBasis< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > pos< / span > , < span class = "org-variable-name" > setpoint< / span > , < span class = "org-variable-name" > ty< / span > , < span class = "org-variable-name" > ry< / span > , < span class = "org-variable-name" > rz< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-comment" > % convertErrorBasis -< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: convertErrorBasis(p_error, ty, ry, rz)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - p_error - Position error of the sample w.r.t. the granite [m, rad]< / span >
< span class = "org-comment" > % - ty - Measured translation of the Ty stage [m]< / span >
< span class = "org-comment" > % - ry - Measured rotation of the Ry stage [rad]< / span >
< span class = "org-comment" > % - rz - Measured rotation of the Rz stage [rad]< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Outputs:< / span >
< span class = "org-comment" > % - P_nass - Position error of the sample w.r.t. the NASS base [m]< / span >
< span class = "org-comment" > % - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Example:< / span >
< span class = "org-comment" > %< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% If line vector => column vector< / span > < / span >
< span class = "org-keyword" > if< / span > size< span class = "org-rainbow-delimiters-depth-1" > (< / span > pos, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 6< / span >
pos = pos< span class = "org-type" > '< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > if< / span > size< span class = "org-rainbow-delimiters-depth-1" > (< / span > setpoint, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 6< / span >
setpoint = setpoint< span class = "org-type" > '< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Position of the sample in the frame fixed to the Granite< / span > < / span >
P_granite = < span class = "org-rainbow-delimiters-depth-1" > [< / span > pos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Position [m]< / span >
R_granite = < span class = "org-rainbow-delimiters-depth-1" > [< / span > setpoint< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Reference [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Transformation matrices of the stages< / span > < / span >
< span class = "org-comment" > % T-y< / span >
TMty = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ty ;
< 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" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % R-y< / span >
TMry = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< 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" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % R-z< / span >
TMrz = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 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" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute Point coordinates in the new reference fixed to the NASS base< / span > < / span >
< span class = "org-comment" > % P_nass = TMrz*TMry*TMty*P_granite;< / span >
P_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > P_granite;
R_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > R_granite;
dx = R_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > P_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
dy = R_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > P_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
dz = R_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > P_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute new basis vectors linked to the NASS base< / span > < / span >
% ux_nass = TMrz< span class = "org-type" > *< / span > TMry< span class = "org-type" > *< / span > TMty< span class = "org-type" > *< / span > [< span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ];
% ux_nass = ux_nass(< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > );
% uy_nass = TMrz< span class = "org-type" > *< / span > TMry< span class = "org-type" > *< / span > TMty< span class = "org-type" > *< / 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 > ];
% uy_nass = uy_nass(< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > );
% uz_nass = TMrz< span class = "org-type" > *< / span > TMry< span class = "org-type" > *< / span > TMty< span class = "org-type" > *< / 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 > ];
% uz_nass = uz_nass(< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > );
ux_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< span class = "org-type" > \< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ux_nass = ux_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
uy_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< 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" > 1< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
uy_nass = uy_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
uz_nass = TMrz< span class = "org-type" > \< / span > TMry< span class = "org-type" > \< / span > TMty< 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-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
uz_nass = uz_nass< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Rotations error w.r.t. granite Frame< / span > < / span >
< span class = "org-comment" > % Rotations error w.r.t. granite Frame< / span >
rx_nass = pos< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry_nass = pos< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz_nass = pos< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % Rotation matrices of the Sample w.r.t. the Granite< / span >
Mrx_error = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rx_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Mry_error = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > ry_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Mrz_error = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-type" > -< / span > rz_nass< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % Rotation matrix of the Sample w.r.t. the Granite< / span >
Mr_error = Mrz_error< span class = "org-type" > *< / span > Mry_error< span class = "org-type" > *< / span > Mrx_error;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Use matrix to solve< / span > < / span >
R = Mr_error< span class = "org-type" > /< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > ux_nass, uy_nass, uz_nass< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Rotation matrix from NASS base to Sample< / span >
< span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = RM2angle< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
error_nass = < span class = "org-rainbow-delimiters-depth-1" > [< / span > dx; dy; dz; thetax; thetay; thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > thetax, thetay, thetaz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > RM2angle< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > R< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > abs< span class = "org-rainbow-delimiters-depth-2" > (< / span > R< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-highlight-numbers-number" > 3< / 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-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > > < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 < span class = "org-type" > !< / span > = < span class = "org-highlight-numbers-number" > 1< / span > and R31 < span class = "org-type" > !< / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-type" > -< / span > asin< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % thetaybis = pi-thetay;< / span >
thetax = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
% thetaxbis = atan2(R(< span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > )< span class = "org-type" > /< / span > cos(thetaybis), R(< span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > )< span class = "org-type" > /< / span > cos(thetaybis));
thetaz = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< 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-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
% thetazbis = atan2(R(< span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > )< span class = "org-type" > /< / span > cos(thetaybis), R(< span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 1< / span > )< span class = "org-type" > /< / span > cos(thetaybis));
< span class = "org-keyword" > else< / span >
thetaz = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / 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-keyword" > else< / span >
thetay = < span class = "org-type" > -< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = < span class = "org-type" > -< / span > thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / 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-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org2d44efa" class = "outline-3" >
< h3 id = "org2d44efa" > < span class = "section-number-3" > 5.4< / span > generateDiagPidControl< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-5-4" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgd1ce773" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/generateDiagPidControl.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > K< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > generateDiagPidControl< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > G< / span > , < span class = "org-variable-name" > fs< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
pid_opts = pidtuneOptions< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'PhaseMargin'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > , ...
< span class = "org-string" > 'DesignFocus'< / span > , < span class = "org-string" > 'disturbance-rejection'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
K = tf< span class = "org-rainbow-delimiters-depth-1" > (< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < 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" > :< / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 6< / span > < / span >
input_name = G.InputName< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
output_name = G.OutputName< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
K< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = tf< span class = "org-rainbow-delimiters-depth-1" > (< / span > pidtune< span class = "org-rainbow-delimiters-depth-2" > (< / span > minreal< span class = "org-rainbow-delimiters-depth-3" > (< / span > G< span class = "org-rainbow-delimiters-depth-4" > (< / span > output_name, input_name< span class = "org-rainbow-delimiters-depth-4" > )< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > , < span class = "org-string" > 'PIDF'< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > fs, pid_opts< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
K.InputName = G.OutputName;
K.OutputName = G.InputName;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org6f7736a" class = "outline-3" >
< h3 id = "org6f7736a" > < span class = "section-number-3" > 5.5< / span > identifyPlant< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-5-5" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgd69607b" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/identifyPlant.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > sys< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > identifyPlant< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Options for Linearized< / span > < / span >
options = linearizeOptions;
options.SampleTime = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Name of the Simulink File< / span > < / span >
mdl = < span class = "org-string" > 'sim_nano_station_id'< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Input/Output definition< / span > < / span >
2019-12-06 12:04:11 +01:00
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Fn'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Cartesian forces applied by NASS< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Dw'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Ground Motion< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Fs'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % External forces on the sample< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Fnl'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Forces applied on the NASS's legs< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Fd'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'input'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Disturbance Forces< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Dsm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Displacement of the sample< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Fnlm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Force sensor in NASS's legs< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 8< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Dnlm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Displacement of NASS's legs< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 9< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Es'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Position Error w.r.t. NASS base< / span >
io< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 10< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = linio< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > [< / span > mdl, < span class = "org-string" > '/Vlm'< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-string" > 'output'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Measured absolute velocity of the legs< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the linearization< / span > < / span >
2019-12-06 12:04:11 +01:00
G = linearize< span class = "org-rainbow-delimiters-depth-1" > (< / span > mdl, io, options< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
G.InputName = < span class = "org-rainbow-delimiters-depth-1" > {< / span > < span class = "org-string" > 'Fnx'< / span > , < span class = "org-string" > 'Fny'< / span > , < span class = "org-string" > 'Fnz'< / span > , < span class = "org-string" > 'Mnx'< / span > , < span class = "org-string" > 'Mny'< / span > , < span class = "org-string" > 'Mnz'< / span > , ...
< span class = "org-string" > 'Dgx'< / span > , < span class = "org-string" > 'Dgy'< / span > , < span class = "org-string" > 'Dgz'< / span > , ...
< span class = "org-string" > 'Fsx'< / span > , < span class = "org-string" > 'Fsy'< / span > , < span class = "org-string" > 'Fsz'< / span > , < span class = "org-string" > 'Msx'< / span > , < span class = "org-string" > 'Msy'< / span > , < span class = "org-string" > 'Msz'< / span > , ...
2019-12-06 12:04:11 +01:00
< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > , ...
< span class = "org-string" > 'Frzz'< / span > , < span class = "org-string" > 'Ftyx'< / span > , < span class = "org-string" > 'Ftyz'< / span > < span class = "org-rainbow-delimiters-depth-1" > }< / span > ;
2019-10-16 11:18:06 +02:00
G.OutputName = < span class = "org-rainbow-delimiters-depth-1" > {< / span > < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > , ...
< span class = "org-string" > 'Fm1'< / span > , < span class = "org-string" > 'Fm2'< / span > , < span class = "org-string" > 'Fm3'< / span > , < span class = "org-string" > 'Fm4'< / span > , < span class = "org-string" > 'Fm5'< / span > , < span class = "org-string" > 'Fm6'< / span > , ...
< span class = "org-string" > 'Dm1'< / span > , < span class = "org-string" > 'Dm2'< / span > , < span class = "org-string" > 'Dm3'< / span > , < span class = "org-string" > 'Dm4'< / span > , < span class = "org-string" > 'Dm5'< / span > , < span class = "org-string" > 'Dm6'< / span > , ...
2019-12-06 12:04:11 +01:00
< span class = "org-string" > 'Edx'< / span > , < span class = "org-string" > 'Edy'< / span > , < span class = "org-string" > 'Edz'< / span > , < span class = "org-string" > 'Erx'< / span > , < span class = "org-string" > 'Ery'< / span > , < span class = "org-string" > 'Erz'< / span > , ...
< span class = "org-string" > 'Vm1'< / span > , < span class = "org-string" > 'Vm2'< / span > , < span class = "org-string" > 'Vm3'< / span > , < span class = "org-string" > 'Vm4'< / span > , < span class = "org-string" > 'Vm5'< / span > , < span class = "org-string" > 'Vm6'< / span > < span class = "org-rainbow-delimiters-depth-1" > }< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Create the sub transfer functions< / span > < / span >
2019-12-06 12:04:11 +01:00
minreal_tol = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > eps< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-comment" > % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame< / span >
2019-12-06 12:04:11 +01:00
sys.G_cart = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fnx'< / span > , < span class = "org-string" > 'Fny'< / span > , < span class = "org-string" > 'Fnz'< / span > , < span class = "org-string" > 'Mnx'< / span > , < span class = "org-string" > 'Mny'< / span > , < span class = "org-string" > 'Mnz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-comment" > % From ground motion to Sample displacement< / span >
2019-12-06 12:04:11 +01:00
sys.G_gm = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dgx'< / span > , < span class = "org-string" > 'Dgy'< / span > , < span class = "org-string" > 'Dgz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-comment" > % From direct forces applied on the sample to displacement of the sample< / span >
2019-12-06 12:04:11 +01:00
sys.G_fs = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fsx'< / span > , < span class = "org-string" > 'Fsy'< / span > , < span class = "org-string" > 'Fsz'< / span > , < span class = "org-string" > 'Msx'< / span > , < span class = "org-string" > 'Msy'< / span > , < span class = "org-string" > 'Msz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-comment" > % From forces applied on NASS's legs to force sensor in each leg< / span >
2019-12-06 12:04:11 +01:00
sys.G_iff = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fm1'< / span > , < span class = "org-string" > 'Fm2'< / span > , < span class = "org-string" > 'Fm3'< / span > , < span class = "org-string" > 'Fm4'< / span > , < span class = "org-string" > 'Fm5'< / span > , < span class = "org-string" > 'Fm6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-comment" > % From forces applied on NASS's legs to displacement of each leg< / span >
2019-12-06 12:04:11 +01:00
sys.G_dleg = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dm1'< / span > , < span class = "org-string" > 'Dm2'< / span > , < span class = "org-string" > 'Dm3'< / span > , < span class = "org-string" > 'Dm4'< / span > , < span class = "org-string" > 'Dm5'< / span > , < span class = "org-string" > 'Dm6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From forces/torques applied by the NASS to position error< / span >
sys.G_plant = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Edx'< / span > , < span class = "org-string" > 'Edy'< / span > , < span class = "org-string" > 'Edz'< / span > , < span class = "org-string" > 'Erx'< / span > , < span class = "org-string" > 'Ery'< / span > , < span class = "org-string" > 'Erz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Fnx'< / span > , < span class = "org-string" > 'Fny'< / span > , < span class = "org-string" > 'Fnz'< / span > , < span class = "org-string" > 'Mnx'< / span > , < span class = "org-string" > 'Mny'< / span > , < span class = "org-string" > 'Mnz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From forces/torques applied by the NASS to velocity of the actuator< / span >
sys.G_geoph = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Vm1'< / span > , < span class = "org-string" > 'Vm2'< / span > , < span class = "org-string" > 'Vm3'< / span > , < span class = "org-string" > 'Vm4'< / span > , < span class = "org-string" > 'Vm5'< / span > , < span class = "org-string" > 'Vm6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-comment" > % From various disturbance forces to position error< / span >
sys.G_dist = minreal< span class = "org-rainbow-delimiters-depth-1" > (< / span > G< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > , < span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-string" > 'Frzz'< / span > , < span class = "org-string" > 'Ftyx'< / span > , < span class = "org-string" > 'Ftyz'< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , minreal_tol, < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% We remove low frequency and high frequency dynamics that are usually unstable< / span > < / span >
< span class = "org-comment" > % using =freqsep= is risky as it may change the shape of the transfer functions< / span >
% f_min = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > ; % [Hz]
% f_max = < span class = "org-highlight-numbers-number" > 1e4< / span > ; % [Hz]
% [< span class = "org-type" > ~< / span > , sys.G_cart] = freqsep(freqsep(sys.G_cart, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_max), < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_min);
% [< span class = "org-type" > ~< / span > , sys.G_gm] = freqsep(freqsep(sys.G_gm, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_max), < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_min);
% [< span class = "org-type" > ~< / span > , sys.G_fs] = freqsep(freqsep(sys.G_fs, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_max), < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_min);
% [< span class = "org-type" > ~< / span > , sys.G_iff] = freqsep(freqsep(sys.G_iff, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_max), < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_min);
% [< span class = "org-type" > ~< / span > , sys.G_dleg] = freqsep(freqsep(sys.G_dleg, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_max), < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_min);
% [< span class = "org-type" > ~< / span > , sys.G_plant] = freqsep(freqsep(sys.G_plant, < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_max), < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > f_min);
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% We finally verify that the system is stable< / span > < / span >
< span class = "org-keyword" > if< / span > < span class = "org-type" > ~< / span > isstable< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys.G_cart< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ||< / span > < span class = "org-type" > ~< / span > isstable< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys.G_gm< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ||< / span > < span class = "org-type" > ~< / span > isstable< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys.G_fs< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ||< / span > < span class = "org-type" > ~< / span > isstable< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys.G_iff< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ||< / span > < span class = "org-type" > ~< / span > isstable< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys.G_dleg< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ||< / span > < span class = "org-type" > ~< / span > isstable< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys.G_plant< span class = "org-rainbow-delimiters-depth-1" > )< / span >
warning< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'One of the identified system is unstable'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-06 12:04:11 +01:00
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org32dd782" class = "outline-3" >
< h3 id = "org32dd782" > < span class = "section-number-3" > 5.6< / span > runSimulation< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-5-6" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org95edf73" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/runSimulation.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > runSimulation< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > sys_name< / span > , < span class = "org-variable-name" > sys_mass< / span > , < span class = "org-variable-name" > ctrl_type< / span > , < span class = "org-variable-name" > act_damp< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Load the controller and save it for the simulation< / span > < / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > ctrl_type, < span class = "org-string" > 'cl'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > act_damp, < span class = "org-string" > 'none'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
K_obj = load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/K_fb.mat'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
K = K_obj.< span class = "org-rainbow-delimiters-depth-1" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'K_%s_%s'< / span > , sys_mass, sys_name)); < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-3" > (< / span > ctrl_type, < span class = "org-string" > 'cl'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > & & < / span > strcmp< span class = "org-rainbow-delimiters-depth-3" > (< / span > act_damp, < span class = "org-string" > 'iff'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span >
K_obj = load< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-string" > './mat/K_fb_iff.mat'< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > ;
K = K_obj.< span class = "org-rainbow-delimiters-depth-3" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-4" > (< / span > < span class = "org-string" > 'K_%s_%s_iff'< / span > , sys_mass, sys_name)); < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-5" > (< / span > ctrl_type, < span class = "org-string" > 'ol'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span >
K = tf< span class = "org-rainbow-delimiters-depth-5" > (< / span > zeros< span class = "org-rainbow-delimiters-depth-6" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-6" > )< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > 'ctrl_type should be cl or ol'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Active Damping< / span > < / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-5" > (< / span > act_damp, < span class = "org-string" > 'iff'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span >
K_iff_crit = load< span class = "org-rainbow-delimiters-depth-5" > (< / span > < span class = "org-string" > './mat/K_iff_crit.mat'< / span > < span class = "org-rainbow-delimiters-depth-5" > )< / span > ;
K_iff = K_iff_crit.< span class = "org-rainbow-delimiters-depth-5" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-6" > (< / span > < span class = "org-string" > 'K_iff_%s_%s'< / span > , sys_mass, sys_name)); < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K_iff'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > act_damp, < span class = "org-string" > 'none'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
K_iff = tf< span class = "org-rainbow-delimiters-depth-7" > (< / span > zeros< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > './mat/controllers.mat'< / span > , < span class = "org-string" > 'K_iff'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_name, < span class = "org-string" > 'pz'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeNanoHexapod< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'actuator'< / span > , < span class = "org-string" > 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_name, < span class = "org-string" > 'vc'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeNanoHexapod< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'actuator'< / span > , < span class = "org-string" > 'lorentz'< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sys_name should be pz or vc'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_mass, < span class = "org-string" > 'light'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeSample< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-7" > (< / span > sys_mass, < span class = "org-string" > 'heavy'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span >
initializeSample< span class = "org-rainbow-delimiters-depth-7" > (< / span > struct< span class = "org-rainbow-delimiters-depth-8" > (< / span > < span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > < span class = "org-rainbow-delimiters-depth-8" > )< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sys_mass should be light or heavy'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the simulation< / span > < / span >
< span class = "org-matlab-simulink-keyword" > sim< / span > < span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sim_nano_station_ctrl.slx'< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Split the Dsample matrix into vectors< / span > < / span >
< span class = "org-rainbow-delimiters-depth-7" > [< / span > Dx, Dy, Dz, Rx, Ry, Rz< span class = "org-rainbow-delimiters-depth-7" > ]< / span > = matSplit< span class = "org-rainbow-delimiters-depth-7" > (< / span > Es.Data, < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-7" > )< / span > ; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
time = Dsample.Time; < span class = "org-comment" > %#< / span > < span class = "org-comment" > < span class = "org-bold" > ok< / span > < / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save the result< / span > < / span >
filename = sprintf< span class = "org-rainbow-delimiters-depth-7" > (< / span > < span class = "org-string" > 'sim_%s_%s_%s_%s'< / span > , sys_mass, sys_name, ctrl_type, act_damp);
save< span class = "org-rainbow-delimiters-depth-8" > (< / span > sprintf< span class = "org-rainbow-delimiters-depth-9" > (< / span > < span class = "org-string" > './mat/%s.mat'< / span > , filename), ...
< span class = "org-string" > 'time'< / span > , < span class = "org-string" > 'Dx'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Dz'< / span > , < span class = "org-string" > 'Rx'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > , < span class = "org-string" > 'K'< / span > < span class = "org-rainbow-delimiters-depth-9" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org5cc2276" class = "outline-2" >
< h2 id = "org5cc2276" > < span class = "section-number-2" > 6< / span > Initialize Elements< / h2 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-2" id = "text-6" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org18b2c56" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org664a7e3" class = "outline-3" >
< h3 id = "org664a7e3" > < span class = "section-number-3" > 6.1< / span > Experiment< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-1" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org395aa48" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeExperiment.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > initializeExperiment< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > exp_name< / span > , < span class = "org-variable-name" > sys_mass< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > exp_name, < span class = "org-string" > 'tomography'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_mass, < span class = "org-string" > 'light'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
opts_inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Dw'< / span > , < span class = "org-constant" > true< / span > , ...
< span class = "org-string" > 'Rz'< / span > , < span class = "org-highlight-numbers-number" > 60< / span > ...< span class = "org-comment" > % rpm< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > strcpm< span class = "org-rainbow-delimiters-depth-1" > (< / span > sys_mass, < span class = "org-string" > 'heavy'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
opts_inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Dw'< / span > , < span class = "org-constant" > true< / span > , ...
< span class = "org-string" > 'Rz'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > ...< span class = "org-comment" > % rpm< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'sys_mass should be light or heavy'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
initializeInputs< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts_inputs< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'exp_name is only configured for tomography'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org89c58f8" class = "outline-3" >
< h3 id = "org89c58f8" > < span class = "section-number-3" > 6.2< / span > Generate Reference Signals< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-2" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgdda511e" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeInputs.m" > here< / a > .
< / p >
2019-12-06 12:04:11 +01:00
< 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" > ref< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeReferences< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Ts'< / span > , < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > , ...< span class = "org-comment" > % Sampling Frequency [s]< / span >
< span class = "org-string" > 'Dy_type'< / span > , < span class = "org-string" > 'constant'< / span > , ...< span class = "org-comment" > % Either "constant" / "triangular" / "sinusoidal"< / span >
< span class = "org-string" > 'Dy_amplitude'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , ...< span class = "org-comment" > % Amplitude of the displacement [m]< / span >
< span class = "org-string" > 'Dy_period'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , ...< span class = "org-comment" > % Period of the displacement [s]< / span >
< span class = "org-string" > 'Ry_type'< / span > , < span class = "org-string" > 'constant'< / span > , ...< span class = "org-comment" > % Either "constant" / "triangular" / "sinusoidal"< / span >
2019-12-10 18:06:33 +01:00
< span class = "org-string" > 'Ry_amplitude'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , ...< span class = "org-comment" > % Amplitude [rad]< / span >
2019-12-06 12:04:11 +01:00
< span class = "org-string" > 'Ry_period'< / span > , < span class = "org-highlight-numbers-number" > 10< / span > , ...< span class = "org-comment" > % Period of the displacement [s]< / span >
< span class = "org-string" > 'Rz_type'< / span > , < span class = "org-string" > 'constant'< / span > , ...< span class = "org-comment" > % Either "constant" / "rotating"< / span >
2019-12-10 18:06:33 +01:00
< span class = "org-string" > 'Rz_amplitude'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , ...< span class = "org-comment" > % Initial angle [rad]< / span >
2019-12-06 12:04:11 +01:00
< span class = "org-string" > 'Rz_period'< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , ...< span class = "org-comment" > % Period of the rotating [s]< / span >
< span class = "org-string" > 'Dh_type'< / span > , < span class = "org-string" > 'constant'< / span > , ...< span class = "org-comment" > % For now, only constant is implemented< / span >
< span class = "org-string" > 'Dh_pos'< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , ...< span class = "org-comment" > % Initial position [m,m,m,rad,rad,rad] of the top platform< / span >
< span class = "org-string" > 'Rm_type'< / span > , < span class = "org-string" > 'constant'< / span > , ...< span class = "org-comment" > % For now, only constant is implemented< / span >
< span class = "org-string" > 'Rm_pos'< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-constant" > pi< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > , ...< span class = "org-comment" > % Initial position of the two masses< / span >
< span class = "org-string" > 'Dn_type'< / span > , < span class = "org-string" > 'constant'< / span > , ...< span class = "org-comment" > % For now, only constant is implemented< / span >
< span class = "org-string" > 'Dn_pos'< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > ...< span class = "org-comment" > % Initial position [m,m,m,rad,rad,rad] of the top platform< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Set Sampling Time< / span > < / span >
Ts = opts.Ts;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Translation stage - Dy< / span > < / span >
t = < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > :< / span > Ts< span class = "org-type" > :< / span > opts.Dy_period< span class = "org-type" > -< / span > Ts; < span class = "org-comment" > % Time Vector [s]< / span >
Dy = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > switch< / span > < span class = "org-constant" > opts.Dy_type< / span >
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'constant'< / span >
Dy< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = opts.Dy_amplitude;
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'triangular'< / span >
Dy< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Dy_amplitude < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Dy_amplitude< span class = "org-type" > /< / span > opts.Dy_period< span class = "org-type" > *< / span > t;
Dy< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 75< / span > < span class = "org-type" > *< / span > opts.Dy_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > opts.Dy_amplitude < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Dy_amplitude< span class = "org-type" > /< / span > opts.Dy_period< span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 75< / span > < span class = "org-type" > *< / span > opts.Dy_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dy< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 25< / span > < span class = "org-type" > *< / span > opts.Dy_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Dy_amplitude< span class = "org-type" > /< / span > opts.Dy_period< span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 25< / span > < span class = "org-type" > *< / span > opts.Dy_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'sinusoidal'< / span >
Dy< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = opts.Dy_amplitude< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > opts.Dy_period< span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > otherwise< / span >
warning< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Dy_type is not set correctly'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Dy = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'time'< / span > , t, < span class = "org-string" > 'signals'< / span > , struct< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'values'< / span > , Dy< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Ry< / span > < / span >
t = < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > :< / span > Ts< span class = "org-type" > :< / span > opts.Ry_period< span class = "org-type" > -< / span > Ts;
Ry = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > switch< / span > < span class = "org-constant" > opts.Ry_type< / span >
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'constant'< / span >
2019-12-10 18:06:33 +01:00
Ry< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = opts.Ry_amplitude;
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'triangular'< / span >
2019-12-10 18:06:33 +01:00
Ry< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Ry_amplitude < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Ry_amplitude< span class = "org-type" > /< / span > opts.Ry_period< span class = "org-type" > *< / span > t;
Ry< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 75< / span > < span class = "org-type" > *< / span > opts.Ry_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > opts.Ry_amplitude < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Ry_amplitude< span class = "org-type" > /< / span > opts.Ry_period< span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 75< / span > < span class = "org-type" > *< / span > opts.Ry_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Ry< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 25< / span > < span class = "org-type" > *< / span > opts.Ry_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > *< / span > opts.Ry_amplitude< span class = "org-type" > /< / span > opts.Ry_period< span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > (< / span > t< span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 25< / span > < span class = "org-type" > *< / span > opts.Ry_period< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'sinusoidal'< / span >
2019-12-10 18:06:33 +01:00
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > otherwise< / span >
warning< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Ry_type is not set correctly'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Ry = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'time'< / span > , t, < span class = "org-string" > 'signals'< / span > , struct< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'values'< / span > , Ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Rz< / span > < / span >
t = < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > :< / span > Ts< span class = "org-type" > :< / span > opts.Rz_period< span class = "org-type" > -< / span > Ts;
Rz = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > switch< / span > < span class = "org-constant" > opts.Rz_type< / span >
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'constant'< / span >
2019-12-10 18:06:33 +01:00
Rz< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = opts.Rz_amplitude;
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'rotating'< / span >
2019-12-10 18:06:33 +01:00
Rz< span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-type" > :< / span > < span class = "org-type" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span > < span class = "org-type" > < / span > = opts.Rz_amplitude< span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > opts.Rz_period< span class = "org-type" > *< / span > t;
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > otherwise< / span >
warning< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Rz_type is not set correctly'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Rz = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'time'< / span > , t, < span class = "org-string" > 'signals'< / span > , struct< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'values'< / span > , Rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Micro-Hexapod< / span > < / span >
t = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > , Ts< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Dh = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > switch< / span > < span class = "org-constant" > opts.Dh_type< / span >
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'constant'< / span >
Dh = < span class = "org-rainbow-delimiters-depth-1" > [< / span > opts.Dh_pos, opts.Dh_pos< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > otherwise< / span >
warning< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Dh_type is not set correctly'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Dh = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'time'< / span > , t, < span class = "org-string" > 'signals'< / span > , struct< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'values'< / span > , Dh< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensation - Rm< / span > < / span >
t = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > , Ts< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Rm = < span class = "org-rainbow-delimiters-depth-1" > [< / span > opts.Rm_pos, opts.Rm_pos< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Rm = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'time'< / span > , t, < span class = "org-string" > 'signals'< / span > , struct< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'values'< / span > , Rm< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Nano-Hexapod< / span > < / span >
t = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > , Ts< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Dn = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > switch< / span > < span class = "org-constant" > opts.Dn_type< / span >
< span class = "org-keyword" > case< / span > < span class = "org-string" > 'constant'< / span >
Dn = < span class = "org-rainbow-delimiters-depth-1" > [< / span > opts.Dn_pos, opts.Dn_pos< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > otherwise< / span >
warning< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Dn_type is not set correctly'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Dn = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'time'< / span > , t, < span class = "org-string" > 'signals'< / span > , struct< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-string" > 'values'< / span > , Dn< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/nass_references.mat'< / span > , < span class = "org-string" > 'Dy'< / span > , < span class = "org-string" > 'Ry'< / span > , < span class = "org-string" > 'Rz'< / span > , < span class = "org-string" > 'Dh'< / span > , < span class = "org-string" > 'Rm'< / span > , < span class = "org-string" > 'Dn'< / span > , < span class = "org-string" > 'Ts'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org77d160f" class = "outline-3" >
< h3 id = "org77d160f" > < span class = "section-number-3" > 6.3< / span > < span class = "todo TODO" > TODO< / span > Inputs< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-3" >
< p >
2019-12-10 18:06:33 +01:00
< a id = "org7501b33" > < / a >
2019-12-06 12:04:11 +01:00
< / p >
< ul class = "org-ul" >
< li class = "off" > < code > [  ]< / code > < b > This function should not be used anymore< / b > . Now there are two functions to initialize disturbances and references.< / li >
< / ul >
< p >
This Matlab function is accessible < a href = "../src/initializeInputs.m" > here< / a > .
< / p >
2019-10-16 11:18:06 +02:00
< 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" > inputs< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeInputs< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Dw'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Dy'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Ry'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Rz'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Dh'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Rm'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'Dn'< / span > , < span class = "org-constant" > false< / span > ...
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Load Sampling Time and Simulation Time< / span > < / span >
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/sim_conf.mat'< / span > , < span class = "org-string" > 'sim_conf'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Define the time vector< / span > < / span >
t = < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-type" > :< / span > sim_conf.Ts< span class = "org-type" > :< / span > sim_conf.Tsim;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ground motion - Dw< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dw< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dw < span class = "org-type" > ==< / span > < span class = "org-constant" > true< / span >
load< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/perturbations.mat'< / span > , < span class = "org-string" > 'Wxg'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / 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-highlight-numbers-number" > 100< / span > < span class = "org-type" > *< / span > random< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'norm'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , < span class = "org-highlight-numbers-number" > 1< / span > , length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = lsim< span class = "org-rainbow-delimiters-depth-1" > (< / span > Wxg, Dw< 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 > , t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = lsim< span class = "org-rainbow-delimiters-depth-1" > (< / span > Wxg, Dw< 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 > , t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Dw< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = lsim< span class = "org-rainbow-delimiters-depth-1" > (< / span > Wxg, Dw< 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 > , t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dw< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dw < span class = "org-type" > ==< / span > < span class = "org-constant" > false< / span >
Dw = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dw = opts.Dw;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Translation stage - Dy< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dy< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dy < span class = "org-type" > ==< / span > < span class = "org-constant" > true< / span >
Dy = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dy< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dy < span class = "org-type" > ==< / span > < span class = "org-constant" > false< / span >
Dy = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dy = opts.Dy;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Ry< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Ry < span class = "org-type" > ==< / span > < span class = "org-constant" > true< / span >
Ry = < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 360< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > t< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Ry < span class = "org-type" > ==< / span > < span class = "org-constant" > false< / span >
Ry = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > isnumeric< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > length< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 1< / span >
Ry = opts.Ry< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 360< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > ones< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Ry = opts.Ry;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Rz< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Rz < span class = "org-type" > ==< / span > < span class = "org-constant" > true< / span >
Rz = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-type" > *< / span > t;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Rz < span class = "org-type" > ==< / span > < span class = "org-constant" > false< / span >
Rz = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > isnumeric< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > length< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > ==< / span > < span class = "org-highlight-numbers-number" > 1< / span >
Rz = opts.Rz< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 60< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > t;
< span class = "org-keyword" > else< / span >
Rz = opts.Rz;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Micro Hexapod - Dh< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dh< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dh < span class = "org-type" > ==< / span > < span class = "org-constant" > true< / span >
Dh = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dh< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dh < span class = "org-type" > ==< / span > < span class = "org-constant" > false< / span >
Dh = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dh = opts.Dh;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensation - Rm< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Rm< span class = "org-rainbow-delimiters-depth-1" > )< / span >
Rm = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
Rm< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > ones< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Rm = opts.Rm;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Nano Hexapod - Dn< / span > < / span >
< span class = "org-keyword" > if< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dn< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dn < span class = "org-type" > ==< / span > < span class = "org-constant" > true< / span >
Dn = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > elseif< / span > islogical< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.Dn< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > & & < / span > opts.Dn < span class = "org-type" > ==< / span > < span class = "org-constant" > false< / span >
Dn = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
Dn = opts.Dn;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Setpoint - Ds< / span > < / span >
Ds = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< 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" > t< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
Ds< 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 > = computeSetpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > Dy< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , Ry< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , Rz< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% External Forces applied on the Granite< / span > < / span >
Fg = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% External Forces applied on the Sample< / span > < / span >
Fs = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > length< span class = "org-rainbow-delimiters-depth-2" > (< / span > t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Create the input Structure that will contain all the inputs< / span > < / span >
inputs = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'Ts'< / span > , sim_conf.Ts, ...
< span class = "org-string" > 'Dw'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dw, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Dy'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dy, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Ry'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Ry, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Rz'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Rz, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Dh'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dh, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Rm'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Rm, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Dn'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Dn, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Ds'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Ds, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Fg'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Fg, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...
< span class = "org-string" > 'Fs'< / span > , timeseries< span class = "org-rainbow-delimiters-depth-2" > (< / span > Fs, t< span class = "org-rainbow-delimiters-depth-2" > )< / span > ...
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/inputs.mat'< / span > , < span class = "org-string" > 'inputs'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > setpoint< / span > = < span class = "org-function-name" > computeSetpoint< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > ty< / span > , < span class = "org-variable-name" > ry< / span > , < span class = "org-variable-name" > rz< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
setpoint = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ty< / span > < / span >
TMTy = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > ty ;
< 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" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Ry< / span > < / span >
TMRy = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< 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" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > ry< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Rz< / span > < / span >
TMRz = < span class = "org-rainbow-delimiters-depth-1" > [< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > rz< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 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" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% All stages< / span > < / span >
TM = TMTy< span class = "org-type" > *< / span > TMRy< span class = "org-type" > *< / span > TMRz;
< span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > = RM2angle< span class = "org-rainbow-delimiters-depth-1" > (< / span > TM< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / 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-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = TM< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
setpoint< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > thetax, thetay, thetaz< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Custom Functions< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > thetax, thetay, thetaz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > RM2angle< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > R< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > abs< span class = "org-rainbow-delimiters-depth-2" > (< / span > R< span class = "org-rainbow-delimiters-depth-3" > (< / span > < span class = "org-highlight-numbers-number" > 3< / 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-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > > < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 < span class = "org-type" > !< / span > = < span class = "org-highlight-numbers-number" > 1< / span > and R31 < span class = "org-type" > !< / span > = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-type" > -< / span > asin< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetax = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
thetaz = atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< 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-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > thetay< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
thetaz = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > if< / span > abs< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > < < / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > % R31 = < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span >
thetay = < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / 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-keyword" > else< / span >
thetay = < span class = "org-type" > -< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > ;
thetax = < span class = "org-type" > -< / span > thetaz < span class = "org-type" > +< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , < span class = "org-type" > -< / span > R< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / 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-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org962187e" class = "outline-3" >
< h3 id = "org962187e" > < span class = "section-number-3" > 6.4< / span > Ground< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-4" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org763b0ff" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeGround.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > ground< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeGround< / span > < span class = "org-rainbow-delimiters-depth-1" > ()< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
ground = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
ground.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 2< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [m]< / span >
ground.density = < span class = "org-highlight-numbers-number" > 2800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ground.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'ground'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org198c366" class = "outline-3" >
< h3 id = "org198c366" > < span class = "section-number-3" > 6.5< / span > Granite< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-5" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgbdb5477" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeGranite.m" > here< / a > .
< / p >
< div class = "org-src-container" >
2019-12-06 12:04:11 +01:00
< 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" > granite< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeGranite< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
granite = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Static Properties< / span > < / span >
granite.density = < span class = "org-highlight-numbers-number" > 2800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
2019-11-22 10:39:08 +01:00
granite.volume = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 749< / span > ; < span class = "org-comment" > % [m3] TODO - should< / span >
2019-10-16 11:18:06 +02:00
granite.mass = granite.density< span class = "org-type" > *< / span > granite.volume; < span class = "org-comment" > % [kg]< / span >
granite.color = < 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 > ;
granite.STEP = < span class = "org-string" > './STEPS/granite/granite.STEP'< / span > ;
2019-11-22 10:39:08 +01:00
granite.mass_top = < span class = "org-highlight-numbers-number" > 4000< / span > ; < span class = "org-comment" > % [kg] TODO< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Dynamical Properties< / span > < / span >
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > if< / span > opts.rigid
granite.k.x = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.k.y = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.k.z = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.k.rx = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
granite.k.ry = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
granite.k.rz = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
< span class = "org-keyword" > else< / span >
granite.k.x = < span class = "org-highlight-numbers-number" > 4e9< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.k.y = < span class = "org-highlight-numbers-number" > 3e8< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.k.z = < span class = "org-highlight-numbers-number" > 8e8< / span > ; < span class = "org-comment" > % [N/m]< / span >
granite.k.rx = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
granite.k.ry = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
granite.k.rz = < span class = "org-highlight-numbers-number" > 1e6< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
< span class = "org-keyword" > end< / span >
2019-11-22 10:39:08 +01:00
2019-12-06 12:04:11 +01:00
granite.c.x = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > granite.mass_top< span class = "org-type" > *< / span > granite.k.x< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
2019-11-22 10:39:08 +01:00
granite.c.y = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > granite.mass_top< span class = "org-type" > *< / span > granite.k.y< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
granite.c.z = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > granite.mass_top< span class = "org-type" > *< / span > granite.k.z< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
granite.c.rx = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > granite.mass_top< span class = "org-type" > *< / span > granite.k.rx< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N*m/(deg/s)]< / span >
granite.c.ry = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > granite.mass_top< span class = "org-type" > *< / span > granite.k.ry< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N*m/(deg/s)]< / span >
granite.c.rz = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > granite.mass_top< span class = "org-type" > *< / span > granite.k.rz< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N*m/(deg/s)]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Positioning parameters< / span > < / span >
granite.sample_pos = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 8< / span > ; < span class = "org-comment" > % Z-offset for the initial position of the sample [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'granite'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org6e967d2" class = "outline-3" >
< h3 id = "org6e967d2" > < span class = "section-number-3" > 6.6< / span > Translation Stage< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-6" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org2597c18" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeTy.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > ty< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeTy< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-12-06 12:04:11 +01:00
< 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 > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
ty = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Y-Translation - Static Properties< / span > < / span >
< span class = "org-comment" > % Ty Granite frame< / span >
ty.granite_frame.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.granite_frame.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.granite_frame.STEP = < span class = "org-string" > './STEPS/Ty/Ty_Granite_Frame.STEP'< / span > ;
< span class = "org-comment" > % Guide Translation Ty< / span >
ty.guide.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide.STEP = < span class = "org-string" > './STEPS/ty/Ty_Guide.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation12< / span >
ty.guide12.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide12.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide12.STEP = < span class = "org-string" > './STEPS/Ty/Ty_Guide_12.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation11< / span >
ty.guide11.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide11.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide11.STEP = < span class = "org-string" > './STEPS/ty/Ty_Guide_11.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation22< / span >
ty.guide22.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide22.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide22.STEP = < span class = "org-string" > './STEPS/ty/Ty_Guide_22.STEP'< / span > ;
< span class = "org-comment" > % Ty - Guide_Translation21< / span >
ty.guide21.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.guide21.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.guide21.STEP = < span class = "org-string" > './STEPS/Ty/Ty_Guide_21.STEP'< / span > ;
< span class = "org-comment" > % Ty - Plateau translation< / span >
ty.frame.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.frame.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.frame.STEP = < span class = "org-string" > './STEPS/ty/Ty_Stage.STEP'< / span > ;
< span class = "org-comment" > % Ty Stator Part< / span >
ty.stator.density = < span class = "org-highlight-numbers-number" > 5400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.stator.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.stator.STEP = < span class = "org-string" > './STEPS/ty/Ty_Motor_Stator.STEP'< / span > ;
< span class = "org-comment" > % Ty Rotor Part< / span >
ty.rotor.density = < span class = "org-highlight-numbers-number" > 5400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ty.rotor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ty.rotor.STEP = < span class = "org-string" > './STEPS/ty/Ty_Motor_Rotor.STEP'< / span > ;
2019-11-22 10:39:08 +01:00
ty.m = < span class = "org-highlight-numbers-number" > 1000< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Y-Translation - Dynamicals Properties< / span > < / span >
< span class = "org-keyword" > if< / span > opts.rigid
2019-12-06 12:04:11 +01:00
ty.k.ax = < span class = "org-highlight-numbers-number" > 1e12< / span > ; % Axial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (y) [N< span class = "org-type" > /< / span > m]
ty.k.rad = < span class = "org-highlight-numbers-number" > 1e12< / span > ; % Radial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (x< span class = "org-type" > -< / span > z) [N< span class = "org-type" > /< / span > m]
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > else< / span >
2019-12-06 12:04:11 +01:00
ty.k.ax = < span class = "org-highlight-numbers-number" > 5e8< / span > ; % Axial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (y) [N< span class = "org-type" > /< / span > m]
ty.k.rad = < span class = "org-highlight-numbers-number" > 5e7< / span > ; % Radial Stiffness < span class = "org-keyword" > for< / span > each of the < span class = "org-highlight-numbers-number" > 4< / span > guidance (x< span class = "org-type" > -< / span > z) [N< span class = "org-type" > /< / span > m]
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
2019-11-22 10:39:08 +01:00
ty.c.ax = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ty.k.ax< span class = "org-type" > *< / span > ty.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ty.c.rad = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ty.k.rad< span class = "org-type" > *< / span > ty.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'ty'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org6c95b53" class = "outline-3" >
< h3 id = "org6c95b53" > < span class = "section-number-3" > 6.7< / span > Tilt Stage< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-7" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org29b1fab" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeRy.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > ry< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeRy< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-12-06 12:04:11 +01:00
< 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 > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
ry = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Static Properties< / span > < / span >
< span class = "org-comment" > % Ry - Guide for the tilt stage< / span >
ry.guide.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.guide.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.guide.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Guide.STEP'< / span > ;
< span class = "org-comment" > % Ry - Rotor of the motor< / span >
ry.rotor.density = < span class = "org-highlight-numbers-number" > 2400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.rotor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.rotor.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Motor_Axis.STEP'< / span > ;
< span class = "org-comment" > % Ry - Motor< / span >
ry.motor.density = < span class = "org-highlight-numbers-number" > 3200< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.motor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.motor.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Motor.STEP'< / span > ;
< span class = "org-comment" > % Ry - Plateau Tilt< / span >
ry.stage.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
ry.stage.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
ry.stage.STEP = < span class = "org-string" > './STEPS/ry/Tilt_Stage.STEP'< / span > ;
2019-11-22 10:39:08 +01:00
ry.m = < span class = "org-highlight-numbers-number" > 800< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Tilt Stage - Dynamical Properties< / span > < / span >
< span class = "org-keyword" > if< / span > opts.rigid
2019-12-06 12:04:11 +01:00
ry.k.tilt = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % Rotation stiffness around y [N*m/deg]< / span >
ry.k.h = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % Stiffness in the direction of the guidance [N/m]< / span >
ry.k.rad = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % Stiffness in the top direction [N/m]< / span >
ry.k.rrad = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % Stiffness in the side direction [N/m]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > else< / span >
2019-12-06 12:04:11 +01:00
ry.k.tilt = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % Rotation stiffness around y [N*m/deg]< / span >
ry.k.h = < span class = "org-highlight-numbers-number" > 1e8< / span > ; < span class = "org-comment" > % Stiffness in the direction of the guidance [N/m]< / span >
ry.k.rad = < span class = "org-highlight-numbers-number" > 1e8< / span > ; < span class = "org-comment" > % Stiffness in the top direction [N/m]< / span >
ry.k.rrad = < span class = "org-highlight-numbers-number" > 1e8< / span > ; < span class = "org-comment" > % Stiffness in the side direction [N/m]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
2019-11-22 10:39:08 +01:00
ry.c.h = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.h< span class = "org-type" > *< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry.c.rad = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.rad< span class = "org-type" > *< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry.c.rrad = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.rrad< span class = "org-type" > *< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
ry.c.tilt = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > ry.k.tilt< span class = "org-type" > *< / span > ry.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Positioning parameters< / span > < / span >
ry.z_offset = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 58178< / span > ; < span class = "org-comment" > % Z-Offset so that the center of rotation matches the sample center [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'ry'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org13271e7" class = "outline-3" >
< h3 id = "org13271e7" > < span class = "section-number-3" > 6.8< / span > Spindle< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-8" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org50164cd" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeRz.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > rz< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeRz< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-12-06 12:04:11 +01:00
< 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 > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
rz = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Static Properties< / span > < / span >
< span class = "org-comment" > % Spindle - Slip Ring< / span >
rz.slipring.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
rz.slipring.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
rz.slipring.STEP = < span class = "org-string" > './STEPS/rz/Spindle_Slip_Ring.STEP'< / span > ;
< span class = "org-comment" > % Spindle - Rotor< / span >
rz.rotor.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
rz.rotor.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
rz.rotor.STEP = < span class = "org-string" > './STEPS/rz/Spindle_Rotor.STEP'< / span > ;
< span class = "org-comment" > % Spindle - Stator< / span >
rz.stator.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
rz.stator.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
rz.stator.STEP = < span class = "org-string" > './STEPS/rz/Spindle_Stator.STEP'< / span > ;
< span class = "org-comment" > % Estimated mass of the mooving part< / span >
rz.m = < span class = "org-highlight-numbers-number" > 250< / span > ; < span class = "org-comment" > % [kg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Spindle - Dynamical Properties< / span > < / span >
< span class = "org-keyword" > if< / span > opts.rigid
2019-12-06 12:04:11 +01:00
rz.k.rot = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % Rotational Stiffness (Rz) [N*m/deg]< / span >
rz.k.tilt = < span class = "org-highlight-numbers-number" > 1e10< / span > ; < span class = "org-comment" > % Rotational Stiffness (Rx, Ry) [N*m/deg]< / span >
rz.k.ax = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % Axial Stiffness (Z) [N/m]< / span >
rz.k.rad = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % Radial Stiffness (X, Y) [N/m]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > else< / span >
2019-12-06 12:04:11 +01:00
rz.k.rot = < span class = "org-highlight-numbers-number" > 1e6< / span > ; < span class = "org-comment" > % TODO - Rotational Stiffness (Rz) [N*m/deg]< / span >
rz.k.tilt = < span class = "org-highlight-numbers-number" > 1e6< / span > ; < span class = "org-comment" > % Rotational Stiffness (Rx, Ry) [N*m/deg]< / span >
rz.k.ax = < span class = "org-highlight-numbers-number" > 2e9< / span > ; < span class = "org-comment" > % Axial Stiffness (Z) [N/m]< / span >
rz.k.rad = < span class = "org-highlight-numbers-number" > 7e8< / span > ; < span class = "org-comment" > % Radial Stiffness (X, Y) [N/m]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
2019-11-22 10:39:08 +01:00
< span class = "org-comment" > % Damping< / span >
rz.c.ax = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.ax< span class = "org-type" > *< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz.c.rad = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.rad< span class = "org-type" > *< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz.c.tilt = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.tilt< span class = "org-type" > *< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rz.c.rot = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > rz.k.rot< span class = "org-type" > *< / span > rz.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'rz'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org3c482fe" class = "outline-3" >
< h3 id = "org3c482fe" > < span class = "section-number-3" > 6.9< / span > Initialize Hexapod legs' length< / h3 >
2019-12-06 12:04:11 +01:00
< div class = "outline-text-3" id = "text-6-9" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgee2252f" > < / a >
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeHexapodPosition.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > hexapod< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeHexapodPosition< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > hexapod< / span > , < span class = "org-variable-name" > AP< / span > , < span class = "org-variable-name" > ARB< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-comment" > % initializeHexapodPosition -< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: initializeHexapodPosition(hexapod, AP, ARB)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - hexapod - Hexapod object containing the geometry of the hexapod< / span >
< span class = "org-comment" > % - AP - Position vector of point OB expressed in frame {A}< / span >
< span class = "org-comment" > % - ARB - Rotation Matrix expressed in frame {A}< / span >
L0 = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :length< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > L< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
Bbi = hexapod.pos_top_tranform< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > '< / span > ;
Aai = hexapod.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > '< / span > ;
L0< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > AP< span class = "org-type" > '*< / span > AP < span class = "org-type" > +< / span > Bbi< span class = "org-type" > '*< / span > Bbi < span class = "org-type" > +< / span > Aai< span class = "org-type" > '*< / span > Aai < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > AP< span class = "org-type" > '*< / span > Aai < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > AP< span class = "org-type" > '*< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > ARB< span class = "org-type" > *< / span > Bbi< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > ARB< span class = "org-type" > *< / span > Bbi< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '*< / span > Aai< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
hexapod.L0 = L0;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org7c295aa" class = "outline-3" >
< h3 id = "org7c295aa" > < span class = "section-number-3" > 6.10< / span > Micro Hexapod< / h3 >
< div class = "outline-text-3" id = "text-6-10" >
< p >
< a id = "org9e25f5d" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeMicroHexapod.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > micro_hexapod< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeMicroHexapod< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
2019-12-10 18:06:33 +01:00
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'rigid'< / span > , < span class = "org-constant" > false< / span > , ...
< span class = "org-string" > 'AP'< / span > , zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , ...< span class = "org-comment" > % Wanted position in [m] of OB with respect to frame {A}< / span >
< span class = "org-string" > 'ARB'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ...< span class = "org-comment" > % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}< / span >
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-12-06 12:04:11 +01:00
< 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 > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Stewart Object< / span > < / span >
micro_hexapod = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
micro_hexapod.h = < span class = "org-highlight-numbers-number" > 350< / span > ; < span class = "org-comment" > % Total height of the platform [mm]< / span >
2019-12-10 18:06:33 +01:00
micro_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 270< / span > ; < span class = "org-comment" > % Distance from the top of the mobile platform to the Jacobian point [mm]< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Bottom Plate - Mechanical Design< / span > < / span >
BP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
BP.rad.int = < span class = "org-highlight-numbers-number" > 110< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
BP.rad.ext = < span class = "org-highlight-numbers-number" > 207< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % External Radius [mm]< / span >
BP.thickness = < span class = "org-highlight-numbers-number" > 26< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
BP.leg.rad = < span class = "org-highlight-numbers-number" > 175< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
BP.leg.ang = < span class = "org-highlight-numbers-number" > 9< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
BP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
BP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
BP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > BP.rad.int BP.thickness; BP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext BP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Top Plate - Mechanical Design< / span > < / span >
TP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
TP.rad.int = < span class = "org-highlight-numbers-number" > 82< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.rad.ext = < span class = "org-highlight-numbers-number" > 150< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.thickness = < span class = "org-highlight-numbers-number" > 26< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
TP.leg.rad = < span class = "org-highlight-numbers-number" > 118< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
TP.leg.ang = < span class = "org-highlight-numbers-number" > 12< / span > .< span class = "org-highlight-numbers-number" > 1< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
TP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
TP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
TP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > TP.rad.int TP.thickness; TP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext TP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Struts< / span > < / span >
Leg = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
Leg.stroke = < span class = "org-highlight-numbers-number" > 10e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > ; < span class = "org-comment" > % Maximum Stroke of each leg [m]< / span >
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > if< / span > opts.rigid
Leg.k.ax = < span class = "org-highlight-numbers-number" > 1e12< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
< span class = "org-keyword" > else< / span >
Leg.k.ax = < span class = "org-highlight-numbers-number" > 2e7< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
< span class = "org-keyword" > end< / span >
2019-12-10 18:06:33 +01:00
Leg.ksi.ax = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > ; % Modal damping ksi = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > c< span class = "org-type" > /< / span > sqrt(km) []
2019-10-16 11:18:06 +02:00
Leg.rad.bottom = < span class = "org-highlight-numbers-number" > 25< / span > ; < span class = "org-comment" > % Radius of the cylinder of the bottom part [mm]< / span >
Leg.rad.top = < span class = "org-highlight-numbers-number" > 17< / span > ; < span class = "org-comment" > % Radius of the cylinder of the top part [mm]< / span >
Leg.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
Leg.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.sphere.bottom = Leg.rad.bottom; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.sphere.top = Leg.rad.top; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.m = TP.density< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.ext< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > ^< / 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-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.int< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > ^< / 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-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / 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-highlight-numbers-number" > 6< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
Leg = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > Leg< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Sphere< / span > < / span >
SP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
SP.height.bottom = < span class = "org-highlight-numbers-number" > 27< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.height.top = < span class = "org-highlight-numbers-number" > 27< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.density.bottom = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.density.top = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.k.ax = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
SP.ksi.ax = < span class = "org-highlight-numbers-number" > 10< / span > ;
SP.thickness.bottom = SP.height.bottom< span class = "org-type" > -< / span > Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.thickness.top = SP.height.top< span class = "org-type" > -< / span > Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.rad.bottom = Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.rad.top = Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.m = SP.density.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > SP.rad.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < 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 > SP.height.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
SP = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > SP< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
Leg.support.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.bottom; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom SP.height.bottom< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Leg.support.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.top; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top SP.height.top< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
micro_hexapod.BP = BP;
micro_hexapod.TP = TP;
micro_hexapod.Leg = Leg;
micro_hexapod.SP = SP;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
micro_hexapod = initializeParameters< span class = "org-rainbow-delimiters-depth-1" > (< / span > micro_hexapod< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-12-10 18:06:33 +01:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Setup equilibrium position of each leg< / span > < / span >
micro_hexapod.L0 = initializeHexapodPosition< span class = "org-rainbow-delimiters-depth-1" > (< / span > micro_hexapod, opts.AP, opts.ARB< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'micro_hexapod'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > element< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > updateDamping< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > element< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-12-06 12:04:11 +01:00
field = fieldnames< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k< 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" > field< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
element.c.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > element.ksi.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k.< span class = "org-rainbow-delimiters-depth-2" > (< / span > field< span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > element.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeParameters< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Connection points on base and top plate w.r.t. World frame at the center of the base plate< / span > < / span >
stewart.pos_base = 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 > ;
stewart.pos_top = 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 > ;
alpha_b = stewart.BP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % angle de dé calage par rapport à < span class = "org-highlight-numbers-number" > 120< / span > deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % < span class = "org-type" > +-< / span > offset angle from < span class = "org-highlight-numbers-number" > 120< / span > degree spacing on top
height = < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.h< span class = "org-type" > -< / span > stewart.BP.thickness< span class = "org-type" > -< / span > stewart.TP.thickness< span class = "org-type" > -< / span > stewart.Leg.sphere.bottom< span class = "org-type" > -< / span > stewart.Leg.sphere.top< span class = "org-type" > -< / span > stewart.SP.thickness.bottom< span class = "org-type" > -< / span > stewart.SP.thickness.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % TODO< / span >
radius_b = stewart.BP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % rayon emplacement support base< / span >
radius_t = stewart.TP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % top radius in meters< / 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" > 3< / span > < / span >
< span class = "org-comment" > % base points< / span >
angle_m_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_b;
angle_p_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_b;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< 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-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< 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-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % top points< / span >
% Top points are < span class = "org-highlight-numbers-number" > 60< / span > degrees offset
angle_m_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
angle_p_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-comment" > % permute pos_top points so that legs are end points of base and top points< / span >
stewart.pos_top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 5< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; %< span class = "org-highlight-numbers-number" > 6th< / span > point on top connects to < span class = "org-highlight-numbers-number" > 1st< / span > on bottom
stewart.pos_top_tranform = stewart.pos_top < span class = "org-type" > -< / span > height< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / 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 > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% leg vectors< / span > < / span >
legs = stewart.pos_top < span class = "org-type" > -< / span > stewart.pos_base;
leg_length = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors = 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 >
leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = norm< 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-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors< 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< 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 > leg_length< 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-keyword" > end< / span >
stewart.Leg.lenght = < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > *< / span > leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ;
stewart.Leg.shape.bot = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; ...
stewart.Leg.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; ...
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
stewart.Leg.rad.top stewart.Leg.lenght; ...
stewart.Leg.rad.top < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght; ...
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Calculate revolute and cylindrical axes< / span > < / span >
rev1 = 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 > ;
rev2 = 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 > ;
cyl1 = 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 >
rev1< 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 > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / 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" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev1< 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 > = rev1< 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 > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< 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 > cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< 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 > = rev2< 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 > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
cyl1< 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 > = leg_vectors< 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-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Coordinate systems< / span > < / span >
stewart.lower_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
stewart.upper_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-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 >
stewart.lower_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.upper_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / 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" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Position Matrix< / span > < / span >
stewart.M_pos_base = stewart.pos_base < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > height< span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > stewart.TP.thickness< span class = "org-type" > +< / span > stewart.Leg.sphere.top< span class = "org-type" > +< / span > stewart.SP.thickness.top< span class = "org-type" > +< / span > stewart.jacobian< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / 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 > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute Jacobian Matrix< / span > < / span >
aa = stewart.pos_top_tranform < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.jacobian < span class = "org-type" > -< / span > stewart.TP.thickness < span class = "org-type" > -< / span > stewart.SP.height.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / 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 > ;
stewart.J = getJacobianMatrix< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-type" > '< / span > , aa< span class = "org-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
2019-12-10 18:06:33 +01:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > J< / span > = < span class = "org-function-name" > getJacobianMatrix< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > RM< / span > , < span class = "org-variable-name" > M_pos_base< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
2019-10-16 11:18:06 +02:00
% RM< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] unit vector of each leg in the fixed frame
% M_pos_base< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] vector of the leg connection at the top platform location in the fixed frame
J = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = RM< span class = "org-type" > '< / span > ;
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > M_pos_base, RM< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > '< / span > ;
< span class = "org-keyword" > end< / span >
2019-12-10 18:06:33 +01:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > L< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeHexapodPosition< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > hexapod< / span > , < span class = "org-variable-name" > AP< / span > , < span class = "org-variable-name" > ARB< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-comment" > % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Syntax: initializeHexapodPosition(hexapod, AP, ARB)< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % Inputs:< / span >
< span class = "org-comment" > % - hexapod - Hexapod object containing the geometry of the hexapod< / span >
< span class = "org-comment" > % - AP - Position vector of point OB expressed in frame {A} in [m]< / span >
< span class = "org-comment" > % - ARB - Rotation Matrix expressed in frame {A}< / span >
< span class = "org-comment" > % Wanted Length of the hexapod's legs [m]< / span >
L = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / span > = < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 1< / span > < / span > < span class = "org-constant" > :length< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > L< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
Bbi = hexapod.pos_top_tranform< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > '< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; hexapod.TP.thickness< span class = "org-type" > +< / span > hexapod.Leg.sphere.top< span class = "org-type" > +< / span > hexapod.SP.thickness.top< span class = "org-type" > +< / span > hexapod.jacobian< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [m]< / span >
Aai = hexapod.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > , < span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > '< / span > < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-highlight-numbers-number" > 0< / span > ; hexapod.BP.thickness< span class = "org-type" > +< / span > hexapod.Leg.sphere.bottom< span class = "org-type" > +< / span > hexapod.SP.thickness.bottom< span class = "org-type" > -< / span > micro_hexapod.h< span class = "org-type" > -< / span > hexapod.jacobian< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [m]< / span >
L< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > AP< span class = "org-type" > '*< / span > AP < span class = "org-type" > +< / span > Bbi< span class = "org-type" > '*< / span > Bbi < span class = "org-type" > +< / span > Aai< span class = "org-type" > '*< / span > Aai < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > AP< span class = "org-type" > '*< / span > Aai < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > AP< span class = "org-type" > '*< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > ARB< span class = "org-type" > *< / span > Bbi< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > ARB< span class = "org-type" > *< / span > Bbi< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '*< / span > Aai< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org72492f3" class = "outline-3" >
< h3 id = "org72492f3" > < span class = "section-number-3" > 6.11< / span > Center of gravity compensation< / h3 >
< div class = "outline-text-3" id = "text-6-11" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org991aec0" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeAxisc.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > axisc< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeAxisc< / span > < span class = "org-rainbow-delimiters-depth-1" > ()< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
axisc = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensator - Static Properties< / span > < / span >
< span class = "org-comment" > % Structure< / span >
axisc.structure.density = < span class = "org-highlight-numbers-number" > 3400< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.structure.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.structure.STEP = < span class = "org-string" > './STEPS/axisc/axisc_structure.STEP'< / span > ;
< span class = "org-comment" > % Wheel< / span >
axisc.wheel.density = < span class = "org-highlight-numbers-number" > 2700< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.wheel.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 753< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.wheel.STEP = < span class = "org-string" > './STEPS/axisc/axisc_wheel.STEP'< / span > ;
< span class = "org-comment" > % Mass< / span >
axisc.mass.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.mass.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.mass.STEP = < span class = "org-string" > './STEPS/axisc/axisc_mass.STEP'< / span > ;
< span class = "org-comment" > % Gear< / span >
axisc.gear.density = < span class = "org-highlight-numbers-number" > 7800< / span > ; < span class = "org-comment" > % [kg/m3]< / span >
axisc.gear.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 792< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 820< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 933< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
axisc.gear.STEP = < span class = "org-string" > './STEPS/axisc/axisc_gear.STEP'< / span > ;
axisc.m = < span class = "org-highlight-numbers-number" > 40< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Axis Compensator - Dynamical Properties< / span > < / span >
2019-12-06 12:04:11 +01:00
% axisc.k.ax = < span class = "org-highlight-numbers-number" > 1< / span > ; % TODO [N< span class = "org-type" > *< / span > m< span class = "org-type" > /< / span > deg)]
% axisc.c.ax = (< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > )< span class = "org-type" > *< / span > sqrt(axisc.k.ax< span class = "org-type" > /< / span > axisc.m);
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'axisc'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-11-22 10:39:08 +01:00
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org68d031b" class = "outline-3" >
< h3 id = "org68d031b" > < span class = "section-number-3" > 6.12< / span > Mirror< / h3 >
< div class = "outline-text-3" id = "text-6-12" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org43cadec" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeMirror.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > []< / span > < / span > = < span class = "org-function-name" > initializeMirror< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > ...
< span class = "org-string" > 'shape'< / span > , < span class = "org-string" > 'spherical'< / span > , ...< span class = "org-comment" > % spherical or conical< / span >
< span class = "org-string" > 'angle'< / span > , < span class = "org-highlight-numbers-number" > 45< / span > ...
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
mirror = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
mirror.h = < span class = "org-highlight-numbers-number" > 50< / span > ; < span class = "org-comment" > % height of the mirror [mm]< / span >
mirror.thickness = < span class = "org-highlight-numbers-number" > 25< / span > ; < span class = "org-comment" > % Thickness of the plate supporting the sample [mm]< / span >
mirror.hole_rad = < span class = "org-highlight-numbers-number" > 120< / span > ; < span class = "org-comment" > % radius of the hole in the mirror [mm]< / span >
mirror.support_rad = < span class = "org-highlight-numbers-number" > 100< / span > ; < span class = "org-comment" > % radius of the support plate [mm]< / span >
mirror.jacobian = < span class = "org-highlight-numbers-number" > 150< / span > ; < span class = "org-comment" > % point of interest offset in z (above the top surfave) [mm]< / span >
mirror.rad = < span class = "org-highlight-numbers-number" > 180< / span > ; < span class = "org-comment" > % radius of the mirror (at the bottom surface) [mm]< / span >
mirror.density = < span class = "org-highlight-numbers-number" > 2400< / span > ; < span class = "org-comment" > % Density of the mirror [kg/m3]< / span >
mirror.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 4< / 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-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color of the mirror< / span >
mirror.cone_length = mirror.rad< span class = "org-type" > *< / span > tand< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.angle< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > mirror.h< span class = "org-type" > +< / span > mirror.jacobian; < span class = "org-comment" > % Distance from Apex point of the cone to jacobian point< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Shape< / span > < / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > ...
< span class = "org-highlight-numbers-number" > 0< / span > mirror.h< span class = "org-type" > -< / span > mirror.thickness
mirror.hole_rad mirror.h< span class = "org-type" > -< / span > mirror.thickness; ...
mirror.hole_rad < span class = "org-highlight-numbers-number" > 0< / span > ; ...
mirror.rad < span class = "org-highlight-numbers-number" > 0< / span > ...
< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.shape, < span class = "org-string" > 'spherical'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
mirror.sphere_radius = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > mirror.jacobian< span class = "org-type" > +< / span > mirror.h< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > +< / span > mirror.rad< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Radius of the sphere [mm]< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > z< / span > = < span class = "org-constant" > linspace< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 0< / span > < / span > < span class = "org-constant" > , mirror.h, < / span > < span class = "org-constant" > < span class = "org-highlight-numbers-number" > 101< / span > < / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > mirror.shape; sqrt< span class = "org-rainbow-delimiters-depth-2" > (< / span > mirror.sphere_radius< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > -< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > z< span class = "org-type" > -< / span > mirror.jacobian< span class = "org-type" > -< / span > mirror.h< span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > z< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.shape, < span class = "org-string" > 'conical'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > mirror.shape; mirror.rad< span class = "org-type" > +< / span > mirror.h< span class = "org-type" > /< / span > tand< span class = "org-rainbow-delimiters-depth-2" > (< / span > opts.angle< span class = "org-rainbow-delimiters-depth-2" > )< / span > mirror.h< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'Shape should be either conical or spherical'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
mirror.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > mirror.shape; < span class = "org-highlight-numbers-number" > 0< / span > mirror.h< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'mirror'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org74149ff" class = "outline-3" >
< h3 id = "org74149ff" > < span class = "section-number-3" > 6.13< / span > Nano Hexapod< / h3 >
< div class = "outline-text-3" id = "text-6-13" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgcb27cac" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeNanoHexapod.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > nano_hexapod< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeNanoHexapod< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'actuator'< / span > , < span class = "org-string" > 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Stewart Object< / span > < / span >
nano_hexapod = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
nano_hexapod.h = < span class = "org-highlight-numbers-number" > 90< / span > ; < span class = "org-comment" > % Total height of the platform [mm]< / span >
nano_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 175< / span > ; < span class = "org-comment" > % Point where the Jacobian is computed => Center of rotation [mm]< / span >
% nano_hexapod.jacobian = < span class = "org-highlight-numbers-number" > 174< / span > .< span class = "org-highlight-numbers-number" > 26< / span > ; % Point where the Jacobian is computed =< span class = "org-type" > > < / span > Center of rotation [mm]
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Bottom Plate< / span > < / span >
BP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
BP.rad.int = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
BP.rad.ext = < span class = "org-highlight-numbers-number" > 150< / span > ; < span class = "org-comment" > % External Radius [mm]< / span >
BP.thickness = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
BP.leg.rad = < span class = "org-highlight-numbers-number" > 100< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
BP.leg.ang = < span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
BP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ;% Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
BP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
BP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > BP.rad.int BP.thickness; BP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; BP.rad.ext BP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Top Plate< / span > < / span >
TP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
TP.rad.int = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.rad.ext = < span class = "org-highlight-numbers-number" > 100< / span > ; < span class = "org-comment" > % Internal Radius [mm]< / span >
TP.thickness = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Thickness [mm]< / span >
TP.leg.rad = < span class = "org-highlight-numbers-number" > 90< / span > ; < span class = "org-comment" > % Radius where the legs articulations are positionned [mm]< / span >
TP.leg.ang = < span class = "org-highlight-numbers-number" > 5< / span > ; < span class = "org-comment" > % Angle Offset [deg]< / span >
TP.density = < span class = "org-highlight-numbers-number" > 8000< / span > ;% Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
TP.color = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
TP.shape = < span class = "org-rainbow-delimiters-depth-1" > [< / span > TP.rad.int TP.thickness; TP.rad.int < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext < span class = "org-highlight-numbers-number" > 0< / span > ; TP.rad.ext TP.thickness< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Leg< / span > < / span >
Leg = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
Leg.stroke = < span class = "org-highlight-numbers-number" > 80e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 6< / span > ; < span class = "org-comment" > % Maximum Stroke of each leg [m]< / span >
< span class = "org-keyword" > if< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.actuator, < span class = "org-string" > 'piezo'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
Leg.k.ax = < span class = "org-highlight-numbers-number" > 1e7< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
< span class = "org-keyword" > elseif< / span > strcmp< span class = "org-rainbow-delimiters-depth-1" > (< / span > opts.actuator, < span class = "org-string" > 'lorentz'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
Leg.k.ax = < span class = "org-highlight-numbers-number" > 1e4< / span > ; < span class = "org-comment" > % Stiffness of each leg [N/m]< / span >
< span class = "org-keyword" > else< / span >
error< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts.actuator should be piezo or lorentz'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
Leg.ksi.ax = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Maximum amplification at resonance []< / span >
Leg.rad.bottom = < span class = "org-highlight-numbers-number" > 12< / span > ; < span class = "org-comment" > % Radius of the cylinder of the bottom part [mm]< / span >
Leg.rad.top = < span class = "org-highlight-numbers-number" > 10< / span > ; < span class = "org-comment" > % Radius of the cylinder of the top part [mm]< / span >
Leg.density = < span class = "org-highlight-numbers-number" > 8000< / span > ; % Density of the material [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
Leg.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 5< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % Color [rgb]< / span >
Leg.sphere.bottom = Leg.rad.bottom; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.sphere.top = Leg.rad.top; < span class = "org-comment" > % Size of the sphere at the end of the leg [mm]< / span >
Leg.m = TP.density< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.ext< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-3" > )< / span > < span class = "org-type" > ^< / 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-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > -< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-3" > (< / span > TP.rad.int< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > ^< / 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-type" > *< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > TP.thickness< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1000< / 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-highlight-numbers-number" > 6< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
Leg = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > Leg< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Sphere< / span > < / span >
SP = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
SP.height.bottom = < span class = "org-highlight-numbers-number" > 15< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.height.top = < span class = "org-highlight-numbers-number" > 15< / span > ; < span class = "org-comment" > % [mm]< / span >
SP.density.bottom = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.density.top = < span class = "org-highlight-numbers-number" > 8000< / span > ; % [kg< span class = "org-type" > /< / span > m< span class = "org-type" > ^< / span > < span class = "org-highlight-numbers-number" > 3< / span > ]
SP.color.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.color.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 7< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; < span class = "org-comment" > % [rgb]< / span >
SP.k.ax = < span class = "org-highlight-numbers-number" > 0< / span > ; < span class = "org-comment" > % [N*m/deg]< / span >
2019-12-06 12:04:11 +01:00
SP.ksi.ax = < span class = "org-highlight-numbers-number" > 0< / span > ;
2019-10-16 11:18:06 +02:00
SP.thickness.bottom = SP.height.bottom< span class = "org-type" > -< / span > Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.thickness.top = SP.height.top< span class = "org-type" > -< / span > Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.rad.bottom = Leg.sphere.bottom; < span class = "org-comment" > % [mm]< / span >
SP.rad.top = Leg.sphere.top; < span class = "org-comment" > % [mm]< / span >
SP.m = SP.density.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > SP.rad.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < 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 > SP.height.bottom< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % TODO [kg]< / span >
SP = updateDamping< span class = "org-rainbow-delimiters-depth-1" > (< / span > SP< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
Leg.support.bottom = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.bottom; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.bottom SP.height.bottom< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
Leg.support.top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > SP.thickness.top; < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top < span class = "org-highlight-numbers-number" > 0< / span > ; SP.rad.top SP.height.top< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
nano_hexapod.BP = BP;
nano_hexapod.TP = TP;
nano_hexapod.Leg = Leg;
nano_hexapod.SP = SP;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
nano_hexapod = initializeParameters< span class = "org-rainbow-delimiters-depth-1" > (< / span > nano_hexapod< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'nano_hexapod'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > element< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > updateDamping< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > element< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
field = fieldnames< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k< 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" > field< / span > < span class = "org-constant" > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < / span >
2019-12-06 12:04:11 +01:00
< span class = "org-keyword" > if< / span > element.ksi.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / 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-highlight-numbers-number" > 0< / span >
element.c.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > /< / span > element.ksi.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > element.k.< span class = "org-rainbow-delimiters-depth-2" > (< / span > field< span class = "org-rainbow-delimiters-depth-3" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-3" > }< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > /< / span > element.m< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > else< / span >
element.c.< span class = "org-rainbow-delimiters-depth-1" > (< / span > field< span class = "org-rainbow-delimiters-depth-2" > {< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-2" > }< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-highlight-numbers-number" > 0< / span > ;
< span class = "org-keyword" > end< / span >
2019-10-16 11:18:06 +02:00
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeParameters< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > stewart< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Connection points on base and top plate w.r.t. World frame at the center of the base plate< / span > < / span >
stewart.pos_base = 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 > ;
stewart.pos_top = 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 > ;
alpha_b = stewart.BP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % angle de dé calage par rapport à < span class = "org-highlight-numbers-number" > 120< / span > deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 180< / span > ; % < span class = "org-type" > +-< / span > offset angle from < span class = "org-highlight-numbers-number" > 120< / span > degree spacing on top
height = < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.h< span class = "org-type" > -< / span > stewart.BP.thickness< span class = "org-type" > -< / span > stewart.TP.thickness< span class = "org-type" > -< / span > stewart.Leg.sphere.bottom< span class = "org-type" > -< / span > stewart.Leg.sphere.top< span class = "org-type" > -< / span > stewart.SP.thickness.bottom< span class = "org-type" > -< / span > stewart.SP.thickness.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % TODO< / span >
radius_b = stewart.BP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % rayon emplacement support base< / span >
radius_t = stewart.TP.leg.rad< span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 001< / span > ; < span class = "org-comment" > % top radius in meters< / 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" > 3< / span > < / span >
< span class = "org-comment" > % base points< / span >
angle_m_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_b;
angle_p_b = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_b;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_b< 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-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_base< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_b< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_b< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_b< 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-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-comment" > % top points< / span >
% Top points are < span class = "org-highlight-numbers-number" > 60< / span > degrees offset
angle_m_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > -< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
angle_p_t = < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 3< / 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-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > +< / span > alpha_t < span class = "org-type" > +< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 6< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 1< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_m_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.pos_top< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = < span class = "org-rainbow-delimiters-depth-1" > [< / span > radius_t< span class = "org-type" > *< / span > cos< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , radius_t< span class = "org-type" > *< / span > sin< span class = "org-rainbow-delimiters-depth-2" > (< / span > angle_p_t< span class = "org-rainbow-delimiters-depth-2" > )< / span > , height< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-comment" > % permute pos_top points so that legs are end points of base and top points< / span >
stewart.pos_top = < span class = "org-rainbow-delimiters-depth-1" > [< / span > stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ; stewart.pos_top< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 5< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ; %< span class = "org-highlight-numbers-number" > 6th< / span > point on top connects to < span class = "org-highlight-numbers-number" > 1st< / span > on bottom
stewart.pos_top_tranform = stewart.pos_top < span class = "org-type" > -< / span > height< span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / 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 > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% leg vectors< / span > < / span >
legs = stewart.pos_top < span class = "org-type" > -< / span > stewart.pos_base;
leg_length = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors = 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 >
leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = norm< 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-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
leg_vectors< 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< 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 > leg_length< 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-keyword" > end< / span >
stewart.Leg.lenght = < span class = "org-highlight-numbers-number" > 1000< / span > < span class = "org-type" > *< / span > leg_length< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 1< / span > .< span class = "org-highlight-numbers-number" > 5< / span > ;
stewart.Leg.shape.bot = < span class = "org-rainbow-delimiters-depth-1" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > ; ...
stewart.Leg.rad.bottom < span class = "org-highlight-numbers-number" > 0< / span > ; ...
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
stewart.Leg.rad.top stewart.Leg.lenght; ...
stewart.Leg.rad.top < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght; ...
< span class = "org-highlight-numbers-number" > 0< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-type" > *< / span > stewart.Leg.lenght< span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Calculate revolute and cylindrical axes< / span > < / span >
rev1 = 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 > ;
rev2 = 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 > ;
cyl1 = 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 >
rev1< 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 > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / 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" > 1< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev1< 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 > = rev1< 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 > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< 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 > cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > , leg_vectors< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
rev2< 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 > = rev2< 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 > norm< span class = "org-rainbow-delimiters-depth-1" > (< / span > rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
cyl1< 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 > = leg_vectors< 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-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Coordinate systems< / span > < / span >
stewart.lower_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
stewart.upper_leg = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'rotation'< / span > , eye< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-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 >
stewart.lower_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > ;
stewart.upper_leg< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-constant" > i< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > .rotation = < span class = "org-rainbow-delimiters-depth-1" > [< / span > rev1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , rev2< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > '< / span > , cyl1< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-constant" > i< / span > ,< span class = "org-type" > :< / 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" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Position Matrix< / span > < / span >
stewart.M_pos_base = stewart.pos_base < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > height< span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > stewart.TP.thickness< span class = "org-type" > +< / span > stewart.Leg.sphere.top< span class = "org-type" > +< / span > stewart.SP.thickness.top< span class = "org-type" > +< / span > stewart.jacobian< span class = "org-rainbow-delimiters-depth-2" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / 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 > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Compute Jacobian Matrix< / span > < / span >
aa = stewart.pos_top_tranform < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > stewart.jacobian < span class = "org-type" > -< / span > stewart.TP.thickness < span class = "org-type" > -< / span > stewart.SP.height.top< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > *< / span > < span class = "org-highlight-numbers-number" > 1e< / span > < span class = "org-type" > -< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-type" > *< / span > < span class = "org-rainbow-delimiters-depth-1" > [< / span > zeros< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > , < span class = "org-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-2" > )< / span > ,ones< span class = "org-rainbow-delimiters-depth-2" > (< / span > < span class = "org-highlight-numbers-number" > 6< / 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 > ;
stewart.J = getJacobianMatrix< span class = "org-rainbow-delimiters-depth-1" > (< / span > leg_vectors< span class = "org-type" > '< / span > , aa< span class = "org-type" > '< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > function< / span > < span class = "org-variable-name" > J< / span > = < span class = "org-function-name" > getJacobianMatrix< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > RM< / span > ,< span class = "org-variable-name" > M_pos_base< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
% RM< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] unit vector of each leg in the fixed frame
% M_pos_base< span class = "org-type" > :< / span > [< span class = "org-highlight-numbers-number" > 3x6< / span > ] vector of the leg connection at the top platform location in the fixed frame
J = zeros< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 3< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = RM< span class = "org-type" > '< / span > ;
J< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-type" > :< / span > , < span class = "org-highlight-numbers-number" > 4< / span > < span class = "org-type" > :< / span > < span class = "org-highlight-numbers-number" > 6< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > = cross< span class = "org-rainbow-delimiters-depth-1" > (< / span > M_pos_base, RM< span class = "org-rainbow-delimiters-depth-1" > )< / span > < span class = "org-type" > '< / span > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org94d9a68" class = "outline-3" >
< h3 id = "org94d9a68" > < span class = "section-number-3" > 6.14< / span > Cedrat Actuator< / h3 >
< div class = "outline-text-3" id = "text-6-14" >
2019-12-06 12:04:11 +01:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "orgdb4500f" > < / a >
2019-12-06 12:04:11 +01:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeCedratPiezo.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > cedrat< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeCedratPiezo< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
opts = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Stewart Object< / span > < / span >
cedrat = struct< span class = "org-rainbow-delimiters-depth-1" > ()< / span > ;
cedrat.k = < span class = "org-highlight-numbers-number" > 10e7< / span > ; < span class = "org-comment" > % Linear Stiffness of each "blade" [N/m]< / span >
cedrat.ka = < span class = "org-highlight-numbers-number" > 10e7< / span > ; < span class = "org-comment" > % Linear Stiffness of the stack [N/m]< / span >
cedrat.c = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > cedrat.k< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
cedrat.ca = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > cedrat.ka< span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % [N/(m/s)]< / span >
cedrat.L = < span class = "org-highlight-numbers-number" > 80< / span > ; < span class = "org-comment" > % Total Width of the Actuator[mm]< / span >
cedrat.H = < span class = "org-highlight-numbers-number" > 45< / span > ; < span class = "org-comment" > % Total Height of the Actuator [mm]< / span >
cedrat.L2 = sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > cedrat.L< span class = "org-type" > /< / 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-highlight-numbers-number" > 2< / span > < span class = "org-type" > +< / span > < span class = "org-rainbow-delimiters-depth-2" > (< / span > cedrat.H< span class = "org-type" > /< / 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-highlight-numbers-number" > 2< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ; < span class = "org-comment" > % Length of the elipsoidal sections [mm]< / span >
cedrat.alpha = < span class = "org-highlight-numbers-number" > 180< / span > < span class = "org-type" > /< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > atan2< span class = "org-rainbow-delimiters-depth-1" > (< / span > cedrat.L< span class = "org-type" > /< / span > < span class = "org-highlight-numbers-number" > 2< / span > , cedrat.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-comment" > % [deg]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'cedrat'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
2019-12-10 18:06:33 +01:00
< div id = "outline-container-org8ceaacc" class = "outline-3" >
< h3 id = "org8ceaacc" > < span class = "section-number-3" > 6.15< / span > Sample< / h3 >
< div class = "outline-text-3" id = "text-6-15" >
2019-10-16 11:18:06 +02:00
< p >
2019-12-10 18:06:33 +01:00
< a id = "org96164b7" > < / a >
2019-10-16 11:18:06 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "../src/initializeSample.m" > here< / a > .
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > [< / span > < / span > < span class = "org-variable-name" > sample< / span > < span class = "org-variable-name" > < span class = "org-rainbow-delimiters-depth-1" > ]< / span > < / span > = < span class = "org-function-name" > initializeSample< / span > < span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-variable-name" > opts_param< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Default values for opts< / span > < / span >
sample = struct< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'radius'< / span > , < span class = "org-highlight-numbers-number" > 100< / span > , ...
< span class = "org-string" > 'height'< / span > , < span class = "org-highlight-numbers-number" > 300< / span > , ...
< span class = "org-string" > 'mass'< / span > , < span class = "org-highlight-numbers-number" > 50< / span > , ...
< span class = "org-string" > 'offset'< / span > , < span class = "org-highlight-numbers-number" > 0< / span > , ...
< span class = "org-string" > 'color'< / span > , < span class = "org-rainbow-delimiters-depth-2" > [< / span > < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 45< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 45< / span > , < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 45< / span > < span class = "org-rainbow-delimiters-depth-2" > ]< / span > ...
< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Populate opts with input parameters< / span > < / span >
< span class = "org-keyword" > if< / span > exist< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > 'opts_param'< / span > ,< span class = "org-string" > 'var'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span >
< 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 >
sample.< 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 > ;
< span class = "org-keyword" > end< / span >
< span class = "org-keyword" > end< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %%< / span > < / span >
sample.k.x = < span class = "org-highlight-numbers-number" > 1e8< / span > ;
2019-12-06 12:04:11 +01:00
sample.c.x = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > sample.k.x< span class = "org-type" > *< / span > sample.mass< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
sample.k.y = < span class = "org-highlight-numbers-number" > 1e8< / span > ;
2019-12-06 12:04:11 +01:00
sample.c.y = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > sample.k.y< span class = "org-type" > *< / span > sample.mass< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
sample.k.z = < span class = "org-highlight-numbers-number" > 1e8< / span > ;
2019-12-06 12:04:11 +01:00
sample.c.z = < span class = "org-highlight-numbers-number" > 0< / span > .< span class = "org-highlight-numbers-number" > 1< / span > < span class = "org-type" > *< / span > sqrt< span class = "org-rainbow-delimiters-depth-1" > (< / span > sample.k.z< span class = "org-type" > *< / span > sample.mass< span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
2019-10-16 11:18:06 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Save< / span > < / span >
save< span class = "org-rainbow-delimiters-depth-1" > (< / span > < span class = "org-string" > './mat/stages.mat'< / span > , < span class = "org-string" > 'sample'< / span > , < span class = "org-string" > '-append'< / span > < span class = "org-rainbow-delimiters-depth-1" > )< / span > ;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< / div >
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Dehaeze Thomas< / p >
2019-12-10 18:06:33 +01:00
< p class = "date" > Created: 2019-12-10 mar. 18:05< / p >
2019-10-16 11:18:06 +02:00
< p class = "validation" > < a href = "http://validator.w3.org/check?uri=referer" > Validate< / a > < / p >
< / div >
< / body >
< / html >
