tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Reworked the compensation of gravity forces

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-04-14 10:04:42 +02:00
parent dce2934bec
commit e1cde2bd5b
1 changed files with 86 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

289
org/compensation_gravity_forces.org
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 - 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: * Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+BEGIN_SRC matlab
<<matlab-dir>> simulinkproject('../');
#+end_src #+END_SRC
#+begin_src matlab :exports none :results silent :noweb yes #+BEGIN_SRC matlab
<<matlab-init>> open('nass_model.slx')
#+end_src #+END_SRC
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
#+begin_src matlab
open('nass_model.slx')
#+end_src
* Initialization of the Experimental Conditions * Initialization of the Experimental Conditions
We don't inject any perturbations and no reference tracking. We don't inject any perturbations and no reference tracking.
#+begin_src matlab #+BEGIN_SRC matlab
initializeReferences(); initializeReferences();
initializeDisturbances('enable', false); initializeDisturbances('enable', false);
initializeController(); initializeController();
#+end_src #+END_SRC
We include the gravity and log all the signals to display. We include the gravity and log all the signals to display.
#+begin_src matlab #+BEGIN_SRC matlab
initializeSimscapeConfiguration('gravity', true); initializeSimscapeConfiguration('gravity', true);
initializeLoggingConfiguration('log', 'all'); initializeLoggingConfiguration('log', 'all');
#+end_src #+END_SRC
* Without compensation * Without compensation
<<sec:no_compensation>> <<sec:no_compensation>>
Let's simulate the system without any compensation of gravity forces. Let's simulate the system without any compensation of gravity forces.
#+begin_src matlab #+BEGIN_SRC matlab
initializeGround(); initializeGround();
initializeGranite(); initializeGranite();
initializeTy(); initializeTy();
initializeRy(); initializeRy();
initializeRz(); initializeRz();
initializeMicroHexapod(); initializeMicroHexapod();
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod(); initializeNanoHexapod();
initializeSample(); initializeSample();
#+end_src #+END_SRC
#+begin_src matlab #+BEGIN_SRC matlab
load('mat/conf_simulink.mat'); load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5'); set_param(conf_simulink, 'StopTime', '0.5');
#+end_src #+END_SRC
#+begin_src matlab #+BEGIN_SRC matlab
sim('nass_model'); sim('nass_model');
sim_no_compensation = simout; sim_no_compensation = simout;
#+end_src #+END_SRC
And we can observe on Figure [[fig:transient_phase_gravity_no_compensation]] that there are some motion in the system. 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 #+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]]) #+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]] [[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>> <<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. 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 #+BEGIN_SRC matlab
initializeGround(); initializeGround();
initializeGranite('type', 'init'); initializeGranite('type', 'init');
initializeTy('type', 'init'); initializeTy('type', 'init');
initializeRy('type', 'init'); initializeRy('type', 'init');
initializeRz('type', 'init'); initializeRz('type', 'init');
initializeMicroHexapod('type', 'init'); initializeMicroHexapod('type', 'init');
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod('type', 'init'); initializeNanoHexapod('type', 'init');
initializeSample('type', 'init'); initializeSample('type', 'init');
#+end_src #+END_SRC
We simulate for a short time period (all the bodies are solid, so nothing should move). We simulate for a short time period (all the bodies are solid, so nothing should move).
#+begin_src matlab #+BEGIN_SRC matlab
load('mat/conf_simulink.mat'); load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.1'); set_param(conf_simulink, 'StopTime', '0.1');
#+end_src #+END_SRC
#+begin_src matlab #+BEGIN_SRC matlab
sim('nass_model'); sim('nass_model');
#+end_src #+END_SRC
Verification that nothing is moving by looking at the maximum displacement of the sample: Verification that nothing is moving by looking at the maximum displacement of the sample:
#+begin_src matlab :results value replace #+BEGIN_SRC matlab
max(max(simout.Em.En.Data)) max(max(simout.Em.En.Data))
#+end_src #+END_SRC
#+RESULTS: #+RESULTS:
: 1.0681e-15 : 1.0681e-15
We here show the measured total force/torque applied at the location of each joint. 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*) #+BEGIN_SRC matlab
data2orgtable([Fgm 0 0 0; Ftym; Fym; Fsm], {'Granite', 'Translation Stage', 'Tilt Stage', 'Sample'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}, ' %.1e '); data2orgtable([Fgm 0 0 0; Ftym; Fym; Fsm], {'Granite', 'Translation Stage', 'Tilt Stage', 'Sample'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}, ' %.1e ');
#+end_src #+END_SRC
#+RESULTS: #+RESULTS:
| | Fx | Fy | Fz | Mx | My | Mz | | | 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 | | 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 | | 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*) #+BEGIN_SRC matlab
data2orgtable([Fhm; Fnm], {'Micro-Hexapod', 'Nano-Hexapod'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}, ' %.1e '); data2orgtable([Fhm; Fnm], {'Micro-Hexapod', 'Nano-Hexapod'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}, ' %.1e ');
#+end_src #+END_SRC
#+RESULTS: #+RESULTS:
| | F1 | F2 | F3 | F4 | F5 | F6 | | | 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 | | Nano-Hexapod | -160.0 | -160.0 | -160.0 | -160.0 | -160.0 | -160.0 |
We save these forces in =Foffset.mat=. We save these forces in =Foffset.mat=.
#+begin_src matlab #+BEGIN_SRC matlab
save('mat/Foffset.mat', 'Fgm', 'Ftym', 'Fym', 'Fzm', 'Fhm', 'Fnm', 'Fsm'); save('mat/Foffset.mat', 'Fgm', 'Ftym', 'Fym', 'Fzm', 'Fhm', 'Fnm', 'Fsm');
#+end_src #+END_SRC
* New simulation with compensation of gravity forces * New simulation with compensation of gravity forces
<<sec:compensation>> <<sec:compensation>>
We now initialize the stages with the option =Foffset=. We now initialize the stages with the option =Foffset=.
#+begin_src matlab #+BEGIN_SRC matlab
initializeGround(); initializeGround();
initializeGranite('Foffset', true); initializeGranite('Foffset', true);
initializeTy('Foffset', true); initializeTy('Foffset', true);
initializeRy('Foffset', true); initializeRy('Foffset', true);
initializeRz('Foffset', true); initializeRz('Foffset', true);
initializeMicroHexapod('Foffset', true); initializeMicroHexapod('Foffset', true);
initializeAxisc(); initializeAxisc();
initializeMirror(); initializeMirror();
initializeNanoHexapod('Foffset', true); initializeNanoHexapod('Foffset', true);
initializeSample('Foffset', true); initializeSample('Foffset', true);
#+end_src #+END_SRC
And we simulate the system for 0.5 seconds. And we simulate the system for 0.5 seconds.
#+begin_src matlab #+BEGIN_SRC matlab
load('mat/conf_simulink.mat'); load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '0.5'); set_param(conf_simulink, 'StopTime', '0.5');
#+end_src #+END_SRC
#+begin_src matlab #+BEGIN_SRC matlab
sim('nass_model'); sim('nass_model');
sim_compensation = simout; sim_compensation = simout;
#+end_src #+END_SRC
Verification that nothing is moving 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 #+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]]) #+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]] [[file:figs/transient_phase_gravity_compensation.png]]