diff --git a/docs/figs/transient_phase_gravity_compensation.pdf b/docs/figs/transient_phase_gravity_compensation.pdf new file mode 100644 index 0000000..d370045 Binary files /dev/null and b/docs/figs/transient_phase_gravity_compensation.pdf differ diff --git a/docs/figs/transient_phase_gravity_compensation.png b/docs/figs/transient_phase_gravity_compensation.png new file mode 100644 index 0000000..322fd16 Binary files /dev/null and b/docs/figs/transient_phase_gravity_compensation.png differ diff --git a/docs/figs/transient_phase_gravity_no_compensation.pdf b/docs/figs/transient_phase_gravity_no_compensation.pdf new file mode 100644 index 0000000..4dca2b7 Binary files /dev/null and b/docs/figs/transient_phase_gravity_no_compensation.pdf differ diff --git a/docs/figs/transient_phase_gravity_no_compensation.png b/docs/figs/transient_phase_gravity_no_compensation.png new file mode 100644 index 0000000..44f4c3d Binary files /dev/null and b/docs/figs/transient_phase_gravity_no_compensation.png differ diff --git a/matlab/nass_model.slx b/matlab/nass_model.slx index ebbffa8..4bfff95 100644 Binary files a/matlab/nass_model.slx and b/matlab/nass_model.slx differ diff --git a/org/simscape_subsystems.org b/org/simscape_subsystems.org index e6034d2..262741d 100644 --- a/org/simscape_subsystems.org +++ b/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 diff --git a/org/stage_initialization.org b/org/stage_initialization.org index d023c67..90acd4f 100644 --- a/org/stage_initialization.org +++ b/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) -<> + <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes -<> + <> #+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 +<> +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") +<> +#+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 +<> +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 +<> +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") +<> #+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]] diff --git a/src/initializeNanoHexapod.m b/src/initializeNanoHexapod.m index 6e89411..0b6e30b 100644 --- a/src/initializeNanoHexapod.m +++ b/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'