nass-simscape/org/compensation_gravity_forces.org

#+TITLE: Compensating the gravity forces to start at steady state
: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: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>

#+HTML_MATHJAX: align: center tagside: right font: TeX

#+PROPERTY: header-args:matlab  :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes

#+PROPERTY: header-args:shell  :eval no-export

#+PROPERTY: header-args:latex  :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{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 raw replace :buffer no
#+PROPERTY: header-args:latex+ :eval no-export
#+PROPERTY: header-args:latex+ :exports both
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
:END:

* Introduction                                                        :ignore:
In this file is shown a technique used to compensate the gravity forces at t=0.

The problem is that in presence of gravity, the system does not start at steady state and experience a transient phase (section [[sec:no_compensation]]).

In order to start the simulation at steady state in presence of gravity:
- section [[sec:compute_forces]]: first the stages are initialize in such a way that they are rigid, and the forces/torques applied at the location of their joints is measured
- section [[sec:compensation]]: Then, the equilibrium position of each joint is modified in such a way that at t=0, the forces in each joints exactly compensate the forces due to gravity forces

* 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 :tangle no
  simulinkproject('../');
#+end_src

#+begin_src matlab
  open('nass_model.slx')
#+end_src

* Initialization of the Experimental Conditions
We don't inject any perturbations and no reference tracking.
#+begin_src matlab
  initializeReferences();
  initializeDisturbances('enable', false);
  initializeController();
#+end_src

We include the gravity and log all the signals to display.
#+begin_src matlab
  initializeSimscapeConfiguration('gravity', true);
  initializeLoggingConfiguration('log', 'all');
#+end_src

* Without compensation
<<sec:no_compensation>>
Let's simulate the system without any compensation of gravity forces.

#+begin_src matlab
  initializeGround();
  initializeGranite();
  initializeTy();
  initializeRy();
  initializeRz();
  initializeMicroHexapod();
  initializeAxisc();
  initializeMirror();
  initializeNanoHexapod();
  initializeSample();
#+end_src

#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '0.5');
#+end_src

#+begin_src matlab
  sim('nass_model');
  sim_no_compensation = simout;
#+end_src

Verification that nothing is moving
#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 1))
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 2))
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 3))
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 4))
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 5))
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 6))
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/transient_phase_gravity_no_compensation.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:transient_phase_gravity_no_compensation
#+caption: Motion of the sample at the start of the simulation in presence of gravity ([[./figs/transient_phase_gravity_no_compensation.png][png]], [[./figs/transient_phase_gravity_no_compensation.pdf][pdf]])
[[file:figs/transient_phase_gravity_no_compensation.png]]

* Simulation to compute the required force in each joint
<<sec:compute_forces>>
We here wish to simulate the system in order to compute the required force in each joint to compensate the gravity forces.

#+begin_src matlab
  initializeGround();
  initializeGranite('type', 'init');
  initializeTy('type', 'init');
  initializeRy('type', 'init');
  initializeRz('type', 'init');
  initializeMicroHexapod('type', 'init');
  initializeAxisc();
  initializeMirror();
  initializeNanoHexapod('type', 'init');
  initializeSample('type', 'init');
#+end_src

We simulate for a short time period (all the bodies are solid, so nothing should move).
#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '0.1');
#+end_src

#+begin_src matlab
  sim('nass_model');
#+end_src

Verification that nothing is moving by looking at the maximum displacement of the sample:
#+begin_src matlab :results value replace
  max(max(simout.Em.En.Data))
#+end_src

#+RESULTS:
: 1.0681e-15

We here show the measured total force/torque applied at the location of each joint.
#+begin_src matlab :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([Fgm 0 0 0; Ftym; Fym; Fsm], {'Granite', 'Translation Stage', 'Tilt Stage', 'Sample'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}, ' %.1e ');
#+end_src

#+RESULTS:
|                   |       Fx |      Fy |       Fz |       Mx |       My |      Mz |
|-------------------+----------+---------+----------+----------+----------+---------|
| Granite           | -7.6e-12 | 1.2e-11 | -34000.0 |      0.0 |      0.0 |     0.0 |
| Translation Stage | -7.6e-12 | 1.2e-11 | -12000.0 |     31.0 |      2.5 | 6.6e-13 |
| Tilt Stage        | -7.6e-12 | 1.2e-11 |  -8800.0 |     33.0 |    -0.52 | 6.6e-13 |
| Sample            | -5.7e-12 | 1.3e-11 |   -490.0 | -2.5e-12 | -8.1e-13 | 2.7e-13 |

#+begin_src matlab :results value table replace :tangle no :post addhdr(*this*)
  data2orgtable([Fhm; Fnm], {'Micro-Hexapod', 'Nano-Hexapod'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}, ' %.1e ');
#+end_src

#+RESULTS:
|               |     F1 |     F2 |     F3 |     F4 |     F5 |     F6 |
|---------------+--------+--------+--------+--------+--------+--------|
| Micro-Hexapod | -180.0 | -180.0 | -180.0 | -180.0 | -180.0 | -180.0 |
| Nano-Hexapod  | -160.0 | -160.0 | -160.0 | -160.0 | -160.0 | -160.0 |

We save these forces in =Foffset.mat=.
#+begin_src matlab
  save('mat/Foffset.mat', 'Fgm', 'Ftym', 'Fym', 'Fzm', 'Fhm', 'Fnm', 'Fsm');
#+end_src

* New simulation with compensation of gravity forces
<<sec:compensation>>
We now initialize the stages with the option =Foffset=.
#+begin_src matlab
  initializeGround();
  initializeGranite('Foffset', true);
  initializeTy('Foffset', true);
  initializeRy('Foffset', true);
  initializeRz('Foffset', true);
  initializeMicroHexapod('Foffset', true);
  initializeAxisc();
  initializeMirror();
  initializeNanoHexapod('Foffset', true);
  initializeSample('Foffset', true);
#+end_src

And we simulate the system for 0.5 seconds.
#+begin_src matlab
  load('mat/conf_simulink.mat');
  set_param(conf_simulink, 'StopTime', '0.5');
#+end_src

#+begin_src matlab
  sim('nass_model');
  sim_compensation = simout;
#+end_src

Verification that nothing is moving
#+begin_src matlab :exports none
  figure;
  ax1 = subplot(2, 3, 1);
  hold on;
  plot(sim_compensation.Em.En.Time, sim_compensation.Em.En.Data(:, 1))
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 1))
  hold off;
  xlabel('Time [s]');
  ylabel('Dx [m]');

  ax2 = subplot(2, 3, 2);
  hold on;
  plot(sim_compensation.Em.En.Time, sim_compensation.Em.En.Data(:, 2))
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 2))
  hold off;
  xlabel('Time [s]');
  ylabel('Dy [m]');

  ax3 = subplot(2, 3, 3);
  hold on;
  plot(sim_compensation.Em.En.Time, sim_compensation.Em.En.Data(:, 3))
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 3))
  hold off;
  xlabel('Time [s]');
  ylabel('Dz [m]');

  ax4 = subplot(2, 3, 4);
  hold on;
  plot(sim_compensation.Em.En.Time, sim_compensation.Em.En.Data(:, 4))
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 4))
  hold off;
  xlabel('Time [s]');
  ylabel('Rx [rad]');

  ax5 = subplot(2, 3, 5);
  hold on;
  plot(sim_compensation.Em.En.Time, sim_compensation.Em.En.Data(:, 5))
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 5))
  hold off;
  xlabel('Time [s]');
  ylabel('Ry [rad]');

  ax6 = subplot(2, 3, 6);
  hold on;
  plot(sim_compensation.Em.En.Time, sim_compensation.Em.En.Data(:, 6))
  plot(sim_no_compensation.Em.En.Time, sim_no_compensation.Em.En.Data(:, 6))
  hold off;
  xlabel('Time [s]');
  ylabel('Rz [rad]');

  linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
  xlim([sim_compensation.Em.En.Time(1), sim_compensation.Em.En.Time(end)])
#+end_src

#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/transient_phase_gravity_compensation.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src

#+name: fig:transient_phase_gravity_compensation
#+caption: Motion of the sample at the start of the simulation in presence of gravity when compensating the gravity forces  ([[./figs/transient_phase_gravity_compensation.png][png]], [[./figs/transient_phase_gravity_compensation.pdf][pdf]])
[[file:figs/transient_phase_gravity_compensation.png]]