Reworked the compensation of gravity forces

This commit is contained in:
Thomas Dehaeze 2020-04-14 10:04:42 +02:00
parent dce2934bec
commit e1cde2bd5b

View File

@ -34,114 +34,56 @@ In order to start the simulation at steady state in presence of gravity:
- 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
simulinkproject('../');
#+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
#+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
#+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
#+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
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
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
#+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]]
@ -150,41 +92,41 @@ And we can observe on Figure [[fig:transient_phase_gravity_no_compensation]] tha
<<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
#+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
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.1');
#+END_SRC
#+begin_src matlab
sim('nass_model');
#+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
#+BEGIN_SRC matlab
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
#+BEGIN_SRC matlab
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 |
@ -194,9 +136,9 @@ We here show the measured total force/torque applied at the location of each joi
| 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
#+BEGIN_SRC matlab
data2orgtable([Fhm; Fnm], {'Micro-Hexapod', 'Nano-Hexapod'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}, ' %.1e ');
#+END_SRC
#+RESULTS:
| | F1 | F2 | F3 | F4 | F5 | F6 |
@ -205,97 +147,38 @@ We here show the measured total force/torque applied at the location of each joi
| 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
#+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
#+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
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
#+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]]