From 7084c7a368eab6ebfcd2b3856dc99875dbf8beaa Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Fri, 25 Oct 2024 17:35:08 +0200 Subject: [PATCH] Remove unused sections --- test-bench-nano-hexapod.org | 330 ------------------------------------ 1 file changed, 330 deletions(-) diff --git a/test-bench-nano-hexapod.org b/test-bench-nano-hexapod.org index bcc4d00..1bae01c 100644 --- a/test-bench-nano-hexapod.org +++ b/test-bench-nano-hexapod.org @@ -6643,336 +6643,6 @@ xlim([1, Fs/2]); ylim([1e-11, 1e-7]); #+end_src -* Feedforward Control :noexport: -<> - -** Introduction :ignore: - -#+begin_src latex :file control_architecture_iff_feedforward.pdf -\begin{tikzpicture} - % Blocs - \node[block={3.0cm}{3.0cm}] (P) {Plant}; - \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); - \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); - \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); - \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); - - \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; - \node[addb, left= of inputF] (addF) {}; - \node[block, left= of addF] (Kff) {$\bm{K}_{\mathcal{L},\text{ff}}$}; - \node[block, align=center, left= of Kff] (J) {Inverse\\Kinematics}; - - % Connections and labels - \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; - \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); - \draw[->] (Kiff.west) -| (addF.north); - \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$}; - - \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$}; - \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$}; - - \draw[->] (Kff.east) -- (addF.west) node[above left]{$\bm{u}_{\text{ff}}$}; - \draw[->] (J.east) -- (Kff.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; - \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0); -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_control_architecture_iff_feedforward -#+caption: Feedforward control in the frame of the legs -#+RESULTS: -[[file:figs/test_nhexa_control_architecture_iff_feedforward.png]] - -Main problems: -- Non-linearity: Creep, Hysteresis -- Variability of the plant - -** 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 :noweb yes -<> -#+end_src - -#+begin_src matlab :eval no :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -<> -#+end_src - -#+begin_src matlab -load('damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt') -#+end_src - -** Simple Feedforward Controller -Let's estimate the mean DC gain for the damped plant (diagonal elements:) -#+begin_src matlab :results value replace :exports results :tangle no -mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:)))))) -#+end_src - -#+RESULTS: -: 1.773e-05 - -The feedforward controller is then taken as the inverse of this gain (the minus sign is there manually added as it is "removed" by the =abs= function): -#+begin_src matlab -Kff_iff_L = -1/mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:)))))); -#+end_src - -The open-loop gain (feedforward controller times the damped plant) is shown in Figure ref:fig:test_nhexa_open_loop_gain_feedforward_iff_struts. - -#+begin_src matlab :exports none -%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - set(gca,'ColorOrderIndex',1); - plot(f, abs(Kff_iff_L*G_enc_iff_opt(:,i,i)), 'k-'); -end - -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [-]'); set(gca, 'XTickLabel',[]); -ylim([1e-2, 1e1]); - -ax2 = nexttile; -hold on; -for i = 1:6 - set(gca,'ColorOrderIndex',1); - plot(f, 180/pi*angle(Kff_iff_L*G_enc_iff_opt(:,i,i)), 'k-') -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); -ylim([-180, 180]); -yticks([-180, -90, 0, 90, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([1, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/open_loop_gain_feedforward_iff_struts.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_open_loop_gain_feedforward_iff_struts -#+caption: Diagonal elements of the "open loop gain" -#+RESULTS: -[[file:figs/test_nhexa_open_loop_gain_feedforward_iff_struts.png]] - -And save the feedforward controller for further use: -#+begin_src matlab -Kff_iff_L = zpk(Kff_iff_L)*eye(6); -#+end_src - -#+begin_src matlab :tangle no -save('matlab/data_sim/feedforward_iff.mat', 'Kff_iff_L') -#+end_src - -#+begin_src matlab :exports none :eval no -save('data_sim/feedforward_iff.mat', 'Kff_iff_L') -#+end_src - -** Test with Simscape Model -#+begin_src matlab -load('reference_path.mat', 'Rx_yz'); -#+end_src - -** Feedback/Feedforward control in the frame of the struts -*** Introduction :ignore: - -#+begin_src latex :file control_architecture_hac_iff_L_feedforward.pdf -\begin{tikzpicture} - % Blocs - \node[block={3.0cm}{3.0cm}] (P) {Plant}; - \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); - \coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$); - \coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$); - \coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$); - - \node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$}; - \node[addb, left= of inputF] (addF) {}; - \node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$}; - \node[block, above= of K] (Kff) {$\bm{K}_{\mathcal{L},\text{ff}}$}; - \node[addb, left= of K] (subr) {}; - \node[block, align=center, left= of subr] (J) {Inverse\\Kinematics}; - - % Connections and labels - \draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$}; - \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); - \draw[->] (Kiff.west) -| (addF.north); - \draw[->] (addF.east) -- (inputF) node[above left]{$\bm{u}$}; - - \draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$}; - \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, -1) -| (subr.south); - \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$}; - \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$}; - - \draw[->] (outputX) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}_n$}; - - \draw[->] (J.east) -- (subr.west); - \draw[->] ($(J.east) + (0.4, 0)$)node[branch]{} node[below]{$\bm{r}_{d\mathcal{L}}$} |- (Kff.west); - \draw[->] (Kff.east) -- ++(0.5, 0) -- (addF.north west); - - \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0); -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_control_architecture_hac_iff_L_feedforward -#+caption: Feedback/Feedforward control in the frame of the legs -#+RESULTS: -[[file:figs/test_nhexa_control_architecture_hac_iff_L_feedforward.png]] - - - -* Advanced Control :noexport: -:PROPERTIES: -:header-args:matlab+: :tangle matlab/nano_hexapod_advanced_control.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -** Matlab Init :noexport:ignore: -#+begin_src matlab -%% frf_enc_plates_comp_simscape.m -% Compare the measured dynamics from u to dL and to taum with the Simscape model -% Encoders are fixed to the plates -#+end_src - -#+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 :noweb yes -<> -#+end_src - -#+begin_src matlab :eval no :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -#+end_src - -#+begin_src matlab -%% Load identification data -frf_ol = load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_tau', 'G_dL'); -#+end_src - -** Identification with the Simscape Model -The nano-hexapod is initialized with the APA taken as 2dof models. -#+begin_src matlab -%% Initialize Nano-Hexapod -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); - -support.type = 0; % Rigid -payload.type = 0; % No Payload -#+end_src - -Then the transfer function from $\bm{u}$ to $\bm{\tau}_m$ is identified using the Simscape model. -#+begin_src matlab -%% Identify the transfer function from u to taum -clear io; io_i = 1; -io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Encoders -io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors - -G = linearize(mdl, io, 0.0, options); -G.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; -G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}; -#+end_src - -** Comparison with the model -#+begin_src matlab :exports none -%% Comparison of the plants (encoder output) when tuning the misalignment -freqs = 2*logspace(1, 3, 1000); - -i_input = 3; - -figure; -tiledlayout(2, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile(); -hold on; -plot(frf_ol.f, abs(frf_ol.G_dL(:, 1, i_input))); -plot(freqs, abs(squeeze(freqresp(G(1, i_input), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Amplitude [m/V]'); -title(sprintf('$d\\mathcal{L}_{m1}/u_{%i}$', i_input)); - -ax2 = nexttile(); -hold on; -plot(frf_ol.f, abs(frf_ol.G_dL(:, 2, i_input))); -plot(freqs, abs(squeeze(freqresp(G(2, i_input), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -title(sprintf('$d\\mathcal{L}_{m2}/u_{%i}$', i_input)); - -ax3 = nexttile(); -hold on; -plot(frf_ol.f, abs(frf_ol.G_dL(:, 3, i_input)), ... - 'DisplayName', 'Meas.'); -plot(freqs, abs(squeeze(freqresp(G(3, i_input), freqs, 'Hz'))), ... - 'DisplayName', 'Model'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); -legend('location', 'southeast', 'FontSize', 8); -title(sprintf('$d\\mathcal{L}_{m3}/u_{%i}$', i_input)); - -ax4 = nexttile(); -hold on; -plot(frf_ol.f, abs(frf_ol.G_dL(:, 4, i_input))); -plot(freqs, abs(squeeze(freqresp(G(4, i_input), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); -title(sprintf('$d\\mathcal{L}_{m4}/u_{%i}$', i_input)); - -ax5 = nexttile(); -hold on; -plot(frf_ol.f, abs(frf_ol.G_dL(:, 5, i_input))); -plot(freqs, abs(squeeze(freqresp(G(5, i_input), freqs, 'Hz')))); -hold off; -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -title(sprintf('$d\\mathcal{L}_{m5}/u_{%i}$', i_input)); - -ax6 = nexttile(); -hold on; -plot(frf_ol.f, abs(frf_ol.G_dL(:, 6, i_input))); -plot(freqs, abs(squeeze(freqresp(G(6, i_input), freqs, 'Hz')))); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); -title(sprintf('$d\\mathcal{L}_{m6}/u_{%i}$', i_input)); - -linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'xy'); -xlim([40, 4e2]); ylim([1e-8, 1e-2]); -#+end_src - * Bibliography :ignore: #+latex: \printbibliography[heading=bibintoc,title={Bibliography}]