Work on HAC-LAC, Control architectures

org/active-damping.org

@@ -93,6 +93,7 @@ To run the script, open the Simulink Project, and type =run active_damping_inert
 #+begin_src matlab
   ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 #+begin_src matlab
@@ -323,18 +324,11 @@ We first initialize the Stewart platform without joint stiffness.
 #+begin_src matlab
   ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
   payload = initializePayload('type', 'none');
-#+end_src
-
-#+begin_src matlab
   controller = initializeController('type', 'open-loop');
 #+end_src
 
 And we identify the dynamics from force actuators to force sensors.
 #+begin_src matlab
-  %% Options for Linearized
-  options = linearizeOptions;
-  options.SampleTime = 0;
-
   %% Name of the Simulink File
   mdl = 'stewart_platform_model';
 
@@ -344,7 +338,7 @@ And we identify the dynamics from force actuators to force sensors.
   io(io_i) = linio([mdl, '/Stewart Platform'],  1, 'openoutput', [], 'Taum'); io_i = io_i + 1; % Force Sensor Outputs [N]
 
   %% Run the linearization
-  G = linearize(mdl, io, options);
+  G = linearize(mdl, io);
   G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
   G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
 #+end_src
@@ -398,7 +392,7 @@ The transfer function from actuator forces to force sensors is shown in Figure [
 We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
 #+begin_src matlab
   stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
-  Gf = linearize(mdl, io, options);
+  Gf = linearize(mdl, io);
   Gf.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
   Gf.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
 #+end_src
@@ -406,7 +400,7 @@ We add some stiffness and damping in the flexible joints and we re-identify the
 We now use the amplified actuators and re-identify the dynamics
 #+begin_src matlab
   stewart = initializeAmplifiedStrutDynamics(stewart);
-  Ga = linearize(mdl, io, options);
+  Ga = linearize(mdl, io);
   Ga.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
   Ga.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
 #+end_src
@@ -596,6 +590,7 @@ We first initialize the Stewart platform without joint stiffness.
 #+begin_src matlab
   ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 And we identify the dynamics from force actuators to force sensors.
@@ -778,6 +773,24 @@ The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]].
   Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
 #+end_important
 * Compliance and Transmissibility Comparison
+** Introduction                                                      :ignore:
+** Matlab Init                                             :noexport:ignore:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+<<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+<<matlab-init>>
+#+end_src
+
+#+begin_src matlab
+  simulinkproject('../');
+#+end_src
+
+#+begin_src matlab
+  open('stewart_platform_model.slx')
+#+end_src
+
 ** Initialization
 We first initialize the Stewart platform without joint stiffness.
 #+begin_src matlab
@@ -798,6 +811,7 @@ The rotation point of the ground is located at the origin of frame $\{A\}$.
 #+begin_src matlab
   ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 ** Identification
@@ -811,7 +825,7 @@ Let's first identify the transmissibility and compliance in the open-loop case.
 Now, let's identify the transmissibility and compliance for the Integral Force Feedback architecture.
 #+begin_src matlab
   controller = initializeController('type', 'iff');
-  G_iff = (2e4/s)*eye(6);
+  K_iff = (1e4/s)*eye(6);
 
   [T_iff, T_norm_iff, ~] = computeTransmissibility();
   [C_iff, C_norm_iff, ~] = computeCompliance();
@@ -820,7 +834,7 @@ Now, let's identify the transmissibility and compliance for the Integral Force F
 And for the Direct Velocity Feedback.
 #+begin_src matlab
   controller = initializeController('type', 'dvf');
-  G_dvf = 1e4*s/(1+s/2/pi/5000)*eye(6);
+  K_dvf = 1e4*s/(1+s/2/pi/5000)*eye(6);
 
   [T_dvf, T_norm_dvf, ~] = computeTransmissibility();
   [C_dvf, C_norm_dvf, ~] = computeCompliance();
@@ -857,8 +871,8 @@ And for the Direct Velocity Feedback.
   han = axes(fig, 'visible', 'off');
   han.XLabel.Visible = 'on';
   han.YLabel.Visible = 'on';
-  ylabel(han, 'Frequency [Hz]');
-  xlabel(han, 'Transmissibility');
+  xlabel(han, 'Frequency [Hz]');
+  ylabel(han, 'Transmissibility');
 #+end_src
 
 #+header: :tangle no :exports results :results none :noweb yes
@@ -900,8 +914,8 @@ And for the Direct Velocity Feedback.
   han = axes(fig, 'visible', 'off');
   han.XLabel.Visible = 'on';
   han.YLabel.Visible = 'on';
-  ylabel(han, 'Frequency [Hz]');
-  xlabel(han, 'Compliance');
+  xlabel(han, 'Frequency [Hz]');
+  ylabel(han, 'Compliance');
 #+end_src
 
 #+header: :tangle no :exports results :results none :noweb yes

org/control-tracking.org

@@ -0,0 +1,257 @@
+#+TITLE: Stewart Platform - Tracking Control
+:DRAWER:
+#+STARTUP: overview
+
+#+LANGUAGE: en
+#+EMAIL: dehaeze.thomas@gmail.com
+#+AUTHOR: Dehaeze Thomas
+
+#+HTML_LINK_HOME: ./index.html
+#+HTML_LINK_UP: ./index.html
+
+#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
+#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
+#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
+#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
+#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
+#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
+
+#+PROPERTY: header-args:matlab  :session *MATLAB*
+#+PROPERTY: header-args:matlab+ :comments org
+#+PROPERTY: header-args:matlab+ :exports both
+#+PROPERTY: header-args:matlab+ :results none
+#+PROPERTY: header-args:matlab+ :eval no-export
+#+PROPERTY: header-args:matlab+ :noweb yes
+#+PROPERTY: header-args:matlab+ :mkdirp yes
+#+PROPERTY: header-args:matlab+ :output-dir figs
+
+#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
+#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
+#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
+#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
+#+PROPERTY: header-args:latex+ :results file raw replace
+#+PROPERTY: header-args:latex+ :buffer no
+#+PROPERTY: header-args:latex+ :eval no-export
+#+PROPERTY: header-args:latex+ :exports results
+#+PROPERTY: header-args:latex+ :mkdirp yes
+#+PROPERTY: header-args:latex+ :output-dir figs
+#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
+:END:
+
+* First Control Architecture
+** Matlab Init                                                     :noexport:
+#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
+  <<matlab-dir>>
+#+end_src
+
+#+begin_src matlab :exports none :results silent :noweb yes
+  <<matlab-init>>
+#+end_src
+
+#+begin_src matlab
+  simulinkproject('../');
+#+end_src
+
+** Control Schematic
+#+begin_src latex :file control_measure_rotating_2dof.pdf
+  \begin{tikzpicture}
+    % Blocs
+    \node[block] (J) at (0, 0) {$J$};
+    \node[addb={+}{}{}{}{-}, right=1 of J] (subr) {};
+    \node[block, right=0.8 of subr] (K) {$K_{L}$};
+    \node[block, right=1 of K] (G) {$G_{L}$};
+
+    % Connections and labels
+    \draw[<-] (J.west)node[above left]{$\bm{r}_{n}$} -- ++(-1, 0);
+    \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{L}$};
+    \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{L}$};
+    \draw[->] (K.east) -- (G.west) node[above left]{$\bm{\tau}$};
+    \draw[->] (G.east) node[above right]{$\bm{L}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
+  \end{tikzpicture}
+#+end_src
+
+#+RESULTS:
+[[file:figs/control_measure_rotating_2dof.png]]
+
+** Initialize the Stewart platform
+#+begin_src matlab
+  stewart = initializeStewartPlatform();
+  stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
+  stewart = generateGeneralConfiguration(stewart);
+  stewart = computeJointsPose(stewart);
+  stewart = initializeStrutDynamics(stewart);
+  stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
+  stewart = initializeCylindricalPlatforms(stewart);
+  stewart = initializeCylindricalStruts(stewart);
+  stewart = computeJacobian(stewart);
+  stewart = initializeStewartPose(stewart);
+  stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
+#+end_src
+
+#+begin_src matlab
+  ground = initializeGround('type', 'none');
+  payload = initializePayload('type', 'none');
+#+end_src
+
+** Identification of the plant
+Let's identify the transfer function from $\bm{\tau}$ to $\bm{L}$.
+#+begin_src matlab
+  %% Name of the Simulink File
+  mdl = 'stewart_platform_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller'],        1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
+  io(io_i) = linio([mdl, '/Stewart Platform'],  1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
+
+  %% Run the linearization
+  G = linearize(mdl, io);
+  G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
+  G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
+#+end_src
+
+** Plant Analysis
+Diagonal terms
+#+begin_src matlab :exports none
+  freqs = logspace(1, 4, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:6
+    plot(freqs, abs(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:6
+    plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
+  end
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+Compare to off-diagonal terms
+#+begin_src matlab :exports none
+  freqs = logspace(1, 4, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+** Controller Design
+One integrator should be present in the controller.
+
+A lead is added around the crossover frequency which is set to be around 500Hz.
+
+#+begin_src matlab
+  % wint = 2*pi*100; % Integrate until [rad]
+  % wlead = 2*pi*500; % Location of the lead [rad]
+  % hlead = 2; % Lead strengh
+
+  % Kl = 1e6 * ... % Gain
+  %      (s + wint)/(s) * ... % Integrator until 100Hz
+  %      (1 + s/(wlead/hlead)/(1 + s/(wlead*hlead))); % Lead
+
+  wc = 2*pi*100;
+  Kl = 1/abs(freqresp(G(1,1), wc)) * wc/s * 1/(1 + s/(3*wc));
+  Kl = Kl * eye(6);
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(1, 3, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  plot(freqs, abs(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src
+
+#+begin_src matlab :exports none
+  freqs = logspace(1, 4, 1000);
+
+  figure;
+
+  ax1 = subplot(2, 1, 1);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, abs(squeeze(freqresp(Kl(i,i)*G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, abs(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+  ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+
+  ax2 = subplot(2, 1, 2);
+  hold on;
+  for i = 1:5
+    for j = i+1:6
+      plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(i, i)*G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+    end
+  end
+  set(gca,'ColorOrderIndex',1);
+  plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
+  hold off;
+  set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+  ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
+  ylim([-180, 180]);
+  yticks([-180, -90, 0, 90, 180]);
+
+  linkaxes([ax1,ax2],'x');
+#+end_src

org/cubic-configuration.org

@@ -695,6 +695,7 @@ No flexibility below the Stewart platform and no payload.
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 The obtain geometry is shown in figure [[fig:stewart_cubic_conf_decouple_dynamics]].
@@ -880,6 +881,7 @@ No flexibility below the Stewart platform and no payload.
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 The obtain geometry is shown in figure [[fig:stewart_cubic_conf_mass_above]].
@@ -1087,6 +1089,7 @@ No flexibility below the Stewart platform and no payload.
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 #+begin_src matlab :exports none
@@ -1258,6 +1261,7 @@ No flexibility below the Stewart platform and no payload.
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 #+begin_src matlab :exports none

org/dynamics-study.org

@@ -80,6 +80,7 @@ We also don't put any payload on top of the Stewart platform.
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 The transfer function from actuator forces $\bm{\tau}$ to the relative displacement of the mobile platform $\mathcal{\bm{X}}$ is extracted.
@@ -327,6 +328,7 @@ No flexibility below the Stewart platform and no payload.
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:

org/identification.org

@@ -83,6 +83,7 @@ In this document, we discuss the various methods to identify the behavior of the
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 ** Identification
@@ -284,6 +285,7 @@ We set the rotation point of the ground to be at the same point at frames $\{A\}
 #+begin_src matlab
   ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
   payload = initializePayload('type', 'rigid');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 ** Transmissibility
@@ -396,6 +398,7 @@ We set the rotation point of the ground to be at the same point at frames $\{A\}
 #+begin_src matlab
   ground = initializeGround('type', 'none');
   payload = initializePayload('type', 'rigid');
+  controller = initializeController('type', 'open-loop');
 #+end_src
 
 ** Compliance
@@ -517,7 +520,7 @@ We can try to use the Frobenius norm to obtain a scalar value representing the 6
   %% Input/Output definition
   clear io; io_i = 1;
   io(io_i) = linio([mdl, '/Disturbances/D_w'],        1, 'openinput');  io_i = io_i + 1; % Base Motion [m, rad]
-  io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
+  io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'output'); io_i = io_i + 1; % Absolute Motion [m, rad]
 
   %% Run the linearization
   T = linearize(mdl, io, options);
@@ -553,8 +556,8 @@ If wanted, the 6x6 transmissibility matrix is plotted.
     han = axes(fig, 'visible', 'off');
     han.XLabel.Visible = 'on';
     han.YLabel.Visible = 'on';
-    ylabel(han, 'Frequency [Hz]');
-    xlabel(han, 'Transmissibility [m/m]');
+    xlabel(han, 'Frequency [Hz]');
+    ylabel(han, 'Transmissibility [m/m]');
   end
 #+end_src
 
@@ -642,7 +645,7 @@ If wanted, the 6x6 transmissibility matrix is plotted.
   %% Input/Output definition
   clear io; io_i = 1;
   io(io_i) = linio([mdl, '/Disturbances/F_ext'],      1, 'openinput');  io_i = io_i + 1; % External forces [N, N*m]
-  io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
+  io(io_i) = linio([mdl, '/Absolute Motion Sensor'],  1, 'output'); io_i = io_i + 1; % Absolute Motion [m, rad]
 
   %% Run the linearization
   C = linearize(mdl, io, options);