Remove unused sections
This commit is contained in:
parent
b02c65b83a
commit
7084c7a368
@ -6643,336 +6643,6 @@ xlim([1, Fs/2]); ylim([1e-11, 1e-7]);
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
|
|
||||||
* Feedforward Control :noexport:
|
|
||||||
<<sec:test_nhexa_feedforward>>
|
|
||||||
|
|
||||||
** 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)
|
|
||||||
<<matlab-dir>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :exports none :results silent :noweb yes
|
|
||||||
<<matlab-init>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :tangle no :noweb yes
|
|
||||||
<<m-init-path>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :eval no :noweb yes
|
|
||||||
<<m-init-path-tangle>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :noweb yes
|
|
||||||
<<m-init-simscape>>
|
|
||||||
<<m-init-other>>
|
|
||||||
#+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)
|
|
||||||
<<matlab-dir>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :exports none :results silent :noweb yes
|
|
||||||
<<matlab-init>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :tangle no :noweb yes
|
|
||||||
<<m-init-path>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :eval no :noweb yes
|
|
||||||
<<m-init-path-tangle>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :noweb yes
|
|
||||||
<<m-init-simscape>>
|
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab :noweb yes
|
|
||||||
<<m-init-other>>
|
|
||||||
#+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:
|
* Bibliography :ignore:
|
||||||
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
|
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user