#+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

And we can observe on Figure [[fig:transient_phase_gravity_no_compensation]] that there are some motion in the system.
#+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]]
* Conclusion
This initialization technique permits to compute the required forces/torques to be applied in each joint in order to compensate for gravity forces.
This initialization should be redone for each configuration (change of sample mass, change of tilt angle), but not when changing the stiffness of joints, for instant when changing from lorentz based nano-hexapod or piezo based.