tdehaeze/stewart-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
master
stewart-simscape/docs/control-tracking.html

1522 lines
68 KiB
HTML
Raw Permalink Blame History

<?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>
<!-- 2021-01-08 ven. 15:53 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<title>Stewart Platform - Tracking Control</title>
<meta name="generator" content="Org mode" />
<meta name="author" content="Dehaeze Thomas" />
<link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
<script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
<script>
MathJax = {
svg: {
scale: 1,
fontCache: "global"
},
tex: {
tags: "ams",
multlineWidth: "%MULTLINEWIDTH",
tagSide: "right",
macros: {bm: ["\\boldsymbol{#1}",1],},
tagIndent: ".8em"
}
};
</script>
<script id="MathJax-script" async
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-svg.js"></script>
</head>
<body>
<div id="org-div-home-and-up">
<a accesskey="h" href="./index.html"> UP </a>
|
<a accesskey="H" href="./index.html"> HOME </a>
</div><div id="content">
<h1 class="title">Stewart Platform - Tracking Control</h1>
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#org38bd29c">1. Decentralized Control Architecture using Strut Length</a>
<ul>
<li><a href="#org26beab0">1.1. Control Schematic</a></li>
<li><a href="#org4974b70">1.2. Initialize the Stewart platform</a></li>
<li><a href="#orgffac384">1.3. Identification of the plant</a></li>
<li><a href="#org6b0f741">1.4. Plant Analysis</a></li>
<li><a href="#org4ac52e4">1.5. Controller Design</a></li>
<li><a href="#orgcc561b1">1.6. Simulation</a></li>
<li><a href="#org27e5895">1.7. Results</a></li>
<li><a href="#orga08a7c7">1.8. Conclusion</a></li>
</ul>
</li>
<li><a href="#org6def216">2. Centralized Control Architecture using Pose Measurement</a>
<ul>
<li><a href="#org0a198bd">2.1. Control Schematic</a></li>
<li><a href="#orgcd92fdd">2.2. Initialize the Stewart platform</a></li>
<li><a href="#org07a44ca">2.3. Identification of the plant</a></li>
<li><a href="#org247c983">2.4. Diagonal Control - Leg&rsquo;s Frame</a>
<ul>
<li><a href="#org89a9e53">2.4.1. Control Architecture</a></li>
<li><a href="#org1ec2254">2.4.2. Plant Analysis</a></li>
<li><a href="#org1153366">2.4.3. Controller Design</a></li>
<li><a href="#orgcdb2d37">2.4.4. Simulation</a></li>
</ul>
</li>
<li><a href="#org016a64d">2.5. Diagonal Control - Cartesian Frame</a>
<ul>
<li><a href="#orgd397e3e">2.5.1. Control Architecture</a></li>
<li><a href="#orgeb6847d">2.5.2. Plant Analysis</a></li>
<li><a href="#org75860e5">2.5.3. Controller Design</a></li>
<li><a href="#org2c9c807">2.5.4. Simulation</a></li>
</ul>
</li>
<li><a href="#orgc91604c">2.6. Diagonal Control - Steady State Decoupling</a>
<ul>
<li><a href="#org6c1baeb">2.6.1. Control Architecture</a></li>
<li><a href="#orgbff8dd9">2.6.2. Plant Analysis</a></li>
<li><a href="#org7bfa1fd">2.6.3. Controller Design</a></li>
</ul>
</li>
<li><a href="#org83bccab">2.7. Comparison</a>
<ul>
<li><a href="#org88c56f3">2.7.1. Obtained MIMO Controllers</a></li>
<li><a href="#org9360078">2.7.2. Simulation Results</a></li>
</ul>
</li>
<li><a href="#org80dbaca">2.8. Conclusion</a></li>
</ul>
</li>
<li><a href="#org7107cf9">3. Hybrid Control Architecture - HAC-LAC with relative DVF</a>
<ul>
<li><a href="#org659eed7">3.1. Control Schematic</a></li>
<li><a href="#orge9def22">3.2. Initialize the Stewart platform</a></li>
<li><a href="#org491cea7">3.3. First Control Loop - \(\bm{K}_\mathcal{L}\)</a>
<ul>
<li><a href="#org5cc334f">3.3.1. Identification</a></li>
<li><a href="#org3be701b">3.3.2. Obtained Plant</a></li>
<li><a href="#org4623e8f">3.3.3. Controller Design</a></li>
</ul>
</li>
<li><a href="#org53cbafc">3.4. Second Control Loop - \(\bm{K}_\mathcal{X}\)</a>
<ul>
<li><a href="#orgfc299ed">3.4.1. Identification</a></li>
<li><a href="#org1680642">3.4.2. Obtained Plant</a></li>
<li><a href="#orgae806c2">3.4.3. Controller Design</a></li>
</ul>
</li>
<li><a href="#org96f8c42">3.5. Simulations</a></li>
<li><a href="#orgcf8f38f">3.6. Conclusion</a></li>
</ul>
</li>
<li><a href="#orgffc4966">4. Comparison of all the methods</a></li>
<li><a href="#org1b77c20">5. Compute the pose error of the Stewart Platform</a></li>
</ul>
</div>
</div>
<p>
Let&rsquo;s suppose the control objective is to position \(\bm{\mathcal{X}}\) of the mobile platform of the Stewart platform such that it is following some reference position \(\bm{r}_\mathcal{X}\).
</p>
<p>
In order to compute the pose error \(\bm{\epsilon}_\mathcal{X}\) from the actual pose \(\bm{\mathcal{X}}\) and the reference position \(\bm{r}_\mathcal{X}\), some computation is required and explained in section <a href="#org2f1ce27">5</a>.
</p>
<p>
Depending of the measured quantity, different control architectures can be used:
</p>
<ul class="org-ul">
<li>If the struts length \(\bm{\mathcal{L}}\) is measured, a decentralized control architecture can be used (Section <a href="#orgf224027">1</a>)</li>
<li>If the pose of the mobile platform \(\bm{\mathcal{X}}\) is directly measured, a centralized control architecture can be used (Section <a href="#org17ac109">2</a>)</li>
<li>If both \(\bm{\mathcal{L}}\) and \(\bm{\mathcal{X}}\) are measured, we can use an hybrid control architecture (Section <a href="#orgb001207">3</a>)</li>
</ul>
<p>
The control configuration are compare in section <a href="#org8ae0d7b">4</a>.
</p>
<div id="outline-container-org38bd29c" class="outline-2">
<h2 id="org38bd29c"><span class="section-number-2">1</span> Decentralized Control Architecture using Strut Length</h2>
<div class="outline-text-2" id="text-1">
<p>
<a id="orgf224027"></a>
</p>
</div>
<div id="outline-container-org26beab0" class="outline-3">
<h3 id="org26beab0"><span class="section-number-3">1.1</span> Control Schematic</h3>
<div class="outline-text-3" id="text-1-1">
<p>
The control architecture is shown in Figure <a href="#orga408812">1</a>.
</p>
<p>
The required leg length \(\bm{r}_\mathcal{L}\) is computed from the reference path \(\bm{r}_\mathcal{X}\) using the inverse kinematics.
</p>
<p>
Then, a diagonal (decentralized) controller \(\bm{K}_\mathcal{L}\) is used such that each leg lengths stays close to its required length.
</p>
<div id="orga408812" class="figure">
<p><img src="figs/decentralized_reference_tracking_L.png" alt="decentralized_reference_tracking_L.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Decentralized control for reference tracking</p>
</div>
</div>
</div>
<div id="outline-container-org4974b70" class="outline-3">
<h3 id="org4974b70"><span class="section-number-3">1.2</span> Initialize the Stewart platform</h3>
<div class="outline-text-3" id="text-1-2">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-comment">% Stewart Platform</span>
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, <span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, <span class="org-string">'type_F'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical_p'</span>);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'accelerometer'</span>, <span class="org-string">'freq'</span>, 5e3);
<span class="org-comment">% Ground and Payload</span>
ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>);
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
<span class="org-comment">% Controller</span>
controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
<span class="org-comment">% Disturbances and References</span>
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
</pre>
</div>
</div>
</div>
<div id="outline-container-orgffac384" class="outline-3">
<h3 id="orgffac384"><span class="section-number-3">1.3</span> Identification of the plant</h3>
<div class="outline-text-3" id="text-1-3">
<p>
Let&rsquo;s identify the transfer function from \(\bm{\tau}\) to \(\bm{\mathcal{L}}\).
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'stewart_platform_model'</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
clear io; io_i = 1;
io(io_i) = linio([mdl, <span class="org-string">'/Controller'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</span>
io(io_i) = linio([mdl, <span class="org-string">'/Stewart Platform'</span>], 1, <span class="org-string">'openoutput'</span>, [], <span class="org-string">'dLm'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Relative Displacement Outputs [m]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G = linearize(mdl, io);
G.InputName = {<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>};
G.OutputName = {<span class="org-string">'L1'</span>, <span class="org-string">'L2'</span>, <span class="org-string">'L3'</span>, <span class="org-string">'L4'</span>, <span class="org-string">'L5'</span>, <span class="org-string">'L6'</span>};
</pre>
</div>
</div>
</div>
<div id="outline-container-org6b0f741" class="outline-3">
<h3 id="org6b0f741"><span class="section-number-3">1.4</span> Plant Analysis</h3>
<div class="outline-text-3" id="text-1-4">
<p>
The diagonal and off-diagonal terms of the plant are shown in Figure <a href="#org98f0b3d">2</a>.
</p>
<p>
All the diagonal terms are equal.
We see that the plant is decoupled at low frequency which indicate that decentralized control may be a good idea.
</p>
<div id="org98f0b3d" class="figure">
<p><img src="figs/plant_decentralized_L.png" alt="plant_decentralized_L.png" />
</p>
<p><span class="figure-number">Figure 2: </span>Obtain Diagonal and off diagonal dynamics (<a href="./figs/plant_decentralized_L.png">png</a>, <a href="./figs/plant_decentralized_L.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org4ac52e4" class="outline-3">
<h3 id="org4ac52e4"><span class="section-number-3">1.5</span> Controller Design</h3>
<div class="outline-text-3" id="text-1-5">
<p>
The controller consists of:
</p>
<ul class="org-ul">
<li>A pure integrator</li>
<li>A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin</li>
</ul>
<p>
The obtained loop gains corresponding to the diagonal elements are shown in Figure <a href="#org7d03f3c">3</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>30;
Kl = diag(1<span class="org-type">./</span>diag(abs(freqresp(G, wc)))) <span class="org-type">*</span> wc<span class="org-type">/</span>s <span class="org-type">*</span> 1<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>3<span class="org-type">/</span>wc);
</pre>
</div>
<div id="org7d03f3c" class="figure">
<p><img src="figs/loop_gain_decentralized_L.png" alt="loop_gain_decentralized_L.png" />
</p>
<p><span class="figure-number">Figure 3: </span>Loop Gain of the diagonal elements (<a href="./figs/loop_gain_decentralized_L.png">png</a>, <a href="./figs/loop_gain_decentralized_L.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgcc561b1" class="outline-3">
<h3 id="orgcc561b1"><span class="section-number-3">1.6</span> Simulation</h3>
<div class="outline-text-3" id="text-1-6">
<p>
Let&rsquo;s define some reference path to follow.
</p>
<div class="org-src-container">
<pre class="src src-matlab">t = [0<span class="org-type">:</span>1e<span class="org-type">-</span>4<span class="org-type">:</span>10];
r = zeros(6, length(t));
r(1, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(2, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>cos(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(3, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t;
references = initializeReferences(stewart, <span class="org-string">'t'</span>, t, <span class="org-string">'r'</span>, r);
</pre>
</div>
<p>
The reference path is shown in Figure <a href="#org6e50430">4</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>;
plot3(squeeze(references.r.Data(1,1,<span class="org-type">:</span>)), squeeze(references.r.Data(2,1,<span class="org-type">:</span>)), squeeze(references.r.Data(3,1,<span class="org-type">:</span>)));
xlabel(<span class="org-string">'X [m]'</span>);
ylabel(<span class="org-string">'Y [m]'</span>);
zlabel(<span class="org-string">'Z [m]'</span>);
</pre>
</div>
<div id="org6e50430" class="figure">
<p><img src="figs/tracking_control_reference_path.png" alt="tracking_control_reference_path.png" />
</p>
<p><span class="figure-number">Figure 4: </span>Reference path used for Tracking control (<a href="./figs/tracking_control_reference_path.png">png</a>, <a href="./figs/tracking_control_reference_path.pdf">pdf</a>)</p>
</div>
<div class="org-src-container">
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'ref-track-L'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-string">'stewart_platform_model'</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'10'</span>)
<span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>);
simout_D = simout;
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">save(<span class="org-string">'./mat/control_tracking.mat'</span>, <span class="org-string">'simout_D'</span>, <span class="org-string">'-append'</span>);
</pre>
</div>
</div>
</div>
<div id="outline-container-org27e5895" class="outline-3">
<h3 id="org27e5895"><span class="section-number-3">1.7</span> Results</h3>
<div class="outline-text-3" id="text-1-7">
<p>
The reference path and the position of the mobile platform are shown in Figure <a href="#org6fd0dfd">5</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>;
hold on;
plot3(simout_D.x.Xr.Data(<span class="org-type">:</span>, 1), simout_D.x.Xr.Data(<span class="org-type">:</span>, 2), simout_D.x.Xr.Data(<span class="org-type">:</span>, 3), <span class="org-string">'k-'</span>);
plot3(squeeze(references.r.Data(1,1,<span class="org-type">:</span>)), squeeze(references.r.Data(2,1,<span class="org-type">:</span>)), squeeze(references.r.Data(3,1,<span class="org-type">:</span>)), <span class="org-string">'--'</span>);
hold off;
xlabel(<span class="org-string">'X [m]'</span>); ylabel(<span class="org-string">'Y [m]'</span>); zlabel(<span class="org-string">'Z [m]'</span>);
view([1,1,1])
zlim([0, <span class="org-constant">inf</span>]);
</pre>
</div>
<div id="org6fd0dfd" class="figure">
<p><img src="figs/decentralized_control_3d_pos.png" alt="decentralized_control_3d_pos.png" />
</p>
<p><span class="figure-number">Figure 5: </span>Reference path and Obtained Position (<a href="./figs/decentralized_control_3d_pos.png">png</a>, <a href="./figs/decentralized_control_3d_pos.pdf">pdf</a>)</p>
</div>
<div id="org00434d5" class="figure">
<p><img src="figs/decentralized_control_Ex.png" alt="decentralized_control_Ex.png" />
</p>
<p><span class="figure-number">Figure 6: </span>Position error \(\bm{\epsilon}_\mathcal{X}\) (<a href="./figs/decentralized_control_Ex.png">png</a>, <a href="./figs/decentralized_control_Ex.pdf">pdf</a>)</p>
</div>
<div id="org9abe540" class="figure">
<p><img src="figs/decentralized_control_El.png" alt="decentralized_control_El.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Position error \(\bm{\epsilon}_\mathcal{L}\) (<a href="./figs/decentralized_control_El.png">png</a>, <a href="./figs/decentralized_control_El.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orga08a7c7" class="outline-3">
<h3 id="orga08a7c7"><span class="section-number-3">1.8</span> Conclusion</h3>
<div class="outline-text-3" id="text-1-8">
<p>
Such control architecture is easy to implement and give good results.
The inverse kinematics is easy to compute.
</p>
<p>
However, as \(\mathcal{X}\) is not directly measured, it is possible that important positioning errors are due to finite stiffness of the joints and other imperfections.
</p>
</div>
</div>
</div>
<div id="outline-container-org6def216" class="outline-2">
<h2 id="org6def216"><span class="section-number-2">2</span> Centralized Control Architecture using Pose Measurement</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="org17ac109"></a>
</p>
</div>
<div id="outline-container-org0a198bd" class="outline-3">
<h3 id="org0a198bd"><span class="section-number-3">2.1</span> Control Schematic</h3>
<div class="outline-text-3" id="text-2-1">
<p>
The centralized controller takes the position error \(\bm{\epsilon}_\mathcal{X}\) as an inputs and generate actuator forces \(\bm{\tau}\) (see Figure <a href="#org70b7a89">8</a>).
The signals are:
</p>
<ul class="org-ul">
<li>reference path \(\bm{r}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}\)</li>
<li>tracking error \(\bm{\epsilon}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}\)</li>
<li>actuator forces \(\bm{\tau} = \begin{bmatrix} \tau_1 & \tau_2 & \tau_3 & \tau_4 & \tau_5 & \tau_6 \end{bmatrix}\)</li>
<li>payload pose \(\bm{\mathcal{X}} = \begin{bmatrix} x & y & z & R_x & R_y & R_z \end{bmatrix}\)</li>
</ul>
<div id="org70b7a89" class="figure">
<p><img src="figs/centralized_reference_tracking.png" alt="centralized_reference_tracking.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Centralized Controller</p>
</div>
<p>
Instead of designing a full MIMO controller \(K\), we first try to make the plant more diagonal by pre- or post-multiplying some constant matrix, then we design a diagonal controller.
</p>
<p>
We can think of two ways to make the plant more diagonal that are described in sections <a href="#org245333d">2.4</a> and <a href="#org391e5c7">2.5</a>.
</p>
<div class="important" id="orgfe9fde9">
<p>
Note here that the subtraction shown in Figure <a href="#org70b7a89">8</a> is not a real subtraction.
It is indeed a more complex computation explained in section <a href="#org2f1ce27">5</a>.
</p>
</div>
</div>
</div>
<div id="outline-container-orgcd92fdd" class="outline-3">
<h3 id="orgcd92fdd"><span class="section-number-3">2.2</span> Initialize the Stewart platform</h3>
<div class="outline-text-3" id="text-2-2">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-comment">% Stewart Platform</span>
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, <span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, <span class="org-string">'type_F'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical_p'</span>);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'accelerometer'</span>, <span class="org-string">'freq'</span>, 5e3);
<span class="org-comment">% Ground and Payload</span>
ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>);
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
<span class="org-comment">% Controller</span>
controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
<span class="org-comment">% Disturbances and References</span>
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
</pre>
</div>
</div>
</div>
<div id="outline-container-org07a44ca" class="outline-3">
<h3 id="org07a44ca"><span class="section-number-3">2.3</span> Identification of the plant</h3>
<div class="outline-text-3" id="text-2-3">
<p>
Let&rsquo;s identify the transfer function from \(\bm{\tau}\) to \(\bm{\mathcal{X}}\).
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'stewart_platform_model'</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
clear io; io_i = 1;
io(io_i) = linio([mdl, <span class="org-string">'/Controller'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</span>
io(io_i) = linio([mdl, <span class="org-string">'/Relative Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Relative Displacement Outputs [m]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G = linearize(mdl, io);
G.InputName = {<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>};
G.OutputName = {<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>};
</pre>
</div>
</div>
</div>
<div id="outline-container-org247c983" class="outline-3">
<h3 id="org247c983"><span class="section-number-3">2.4</span> Diagonal Control - Leg&rsquo;s Frame</h3>
<div class="outline-text-3" id="text-2-4">
<p>
<a id="org245333d"></a>
</p>
</div>
<div id="outline-container-org89a9e53" class="outline-4">
<h4 id="org89a9e53"><span class="section-number-4">2.4.1</span> Control Architecture</h4>
<div class="outline-text-4" id="text-2-4-1">
<p>
The pose error \(\bm{\epsilon}_\mathcal{X}\) is first converted in the frame of the leg by using the Jacobian matrix.
Then a diagonal controller \(\bm{K}_\mathcal{L}\) is designed.
The final implemented controller is \(\bm{K} = \bm{K}_\mathcal{L} \cdot \bm{J}\) as shown in Figure <a href="#org622fa29">9</a>
</p>
<p>
Note here that the transformation from the pose error \(\bm{\epsilon}_\mathcal{X}\) to the leg&rsquo;s length errors by using the Jacobian matrix is only valid for small errors.
</p>
<div id="org622fa29" class="figure">
<p><img src="figs/centralized_reference_tracking_L.png" alt="centralized_reference_tracking_L.png" />
</p>
<p><span class="figure-number">Figure 9: </span>Controller in the frame of the legs</p>
</div>
</div>
</div>
<div id="outline-container-org1ec2254" class="outline-4">
<h4 id="org1ec2254"><span class="section-number-4">2.4.2</span> Plant Analysis</h4>
<div class="outline-text-4" id="text-2-4-2">
<p>
We now multiply the plant by the Jacobian matrix as shown in Figure <a href="#org622fa29">9</a> to obtain a more diagonal plant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Gl = stewart.kinematics.J<span class="org-type">*</span>G;
Gl.OutputName = {<span class="org-string">'D1'</span>, <span class="org-string">'D2'</span>, <span class="org-string">'D3'</span>, <span class="org-string">'D4'</span>, <span class="org-string">'D5'</span>, <span class="org-string">'D6'</span>};
</pre>
</div>
<p>
The bode plot of the plant is shown in Figure <a href="#orge26bef3">10</a>.
We can see that the diagonal elements are identical.
This will simplify the design of the controller as all the elements of the diagonal controller can be made identical.
</p>
<div id="orge26bef3" class="figure">
<p><img src="figs/plant_centralized_L.png" alt="plant_centralized_L.png" />
</p>
<p><span class="figure-number">Figure 10: </span>Diagonal and off-diagonal elements of the plant \(\bm{J}\bm{G}\) (<a href="./figs/plant_centralized_L.png">png</a>, <a href="./figs/plant_centralized_L.pdf">pdf</a>)</p>
</div>
<p>
We can see that this <b>totally decouples the system at low frequency</b>.
</p>
<p>
This was expected since:
\[ \bm{G}(\omega = 0) = \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \]
</p>
<p>
Thus \(J \cdot G(\omega = 0) = J \cdot \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\) is a diagonal matrix containing the inverse of the joint&rsquo;s stiffness.
</p>
</div>
</div>
<div id="outline-container-org1153366" class="outline-4">
<h4 id="org1153366"><span class="section-number-4">2.4.3</span> Controller Design</h4>
<div class="outline-text-4" id="text-2-4-3">
<p>
The controller consists of:
</p>
<ul class="org-ul">
<li>A pure integrator</li>
<li>A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin</li>
</ul>
<p>
The obtained loop gains corresponding to the diagonal elements are shown in Figure <a href="#org614c5d1">11</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>30;
Kl = diag(1<span class="org-type">./</span>diag(abs(freqresp(Gl, wc)))) <span class="org-type">*</span> wc<span class="org-type">/</span>s <span class="org-type">*</span> 1<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>3<span class="org-type">/</span>wc);
</pre>
</div>
<div id="org614c5d1" class="figure">
<p><img src="figs/loop_gain_centralized_L.png" alt="loop_gain_centralized_L.png" />
</p>
<p><span class="figure-number">Figure 11: </span>Loop Gain of the diagonal elements (<a href="./figs/loop_gain_centralized_L.png">png</a>, <a href="./figs/loop_gain_centralized_L.pdf">pdf</a>)</p>
</div>
<p>
The controller \(\bm{K} = \bm{K}_\mathcal{L} \bm{J}\) is computed.
</p>
<div class="org-src-container">
<pre class="src src-matlab">K = Kl<span class="org-type">*</span>stewart.kinematics.J;
</pre>
</div>
</div>
</div>
<div id="outline-container-orgcdb2d37" class="outline-4">
<h4 id="orgcdb2d37"><span class="section-number-4">2.4.4</span> Simulation</h4>
<div class="outline-text-4" id="text-2-4-4">
<p>
We specify the reference path to follow.
</p>
<div class="org-src-container">
<pre class="src src-matlab">t = [0<span class="org-type">:</span>1e<span class="org-type">-</span>4<span class="org-type">:</span>10];
r = zeros(6, length(t));
r(1, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(2, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>cos(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(3, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t;
references = initializeReferences(stewart, <span class="org-string">'t'</span>, t, <span class="org-string">'r'</span>, r);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'ref-track-X'</span>);
</pre>
</div>
<p>
We run the simulation and we save the results.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-string">'stewart_platform_model'</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'10'</span>)
<span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>)
simout_L = simout;
save(<span class="org-string">'./mat/control_tracking.mat'</span>, <span class="org-string">'simout_L'</span>, <span class="org-string">'-append'</span>);
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org016a64d" class="outline-3">
<h3 id="org016a64d"><span class="section-number-3">2.5</span> Diagonal Control - Cartesian Frame</h3>
<div class="outline-text-3" id="text-2-5">
<p>
<a id="org391e5c7"></a>
</p>
</div>
<div id="outline-container-orgd397e3e" class="outline-4">
<h4 id="orgd397e3e"><span class="section-number-4">2.5.1</span> Control Architecture</h4>
<div class="outline-text-4" id="text-2-5-1">
<p>
A diagonal controller \(\bm{K}_\mathcal{X}\) take the pose error \(\bm{\epsilon}_\mathcal{X}\) and generate cartesian forces \(\bm{\mathcal{F}}\) that are then converted to actuators forces using the Jacobian as shown in Figure e <a href="#org3aa556e">12</a>.
</p>
<p>
The final implemented controller is \(\bm{K} = \bm{J}^{-T} \cdot \bm{K}_\mathcal{X}\).
</p>
<div id="org3aa556e" class="figure">
<p><img src="figs/centralized_reference_tracking_X.png" alt="centralized_reference_tracking_X.png" />
</p>
<p><span class="figure-number">Figure 12: </span>Controller in the cartesian frame</p>
</div>
</div>
</div>
<div id="outline-container-orgeb6847d" class="outline-4">
<h4 id="orgeb6847d"><span class="section-number-4">2.5.2</span> Plant Analysis</h4>
<div class="outline-text-4" id="text-2-5-2">
<p>
We now multiply the plant by the Jacobian matrix as shown in Figure <a href="#org3aa556e">12</a> to obtain a more diagonal plant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Gx = G<span class="org-type">*</span>inv(stewart.kinematics.J<span class="org-type">'</span>);
Gx.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
</pre>
</div>
<div id="org087a944" class="figure">
<p><img src="figs/plant_centralized_X.png" alt="plant_centralized_X.png" />
</p>
<p><span class="figure-number">Figure 13: </span>Diagonal and off-diagonal elements of the plant \(\bm{G} \bm{J}^{-T}\) (<a href="./figs/plant_centralized_X.png">png</a>, <a href="./figs/plant_centralized_X.pdf">pdf</a>)</p>
</div>
<p>
The diagonal terms are not the same.
The resonances of the system are &ldquo;decoupled&rdquo;.
For instance, the vertical resonance of the system is only present on the diagonal term corresponding to \(D_z/\mathcal{F}_z\).
</p>
<p>
Here the system is almost decoupled at all frequencies except for the transfer functions \(\frac{R_y}{\mathcal{F}_x}\) and \(\frac{R_x}{\mathcal{F}_y}\).
</p>
<p>
This is due to the fact that the compliance matrix of the Stewart platform is not diagonal.
</p>
<table border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<colgroup>
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
<col class="org-right" />
</colgroup>
<tbody>
<tr>
<td class="org-right">4.75e-08</td>
<td class="org-right">-1.9751e-24</td>
<td class="org-right">7.3536e-25</td>
<td class="org-right">5.915e-23</td>
<td class="org-right">3.2093e-07</td>
<td class="org-right">5.8696e-24</td>
</tr>
<tr>
<td class="org-right">-7.1302e-25</td>
<td class="org-right">4.75e-08</td>
<td class="org-right">2.8866e-25</td>
<td class="org-right">-3.2093e-07</td>
<td class="org-right">-5.38e-24</td>
<td class="org-right">-3.2725e-23</td>
</tr>
<tr>
<td class="org-right">7.9012e-26</td>
<td class="org-right">-6.3991e-25</td>
<td class="org-right">2.099e-08</td>
<td class="org-right">1.9073e-23</td>
<td class="org-right">5.3384e-25</td>
<td class="org-right">-6.4867e-40</td>
</tr>
<tr>
<td class="org-right">1.3724e-23</td>
<td class="org-right">-3.2093e-07</td>
<td class="org-right">1.2799e-23</td>
<td class="org-right">5.1863e-06</td>
<td class="org-right">4.9412e-22</td>
<td class="org-right">-3.8269e-24</td>
</tr>
<tr>
<td class="org-right">3.2093e-07</td>
<td class="org-right">7.6013e-24</td>
<td class="org-right">1.2239e-23</td>
<td class="org-right">6.8886e-22</td>
<td class="org-right">5.1863e-06</td>
<td class="org-right">-2.6025e-22</td>
</tr>
<tr>
<td class="org-right">7.337e-24</td>
<td class="org-right">-3.2395e-23</td>
<td class="org-right">-1.571e-39</td>
<td class="org-right">9.927e-23</td>
<td class="org-right">-3.2531e-22</td>
<td class="org-right">1.7073e-06</td>
</tr>
</tbody>
</table>
<p>
One way to have this compliance matrix diagonal (and thus having a decoupled plant at DC) is to use a <b>cubic architecture</b> with the center of the cube&rsquo;s coincident with frame \(\{A\}\).
</p>
<p>
This control architecture can also give a dynamically decoupled plant if the Center of mass of the payload is also coincident with frame \(\{A\}\).
</p>
</div>
</div>
<div id="outline-container-org75860e5" class="outline-4">
<h4 id="org75860e5"><span class="section-number-4">2.5.3</span> Controller Design</h4>
<div class="outline-text-4" id="text-2-5-3">
<p>
The controller consists of:
</p>
<ul class="org-ul">
<li>A pure integrator</li>
<li>A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin</li>
</ul>
<p>
The obtained loop gains corresponding to the diagonal elements are shown in Figure <a href="#org25406f9">14</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>30;
Kx = diag(1<span class="org-type">./</span>diag(abs(freqresp(Gx, wc)))) <span class="org-type">*</span> wc<span class="org-type">/</span>s <span class="org-type">*</span> 1<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>3<span class="org-type">/</span>wc);
</pre>
</div>
<div id="org25406f9" class="figure">
<p><img src="figs/loop_gain_centralized_X.png" alt="loop_gain_centralized_X.png" />
</p>
<p><span class="figure-number">Figure 14: </span>Loop Gain of the diagonal elements (<a href="./figs/loop_gain_centralized_X.png">png</a>, <a href="./figs/loop_gain_centralized_X.pdf">pdf</a>)</p>
</div>
<p>
The controller \(\bm{K} = \bm{J}^{-T} \bm{K}_\mathcal{X}\) is computed.
</p>
<div class="org-src-container">
<pre class="src src-matlab">K = inv(stewart.kinematics.J<span class="org-type">'</span>)<span class="org-type">*</span>Kx;
</pre>
</div>
</div>
</div>
<div id="outline-container-org2c9c807" class="outline-4">
<h4 id="org2c9c807"><span class="section-number-4">2.5.4</span> Simulation</h4>
<div class="outline-text-4" id="text-2-5-4">
<p>
We specify the reference path to follow.
</p>
<div class="org-src-container">
<pre class="src src-matlab">t = [0<span class="org-type">:</span>1e<span class="org-type">-</span>4<span class="org-type">:</span>10];
r = zeros(6, length(t));
r(1, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(2, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>cos(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(3, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t;
references = initializeReferences(stewart, <span class="org-string">'t'</span>, t, <span class="org-string">'r'</span>, r);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'ref-track-X'</span>);
</pre>
</div>
<p>
We run the simulation and we save the results.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-string">'stewart_platform_model'</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'10'</span>)
<span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>)
simout_X = simout;
save(<span class="org-string">'./mat/control_tracking.mat'</span>, <span class="org-string">'simout_X'</span>, <span class="org-string">'-append'</span>);
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-orgc91604c" class="outline-3">
<h3 id="orgc91604c"><span class="section-number-3">2.6</span> Diagonal Control - Steady State Decoupling</h3>
<div class="outline-text-3" id="text-2-6">
<p>
<a id="org2fcbaba"></a>
</p>
</div>
<div id="outline-container-org6c1baeb" class="outline-4">
<h4 id="org6c1baeb"><span class="section-number-4">2.6.1</span> Control Architecture</h4>
<div class="outline-text-4" id="text-2-6-1">
<p>
The plant \(\bm{G}\) is pre-multiply by \(\bm{G}^{-1}(\omega = 0)\) such that the &ldquo;shaped plant&rdquo; \(\bm{G}_0 = \bm{G} \bm{G}^{-1}(\omega = 0)\) is diagonal at low frequency.
</p>
<p>
Then a diagonal controller \(\bm{K}_0\) is designed.
</p>
<p>
The control architecture is shown in Figure <a href="#orgcf8fa7b">15</a>.
</p>
<div id="orgcf8fa7b" class="figure">
<p><img src="figs/centralized_reference_tracking_S.png" alt="centralized_reference_tracking_S.png" />
</p>
<p><span class="figure-number">Figure 15: </span>Static Decoupling of the Plant</p>
</div>
</div>
</div>
<div id="outline-container-orgbff8dd9" class="outline-4">
<h4 id="orgbff8dd9"><span class="section-number-4">2.6.2</span> Plant Analysis</h4>
<div class="outline-text-4" id="text-2-6-2">
<p>
The plant is pre-multiplied by \(\bm{G}^{-1}(\omega = 0)\).
The diagonal and off-diagonal elements of the shaped plant are shown in Figure <a href="#orge77666a">16</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">G0 = G<span class="org-type">*</span>inv(freqresp(G, 0));
</pre>
</div>
<div id="orge77666a" class="figure">
<p><img src="figs/plant_centralized_SD.png" alt="plant_centralized_SD.png" />
</p>
<p><span class="figure-number">Figure 16: </span>Diagonal and off-diagonal elements of the plant \(\bm{G} \bm{G}^{-1}(\omega = 0)\) (<a href="./figs/plant_centralized_SD.png">png</a>, <a href="./figs/plant_centralized_SD.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org7bfa1fd" class="outline-4">
<h4 id="org7bfa1fd"><span class="section-number-4">2.6.3</span> Controller Design</h4>
<div class="outline-text-4" id="text-2-6-3">
<p>
We have that:
\[ \bm{G}^{-1}(\omega = 0) = \left(\frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\right)^{-1} = \left( \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) \right)^{-1} = \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \bm{J} \]
</p>
<p>
And because:
</p>
<ul class="org-ul">
<li>all the leg stiffness are equal</li>
<li>the controller equal to a \(\bm{K}_0(s) = k(s) \bm{I}_6\)</li>
</ul>
<p>
We have that \(\bm{K}_0(s)\) commutes with \(\bm{G}^{-1}(\omega = 0)\) and thus the overall controller \(\bm{K}\) is the same as the one obtain in section <a href="#org245333d">2.4</a>.
</p>
</div>
</div>
</div>
<div id="outline-container-org83bccab" class="outline-3">
<h3 id="org83bccab"><span class="section-number-3">2.7</span> Comparison</h3>
<div class="outline-text-3" id="text-2-7">
</div>
<div id="outline-container-org88c56f3" class="outline-4">
<h4 id="org88c56f3"><span class="section-number-4">2.7.1</span> Obtained MIMO Controllers</h4>
<div class="outline-text-4" id="text-2-7-1">
<div id="org53130a2" class="figure">
<p><img src="figs/centralized_control_comp_K.png" alt="centralized_control_comp_K.png" />
</p>
<p><span class="figure-number">Figure 17: </span>Comparison of the MIMO controller \(\bm{K}\) for both centralized architectures (<a href="./figs/centralized_control_comp_K.png">png</a>, <a href="./figs/centralized_control_comp_K.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org9360078" class="outline-4">
<h4 id="org9360078"><span class="section-number-4">2.7.2</span> Simulation Results</h4>
<div class="outline-text-4" id="text-2-7-2">
<p>
The position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized architecture are shown in Figure <a href="#org3a8ff4e">18</a>.
</p>
<p>
Based on Figure <a href="#org3a8ff4e">18</a>, we can see that:
</p>
<ul class="org-ul">
<li>There is some tracking error \(\epsilon_x\)</li>
<li>The errors \(\epsilon_y\), \(\epsilon_{R_x}\) and \(\epsilon_{R_z}\) are quite negligible</li>
<li>There is some error in the vertical position \(\epsilon_z\).
The frequency of the error \(\epsilon_z\) is twice the frequency of the reference path \(r_x\).</li>
<li>There is some error \(\epsilon_{R_y}\).
This error is much lower when using the diagonal control in the frame of the leg instead of the cartesian frame.</li>
</ul>
<div id="org3a8ff4e" class="figure">
<p><img src="figs/centralized_control_comp_Ex.png" alt="centralized_control_comp_Ex.png" />
</p>
<p><span class="figure-number">Figure 18: </span>Comparison of the position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized controllers (<a href="./figs/centralized_control_comp_Ex.png">png</a>, <a href="./figs/centralized_control_comp_Ex.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org80dbaca" class="outline-3">
<h3 id="org80dbaca"><span class="section-number-3">2.8</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-8">
<p>
Both control architecture gives similar results even tough the control in the Leg&rsquo;s frame gives slightly better results.
</p>
<p>
The main differences between the control architectures used in sections <a href="#org245333d">2.4</a> and <a href="#org391e5c7">2.5</a> are summarized in Table <a href="#org52e6569">1</a>.
</p>
<table id="org52e6569" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
<caption class="t-above"><span class="table-number">Table 1:</span> Comparison of the two centralized control architectures</caption>
<colgroup>
<col class="org-left" />
<col class="org-left" />
<col class="org-left" />
<col class="org-left" />
</colgroup>
<thead>
<tr>
<th scope="col" class="org-left">&#xa0;</th>
<th scope="col" class="org-left"><b>Leg&rsquo;s Frame</b></th>
<th scope="col" class="org-left"><b>Cartesian Frame</b></th>
<th scope="col" class="org-left"><b>Static Decoupling</b></th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left"><b>Plant Meaning</b></td>
<td class="org-left">\(\delta\mathcal{L}_i/\tau_i\)</td>
<td class="org-left">\(\delta\mathcal{X}_i/\mathcal{F}_i\)</td>
<td class="org-left">No physical meaning</td>
</tr>
<tr>
<td class="org-left"><b>Obtained Decoupling</b></td>
<td class="org-left">Decoupled at DC</td>
<td class="org-left">Dynamical decoupling except few terms</td>
<td class="org-left">Decoupled at DC</td>
</tr>
<tr>
<td class="org-left"><b>Diagonal Elements</b></td>
<td class="org-left">Identical with all the Resonances</td>
<td class="org-left">Different, resonances are cancel out</td>
<td class="org-left">No Alternating poles and zeros</td>
</tr>
<tr>
<td class="org-left"><b>Mechanical Architecture</b></td>
<td class="org-left">Architecture Independent</td>
<td class="org-left">Better with Cubic Architecture</td>
<td class="org-left">&#xa0;</td>
</tr>
<tr>
<td class="org-left"><b>Robustness to Uncertainty</b></td>
<td class="org-left">Good (only depends on \(J\))</td>
<td class="org-left">Good (only depends on \(J\))</td>
<td class="org-left">Bad (depends on the mass)</td>
</tr>
</tbody>
</table>
<p>
These decoupling methods only uses the Jacobian matrix which only depends on the Stewart platform geometry.
Thus, this method should be quite robust against parameter variation (e.g. the payload mass).
</p>
</div>
</div>
</div>
<div id="outline-container-org7107cf9" class="outline-2">
<h2 id="org7107cf9"><span class="section-number-2">3</span> Hybrid Control Architecture - HAC-LAC with relative DVF</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="orgb001207"></a>
</p>
</div>
<div id="outline-container-org659eed7" class="outline-3">
<h3 id="org659eed7"><span class="section-number-3">3.1</span> Control Schematic</h3>
<div class="outline-text-3" id="text-3-1">
<p>
Let&rsquo;s consider the control schematic shown in Figure <a href="#orga867420">19</a>.
</p>
<p>
The first loop containing \(\bm{K}_\mathcal{L}\) is a Decentralized Direct (Relative) Velocity Feedback.
</p>
<p>
A reference \(\bm{r}_\mathcal{L}\) is computed using the inverse kinematics and corresponds to the wanted motion of each leg.
The actual length of each leg \(\bm{\mathcal{L}}\) is subtracted and then passed trough the controller \(\bm{K}_\mathcal{L}\).
</p>
<p>
The controller is a diagonal controller with pure derivative action on the diagonal.
</p>
<p>
The effect of this loop is:
</p>
<ul class="org-ul">
<li>it adds damping to the system (the force applied in each actuator is proportional to the relative velocity of the strut)</li>
<li>it however does not go &ldquo;against&rdquo; the reference path \(\bm{r}_\mathcal{X}\) thanks to the use of the inverse kinematics</li>
</ul>
<p>
Then, the second loop containing \(\bm{K}_\mathcal{X}\) is designed based on the already damped plant (represented by the gray area).
This second loop is responsible for the reference tracking.
</p>
<div id="orga867420" class="figure">
<p><img src="figs/hybrid_reference_tracking.png" alt="hybrid_reference_tracking.png" />
</p>
<p><span class="figure-number">Figure 19: </span>Hybrid Control Architecture</p>
</div>
</div>
</div>
<div id="outline-container-orge9def22" class="outline-3">
<h3 id="orge9def22"><span class="section-number-3">3.2</span> Initialize the Stewart platform</h3>
<div class="outline-text-3" id="text-3-2">
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-comment">% Stewart Platform</span>
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, <span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, <span class="org-string">'type_F'</span>, <span class="org-string">'universal_p'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical_p'</span>);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'accelerometer'</span>, <span class="org-string">'freq'</span>, 5e3);
<span class="org-comment">% Ground and Payload</span>
ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>);
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
<span class="org-comment">% Controller</span>
controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
<span class="org-comment">% Disturbances and References</span>
disturbances = initializeDisturbances();
references = initializeReferences(stewart);
</pre>
</div>
</div>
</div>
<div id="outline-container-org491cea7" class="outline-3">
<h3 id="org491cea7"><span class="section-number-3">3.3</span> First Control Loop - \(\bm{K}_\mathcal{L}\)</h3>
<div class="outline-text-3" id="text-3-3">
</div>
<div id="outline-container-org5cc334f" class="outline-4">
<h4 id="org5cc334f"><span class="section-number-4">3.3.1</span> Identification</h4>
<div class="outline-text-4" id="text-3-3-1">
<p>
Let&rsquo;s identify the transfer function from \(\bm{\tau}\) to \(\bm{L}\).
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'stewart_platform_model'</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
clear io; io_i = 1;
io(io_i) = linio([mdl, <span class="org-string">'/Controller'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</span>
io(io_i) = linio([mdl, <span class="org-string">'/Stewart Platform'</span>], 1, <span class="org-string">'openoutput'</span>, [], <span class="org-string">'dLm'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Relative Displacement Outputs [m]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
Gl = linearize(mdl, io);
Gl.InputName = {<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>};
Gl.OutputName = {<span class="org-string">'L1'</span>, <span class="org-string">'L2'</span>, <span class="org-string">'L3'</span>, <span class="org-string">'L4'</span>, <span class="org-string">'L5'</span>, <span class="org-string">'L6'</span>};
</pre>
</div>
</div>
</div>
<div id="outline-container-org3be701b" class="outline-4">
<h4 id="org3be701b"><span class="section-number-4">3.3.2</span> Obtained Plant</h4>
<div class="outline-text-4" id="text-3-3-2">
<p>
The obtained plant is shown in Figure <a href="#org54b5aae">20</a>.
</p>
<div id="org54b5aae" class="figure">
<p><img src="figs/hybrid_control_Kl_plant.png" alt="hybrid_control_Kl_plant.png" />
</p>
<p><span class="figure-number">Figure 20: </span>Diagonal and off-diagonal elements of the plant for the design of \(\bm{K}_\mathcal{L}\) (<a href="./figs/hybrid_control_Kl_plant.png">png</a>, <a href="./figs/hybrid_control_Kl_plant.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org4623e8f" class="outline-4">
<h4 id="org4623e8f"><span class="section-number-4">3.3.3</span> Controller Design</h4>
<div class="outline-text-4" id="text-3-3-3">
<p>
We apply a decentralized (diagonal) direct velocity feedback.
Thus, we apply a pure derivative action.
In order to make the controller realizable, we add a low pass filter at high frequency.
The gain of the controller is chosen such that the resonances are critically damped.
</p>
<p>
The obtain loop gain is shown in Figure <a href="#org66bd8fb">21</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Kl = 1e4 <span class="org-type">*</span> s <span class="org-type">/</span> (1 <span class="org-type">+</span> s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>1e4) <span class="org-type">*</span> eye(6);
</pre>
</div>
<div id="org66bd8fb" class="figure">
<p><img src="figs/hybrid_control_Kl_loop_gain.png" alt="hybrid_control_Kl_loop_gain.png" />
</p>
<p><span class="figure-number">Figure 21: </span>Obtain Loop Gain for the DVF control loop (<a href="./figs/hybrid_control_Kl_loop_gain.png">png</a>, <a href="./figs/hybrid_control_Kl_loop_gain.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org53cbafc" class="outline-3">
<h3 id="org53cbafc"><span class="section-number-3">3.4</span> Second Control Loop - \(\bm{K}_\mathcal{X}\)</h3>
<div class="outline-text-3" id="text-3-4">
</div>
<div id="outline-container-orgfc299ed" class="outline-4">
<h4 id="orgfc299ed"><span class="section-number-4">3.4.1</span> Identification</h4>
<div class="outline-text-4" id="text-3-4-1">
<div class="org-src-container">
<pre class="src src-matlab">Kx = tf(zeros(6));
controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'ref-track-hac-dvf'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'stewart_platform_model'</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
clear io; io_i = 1;
io(io_i) = linio([mdl, <span class="org-string">'/Controller'</span>], 1, <span class="org-string">'input'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</span>
io(io_i) = linio([mdl, <span class="org-string">'/Relative Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Relative Displacement Outputs [m]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G = linearize(mdl, io);
G.InputName = {<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>};
G.OutputName = {<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>};
</pre>
</div>
</div>
</div>
<div id="outline-container-org1680642" class="outline-4">
<h4 id="org1680642"><span class="section-number-4">3.4.2</span> Obtained Plant</h4>
<div class="outline-text-4" id="text-3-4-2">
<p>
We use the Jacobian matrix to apply forces in the cartesian frame.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Gx = G<span class="org-type">*</span>inv(stewart.kinematics.J<span class="org-type">'</span>);
Gx.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
</pre>
</div>
<p>
The obtained plant is shown in Figure <a href="#org6d6ab43">22</a>.
</p>
<div id="org6d6ab43" class="figure">
<p><img src="figs/hybrid_control_Kx_plant.png" alt="hybrid_control_Kx_plant.png" />
</p>
<p><span class="figure-number">Figure 22: </span>Diagonal and Off-diagonal elements of the plant for the design of \(\bm{K}_\mathcal{L}\) (<a href="./figs/hybrid_control_Kx_plant.png">png</a>, <a href="./figs/hybrid_control_Kx_plant.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgae806c2" class="outline-4">
<h4 id="orgae806c2"><span class="section-number-4">3.4.3</span> Controller Design</h4>
<div class="outline-text-4" id="text-3-4-3">
<p>
The controller consists of:
</p>
<ul class="org-ul">
<li>A pure integrator</li>
<li>A Second integrator up to half the wanted bandwidth</li>
<li>A Lead around the cross-over frequency</li>
<li>A low pass filter with a cut-off equal to three times the wanted bandwidth</li>
</ul>
<div class="org-src-container">
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>200; <span class="org-comment">% Bandwidth Bandwidth [rad/s]</span>
h = 3; <span class="org-comment">% Lead parameter</span>
Kx = (1<span class="org-type">/</span>h) <span class="org-type">*</span> (1 <span class="org-type">+</span> s<span class="org-type">/</span>wc<span class="org-type">*</span>h)<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>wc<span class="org-type">/</span>h) <span class="org-type">*</span> wc<span class="org-type">/</span>s <span class="org-type">*</span> ((s<span class="org-type">/</span>wc<span class="org-type">*</span>2 <span class="org-type">+</span> 1)<span class="org-type">/</span>(s<span class="org-type">/</span>wc<span class="org-type">*</span>2)) <span class="org-type">*</span> (1<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>wc<span class="org-type">/</span>3));
<span class="org-comment">% Normalization of the gain of have a loop gain of 1 at frequency wc</span>
Kx = Kx<span class="org-type">.*</span>diag(1<span class="org-type">./</span>diag(abs(freqresp(Gx<span class="org-type">*</span>Kx, wc))));
</pre>
</div>
<div id="orgaec2d41" class="figure">
<p><img src="figs/hybrid_control_Kx_loop_gain.png" alt="hybrid_control_Kx_loop_gain.png" />
</p>
<p><span class="figure-number">Figure 23: </span>Obtained Loop Gain for the controller \(\bm{K}_\mathcal{X}\) (<a href="./figs/hybrid_control_Kx_loop_gain.png">png</a>, <a href="./figs/hybrid_control_Kx_loop_gain.pdf">pdf</a>)</p>
</div>
<p>
Then we include the Jacobian in the controller matrix.
</p>
<div class="org-src-container">
<pre class="src src-matlab">Kx = inv(stewart.kinematics.J<span class="org-type">'</span>)<span class="org-type">*</span>Kx;
</pre>
</div>
</div>
</div>
</div>
<div id="outline-container-org96f8c42" class="outline-3">
<h3 id="org96f8c42"><span class="section-number-3">3.5</span> Simulations</h3>
<div class="outline-text-3" id="text-3-5">
<p>
We specify the reference path to follow.
</p>
<div class="org-src-container">
<pre class="src src-matlab">t = [0<span class="org-type">:</span>1e<span class="org-type">-</span>4<span class="org-type">:</span>10];
r = zeros(6, length(t));
r(1, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(2, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t<span class="org-type">.*</span>cos(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
r(3, <span class="org-type">:</span>) = 1e<span class="org-type">-</span>3<span class="org-type">.*</span>t;
references = initializeReferences(stewart, <span class="org-string">'t'</span>, t, <span class="org-string">'r'</span>, r);
</pre>
</div>
<p>
We run the simulation and we save the results.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-string">'stewart_platform_model'</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'10'</span>)
<span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>)
simout_H = simout;
save(<span class="org-string">'./mat/control_tracking.mat'</span>, <span class="org-string">'simout_H'</span>, <span class="org-string">'-append'</span>);
</pre>
</div>
<p>
The obtained position error is shown in Figure <a href="#org32b868d">24</a>.
</p>
<div id="org32b868d" class="figure">
<p><img src="figs/hybrid_control_Ex.png" alt="hybrid_control_Ex.png" />
</p>
<p><span class="figure-number">Figure 24: </span>Obtained position error \(\bm{\epsilon}_\mathcal{X}\) (<a href="./figs/hybrid_control_Ex.png">png</a>, <a href="./figs/hybrid_control_Ex.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgcf8f38f" class="outline-3">
<h3 id="orgcf8f38f"><span class="section-number-3">3.6</span> Conclusion</h3>
</div>
</div>
<div id="outline-container-orgffc4966" class="outline-2">
<h2 id="orgffc4966"><span class="section-number-2">4</span> Comparison of all the methods</h2>
<div class="outline-text-2" id="text-4">
<p>
<a id="org8ae0d7b"></a>
</p>
<p>
Let&rsquo;s load the simulation results and compare them in Figure <a href="#orgee46406">25</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">load(<span class="org-string">'./mat/control_tracking.mat'</span>, <span class="org-string">'simout_D'</span>, <span class="org-string">'simout_L'</span>, <span class="org-string">'simout_X'</span>, <span class="org-string">'simout_H'</span>);
</pre>
</div>
<div id="orgee46406" class="figure">
<p><img src="figs/reference_tracking_performance_comparison.png" alt="reference_tracking_performance_comparison.png" />
</p>
<p><span class="figure-number">Figure 25: </span>Comparison of the position errors for all the Control architecture used (<a href="./figs/reference_tracking_performance_comparison.png">png</a>, <a href="./figs/reference_tracking_performance_comparison.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org1b77c20" class="outline-2">
<h2 id="org1b77c20"><span class="section-number-2">5</span> Compute the pose error of the Stewart Platform</h2>
<div class="outline-text-2" id="text-5">
<p>
<a id="org2f1ce27"></a>
</p>
<p>
Let&rsquo;s note:
</p>
<ul class="org-ul">
<li>\(\{W\}\) the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed).
The center of the frame if \(O_W\)</li>
<li>\(\{M\}\) is the frame fixed to the measured elements.
\(O_M\) is the point where the pose of the element is measured</li>
<li>\(\{R\}\) is a virtual frame corresponding to the wanted pose of the element.
\(O_R\) is the origin of this frame where the we want to position the point \(O_M\) of the element.</li>
<li>\(\{V\}\) is a frame which its axes are aligned with \(\{W\}\) and its origin \(O_V\) is coincident with the \(O_M\)</li>
</ul>
<p>
Reference Position with respect to fixed frame {W}: \({}^WT_R\)
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dxr = 0;
Dyr = 0;
Dzr = 0.1;
Rxr = <span class="org-constant">pi</span>;
Ryr = 0;
Rzr = 0;
</pre>
</div>
<p>
Measured Position with respect to fixed frame {W}: \({}^WT_M\)
</p>
<div class="org-src-container">
<pre class="src src-matlab">Dxm = 0;
Dym = 0;
Dzm = 0;
Rxm = <span class="org-constant">pi</span>;
Rym = 0;
Rzm = 0;
</pre>
</div>
<p>
We measure the position and orientation (pose) of the element represented by the frame \(\{M\}\) with respect to frame \(\{W\}\).
Thus we can compute the Homogeneous transformation matrix \({}^WT_M\).
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Measured Pose</span></span>
WTm = zeros(4,4);
WTm(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = [cos(Rzm) <span class="org-type">-</span>sin(Rzm) 0;
sin(Rzm) cos(Rzm) 0;
0 0 1] <span class="org-type">*</span> ...
[cos(Rym) 0 sin(Rym);
0 1 0;
<span class="org-type">-</span>sin(Rym) 0 cos(Rym)] <span class="org-type">*</span> ...
[1 0 0;
0 cos(Rxm) <span class="org-type">-</span>sin(Rxm);
0 sin(Rxm) cos(Rxm)];
WTm(1<span class="org-type">:</span>4, 4) = [Dxm ; Dym ; Dzm; 1];
</pre>
</div>
<p>
We can also compute the Homogeneous transformation matrix \({}^WT_R\) corresponding to the transformation required to go from fixed frame \(\{W\}\) to the wanted frame \(\{R\}\).
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Reference Pose</span></span>
WTr = zeros(4,4);
WTr(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = [cos(Rzr) <span class="org-type">-</span>sin(Rzr) 0;
sin(Rzr) cos(Rzr) 0;
0 0 1] <span class="org-type">*</span> ...
[cos(Ryr) 0 sin(Ryr);
0 1 0;
<span class="org-type">-</span>sin(Ryr) 0 cos(Ryr)] <span class="org-type">*</span> ...
[1 0 0;
0 cos(Rxr) <span class="org-type">-</span>sin(Rxr);
0 sin(Rxr) cos(Rxr)];
WTr(1<span class="org-type">:</span>4, 4) = [Dxr ; Dyr ; Dzr; 1];
</pre>
</div>
<p>
We can also compute \({}^WT_V\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">WTv = eye(4);
WTv(1<span class="org-type">:</span>3, 4) = WTm(1<span class="org-type">:</span>3, 4);
</pre>
</div>
<p>
Now we want to express \({}^MT_R\) which corresponds to the transformation required to go to wanted position expressed in the frame of the measured element.
This homogeneous transformation can be computed from the previously computed matrices:
\[ {}^MT_R = ({{}^WT_M}^{-1}) {}^WT_R \]
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Wanted pose expressed in a frame corresponding to the actual measured pose</span></span>
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;
</pre>
</div>
<p>
Now we want to express \({}^VT_R\):
\[ {}^VT_R = ({{}^WT_V}^{-1}) {}^WT_R \]
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Wanted pose expressed in a frame coincident with the actual position but with no rotation</span></span>
VTr = [WTv(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">'</span>, <span class="org-type">-</span>WTv(1<span class="org-type">:</span>3,1<span class="org-type">:</span>3)<span class="org-type">'*</span>WTv(1<span class="org-type">:</span>3,4) ; 0 0 0 1] <span class="org-type">*</span> WTr;
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Extract Translations and Rotations from the Homogeneous Matrix</span></span>
T = MTr;
Edx = T(1, 4);
Edy = T(2, 4);
Edz = T(3, 4);
<span class="org-comment">% The angles obtained are u-v-w Euler angles (rotations in the moving frame)</span>
Ery = atan2( T(1, 3), sqrt(T(1, 1)<span class="org-type">^</span>2 <span class="org-type">+</span> T(1, 2)<span class="org-type">^</span>2));
Erx = atan2(<span class="org-type">-</span>T(2, 3)<span class="org-type">/</span>cos(Ery), T(3, 3)<span class="org-type">/</span>cos(Ery));
Erz = atan2(<span class="org-type">-</span>T(1, 2)<span class="org-type">/</span>cos(Ery), T(1, 1)<span class="org-type">/</span>cos(Ery));
</pre>
</div>
</div>
</div>
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2021-01-08 ven. 15:53</p>
</div>
</body>
</html>
Reference in New Issue View Git Blame Copy Permalink