Remove main usefull heading
This commit is contained in:
parent
117430c8d1
commit
302c63964f
@ -1,9 +1,10 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2019-12-12 jeu. 11:25 -->
|
||||
<!-- 2020-01-29 mer. 20:25 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
||||
<title>Matlab Functions used for the NASS Project</title>
|
||||
@ -205,7 +206,7 @@
|
||||
@licstart The following is the entire license notice for the
|
||||
JavaScript code in this tag.
|
||||
|
||||
Copyright (C) 2012-2019 Free Software Foundation, Inc.
|
||||
Copyright (C) 2012-2020 Free Software Foundation, Inc.
|
||||
|
||||
The JavaScript code in this tag is free software: you can
|
||||
redistribute it and/or modify it under the terms of the GNU
|
||||
@ -258,33 +259,21 @@ for the JavaScript code in this tag.
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#orgac71426">1. Functions</a>
|
||||
<ul>
|
||||
<li><a href="#orga30ed3e">1.1. computePsdDispl</a></li>
|
||||
<li><a href="#org164599d">1.2. computeSetpoint</a></li>
|
||||
<li><a href="#org0d7c91f">1.3. converErrorBasis</a></li>
|
||||
<li><a href="#orge57e769">1.4. generateDiagPidControl</a></li>
|
||||
<li><a href="#orgb953a5b">1.5. identifyPlant</a></li>
|
||||
<li><a href="#org442df30">1.6. Inverse Kinematics of the Hexapod</a></li>
|
||||
<li><a href="#orgce14673">1.7. computeReferencePose</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgf4ff272">1. <span class="todo TODO">TODO</span> computePsdDispl</a></li>
|
||||
<li><a href="#org4e69743">2. <span class="todo TODO">TODO</span> computeSetpoint</a></li>
|
||||
<li><a href="#org293101b">3. <span class="todo TODO">TODO</span> converErrorBasis</a></li>
|
||||
<li><a href="#orgebd9e8d">4. Inverse Kinematics of the Hexapod</a></li>
|
||||
<li><a href="#orga234cde">5. computeReferencePose</a></li>
|
||||
<li><a href="#orgf5cf06b">6. Compute the Sample Position Error w.r.t. the NASS</a></li>
|
||||
</ul>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgac71426" class="outline-2">
|
||||
<h2 id="orgac71426"><span class="section-number-2">1</span> Functions</h2>
|
||||
<div id="outline-container-orgf4ff272" class="outline-2">
|
||||
<h2 id="orgf4ff272"><span class="section-number-2">1</span> <span class="todo TODO">TODO</span> computePsdDispl</h2>
|
||||
<div class="outline-text-2" id="text-1">
|
||||
<p>
|
||||
<a id="org8643823"></a>
|
||||
</p>
|
||||
</div>
|
||||
<div id="outline-container-orga30ed3e" class="outline-3">
|
||||
<h3 id="orga30ed3e"><span class="section-number-3">1.1</span> computePsdDispl</h3>
|
||||
<div class="outline-text-3" id="text-1-1">
|
||||
<p>
|
||||
<a id="orgeb6170e"></a>
|
||||
<a id="orgf1d0895"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -292,39 +281,39 @@ 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>;
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[psd_object]</span> = <span class="org-function-name">computePsdDispl</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>)
|
||||
i_init = find(sys_data.time <span class="org-type">></span> t_init, 1);
|
||||
|
||||
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>;
|
||||
han_win = hanning(ceil(length(sys_data.Dx(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>))<span class="org-type">/</span>n_av));
|
||||
Fs = 1<span class="org-type">/</span>sys_data.time(2);
|
||||
|
||||
<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>;
|
||||
[pdx, f] = pwelch(sys_data.Dx(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>), han_win, [], [], Fs);
|
||||
[pdy, <span class="org-type">~</span>] = pwelch(sys_data.Dy(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>), han_win, [], [], Fs);
|
||||
[pdz, <span class="org-type">~</span>] = pwelch(sys_data.Dz(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>), han_win, [], [], Fs);
|
||||
|
||||
<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>;
|
||||
[prx, <span class="org-type">~</span>] = pwelch(sys_data.Rx(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>), han_win, [], [], Fs);
|
||||
[pry, <span class="org-type">~</span>] = pwelch(sys_data.Ry(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>), han_win, [], [], Fs);
|
||||
[prz, <span class="org-type">~</span>] = pwelch(sys_data.Rz(i_init<span class="org-type">:</span>end, <span class="org-type">:</span>), han_win, [], [], Fs);
|
||||
|
||||
psd_object = struct<span class="org-rainbow-delimiters-depth-1">(</span>...
|
||||
psd_object = struct(...
|
||||
<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-string">'rz'</span>, prz);
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org164599d" class="outline-3">
|
||||
<h3 id="org164599d"><span class="section-number-3">1.2</span> computeSetpoint</h3>
|
||||
<div class="outline-text-3" id="text-1-2">
|
||||
<div id="outline-container-org4e69743" class="outline-2">
|
||||
<h2 id="org4e69743"><span class="section-number-2">2</span> <span class="todo TODO">TODO</span> computeSetpoint</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<p>
|
||||
<a id="org5f98a47"></a>
|
||||
<a id="org2ff14b2"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -332,34 +321,34 @@ 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>
|
||||
<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-variable-name">ty</span>, <span class="org-variable-name">ry</span>, <span class="org-variable-name">rz</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>;
|
||||
setpoint = zeros(<span class="org-variable-name">6</span>, 1);
|
||||
|
||||
<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>;
|
||||
Ty = [1 0 0 0 ;
|
||||
0 1 0 ty ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
% 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-comment">% Tyinv = [1 0 0 0 ;</span>
|
||||
<span class="org-comment">% 0 1 0 -ty ;</span>
|
||||
<span class="org-comment">% 0 0 1 0 ;</span>
|
||||
<span class="org-comment">% 0 0 0 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>;
|
||||
Ry = [ cos(ry) 0 sin(ry) 0 ;
|
||||
0 1 0 0 ;
|
||||
<span class="org-type">-</span>sin(ry) 0 cos(ry) 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
<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>;
|
||||
Rz = [cos(rz) <span class="org-type">-</span>sin(rz) 0 0 ;
|
||||
sin(rz) cos(rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
<span class="org-comment">% TMrz = Ty*TMry*Rz*TMry'*Tyinv;</span>
|
||||
|
||||
@ -368,25 +357,25 @@ Rz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-ra
|
||||
|
||||
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>;
|
||||
[thetax, thetay, thetaz] = RM2angle(TM(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3));
|
||||
|
||||
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>;
|
||||
setpoint<span class="org-type">(1:3) </span>= TM(1<span class="org-type">:</span>3, 4);
|
||||
setpoint<span class="org-type">(4:6) </span>= [thetax, thetay, thetaz];
|
||||
|
||||
<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">function</span> <span class="org-variable-name">[thetax, thetay, thetaz]</span> = <span class="org-function-name">RM2angle</span>(<span class="org-variable-name">R</span>)
|
||||
<span class="org-keyword">if</span> abs(abs(R(3, 1)) <span class="org-type">-</span> 1) <span class="org-type">></span> 1e<span class="org-type">-</span>6 <span class="org-comment">% R31 != 1 and R31 != -1</span>
|
||||
thetay = <span class="org-type">-</span>asin(R(3, 1));
|
||||
thetax = atan2(R(3, 2)<span class="org-type">/</span>cos(thetay), R(3, 3)<span class="org-type">/</span>cos(thetay));
|
||||
thetaz = atan2(R(2, 1)<span class="org-type">/</span>cos(thetay), R(1, 1)<span class="org-type">/</span>cos(thetay));
|
||||
<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>;
|
||||
thetaz = 0;
|
||||
<span class="org-keyword">if</span> abs(R(3, 1)<span class="org-type">+</span>1) <span class="org-type"><</span> 1e<span class="org-type">-</span>6 <span class="org-comment">% R31 = -1</span>
|
||||
thetay = <span class="org-constant">pi</span><span class="org-type">/</span>2;
|
||||
thetax = thetaz <span class="org-type">+</span> atan2(R(1, 2), R(1, 3));
|
||||
<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>;
|
||||
thetay = <span class="org-type">-</span><span class="org-constant">pi</span><span class="org-type">/</span>2;
|
||||
thetax = <span class="org-type">-</span>thetaz <span class="org-type">+</span> atan2(<span class="org-type">-</span>R(1, 2), <span class="org-type">-</span>R(1, 3));
|
||||
<span class="org-keyword">end</span>
|
||||
<span class="org-keyword">end</span>
|
||||
<span class="org-keyword">end</span>
|
||||
@ -396,11 +385,11 @@ setpoint<span class="org-rainbow-delimiters-depth-1">(</span><span class="org-hi
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org0d7c91f" class="outline-3">
|
||||
<h3 id="org0d7c91f"><span class="section-number-3">1.3</span> converErrorBasis</h3>
|
||||
<div class="outline-text-3" id="text-1-3">
|
||||
<div id="outline-container-org293101b" class="outline-2">
|
||||
<h2 id="org293101b"><span class="section-number-2">3</span> <span class="todo TODO">TODO</span> converErrorBasis</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
<p>
|
||||
<a id="orgdf67397"></a>
|
||||
<a id="orged36710"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -408,7 +397,7 @@ 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>
|
||||
<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-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-comment">% convertErrorBasis -</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Syntax: convertErrorBasis(p_error, ty, ry, rz)</span>
|
||||
@ -427,107 +416,107 @@ This Matlab function is accessible <a href="../src/converErrorBasis.m">here</a>.
|
||||
<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>
|
||||
<span class="org-keyword">if</span> size(pos, 2) <span class="org-type">==</span> 6
|
||||
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>
|
||||
<span class="org-keyword">if</span> size(setpoint, 2) <span class="org-type">==</span> 6
|
||||
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>
|
||||
P_granite = [pos(1<span class="org-type">:</span>3); 1]; <span class="org-comment">% Position [m]</span>
|
||||
R_granite = [setpoint(1<span class="org-type">:</span>3); 1]; <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>;
|
||||
TMty = [1 0 0 0 ;
|
||||
0 1 0 ty ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
<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>;
|
||||
TMry = [ cos(ry) 0 sin(ry) 0 ;
|
||||
0 1 0 0 ;
|
||||
<span class="org-type">-</span>sin(ry) 0 cos(ry) 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
<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>;
|
||||
TMrz = [cos(rz) <span class="org-type">-</span>sin(rz) 0 0 ;
|
||||
sin(rz) cos(rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
<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>;
|
||||
dx = R_nass(1)<span class="org-type">-</span>P_nass(1);
|
||||
dy = R_nass(2)<span class="org-type">-</span>P_nass(2);
|
||||
dz = R_nass(3)<span class="org-type">-</span>P_nass(3);
|
||||
|
||||
<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>);
|
||||
<span class="org-comment">% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];</span>
|
||||
<span class="org-comment">% ux_nass = ux_nass(1:3);</span>
|
||||
<span class="org-comment">% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];</span>
|
||||
<span class="org-comment">% uy_nass = uy_nass(1:3);</span>
|
||||
<span class="org-comment">% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];</span>
|
||||
<span class="org-comment">% uz_nass = uz_nass(1: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>;
|
||||
ux_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span>[1; 0; 0; 0];
|
||||
ux_nass = ux_nass(1<span class="org-type">:</span>3);
|
||||
uy_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span>[0; 1; 0; 0];
|
||||
uy_nass = uy_nass(1<span class="org-type">:</span>3);
|
||||
uz_nass = TMrz<span class="org-type">\</span>TMry<span class="org-type">\</span>TMty<span class="org-type">\</span>[0; 0; 1; 0];
|
||||
uz_nass = uz_nass(1<span class="org-type">:</span>3);
|
||||
|
||||
<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>;
|
||||
rx_nass = pos(4);
|
||||
ry_nass = pos(5);
|
||||
rz_nass = pos(6);
|
||||
|
||||
<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>;
|
||||
Mrx_error = [1 0 0 ;
|
||||
0 cos(<span class="org-type">-</span>rx_nass) <span class="org-type">-</span>sin(<span class="org-type">-</span>rx_nass) ;
|
||||
0 sin(<span class="org-type">-</span>rx_nass) cos(<span class="org-type">-</span>rx_nass)];
|
||||
|
||||
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>;
|
||||
Mry_error = [ cos(<span class="org-type">-</span>ry_nass) 0 sin(<span class="org-type">-</span>ry_nass) ;
|
||||
0 1 0 ;
|
||||
<span class="org-type">-</span>sin(<span class="org-type">-</span>ry_nass) 0 cos(<span class="org-type">-</span>ry_nass)];
|
||||
|
||||
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>;
|
||||
Mrz_error = [cos(<span class="org-type">-</span>rz_nass) <span class="org-type">-</span>sin(<span class="org-type">-</span>rz_nass) 0 ;
|
||||
sin(<span class="org-type">-</span>rz_nass) cos(<span class="org-type">-</span>rz_nass) 0 ;
|
||||
0 0 1];
|
||||
|
||||
<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>
|
||||
R = Mr_error<span class="org-type">/</span>[ux_nass, uy_nass, uz_nass]; <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>;
|
||||
[thetax, thetay, thetaz] = RM2angle(R);
|
||||
|
||||
error_nass = <span class="org-rainbow-delimiters-depth-1">[</span>dx; dy; dz; thetax; thetay; thetaz<span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
error_nass = [dx; dy; dz; thetax; thetay; thetaz];
|
||||
|
||||
<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-keyword">function</span> <span class="org-variable-name">[thetax, thetay, thetaz]</span> = <span class="org-function-name">RM2angle</span>(<span class="org-variable-name">R</span>)
|
||||
<span class="org-keyword">if</span> abs(abs(R(3, 1)) <span class="org-type">-</span> 1) <span class="org-type">></span> 1e<span class="org-type">-</span>6 <span class="org-comment">% R31 != 1 and R31 != -1</span>
|
||||
thetay = <span class="org-type">-</span>asin(R(3, 1));
|
||||
<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));
|
||||
thetax = atan2(R(3, 2)<span class="org-type">/</span>cos(thetay), R(3, 3)<span class="org-type">/</span>cos(thetay));
|
||||
<span class="org-comment">% thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));</span>
|
||||
thetaz = atan2(R(2, 1)<span class="org-type">/</span>cos(thetay), R(1, 1)<span class="org-type">/</span>cos(thetay));
|
||||
<span class="org-comment">% thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));</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>;
|
||||
thetaz = 0;
|
||||
<span class="org-keyword">if</span> abs(R(3, 1)<span class="org-type">+</span>1) <span class="org-type"><</span> 1e<span class="org-type">-</span>6 <span class="org-comment">% R31 = -1</span>
|
||||
thetay = <span class="org-constant">pi</span><span class="org-type">/</span>2;
|
||||
thetax = thetaz <span class="org-type">+</span> atan2(R(1, 2), R(1, 3));
|
||||
<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>;
|
||||
thetay = <span class="org-type">-</span><span class="org-constant">pi</span><span class="org-type">/</span>2;
|
||||
thetax = <span class="org-type">-</span>thetaz <span class="org-type">+</span> atan2(<span class="org-type">-</span>R(1, 2), <span class="org-type">-</span>R(1, 3));
|
||||
<span class="org-keyword">end</span>
|
||||
<span class="org-keyword">end</span>
|
||||
<span class="org-keyword">end</span>
|
||||
@ -538,142 +527,11 @@ error_nass = <span class="org-rainbow-delimiters-depth-1">[</span>dx; dy; dz; th
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orge57e769" class="outline-3">
|
||||
<h3 id="orge57e769"><span class="section-number-3">1.4</span> generateDiagPidControl</h3>
|
||||
<div class="outline-text-3" id="text-1-4">
|
||||
<div id="outline-container-orgebd9e8d" class="outline-2">
|
||||
<h2 id="orgebd9e8d"><span class="section-number-2">4</span> Inverse Kinematics of the Hexapod</h2>
|
||||
<div class="outline-text-2" id="text-4">
|
||||
<p>
|
||||
<a id="org94ba4ce"></a>
|
||||
</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>
|
||||
|
||||
<div id="outline-container-orgb953a5b" class="outline-3">
|
||||
<h3 id="orgb953a5b"><span class="section-number-3">1.5</span> identifyPlant</h3>
|
||||
<div class="outline-text-3" id="text-1-5">
|
||||
<p>
|
||||
<a id="org2662524"></a>
|
||||
</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>
|
||||
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>
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
||||
G = linearize<span class="org-rainbow-delimiters-depth-1">(</span>mdl, io, options<span class="org-rainbow-delimiters-depth-1">)</span>;
|
||||
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>, ...
|
||||
<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>;
|
||||
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>, ...
|
||||
<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>;
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Create the sub transfer functions</span></span>
|
||||
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>;
|
||||
<span class="org-comment">% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame</span>
|
||||
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>;
|
||||
<span class="org-comment">% From ground motion to Sample displacement</span>
|
||||
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>;
|
||||
<span class="org-comment">% From direct forces applied on the sample to displacement of the sample</span>
|
||||
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>;
|
||||
<span class="org-comment">% From forces applied on NASS's legs to force sensor in each leg</span>
|
||||
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>;
|
||||
<span class="org-comment">% From forces applied on NASS's legs to displacement of each leg</span>
|
||||
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>
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org442df30" class="outline-3">
|
||||
<h3 id="org442df30"><span class="section-number-3">1.6</span> Inverse Kinematics of the Hexapod</h3>
|
||||
<div class="outline-text-3" id="text-1-6">
|
||||
<p>
|
||||
<a id="org7102666"></a>
|
||||
<a id="orgfcd14bf"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -681,7 +539,7 @@ This Matlab function is accessible <a href="src/inverseKinematicsHexapod.m">here
|
||||
</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">L</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">inverseKinematicsHexapod</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>
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[L]</span> = <span class="org-function-name">inverseKinematicsHexapod</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-comment">% inverseKinematicsHexapod - 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: inverseKinematicsHexapod(hexapod, AP, ARB)</span>
|
||||
@ -692,24 +550,25 @@ This Matlab function is accessible <a href="src/inverseKinematicsHexapod.m">here
|
||||
<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>;
|
||||
L = zeros(6, 1);
|
||||
|
||||
<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>hexapod.h<span class="org-type">-</span>hexapod.jacobian<span class="org-rainbow-delimiters-depth-1">]</span>; <span class="org-comment">% [m]</span>
|
||||
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(L)</span>
|
||||
Bbi = hexapod.pos_top_tranform(<span class="org-constant">i</span>, <span class="org-type">:</span>)<span class="org-type">'</span> <span class="org-type">-</span> 1e<span class="org-type">-</span>3<span class="org-type">*</span>[0 ; 0 ; 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-comment">% [m]</span>
|
||||
Aai = hexapod.pos_base(<span class="org-constant">i</span>, <span class="org-type">:</span>)<span class="org-type">'</span> <span class="org-type">+</span> 1e<span class="org-type">-</span>3<span class="org-type">*</span>[0 ; 0 ; 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>hexapod.h<span class="org-type">-</span>hexapod.jacobian]; <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>;
|
||||
L(<span class="org-constant">i</span>) = sqrt(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> 2<span class="org-type">*</span>AP<span class="org-type">'*</span>Aai <span class="org-type">+</span> 2<span class="org-type">*</span>AP<span class="org-type">'*</span>(ARB<span class="org-type">*</span>Bbi) <span class="org-type">-</span> 2<span class="org-type">*</span>(ARB<span class="org-type">*</span>Bbi)<span class="org-type">'*</span>Aai);
|
||||
<span class="org-keyword">end</span>
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="outline-container-orgce14673" class="outline-3">
|
||||
<h3 id="orgce14673"><span class="section-number-3">1.7</span> computeReferencePose</h3>
|
||||
<div class="outline-text-3" id="text-1-7">
|
||||
|
||||
<div id="outline-container-orga234cde" class="outline-2">
|
||||
<h2 id="orga234cde"><span class="section-number-2">5</span> computeReferencePose</h2>
|
||||
<div class="outline-text-2" id="text-5">
|
||||
<p>
|
||||
<a id="org11721fe"></a>
|
||||
<a id="org14a5918"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -717,7 +576,7 @@ This Matlab function is accessible <a href="src/computeReferencePose.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">WTr</span><span class="org-variable-name"><span class="org-rainbow-delimiters-depth-1">]</span></span> = <span class="org-function-name">computeReferencePose</span><span class="org-rainbow-delimiters-depth-1">(</span><span class="org-variable-name">Dy</span>, <span class="org-variable-name">Ry</span>, <span class="org-variable-name">Rz</span>, <span class="org-variable-name">Dh</span>, <span class="org-variable-name">Dn</span><span class="org-rainbow-delimiters-depth-1">)</span>
|
||||
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[WTr]</span> = <span class="org-function-name">computeReferencePose</span>(<span class="org-variable-name">Dy</span>, <span class="org-variable-name">Ry</span>, <span class="org-variable-name">Rz</span>, <span class="org-variable-name">Dh</span>, <span class="org-variable-name">Dn</span>)
|
||||
<span class="org-comment">% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)</span>
|
||||
@ -733,63 +592,63 @@ This Matlab function is accessible <a href="src/computeReferencePose.m">here</a>
|
||||
<span class="org-comment">% - WTr -</span>
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Translation Stage</span></span>
|
||||
Rty = <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> Dy;
|
||||
<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>;
|
||||
Rty = [1 0 0 0;
|
||||
0 1 0 Dy;
|
||||
0 0 1 0;
|
||||
0 0 0 1];
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Tilt Stage - Pure rotating aligned with Ob</span></span>
|
||||
Rry = <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>;
|
||||
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
||||
0 1 0 0;
|
||||
<span class="org-type">-</span>sin(Ry) 0 cos(Ry) 0;
|
||||
0 0 0 1];
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Spindle - Rotation along the Z axis</span></span>
|
||||
Rrz = <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>;
|
||||
Rrz = [cos(Rz) <span class="org-type">-</span>sin(Rz) 0 0 ;
|
||||
sin(Rz) cos(Rz) 0 0 ;
|
||||
0 0 1 0 ;
|
||||
0 0 0 1 ];
|
||||
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Micro-Hexapod</span></span>
|
||||
Rhx = <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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
Rhx = [1 0 0;
|
||||
0 cos(Dh(4)) <span class="org-type">-</span>sin(Dh(4));
|
||||
0 sin(Dh(4)) cos(Dh(4))];
|
||||
|
||||
Rhy = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
||||
0 1 0;
|
||||
<span class="org-type">-</span>sin(Dh(5)) 0 cos(Dh(5))];
|
||||
|
||||
Rhz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dh<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</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">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
Rhz = [cos(Dh(6)) <span class="org-type">-</span>sin(Dh(6)) 0;
|
||||
sin(Dh(6)) cos(Dh(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rh = <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> Dh<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-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Dh<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">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> Dh<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-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>;
|
||||
Rh = [1 0 0 Dh(1) ;
|
||||
0 1 0 Dh(2) ;
|
||||
0 0 1 Dh(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rh<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">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = Rhz<span class="org-type">*</span>Rhy<span class="org-type">*</span>Rhx;
|
||||
Rh(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = Rhz<span class="org-type">*</span>Rhy<span class="org-type">*</span>Rhx;
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Nano-Hexapod</span></span>
|
||||
Rnx = <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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">4</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
Rnx = [1 0 0;
|
||||
0 cos(Dn(4)) <span class="org-type">-</span>sin(Dn(4));
|
||||
0 sin(Dn(4)) cos(Dn(4))];
|
||||
|
||||
Rny = <span class="org-rainbow-delimiters-depth-1">[</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">5</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
||||
0 1 0;
|
||||
<span class="org-type">-</span>sin(Dn(5)) 0 cos(Dn(5))];
|
||||
|
||||
Rnz = <span class="org-rainbow-delimiters-depth-1">[</span>cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> <span class="org-type">-</span>sin<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><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>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</span><span class="org-rainbow-delimiters-depth-2">)</span> cos<span class="org-rainbow-delimiters-depth-2">(</span>Dn<span class="org-rainbow-delimiters-depth-3">(</span><span class="org-highlight-numbers-number">6</span><span class="org-rainbow-delimiters-depth-3">)</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">1</span><span class="org-rainbow-delimiters-depth-1">]</span>;
|
||||
Rnz = [cos(Dn(6)) <span class="org-type">-</span>sin(Dn(6)) 0;
|
||||
sin(Dn(6)) cos(Dn(6)) 0;
|
||||
0 0 1];
|
||||
|
||||
Rn = <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> Dn<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-highlight-numbers-number">0</span> <span class="org-highlight-numbers-number">1</span> <span class="org-highlight-numbers-number">0</span> Dn<span class="org-rainbow-delimiters-depth-2">(</span><span class="org-highlight-numbers-number">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> Dn<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-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>;
|
||||
Rn = [1 0 0 Dn(1) ;
|
||||
0 1 0 Dn(2) ;
|
||||
0 0 1 Dn(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rn<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">1</span><span class="org-type">:</span><span class="org-highlight-numbers-number">3</span><span class="org-rainbow-delimiters-depth-1">)</span> = Rnx<span class="org-type">*</span>Rny<span class="org-type">*</span>Rnz;
|
||||
Rn(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = Rnx<span class="org-type">*</span>Rny<span class="org-type">*</span>Rnz;
|
||||
|
||||
<span class="org-matlab-cellbreak"><span class="org-comment">%% Total Homogeneous transformation</span></span>
|
||||
WTr = Rty<span class="org-type">*</span>Rry<span class="org-type">*</span>Rrz<span class="org-type">*</span>Rh<span class="org-type">*</span>Rn;
|
||||
@ -798,12 +657,46 @@ This Matlab function is accessible <a href="src/computeReferencePose.m">here</a>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="outline-container-orgf5cf06b" class="outline-2">
|
||||
<h2 id="orgf5cf06b"><span class="section-number-2">6</span> Compute the Sample Position Error w.r.t. the NASS</h2>
|
||||
<div class="outline-text-2" id="text-6">
|
||||
<p>
|
||||
<a id="org6442eec"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
This Matlab function is accessible <a href="src/computeSampleError.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">[MTr]</span> = <span class="org-function-name">computeSampleError</span>(<span class="org-variable-name">WTm</span>, <span class="org-variable-name">WTr</span>)
|
||||
<span class="org-comment">% computeSampleError -</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Syntax: [MTr] = computeSampleError(WTm, WTr)</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Inputs:</span>
|
||||
<span class="org-comment">% - WTm - Homoegeneous transformation that represent the</span>
|
||||
<span class="org-comment">% wanted pose of the sample with respect to the granite</span>
|
||||
<span class="org-comment">% - WTr - Homoegeneous transformation that represent the</span>
|
||||
<span class="org-comment">% measured pose of the sample with respect to the granite</span>
|
||||
<span class="org-comment">%</span>
|
||||
<span class="org-comment">% Outputs:</span>
|
||||
<span class="org-comment">% - MTr - Homoegeneous transformation that represent the</span>
|
||||
<span class="org-comment">% wanted pose of the sample expressed in a frame</span>
|
||||
<span class="org-comment">% attached to the top platform of the nano-hexapod</span>
|
||||
|
||||
MTr = zeros(4,4);
|
||||
|
||||
MTr = [WTm(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">'</span>, <span class="org-type">-</span>WTm(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">'*</span>WTm(1<span class="org-type">:</span>3,4) ; 0 0 0 1]<span class="org-type">*</span>WTr;
|
||||
<span class="org-keyword">end</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2019-12-12 jeu. 11:25</p>
|
||||
<p class="validation"><a href="http://validator.w3.org/check?uri=referer">Validate</a></p>
|
||||
<p class="date">Created: 2020-01-29 mer. 20:25</p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
@ -41,12 +41,7 @@
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Functions
|
||||
:PROPERTIES:
|
||||
:ID: 8b6ae566-b1a0-448c-8cb1-9d5b3b846ed4
|
||||
:END:
|
||||
<<sec:functions>>
|
||||
** TODO computePsdDispl
|
||||
* TODO computePsdDispl
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computePsdDispl.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
@ -81,7 +76,7 @@ This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** TODO computeSetpoint
|
||||
* TODO computeSetpoint
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeSetpoint.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
@ -152,7 +147,7 @@ This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** TODO converErrorBasis
|
||||
* TODO converErrorBasis
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/converErrorBasis.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
@ -289,7 +284,7 @@ This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Inverse Kinematics of the Hexapod
|
||||
* Inverse Kinematics of the Hexapod
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
@ -321,7 +316,7 @@ This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]]
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** computeReferencePose
|
||||
* computeReferencePose
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeReferencePose.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
@ -409,7 +404,7 @@ This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
|
||||
WTr = Rty*Rry*Rrz*Rh*Rn;
|
||||
end
|
||||
#+end_src
|
||||
** Compute the Sample Position Error w.r.t. the NASS
|
||||
* Compute the Sample Position Error w.r.t. the NASS
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeSampleError.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
|
||||
Loading…
Reference in New Issue
Block a user