Finalize the initialization to compensate gravity

org/simscape_subsystems.org

@@ -1261,6 +1261,8 @@ The =mirror= structure is saved.
       args.ARB (3,3) double {mustBeNumeric} = eye(3)
       % Equilibrium position of each leg
       args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
+      % Force that stiffness of each joint should apply at t=0
+      args.Foffset      logical {mustBeNumericOrLogical} = false
   end
 #+end_src
 
@@ -1287,8 +1289,15 @@ The =mirror= structure is saved.
   nano_hexapod.dLi = dLi;
 #+end_src
 
+Equilibrium position of the each joint.
 #+begin_src matlab
-  nano_hexapod.dLeq = args.dLeq;
+
+  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+    load('mat/Foffset.mat', 'Fnm');
+    nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
+  else
+    nano_hexapod.dLeq = args.dLeq;
+  end
 #+end_src
 
 ** Add Type

org/stage_initialization.org

@@ -1,4 +1,4 @@
-#+TITLE: Evaluating the Plant Uncertainty in various experimental conditions
+#+TITLE: Compensating the gravity forces to start at steady state
 :DRAWER:
 #+STARTUP: overview
 
@@ -41,13 +41,22 @@
 #+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>>
+  <<matlab-dir>>
 #+end_src
 
 #+begin_src matlab :exports none :results silent :noweb yes
-<<matlab-init>>
+  <<matlab-init>>
 #+end_src
 
 #+begin_src matlab :tangle no
@@ -58,7 +67,106 @@
   open('nass_model.slx')
 #+end_src
 
-* Initialization
+* 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');
@@ -72,18 +180,69 @@
   initializeSample('type', 'init');
 #+end_src
 
+We simulate for a short time period (all the bodies are solid, so nothing should move).
 #+begin_src matlab
-  initializeReferences();
-  initializeDisturbances('enable', false);
-  initializeController();
+  load('mat/conf_simulink.mat');
+  set_param(conf_simulink, 'StopTime', '0.1');
 #+end_src
 
 #+begin_src matlab
-  initializeSimscapeConfiguration('gravity', true);
-  initializeLoggingConfiguration('log', 'all');
+  sim('nass_model');
 #+end_src
 
-* Simulation
+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');
@@ -91,6 +250,7 @@
 
 #+begin_src matlab
   sim('nass_model');
+  sim_compensation = simout;
 #+end_src
 
 Verification that nothing is moving
@@ -98,54 +258,61 @@ Verification that nothing is moving
   figure;
   ax1 = subplot(2, 3, 1);
   hold on;
-  plot(simout.Em.En.Time, simout.Em.En.Data(:, 1))
+  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(simout.Em.En.Time, simout.Em.En.Data(:, 2))
+  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(simout.Em.En.Time, simout.Em.En.Data(:, 3))
+  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(simout.Em.En.Time, simout.Em.En.Data(:, 4))
+  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(simout.Em.En.Time, simout.Em.En.Data(:, 5))
+  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(simout.Em.En.Time, simout.Em.En.Data(:, 6))
+  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
 
-Measured Force in each leg
-#+begin_src matlab
-  Fgm
-  Ftym
-  Fym
-  Fzm
-  Fhm
-  Fnm
-  Fsm
+#+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]]

src/initializeNanoHexapod.m

@@ -33,6 +33,8 @@ arguments
     args.ARB (3,3) double {mustBeNumeric} = eye(3)
     % Equilibrium position of each leg
     args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
+    % Force that stiffness of each joint should apply at t=0
+    args.Foffset      logical {mustBeNumericOrLogical} = false
 end
 
 nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
@@ -52,7 +54,12 @@ nano_hexapod = computeJacobian(nano_hexapod);
 nano_hexapod.Li = Li;
 nano_hexapod.dLi = dLi;
 
-nano_hexapod.dLeq = args.dLeq;
+if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
+  load('mat/Foffset.mat', 'Fnm');
+  nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
+else
+  nano_hexapod.dLeq = args.dLeq;
+end
 
 switch args.type
   case 'none'