diff --git a/backup.org b/backup.org new file mode 100644 index 0000000..0fd66c7 --- /dev/null +++ b/backup.org @@ -0,0 +1,3866 @@ + +** Analysis backup of HAC - Decoupling analysis +<> + +*** Introduction :ignore: + +In this section is studied the HAC-IFF architecture for the Nano-Hexapod. +More precisely: +- The LAC control is a decentralized integral force feedback as studied in Section ref:sec:test_nhexa_enc_plates_iff +- The HAC control is a decentralized controller working in the frame of the struts + +The corresponding control architecture is shown in Figure ref:fig:test_nhexa_control_architecture_hac_iff_struts with: +- $\bm{r}_{\mathcal{X}_n}$: the $6 \times 1$ reference signal in the cartesian frame +- $\bm{r}_{d\mathcal{L}}$: the $6 \times 1$ reference signal transformed in the frame of the struts thanks to the inverse kinematic +- $\bm{\epsilon}_{d\mathcal{L}}$: the $6 \times 1$ length error of the 6 struts +- $\bm{u}^\prime$: input of the damped plant +- $\bm{u}$: generated DAC voltages +- $\bm{\tau}_m$: measured force sensors +- $d\bm{\mathcal{L}}_m$: measured displacement of the struts by the encoders + +#+begin_src latex :file control_architecture_hac_iff_struts.pdf +\definecolor{instrumentation}{rgb}{0, 0.447, 0.741} +\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098} +\definecolor{control}{rgb}{0.4660, 0.6740, 0.1880} + +\begin{tikzpicture} + % Blocs + \node[block={3.0cm}{2.0cm}, fill=black!20!white] (P) {Plant}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputF) at ($(P.south east)!0.2!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$); + + \node[block, below=0.4 of P, fill=control!20!white] (Kiff) {$\bm{K}_\text{IFF}$}; + \node[block, left=0.8 of inputF, fill=instrumentation!20!white] (pd200) {\tiny PD200}; + \node[addb, left=0.8 of pd200, fill=control!20!white] (addF) {}; + \node[block, left=0.8 of addF, fill=control!20!white] (K) {$\bm{K}_\mathcal{L}$}; + \node[addb={+}{}{-}{}{}, left=0.8 of K, fill=control!20!white] (subr) {}; + \node[block, align=center, left= of subr, fill=control!20!white] (J) {\tiny Inverse\\\tiny Kinematics}; + + % Connections and labels + \draw[->] (outputF) -- ++(1.0, 0) node[above left]{$\bm{\tau}_m$}; + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.south); + \draw[->] (addF.east) -- (pd200.west) node[above left]{$\bm{u}$}; + \draw[->] (pd200.east) -- (inputF) node[above left]{$\bm{u}_a$}; + + \draw[->] (outputL) -- ++(1.0, 0) node[below left]{$d\bm{\mathcal{L}_m}$}; + \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, 1) -| (subr.north); + \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[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; + \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_struts +#+caption: HAC-LAC: IFF + Control in the frame of the legs +#+RESULTS: +[[file:figs/test_nhexa_control_architecture_hac_iff_struts.png]] + +This part is structured as follow: +- Section ref:sec:test_nhexa_hac_iff_struts_ref_track: some reference tracking tests are performed +- Section ref:sec:test_nhexa_hac_iff_struts_controller: the decentralized high authority controller is tuned using the Simscape model and is implemented and tested experimentally +- Section ref:sec:test_nhexa_interaction_analysis: an interaction analysis is performed, from which the best decoupling strategy can be determined +- Section ref:sec:test_nhexa_robust_hac_design: Robust High Authority Controller are designed + +*** Reference Tracking - Trajectories +:PROPERTIES: +:header-args:matlab+: :tangle matlab/scripts/reference_tracking_paths.m +:END: +<> +**** Introduction :ignore: +In this section, several trajectories representing the wanted pose (position and orientation) of the top platform with respect to the bottom platform are defined. + +These trajectories will be used to test the HAC-LAC architecture. + +In order to transform the wanted pose to the wanted displacement of the 6 struts, the inverse kinematic is required. +As a first approximation, the Jacobian matrix $\bm{J}$ can be used instead of using the full inverse kinematic equations. + +Therefore, the control architecture with the input trajectory $\bm{r}_{\mathcal{X}_n}$ is shown in Figure ref:fig:test_nhexa_control_architecture_hac_iff_L. + +#+begin_src latex :file control_architecture_hac_iff_struts_L.pdf +\definecolor{instrumentation}{rgb}{0, 0.447, 0.741} +\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098} +\definecolor{control}{rgb}{0.4660, 0.6740, 0.1880} + +\begin{tikzpicture} + % Blocs + \node[block={3.0cm}{2.0cm}, fill=black!20!white] (P) {Plant}; + \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); + \coordinate[] (outputF) at ($(P.south east)!0.2!(P.north east)$); + \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$); + + \node[block, below=0.4 of P, fill=control!20!white] (Kiff) {$\bm{K}_\text{IFF}$}; + \node[block, left=0.8 of inputF, fill=instrumentation!20!white] (pd200) {\tiny PD200}; + \node[addb, left=0.8 of pd200, fill=control!20!white] (addF) {}; + \node[block, left=0.8 of addF, fill=control!20!white] (K) {$\bm{K}_\mathcal{L}$}; + \node[addb={+}{}{-}{}{}, left=0.8 of K, fill=control!20!white] (subr) {}; + \node[block, align=center, left= of subr, fill=control!20!white] (J) {$\bm{J}$}; + + % Connections and labels + \draw[->] (outputF) -- ++(1.0, 0) node[above left]{$\bm{\tau}_m$}; + \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); + \draw[->] (Kiff.west) -| (addF.south); + \draw[->] (addF.east) -- (pd200.west) node[above left]{$\bm{u}$}; + \draw[->] (pd200.east) -- (inputF) node[above left]{$\bm{u}_a$}; + + \draw[->] (outputL) -- ++(1.0, 0) node[below left]{$d\bm{\mathcal{L}_m}$}; + \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, 1) -| (subr.north); + \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[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; + \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 +#+caption: HAC-LAC: IFF + Control in the frame of the legs +#+RESULTS: +[[file:figs/test_nhexa_control_architecture_hac_iff_struts_L.png]] + +In the following sections, several reference trajectories are defined: +- Section ref:sec:test_nhexa_yz_scans: simple scans in the Y-Z plane +- Section ref:sec:test_nhexa_tilt_scans: scans in tilt are performed +- Section ref:sec:test_nhexa_nass_scans: scans with X-Y-Z translations in order to draw the word "NASS" + +**** Matlab Init :noexport:ignore: +#+begin_src matlab +%% reference_tracking_paths.m +% Computation of several reference paths +#+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 + +**** Y-Z Scans +<> +A function =generateYZScanTrajectory= has been developed in order to easily generate scans in the Y-Z plane. + +For instance, the following generated trajectory is represented in Figure ref:fig:test_nhexa_yz_scan_example_trajectory_yz_plane. +#+begin_src matlab +%% Generate the Y-Z trajectory scan +Rx_yz = generateYZScanTrajectory(... + 'y_tot', 4e-6, ... % Length of Y scans [m] + 'z_tot', 4e-6, ... % Total Z distance [m] + 'n', 5, ... % Number of Y scans + 'Ts', 1e-3, ... % Sampling Time [s] + 'ti', 1, ... % Time to go to initial position [s] + 'tw', 0, ... % Waiting time between each points [s] + 'ty', 0.6, ... % Time for a scan in Y [s] + 'tz', 0.2); % Time for a scan in Z [s] +#+end_src + +#+begin_src matlab :exports none +%% Plot the trajectory in the Y-Z plane +figure; +plot(Rx_yz(:,3), Rx_yz(:,4)); +xlabel('y [m]'); ylabel('z [m]'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/yz_scan_example_trajectory_yz_plane.pdf', 'width', 'normal', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_yz_scan_example_trajectory_yz_plane +#+caption: Generated scan in the Y-Z plane +#+RESULTS: +[[file:figs/test_nhexa_yz_scan_example_trajectory_yz_plane.png]] + +The Y and Z positions as a function of time are shown in Figure ref:fig:test_nhexa_yz_scan_example_trajectory. + +#+begin_src matlab :exports none +%% Plot the Y-Z trajectory as a function of time +figure; +hold on; +plot(Rx_yz(:,1), Rx_yz(:,3), ... + 'DisplayName', 'Y motion') +plot(Rx_yz(:,1), Rx_yz(:,4), ... + 'DisplayName', 'Z motion') +hold off; +xlabel('Time [s]'); +ylabel('Displacement [m]'); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/yz_scan_example_trajectory.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_yz_scan_example_trajectory +#+caption: Y and Z trajectories as a function of time +#+RESULTS: +[[file:figs/test_nhexa_yz_scan_example_trajectory.png]] + +Using the Jacobian matrix, it is possible to compute the wanted struts lengths as a function of time: +\begin{equation} + \bm{r}_{d\mathcal{L}} = \bm{J} \bm{r}_{\mathcal{X}_n} +\end{equation} + +#+begin_src matlab :exports none +load('jacobian.mat', 'J'); +#+end_src + +#+begin_src matlab +%% Compute the reference in the frame of the legs +dL_ref = [J*Rx_yz(:, 2:7)']'; +#+end_src + +The reference signal for the strut length is shown in Figure ref:fig:test_nhexa_yz_scan_example_trajectory_struts. +#+begin_src matlab :exports none +%% Plot the reference in the frame of the legs +figure; +hold on; +for i=1:6 + plot(Rx_yz(:,1), dL_ref(:, i), ... + 'DisplayName', sprintf('$r_{d\\mathcal{L}_%i}$', i)) +end +xlabel('Time [s]'); ylabel('Strut Motion [m]'); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); +yticks(1e-6*[-5:5]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/yz_scan_example_trajectory_struts.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_yz_scan_example_trajectory_struts +#+caption: Trajectories for the 6 individual struts +#+RESULTS: +[[file:figs/test_nhexa_yz_scan_example_trajectory_struts.png]] + +**** Tilt Scans +<> + +A function =generalSpiralAngleTrajectory= has been developed in order to easily generate $R_x,R_y$ tilt scans. + +For instance, the following generated trajectory is represented in Figure ref:fig:test_nhexa_tilt_scan_example_trajectory. +#+begin_src matlab +%% Generate the "tilt-spiral" trajectory scan +R_tilt = generateSpiralAngleTrajectory(... + 'R_tot', 20e-6, ... % Total Tilt [ad] + 'n_turn', 5, ... % Number of scans + 'Ts', 1e-3, ... % Sampling Time [s] + 't_turn', 1, ... % Turn time [s] + 't_end', 1); % End time to go back to zero [s] +#+end_src + +#+begin_src matlab :exports none +%% Plot the trajectory +figure; +plot(1e6*R_tilt(:,5), 1e6*R_tilt(:,6)); +xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/tilt_scan_example_trajectory.pdf', 'width', 'normal', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_tilt_scan_example_trajectory +#+caption: Generated "spiral" scan +#+RESULTS: +[[file:figs/test_nhexa_tilt_scan_example_trajectory.png]] + +#+begin_src matlab :exports none +%% Compute the reference in the frame of the legs +load('jacobian.mat', 'J'); +dL_ref = [J*R_tilt(:, 2:7)']'; +#+end_src + +The reference signal for the strut length is shown in Figure ref:fig:test_nhexa_tilt_scan_example_trajectory_struts. +#+begin_src matlab :exports none +%% Plot the reference in the frame of the legs +figure; +hold on; +for i=1:6 + plot(R_tilt(:,1), dL_ref(:, i), ... + 'DisplayName', sprintf('$r_{d\\mathcal{L}_%i}$', i)) +end +xlabel('Time [s]'); ylabel('Strut Motion [m]'); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +yticks(1e-6*[-5:5]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/tilt_scan_example_trajectory_struts.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_tilt_scan_example_trajectory_struts +#+caption: Trajectories for the 6 individual struts - Tilt scan +#+RESULTS: +[[file:figs/test_nhexa_tilt_scan_example_trajectory_struts.png]] + +**** "NASS" reference path +<> +In this section, a reference path that "draws" the work "NASS" is developed. + +First, a series of points representing each letter are defined. +Between each letter, a negative Z motion is performed. +#+begin_src matlab +%% List of points that draws "NASS" +ref_path = [ ... + 0, 0,0; % Initial Position + 0,0,1; 0,4,1; 3,0,1; 3,4,1; % N + 3,4,0; 4,0,0; % Transition + 4,0,1; 4,3,1; 5,4,1; 6,4,1; 7,3,1; 7,2,1; 4,2,1; 4,3,1; 5,4,1; 6,4,1; 7,3,1; 7,0,1; % A + 7,0,0; 8,0,0; % Transition + 8,0,1; 11,0,1; 11,2,1; 8,2,1; 8,4,1; 11,4,1; % S + 11,4,0; 12,0,0; % Transition + 12,0,1; 15,0,1; 15,2,1; 12,2,1; 12,4,1; 15,4,1; % S + 15,4,0; + ]; + +%% Center the trajectory arround zero +ref_path = ref_path - (max(ref_path) - min(ref_path))/2; + +%% Define the X-Y-Z cuboid dimensions containing the trajectory +X_max = 10e-6; +Y_max = 4e-6; +Z_max = 2e-6; + +ref_path = ([X_max, Y_max, Z_max]./max(ref_path)).*ref_path; % [m] +#+end_src + +Then, using the =generateXYZTrajectory= function, the $6 \times 1$ trajectory signal is computed. +#+begin_src matlab +%% Generating the trajectory +Rx_nass = generateXYZTrajectory('points', ref_path); +#+end_src + +The trajectory in the X-Y plane is shown in Figure ref:fig:test_nhexa_ref_track_test_nass (the transitions between the letters are removed). +#+begin_src matlab :exports none +%% "NASS" trajectory in the X-Y plane +figure; +plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k.') +xlabel('X [$\mu m$]'); +ylabel('Y [$\mu m$]'); +axis equal; +xlim(1e6*[min(Rx_nass(:,2)), max(Rx_nass(:,2))]); +ylim(1e6*[min(Rx_nass(:,3)), max(Rx_nass(:,3))]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_test_nass.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_ref_track_test_nass +#+caption: Reference path corresponding to the "NASS" acronym +#+RESULTS: +[[file:figs/test_nhexa_ref_track_test_nass.png]] + +It can also be better viewed in a 3D representation as in Figure ref:fig:test_nhexa_ref_track_test_nass_3d. + +#+begin_src matlab :exports none +figure; +plot3(1e6*Rx_nass(:,2), 1e6*Rx_nass(:,3), 1e6*Rx_nass(:,4), 'k-'); +xlabel('x [$\mu m$]'); ylabel('y [$\mu m$]'); zlabel('z [$\mu m$]'); +view(-13, 41) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_test_nass_3d.pdf', 'width', 'normal', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_ref_track_test_nass_3d +#+caption: Reference path that draws "NASS" - 3D view +#+RESULTS: +[[file:figs/test_nhexa_ref_track_test_nass_3d.png]] + +*** First Basic High Authority Controller +:PROPERTIES: +:header-args:matlab+: :tangle matlab/scripts/hac_lac_first_try.m +:END: +<> +**** Introduction :ignore: +In this section, a simple decentralized high authority controller $\bm{K}_{\mathcal{L}}$ is developed to work without any payload. + +The diagonal controller is tuned using classical Loop Shaping in Section ref:sec:test_nhexa_hac_iff_no_payload_tuning. +The stability is verified in Section ref:sec:test_nhexa_hac_iff_no_payload_stability using the Simscape model. + +**** Matlab Init :noexport:ignore: +#+begin_src matlab +%% hac_lac_first_try.m +% Development and analysis of a first basic High Authority Controller +#+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 +%% Load the identified FRF and Simscape model +frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); +sim_iff = load('sim_iff_vib_table_m.mat', 'G_de'); +#+end_src + +**** HAC Controller +<> + +Let's first try to design a first decentralized controller with: +- a bandwidth of 100Hz +- sufficient phase margin +- simple and understandable components + +After some very basic and manual loop shaping, A diagonal controller is developed. +Each diagonal terms are identical and are composed of: +- A lead around 100Hz +- A first order low pass filter starting at 200Hz to add some robustness to high frequency modes +- A notch at 700Hz to cancel the flexible modes of the top plate +- A pure integrator + +#+begin_src matlab +%% Lead to increase phase margin +a = 2; % Amount of phase lead / width of the phase lead / high frequency gain +wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s] + +H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); + +%% Low Pass filter to increase robustness +H_lpf = 1/(1 + s/2/pi/200); + +%% Notch at the top-plate resonance +gm = 0.02; +xi = 0.3; +wn = 2*pi*700; + +H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); + +%% Decentralized HAC +Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain + H_lead * ... % Lead + H_notch * ... % Notch + (2*pi*100/s) * ... % Integrator + eye(6); % 6x6 Diagonal +#+end_src + +This controller is saved for further use. +#+begin_src matlab :exports none :tangle no +save('matlab/data_sim/Khac_iff_struts.mat', 'Khac_iff_struts') +#+end_src + +#+begin_src matlab :eval no +save('data_sim/Khac_iff_struts.mat', 'Khac_iff_struts') +#+end_src + +The experimental loop gain is computed and shown in Figure ref:fig:test_nhexa_loop_gain_hac_iff_struts. +#+begin_src matlab +L_hac_iff_struts = pagemtimes(permute(frf_iff.G_de{1}, [2 3 1]), squeeze(freqresp(Khac_iff_struts, frf_iff.f, 'Hz'))); +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the Loop Gain +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + + +ax1 = nexttile([2,1]); +hold on; +% Diagonal Elements Model +plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(1,1,:))), 'color', colors(1,:), ... + 'DisplayName', 'Diagonal'); +for i = 2:6 + plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(i,i,:))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(1,2,:))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', 'Off-Diag'); +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(i,j,:))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e2]); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(squeeze(L_hac_iff_struts(i,i,:))), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([2, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/loop_gain_hac_iff_struts.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_loop_gain_hac_iff_struts +#+caption: Diagonal and off-diagonal elements of the Loop gain for "HAC-IFF-Struts" +#+RESULTS: +[[file:figs/test_nhexa_loop_gain_hac_iff_struts.png]] + +**** Verification of the Stability using the Simscape model +<> + +The HAC-IFF control strategy is implemented using Simscape. +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'controller_type', 'hac-iff-struts'); +#+end_src + +#+begin_src matlab :exports none +support.type = 1; % On top of vibration table +payload.type = 3; % Payload / 1 "mass layer" + +load('Kiff_opt.mat', 'Kiff'); +#+end_src + +#+begin_src matlab +%% Identify the (damped) transfer function from u to dLm +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; % Plate Displacement (encoder) +#+end_src + +We identify the closed-loop system. +#+begin_src matlab +%% Identification +Gd_iff_hac_opt = linearize(mdl, io, 0.0, options); +#+end_src + +And verify that it is indeed stable. +#+begin_src matlab :results value replace :exports both +%% Verify the stability +isstable(Gd_iff_hac_opt) +#+end_src + +#+RESULTS: +: 1 + +**** Experimental Validation +Both the Integral Force Feedback controller (developed in Section ref:sec:test_nhexa_enc_plates_iff) and the high authority controller working in the frame of the struts (developed in Section ref:sec:test_nhexa_hac_iff_struts_controller) are implemented experimentally. + +Two reference tracking experiments are performed to evaluate the stability and performances of the implemented control. + +#+begin_src matlab +%% Load the experimental data +load('hac_iff_struts_yz_scans.mat', 't', 'de') +#+end_src + +#+begin_src matlab :exports none +%% Reset initial time +t = t - t(1); +#+end_src + +The position of the top-platform is estimated using the Jacobian matrix: +#+begin_src matlab +%% Pose of the top platform from the encoder values +load('jacobian.mat', 'J'); +Xe = [inv(J)*de']'; +#+end_src + +#+begin_src matlab +%% Generate the Y-Z trajectory scan +Rx_yz = generateYZScanTrajectory(... + 'y_tot', 4e-6, ... % Length of Y scans [m] + 'z_tot', 8e-6, ... % Total Z distance [m] + 'n', 5, ... % Number of Y scans + 'Ts', 1e-3, ... % Sampling Time [s] + 'ti', 1, ... % Time to go to initial position [s] + 'tw', 0, ... % Waiting time between each points [s] + 'ty', 0.6, ... % Time for a scan in Y [s] + 'tz', 0.2); % Time for a scan in Z [s] +#+end_src + +The reference path as well as the measured position are partially shown in the Y-Z plane in Figure ref:fig:test_nhexa_yz_scans_exp_results_first_K. +#+begin_src matlab :exports none +%% Position and reference signal in the Y-Z plane +figure; +tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile; +hold on; +plot(1e6*Xe(t>2,2), 1e6*Xe(t>2,3)); +plot(1e6*Rx_yz(:,3), 1e6*Rx_yz(:,4), '--'); +hold off; +xlabel('Y [$\mu m$]'); ylabel('Z [$\mu m$]'); +xlim([-2.05, 2.05]); ylim([-4.1, 4.1]); +axis equal; + +ax2 = nexttile([1,2]); +hold on; +plot(1e6*Xe(:,2), 1e6*Xe(:,3), ... + 'DisplayName', '$\mathcal{X}_n$'); +plot(1e6*Rx_yz(:,3), 1e6*Rx_yz(:,4), '--', ... + 'DisplayName', '$r_{\mathcal{X}_n}$'); +hold off; +legend('location', 'northwest'); +xlabel('Y [$\mu m$]'); ylabel('Z [$\mu m$]'); +axis equal; +xlim([1.6, 2.1]); ylim([-4.1, -3.6]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/yz_scans_exp_results_first_K.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_yz_scans_exp_results_first_K +#+caption: Measured position $\bm{\mathcal{X}}_n$ and reference signal $\bm{r}_{\mathcal{X}_n}$ in the Y-Z plane - Zoom on a change of direction +#+RESULTS: +[[file:figs/test_nhexa_yz_scans_exp_results_first_K.png]] + +#+begin_important +It is clear from Figure ref:fig:test_nhexa_yz_scans_exp_results_first_K that the position of the nano-hexapod effectively tracks to reference signal. +However, oscillations with amplitudes as large as 50nm can be observe. + +It turns out that the frequency of these oscillations is 100Hz which is corresponding to the crossover frequency of the High Authority Control loop. +This clearly indicates poor stability margins. +In the next section, the controller is re-designed to improve the stability margins. +#+end_important + +**** Controller with increased stability margins +The High Authority Controller is re-designed in order to improve the stability margins. +#+begin_src matlab +%% Lead +a = 5; % Amount of phase lead / width of the phase lead / high frequency gain +wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s] + +H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); + +%% Low Pass Filter +H_lpf = 1/(1 + s/2/pi/300); + +%% Notch +gm = 0.02; +xi = 0.5; +wn = 2*pi*700; + +H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); + +%% HAC Controller +Khac_iff_struts = -2.2e4 * ... % Gain + H_lead * ... % Lead + H_lpf * ... % Lead + H_notch * ... % Notch + (2*pi*100/s) * ... % Integrator + eye(6); % 6x6 Diagonal +#+end_src + +#+begin_src matlab :exports none +%% Load the FRF of the transfer function from u to dL with IFF +frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); +#+end_src + +#+begin_src matlab :exports none +%% Compute the Loop Gain +L_frf = pagemtimes(permute(frf_iff.G_de{1}, [2 3 1]), squeeze(freqresp(Khac_iff_struts, frf_iff.f, 'Hz'))); +#+end_src + +The bode plot of the new loop gain is shown in Figure ref:fig:test_nhexa_hac_iff_plates_exp_loop_gain_redesigned_K. +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = 2*logspace(1, 3, 1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Diagonal Elements FRF +plot(frf_iff.f, abs(squeeze(L_frf(1,1,:))), 'color', colors(1,:), ... + 'DisplayName', 'Diagonal'); +for i = 2:6 + plot(frf_iff.f, abs(squeeze(L_frf(i,i,:))), 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end +plot(frf_iff.f, abs(squeeze(L_frf(1,2,:))), 'color', [colors(2,:), 0.2], ... + 'DisplayName', 'Off-Diag'); +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(squeeze(L_frf(i,j,:))), 'color', [colors(2,:), 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e2]); +legend('location', 'northeast'); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(squeeze(L_frf(i,i,:))), 'color', colors(1,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([1, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/hac_iff_plates_exp_loop_gain_redesigned_K.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_hac_iff_plates_exp_loop_gain_redesigned_K +#+caption: Loop Gain for the updated decentralized HAC controller +#+RESULTS: +[[file:figs/test_nhexa_hac_iff_plates_exp_loop_gain_redesigned_K.png]] + +This new controller is implemented experimentally and several tracking tests are performed. +#+begin_src matlab +%% Load Measurements +load('hac_iff_more_lead_nass_scan.mat', 't', 'de') +#+end_src + +#+begin_src matlab :exports none +%% Reset Time +t = t - t(1); +#+end_src + +The pose of the top platform is estimated from the encoder position using the Jacobian matrix. +#+begin_src matlab +%% Compute the pose of the top platform +load('jacobian.mat', 'J'); +Xe = [inv(J)*de']'; +#+end_src + +#+begin_src matlab :exports none +%% Load the reference path +load('reference_path.mat', 'Rx_nass') +#+end_src + +The measured motion as well as the trajectory are shown in Figure ref:fig:test_nhexa_nass_scans_first_test_exp. +#+begin_src matlab :exports none +%% Plot the X-Y-Z "NASS" trajectory +figure; +hold on; +plot3(Xe(1:100:end,1), Xe(1:100:end,2), Xe(1:100:end,3)) +plot3(Rx_nass(1:100:end,2), Rx_nass(1:100:end,3), Rx_nass(1:100:end,4)) +hold off; +xlabel('x [$\mu m$]'); ylabel('y [$\mu m$]'); zlabel('z [$\mu m$]'); +view(-13, 41) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/nass_scans_first_test_exp.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_nass_scans_first_test_exp +#+caption: Measured position $\bm{\mathcal{X}}_n$ and reference signal $\bm{r}_{\mathcal{X}_n}$ for the "NASS" trajectory +#+RESULTS: +[[file:figs/test_nhexa_nass_scans_first_test_exp.png]] + +The trajectory and measured motion are also shown in the X-Y plane in Figure ref:fig:test_nhexa_ref_track_nass_exp_hac_iff_struts. +#+begin_src matlab :exports none +%% Estimate when the hexpod is on top position and drawing the letters +i_top = Xe(:,3) > 1.9e-6; +i_rx = Rx_nass(:,4) > 0; +#+end_src + +#+begin_src matlab :exports none +%% Plot the reference as well as the measurement in the X-Y plane +figure; +tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([1,2]); +hold on; +scatter(1e6*Xe(i_top,1), 1e6*Xe(i_top,2),'.'); +plot(1e6*Rx_nass(i_rx,2), 1e6*Rx_nass(i_rx,3), '--'); +hold off; +xlabel('X [$\mu m$]'); ylabel('Y [$\mu m$]'); +axis equal; +xlim([-10.5, 10.5]); ylim([-4.5, 4.5]); + +ax2 = nexttile; +hold on; +scatter(1e6*Xe(i_top,1), 1e6*Xe(i_top,2),'.'); +plot(1e6*Rx_nass(i_rx,2), 1e6*Rx_nass(i_rx,3), '--'); +hold off; +xlabel('X [$\mu m$]'); ylabel('Y [$\mu m$]'); +axis equal; +xlim([4.5, 4.7]); ylim([-0.15, 0.05]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_nass_exp_hac_iff_struts.pdf', 'width', 'full', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_ref_track_nass_exp_hac_iff_struts +#+caption: Reference path and measured motion in the X-Y plane +#+RESULTS: +[[file:figs/test_nhexa_ref_track_nass_exp_hac_iff_struts.png]] + +The orientation errors during all the scans are shown in Figure ref:fig:test_nhexa_nass_ref_rx_ry. +#+begin_src matlab :exports none +%% Orientation Errors +figure; +hold on; +plot(t(t>20&t<20.1), 1e6*Xe(t>20&t<20.1,4), '-', 'DisplayName', '$\epsilon_{\theta_x}$'); +plot(t(t>20&t<20.1), 1e6*Xe(t>20&t<20.1,5), '-', 'DisplayName', '$\epsilon_{\theta_y}$'); +plot(t(t>20&t<20.1), 1e6*Xe(t>20&t<20.1,6), '-', 'DisplayName', '$\epsilon_{\theta_z}$'); +hold off; +xlabel('Time [s]'); ylabel('Orientation Error [$\mu$ rad]'); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :exports none +%% Orientation Errors +figure; +hold on; +plot(1e9*Xe(100000:100:end,4), 1e9*Xe(100000:100:end,5), '.'); +th = 0:pi/50:2*pi; +xunit = 90 * cos(th); +yunit = 90 * sin(th); +plot(xunit, yunit, '--'); +hold off; +xlabel('$R_x$ [nrad]'); ylabel('$R_y$ [nrad]'); +xlim([-100, 100]); +ylim([-100, 100]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/nass_ref_rx_ry.pdf', 'width', 500, 'height', 500); +#+end_src + +#+name: fig:test_nhexa_nass_ref_rx_ry +#+caption: Orientation errors during the scan +#+RESULTS: +[[file:figs/test_nhexa_nass_ref_rx_ry.png]] + +#+begin_important +Using the updated High Authority Controller, the nano-hexapod can follow trajectories with high accuracy (the position errors are in the order of 50nm peak to peak, and the orientation errors 300nrad peak to peak). +#+end_important + +*** Interaction Analysis and Decoupling +:PROPERTIES: +:header-args:matlab+: :tangle matlab/scripts/interaction_analysis_enc_plates.m +:END: +<> +**** Introduction :ignore: + +In this section, the interaction in the identified plant is estimated using the Relative Gain Array (RGA) [[cite:skogestad07_multiv_feedb_contr][Chap. 3.4]]. + +Then, several decoupling strategies are compared for the nano-hexapod. + +The RGA Matrix is defined as follow: +\begin{equation} + \text{RGA}(G(f)) = G(f) \times (G(f)^{-1})^T +\end{equation} + +Then, the RGA number is defined: +\begin{equation} +\text{RGA-num}(f) = \| \text{I - RGA(G(f))} \|_{\text{sum}} +\end{equation} + + +In this section, the plant with 2 added mass is studied. + +**** Matlab Init :noexport:ignore: +#+begin_src matlab +%% interaction_analysis_enc_plates.m +% Interaction analysis of several decoupling strategies +#+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 +%% Load the identified FRF and Simscape model +frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); +sim_iff = load('sim_iff_vib_table_m.mat', 'G_de'); +#+end_src + +**** Parameters +#+begin_src matlab +wc = 100; % Wanted crossover frequency [Hz] +[~, i_wc] = min(abs(frf_iff.f - wc)); % Indice corresponding to wc +#+end_src + +#+begin_src matlab +%% Plant to be decoupled +frf_coupled = frf_iff.G_de{2}; +G_coupled = sim_iff.G_de{2}; +#+end_src + +**** No Decoupling (Decentralized) +<> + +#+begin_src latex :file decoupling_arch_decentralized.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + + % Connections and labels + \draw[<-] (G.west) -- ++(-1.8, 0) node[above right]{$\bm{\tau}$}; + \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; + + \begin{scope}[on background layer] + \node[fit={(G.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gdec) {}; + \node[below right] at (Gdec.north west) {$\bm{G}_{\text{dec}}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_decentralized +#+caption: Block diagram representing the plant. +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_decentralized.png]] + +#+begin_src matlab :exports none +%% Decentralized Plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(frf_coupled(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +for i = 1:6 + plot(frf_iff.f, abs(frf_coupled(:,i,i)), ... + 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); +end +plot(frf_iff.f, abs(frf_coupled(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(frf_coupled(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_decentralized_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_decentralized_plant +#+caption: Bode Plot of the decentralized plant (diagonal and off-diagonal terms) +#+RESULTS: +[[file:figs/test_nhexa_interaction_decentralized_plant.png]] + +#+begin_src matlab :exports none +%% Decentralized RGA +RGA_dec = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_dec(i,:,:) = squeeze(frf_coupled(i,:,:)).*inv(squeeze(frf_coupled(i,:,:))).'; +end + +RGA_dec_sum = zeros(length(frf_iff), 1); +for i = 1:length(frf_iff.f) + RGA_dec_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_dec(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% RGA for Decentralized plant +figure; +plot(frf_iff.f, RGA_dec_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_decentralized.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_decentralized +#+caption: RGA number for the decentralized plant +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_decentralized.png]] + +**** Static Decoupling +<> + +#+begin_src latex :file decoupling_arch_static.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + \node[block, left=0.8 of G] (Ginv) {$\bm{\hat{G}}(j0)^{-1}$}; + + % Connections and labels + \draw[<-] (Ginv.west) -- ++(-1.8, 0) node[above right]{$\bm{u}$}; + \draw[->] (Ginv.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; + + \begin{scope}[on background layer] + \node[fit={(Ginv.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; + \node[below right] at (Gx.north west) {$\bm{G}_{\text{static}}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_static +#+caption: Decoupling using the inverse of the DC gain of the plant +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_static.png]] + +The DC gain is evaluated from the model as be have bad low frequency identification. + +#+begin_src matlab :exports none +%% Compute the inverse of the DC gain +G_model = G_coupled; +G_model.outputdelay = 0; % necessary for further inversion +dc_inv = inv(dcgain(G_model)); + +%% Compute the inversed plant +G_de_sta = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + G_de_sta(i,:,:) = squeeze(frf_coupled(i,:,:))*dc_inv; +end +#+end_src + +#+begin_src matlab :exports results :results value table replace :tangle no +data2orgtable(dc_inv, {}, {}, ' %.1f '); +#+end_src + +#+RESULTS: +| -62011.5 | 3910.6 | 4299.3 | 660.7 | -4016.5 | -4373.6 | +| 3914.4 | -61991.2 | -4356.8 | -4019.2 | 640.2 | 4281.6 | +| -4020.0 | -4370.5 | -62004.5 | 3914.6 | 4295.8 | 653.8 | +| 660.9 | 4292.4 | 3903.3 | -62012.2 | -4366.5 | -4008.9 | +| 4302.8 | 655.6 | -4025.8 | -4377.8 | -62006.0 | 3919.7 | +| -4377.9 | -4013.2 | 668.6 | 4303.7 | 3906.8 | -62019.3 | + +#+begin_src matlab :exports none +%% Bode plot of the static decoupled plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_sta(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +for i = 1:6 + plot(frf_iff.f, abs(G_de_sta(:,i,i)), ... + 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); +end +plot(frf_iff.f, abs(G_de_sta(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e1]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_sta(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_static_dec_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_static_dec_plant +#+caption: Bode Plot of the static decoupled plant +#+RESULTS: +[[file:figs/test_nhexa_interaction_static_dec_plant.png]] + +#+begin_src matlab :exports none +%% Compute RGA Matrix +RGA_sta = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_sta(i,:,:) = squeeze(G_de_sta(i,:,:)).*inv(squeeze(G_de_sta(i,:,:))).'; +end + +%% Compute RGA-number +RGA_sta_sum = zeros(length(frf_iff), 1); +for i = 1:size(RGA_sta, 1) + RGA_sta_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_sta(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the RGA-number for statically decoupled plant +figure; +plot(frf_iff.f, RGA_sta_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_static_dec.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_static_dec +#+caption: RGA number for the statically decoupled plant +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_static_dec.png]] + +**** Decoupling at the Crossover +<> + +#+begin_src latex :file decoupling_arch_crossover.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + \node[block, left=0.8 of G] (Ginv) {$\bm{\hat{G}}(j\omega_c)^{-1}$}; + + % Connections and labels + \draw[<-] (Ginv.west) -- ++(-1.8, 0) node[above right]{$\bm{u}$}; + \draw[->] (Ginv.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; + + \begin{scope}[on background layer] + \node[fit={(Ginv.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; + \node[below right] at (Gx.north west) {$\bm{G}_{\omega_c}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_crossover +#+caption: Decoupling using the inverse of a dynamical model $\bm{\hat{G}}$ of the plant dynamics $\bm{G}$ +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_crossover.png]] + +#+begin_src matlab :exports none +%% Take complex matrix corresponding to the plant at 100Hz +V = squeeze(frf_coupled(i_wc,:,:)); + +%% Real approximation of inv(G(100Hz)) +D = pinv(real(V'*V)); +H1 = D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2))); + +%% Compute the decoupled plant +G_de_wc = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + G_de_wc(i,:,:) = squeeze(frf_coupled(i,:,:))*H1; +end +#+end_src + +#+begin_src matlab :exports results :results value table replace :tangle no +data2orgtable(H1, {}, {}, ' %.1f '); +#+end_src + +#+RESULTS: +| 67229.8 | 3769.3 | -13704.6 | -23084.8 | -6318.2 | 23378.7 | +| 3486.2 | 67708.9 | 23220.0 | -6314.5 | -22699.8 | -14060.6 | +| -5731.7 | 22471.7 | 66701.4 | 3070.2 | -13205.6 | -21944.6 | +| -23305.5 | -14542.6 | 2743.2 | 70097.6 | 24846.8 | -5295.0 | +| -14882.9 | -22957.8 | -5344.4 | 25786.2 | 70484.6 | 2979.9 | +| 24353.3 | -5195.2 | -22449.0 | -14459.2 | 2203.6 | 69484.2 | + +#+begin_src matlab :exports none +%% Bode plot of the plant decoupled at the crossover +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_wc(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +for i = 1:6 + plot(frf_iff.f, abs(G_de_wc(:,i,i)), ... + 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); +end +plot(frf_iff.f, abs(G_de_wc(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e1]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_wc(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_wc_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_wc_plant +#+caption: Bode Plot of the plant decoupled at the crossover +#+RESULTS: +[[file:figs/test_nhexa_interaction_wc_plant.png]] + +#+begin_src matlab +%% Compute RGA Matrix +RGA_wc = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_wc(i,:,:) = squeeze(G_de_wc(i,:,:)).*inv(squeeze(G_de_wc(i,:,:))).'; +end + +%% Compute RGA-number +RGA_wc_sum = zeros(size(RGA_wc, 1), 1); +for i = 1:size(RGA_wc, 1) + RGA_wc_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_wc(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the RGA-Number for the plant decoupled at crossover +figure; +plot(frf_iff.f, RGA_wc_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_wc.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_wc +#+caption: RGA number for the plant decoupled at the crossover +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_wc.png]] + +**** SVD Decoupling +<> + +#+begin_src latex :file decoupling_arch_svd.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + + \node[block, left=0.8 of G.west] (V) {$V^{-T}$}; + \node[block, right=0.8 of G.east] (U) {$U^{-1}$}; + + % Connections and labels + \draw[<-] (V.west) -- ++(-1.0, 0) node[above right]{$u$}; + \draw[->] (V.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- (U.west) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] (U.east) -- ++( 1.0, 0) node[above left]{$y$}; + + \begin{scope}[on background layer] + \node[fit={(V.south west) (G.north-|U.east)}, fill=black!10!white, draw, dashed, inner sep=14pt] (Gsvd) {}; + \node[below right] at (Gsvd.north west) {$\bm{G}_{SVD}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_svd +#+caption: Decoupling using the Singular Value Decomposition +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_svd.png]] + +#+begin_src matlab :exports none +%% Take complex matrix corresponding to the plant at 100Hz +V = squeeze(frf_coupled(i_wc,:,:)); + +%% Real approximation of G(100Hz) +D = pinv(real(V'*V)); +H1 = pinv(D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2)))); + +%% Singular Value Decomposition +[U,S,V] = svd(H1); + +%% Compute the decoupled plant using SVD +G_de_svd = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + G_de_svd(i,:,:) = inv(U)*squeeze(frf_coupled(i,:,:))*inv(V'); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode Plot of the SVD decoupled plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_svd(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +for i = 1:6 + plot(frf_iff.f, abs(G_de_svd(:,i,i)), ... + 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); +end +plot(frf_iff.f, abs(G_de_svd(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_svd(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_svd_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_svd_plant +#+caption: Bode Plot of the plant decoupled using the Singular Value Decomposition +#+RESULTS: +[[file:figs/test_nhexa_interaction_svd_plant.png]] + +#+begin_src matlab +%% Compute the RGA matrix for the SVD decoupled plant +RGA_svd = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_svd(i,:,:) = squeeze(G_de_svd(i,:,:)).*inv(squeeze(G_de_svd(i,:,:))).'; +end + +%% Compute the RGA-number +RGA_svd_sum = zeros(size(RGA_svd, 1), 1); +for i = 1:length(frf_iff.f) + RGA_svd_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_svd(i,:,:))))); +end +#+end_src + +#+begin_src matlab +%% RGA Number for the SVD decoupled plant +figure; +plot(frf_iff.f, RGA_svd_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_svd.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_svd +#+caption: RGA number for the plant decoupled using the SVD +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_svd.png]] + +**** Dynamic decoupling +<> + +#+begin_src latex :file decoupling_arch_dynamic.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + \node[block, left=0.8 of G] (Ginv) {$\bm{\hat{G}}^{-1}$}; + + % Connections and labels + \draw[<-] (Ginv.west) -- ++(-1.8, 0) node[above right]{$\bm{u}$}; + \draw[->] (Ginv.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; + + \begin{scope}[on background layer] + \node[fit={(Ginv.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; + \node[below right] at (Gx.north west) {$\bm{G}_{\text{inv}}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_dynamic +#+caption: Decoupling using the inverse of a dynamical model $\bm{\hat{G}}$ of the plant dynamics $\bm{G}$ +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_dynamic.png]] + +#+begin_src matlab :exports none +%% Compute the plant inverse from the model +G_model = G_coupled; +G_model.outputdelay = 0; % necessary for further inversion +G_inv = inv(G_model); + +%% Compute the decoupled plant +G_de_inv = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + G_de_inv(i,:,:) = squeeze(frf_coupled(i,:,:))*squeeze(evalfr(G_inv, 1j*2*pi*frf_iff.f(i))); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the decoupled plant by full inversion +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_inv(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +for i = 1:6 + plot(frf_iff.f, abs(G_de_inv(:,i,i)), ... + 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); +end +plot(frf_iff.f, abs(G_de_inv(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e1]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_inv(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_dynamic_dec_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_dynamic_dec_plant +#+caption: Bode Plot of the dynamically decoupled plant +#+RESULTS: +[[file:figs/test_nhexa_interaction_dynamic_dec_plant.png]] + +#+begin_src matlab :exports none +%% Compute the RGA matrix for the inverse based decoupled plant +RGA_inv = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_inv(i,:,:) = squeeze(G_de_inv(i,:,:)).*inv(squeeze(G_de_inv(i,:,:))).'; +end + +%% Compute the RGA-number +RGA_inv_sum = zeros(size(RGA_inv, 1), 1); +for i = 1:size(RGA_inv, 1) + RGA_inv_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_inv(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% RGA Number for the decoupled plant using full inversion +figure; +plot(frf_iff.f, RGA_inv_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_dynamic_dec.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_dynamic_dec +#+caption: RGA number for the dynamically decoupled plant +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_dynamic_dec.png]] + +**** Jacobian Decoupling - Center of Stiffness +<> + +#+begin_src latex :file decoupling_arch_jacobian_cok.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + \node[block, left=0.8 of G] (Jt) {$J_{s,\{K\}}^{-T}$}; + \node[block, right=0.8 of G] (Ja) {$J_{a,\{K\}}^{-1}$}; + + % Connections and labels + \draw[<-] (Jt.west) -- ++(-1.8, 0) node[above right]{$\bm{\mathcal{F}}_{\{K\}}$}; + \draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- (Ja.west) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] (Ja.east) -- ++( 1.8, 0) node[above left]{$\bm{\mathcal{X}}_{\{K\}}$}; + + \begin{scope}[on background layer] + \node[fit={(Jt.south west) (Ja.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; + \node[below right] at (Gx.north west) {$\bm{G}_{\{K\}}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_jacobian_cok +#+caption: Decoupling using Jacobian matrices evaluated at the Center of Stiffness +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_jacobian_cok.png]] + +#+begin_src matlab :exports none +%% Initialize the Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('MO_B', -42e-3, ... + 'motion_sensor_type', 'plates'); + +%% Get the Jacobians +J_cok = n_hexapod.geometry.J; +Js_cok = n_hexapod.geometry.Js; + +%% Decouple plant using Jacobian (CoM) +G_de_J_cok = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + G_de_J_cok(i,:,:) = inv(Js_cok)*squeeze(frf_coupled(i,:,:))*inv(J_cok'); +end +#+end_src + +The obtained plant is shown in Figure ref:fig:test_nhexa_interaction_J_cok_plant_not_normalized. +We can see that the stiffness in the $x$, $y$ and $z$ directions are equal, which is due to the cubic architecture of the Stewart platform. + +#+begin_src matlab :exports none +%% Bode Plot of the SVD decoupled plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_J_cok(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +plot(frf_iff.f, abs(G_de_J_cok(:,1,1)), ... + 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); +plot(frf_iff.f, abs(G_de_J_cok(:,2,2)), ... + 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); +plot(frf_iff.f, abs(G_de_J_cok(:,3,3)), ... + 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); +plot(frf_iff.f, abs(G_de_J_cok(:,4,4)), ... + 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); +plot(frf_iff.f, abs(G_de_J_cok(:,5,5)), ... + 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); +plot(frf_iff.f, abs(G_de_J_cok(:,6,6)), ... + 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); +plot(frf_iff.f, abs(G_de_J_cok(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-8, 2e-2]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_J_cok(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_J_cok_plant_not_normalized.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_J_cok_plant_not_normalized +#+caption: Bode Plot of the plant decoupled using the Jacobian evaluated at the "center of stiffness" +#+RESULTS: +[[file:figs/test_nhexa_interaction_J_cok_plant_not_normalized.png]] + +Because the plant in translation and rotation has very different gains, we choose to normalize the plant inputs such that the gain of the diagonal term is equal to $1$ at 100Hz. + +The results is shown in Figure ref:fig:test_nhexa_interaction_J_cok_plant. +#+begin_src matlab :exports none +%% Normalize the plant input +[~, i_100] = min(abs(frf_iff.f - 100)); +input_normalize = diag(1./diag(abs(squeeze(G_de_J_cok(i_100,:,:))))); + +for i = 1:length(frf_iff.f) + G_de_J_cok(i,:,:) = squeeze(G_de_J_cok(i,:,:))*input_normalize; +end +#+end_src + +#+begin_src matlab :exports none +%% Bode Plot of the SVD decoupled plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_J_cok(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +plot(frf_iff.f, abs(G_de_J_cok(:,1,1)), ... + 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); +plot(frf_iff.f, abs(G_de_J_cok(:,2,2)), ... + 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); +plot(frf_iff.f, abs(G_de_J_cok(:,3,3)), ... + 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); +plot(frf_iff.f, abs(G_de_J_cok(:,4,4)), ... + 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); +plot(frf_iff.f, abs(G_de_J_cok(:,5,5)), ... + 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); +plot(frf_iff.f, abs(G_de_J_cok(:,6,6)), ... + 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); +plot(frf_iff.f, abs(G_de_J_cok(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-4, 1e1]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_J_cok(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_J_cok_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_J_cok_plant +#+caption: Bode Plot of the plant decoupled using the Jacobian evaluated at the "center of stiffness" +#+RESULTS: +[[file:figs/test_nhexa_interaction_J_cok_plant.png]] + +#+begin_src matlab :exports none +%% Compute RGA Matrix +RGA_cok = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_cok(i,:,:) = squeeze(G_de_J_cok(i,:,:)).*inv(squeeze(G_de_J_cok(i,:,:))).'; +end + +%% Compute RGA-number +RGA_cok_sum = zeros(length(frf_iff.f), 1); +for i = 1:length(frf_iff.f) + RGA_cok_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_cok(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the RGA-Number for the Jacobian (CoK) decoupled plant +figure; +plot(frf_iff.f, RGA_cok_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_J_cok.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_J_cok +#+caption: RGA number for the plant decoupled using the Jacobian evaluted at the Center of Stiffness +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_J_cok.png]] + +**** Jacobian Decoupling - Center of Mass +<> + +#+begin_src latex :file decoupling_arch_jacobian_com.pdf +\begin{tikzpicture} + \node[block] (G) {$\bm{G}$}; + \node[block, left=0.8 of G] (Jt) {$J_{s,\{M\}}^{-T}$}; + \node[block, right=0.8 of G] (Ja) {$J_{a,\{M\}}^{-1}$}; + + % Connections and labels + \draw[<-] (Jt.west) -- ++(-1.8, 0) node[above right]{$\bm{\mathcal{F}}_{\{M\}}$}; + \draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- (Ja.west) node[above left]{$d\bm{\mathcal{L}}$}; + \draw[->] (Ja.east) -- ++( 1.8, 0) node[above left]{$\bm{\mathcal{X}}_{\{M\}}$}; + + \begin{scope}[on background layer] + \node[fit={(Jt.south west) (Ja.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; + \node[below right] at (Gx.north west) {$\bm{G}_{\{M\}}$}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+name: fig:test_nhexa_decoupling_arch_jacobian_com +#+caption: Decoupling using Jacobian matrices evaluated at the Center of Mass +#+RESULTS: +[[file:figs/test_nhexa_decoupling_arch_jacobian_com.png]] + +#+begin_src matlab :exports none +%% Initialize the Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('MO_B', 25e-3, ... + 'motion_sensor_type', 'plates'); + +%% Get the Jacobians +J_com = n_hexapod.geometry.J; +Js_com = n_hexapod.geometry.Js; + +%% Decouple plant using Jacobian (CoM) +G_de_J_com = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + G_de_J_com(i,:,:) = inv(Js_com)*squeeze(frf_coupled(i,:,:))*inv(J_com'); +end + +%% Normalize the plant input +[~, i_100] = min(abs(frf_iff.f - 100)); +input_normalize = diag(1./diag(abs(squeeze(G_de_J_com(i_100,:,:))))); + +for i = 1:length(frf_iff.f) + G_de_J_com(i,:,:) = squeeze(G_de_J_com(i,:,:))*input_normalize; +end +#+end_src + +#+begin_src matlab :exports none +%% Bode Plot of the SVD decoupled plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_J_com(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +plot(frf_iff.f, abs(G_de_J_com(:,1,1)), ... + 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); +plot(frf_iff.f, abs(G_de_J_com(:,2,2)), ... + 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); +plot(frf_iff.f, abs(G_de_J_com(:,3,3)), ... + 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); +plot(frf_iff.f, abs(G_de_J_com(:,4,4)), ... + 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); +plot(frf_iff.f, abs(G_de_J_com(:,5,5)), ... + 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); +plot(frf_iff.f, abs(G_de_J_com(:,6,6)), ... + 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); +plot(frf_iff.f, abs(G_de_J_com(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e1]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_J_com(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_J_com_plant.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_J_com_plant +#+caption: Bode Plot of the plant decoupled using the Jacobian evaluated at the Center of Mass +#+RESULTS: +[[file:figs/test_nhexa_interaction_J_com_plant.png]] + +#+begin_src matlab :exports none +%% Compute RGA Matrix +RGA_com = zeros(size(frf_coupled)); +for i = 1:length(frf_iff.f) + RGA_com(i,:,:) = squeeze(G_de_J_com(i,:,:)).*inv(squeeze(G_de_J_com(i,:,:))).'; +end + +%% Compute RGA-number +RGA_com_sum = zeros(size(RGA_com, 1), 1); +for i = 1:size(RGA_com, 1) + RGA_com_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_com(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Plot the RGA-Number for the Jacobian (CoM) decoupled plant +figure; +plot(frf_iff.f, RGA_com_sum, 'k-'); +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_rga_J_com.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_interaction_rga_J_com +#+caption: RGA number for the plant decoupled using the Jacobian evaluted at the Center of Mass +#+RESULTS: +[[file:figs/test_nhexa_interaction_rga_J_com.png]] + +**** Decoupling Comparison +<> + +Let's now compare all of the decoupling methods (Figure ref:fig:test_nhexa_interaction_compare_rga_numbers). + +#+begin_important +From Figure ref:fig:test_nhexa_interaction_compare_rga_numbers, the following remarks are made: +- *Decentralized plant*: well decoupled below suspension modes +- *Static inversion*: similar to the decentralized plant as the decentralized plant has already a good decoupling at low frequency +- *Crossover inversion*: the decoupling is improved around the crossover frequency as compared to the decentralized plant. However, the decoupling is increased at lower frequency. +- *SVD decoupling*: Very good decoupling up to 235Hz. Especially between 100Hz and 200Hz. +- *Dynamic Inversion*: the plant is very well decoupled at frequencies where the model is accurate (below 235Hz where flexible modes are not modelled). +- *Jacobian - Stiffness*: good decoupling at low frequency. The decoupling increases at the frequency of the suspension modes, but is acceptable up to the strut flexible modes (235Hz). +- *Jacobian - Mass*: bad decoupling at low frequency. Better decoupling above the frequency of the suspension modes, and acceptable decoupling up to the strut flexible modes (235Hz). +#+end_important + +#+begin_src matlab :exports none +%% Comparison of the RGA-Numbers +figure; +hold on; +plot(frf_iff.f, RGA_dec_sum, 'DisplayName', 'Decentralized'); +plot(frf_iff.f, RGA_sta_sum, 'DisplayName', 'Static inv.'); +plot(frf_iff.f, RGA_wc_sum, 'DisplayName', 'Crossover inv.'); +plot(frf_iff.f, RGA_svd_sum, 'DisplayName', 'SVD'); +plot(frf_iff.f, RGA_inv_sum, 'DisplayName', 'Dynamic inv.'); +plot(frf_iff.f, RGA_cok_sum, 'DisplayName', 'Jacobian - CoK'); +plot(frf_iff.f, RGA_com_sum, 'DisplayName', 'Jacobian - CoM'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_compare_rga_numbers.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_compare_rga_numbers +#+caption: Comparison of the obtained RGA-numbers for all the decoupling methods +#+RESULTS: +[[file:figs/test_nhexa_interaction_compare_rga_numbers.png]] + +**** Decoupling Robustness +<> + +Let's now see how the decoupling is changing when changing the payload's mass. +#+begin_src matlab +frf_new = frf_iff.G_de{3}; +#+end_src + +#+begin_src matlab :exports none +%% Decentralized RGA +RGA_dec_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_dec_b(i,:,:) = squeeze(frf_new(i,:,:)).*inv(squeeze(frf_new(i,:,:))).'; +end + +RGA_dec_sum_b = zeros(length(frf_iff), 1); +for i = 1:length(frf_iff.f) + RGA_dec_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_dec_b(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Static Decoupling +G_de_sta_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + G_de_sta_b(i,:,:) = squeeze(frf_new(i,:,:))*dc_inv; +end + +RGA_sta_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_sta_b(i,:,:) = squeeze(G_de_sta_b(i,:,:)).*inv(squeeze(G_de_sta_b(i,:,:))).'; +end + +RGA_sta_sum_b = zeros(size(RGA_sta_b, 1), 1); +for i = 1:size(RGA_sta_b, 1) + RGA_sta_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_sta_b(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Crossover Decoupling +V = squeeze(frf_coupled(i_wc,:,:)); +D = pinv(real(V'*V)); +H1 = D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2))); + +G_de_wc_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + G_de_wc_b(i,:,:) = squeeze(frf_new(i,:,:))*H1; +end + +RGA_wc_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_wc_b(i,:,:) = squeeze(G_de_wc_b(i,:,:)).*inv(squeeze(G_de_wc_b(i,:,:))).'; +end + +RGA_wc_sum_b = zeros(size(RGA_wc_b, 1), 1); +for i = 1:size(RGA_wc_b, 1) + RGA_wc_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_wc_b(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% SVD +V = squeeze(frf_coupled(i_wc,:,:)); +D = pinv(real(V'*V)); +H1 = pinv(D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2)))); +[U,S,V] = svd(H1); + +G_de_svd_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + G_de_svd_b(i,:,:) = inv(U)*squeeze(frf_new(i,:,:))*inv(V'); +end + +RGA_svd_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_svd_b(i,:,:) = squeeze(G_de_svd_b(i,:,:)).*inv(squeeze(G_de_svd_b(i,:,:))).'; +end + +RGA_svd_sum_b = zeros(size(RGA_svd_b, 1), 1); +for i = 1:size(RGA_svd, 1) + RGA_svd_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_svd_b(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Dynamic Decoupling +G_model = G_coupled; +G_model.outputdelay = 0; % necessary for further inversion +G_inv = inv(G_model); + +G_de_inv_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + G_de_inv_b(i,:,:) = squeeze(frf_new(i,:,:))*squeeze(evalfr(G_inv, 1j*2*pi*frf_iff.f(i))); +end + +RGA_inv_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_inv_b(i,:,:) = squeeze(G_de_inv_b(i,:,:)).*inv(squeeze(G_de_inv_b(i,:,:))).'; +end + +RGA_inv_sum_b = zeros(size(RGA_inv_b, 1), 1); +for i = 1:size(RGA_inv_b, 1) + RGA_inv_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_inv_b(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Jacobian (CoK) +G_de_J_cok_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + G_de_J_cok_b(i,:,:) = inv(Js_cok)*squeeze(frf_new(i,:,:))*inv(J_cok'); +end + +RGA_cok_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_cok_b(i,:,:) = squeeze(G_de_J_cok_b(i,:,:)).*inv(squeeze(G_de_J_cok_b(i,:,:))).'; +end + +RGA_cok_sum_b = zeros(size(RGA_cok_b, 1), 1); +for i = 1:size(RGA_cok_b, 1) + RGA_cok_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_cok_b(i,:,:))))); +end +#+end_src + +#+begin_src matlab :exports none +%% Jacobian (CoM) +G_de_J_com_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + G_de_J_com_b(i,:,:) = inv(Js_com)*squeeze(frf_new(i,:,:))*inv(J_com'); +end + +RGA_com_b = zeros(size(frf_new)); +for i = 1:length(frf_iff.f) + RGA_com_b(i,:,:) = squeeze(G_de_J_com_b(i,:,:)).*inv(squeeze(G_de_J_com_b(i,:,:))).'; +end + +RGA_com_sum_b = zeros(size(RGA_com_b, 1), 1); +for i = 1:size(RGA_com_b, 1) + RGA_com_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_com_b(i,:,:))))); +end +#+end_src + +The obtained RGA-numbers are shown in Figure ref:fig:test_nhexa_interaction_compare_rga_numbers_rob. + +#+begin_important +From Figure ref:fig:test_nhexa_interaction_compare_rga_numbers_rob: +- The decoupling using the Jacobian evaluated at the "center of stiffness" seems to give the most robust results. +#+end_important + +#+begin_src matlab :exports none +%% Robustness of the Decoupling method +figure; +hold on; +plot(frf_iff.f, RGA_dec_sum, '-', 'DisplayName', 'Decentralized'); +plot(frf_iff.f, RGA_sta_sum, '-', 'DisplayName', 'Static inv.'); +plot(frf_iff.f, RGA_wc_sum, '-', 'DisplayName', 'Crossover inv.'); +plot(frf_iff.f, RGA_svd_sum, '-', 'DisplayName', 'SVD'); +plot(frf_iff.f, RGA_inv_sum, '-', 'DisplayName', 'Dynamic inv.'); +plot(frf_iff.f, RGA_cok_sum, '-', 'DisplayName', 'Jacobian - CoK'); +plot(frf_iff.f, RGA_com_sum, '-', 'DisplayName', 'Jacobian - CoM'); +set(gca,'ColorOrderIndex',1) +plot(frf_iff.f, RGA_dec_sum_b, '--', 'HandleVisibility', 'off'); +plot(frf_iff.f, RGA_sta_sum_b, '--', 'HandleVisibility', 'off'); +plot(frf_iff.f, RGA_wc_sum_b, '--', 'HandleVisibility', 'off'); +plot(frf_iff.f, RGA_svd_sum_b, '--', 'HandleVisibility', 'off'); +plot(frf_iff.f, RGA_inv_sum_b, '--', 'HandleVisibility', 'off'); +plot(frf_iff.f, RGA_cok_sum_b, '--', 'HandleVisibility', 'off'); +plot(frf_iff.f, RGA_com_sum_b, '--', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/interaction_compare_rga_numbers_rob.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_interaction_compare_rga_numbers_rob +#+caption: Change of the RGA-number with a change of the payload. Indication of the robustness of the inversion method. +#+RESULTS: +[[file:figs/test_nhexa_interaction_compare_rga_numbers_rob.png]] + +**** Conclusion + +#+begin_important +Several decoupling methods can be used: +- SVD +- Inverse +- Jacobian (CoK) +#+end_important + +#+name: tab:interaction_analysis_conclusion +#+caption: Summary of the interaction analysis and different decoupling strategies +#+attr_latex: :environment tabularx :width \linewidth :align lccc +#+attr_latex: :center t :booktabs t +| *Method* | *RGA* | *Diag Plant* | *Robustness* | +|----------------+-------+--------------+--------------| +| Decentralized | -- | Equal | ++ | +| Static dec. | -- | Equal | ++ | +| Crossover dec. | - | Equal | 0 | +| SVD | ++ | Diff | + | +| Dynamic dec. | ++ | Unity, equal | - | +| Jacobian - CoK | + | Diff | ++ | +| Jacobian - CoM | 0 | Diff | + | + +*** Robust High Authority Controller +:PROPERTIES: +:header-args:matlab+: :tangle matlab/scripts/hac_lac_enc_plates_suspended_table.m +:END: +<> +**** Introduction :ignore: +In this section we wish to develop a robust High Authority Controller (HAC) that is working for all payloads. + +cite:indri20_mechat_robot + +**** Matlab Init :noexport:ignore: +#+begin_src matlab +%% hac_lac_enc_plates_suspended_table.m +% Development and analysis of a robust High Authority Controller +#+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 +%% Load the identified FRF and Simscape model +frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); +sim_iff = load('sim_iff_vib_table_m.mat', 'G_de'); +#+end_src + +**** Using Jacobian evaluated at the center of stiffness +***** Decoupled Plant +#+begin_src matlab +G_nom = frf_iff.G_de{2}; % Nominal Plant +#+end_src + +#+begin_src matlab :exports none +%% Initialize the Nano-Hexapod +n_hexapod = initializeNanoHexapodFinal('MO_B', -42e-3, ... + 'motion_sensor_type', 'plates'); + +%% Get the Jacobians +J_cok = n_hexapod.geometry.J; +Js_cok = n_hexapod.geometry.Js; + +%% Decouple plant using Jacobian (CoM) +G_de_J_cok = zeros(size(G_nom)); +for i = 1:length(frf_iff.f) + G_de_J_cok(i,:,:) = inv(Js_cok)*squeeze(G_nom(i,:,:))*inv(J_cok'); +end + +%% Normalize the plant input +[~, i_100] = min(abs(frf_iff.f - 10)); +input_normalize = diag(1./diag(abs(squeeze(G_de_J_cok(i_100,:,:))))); + +for i = 1:length(frf_iff.f) + G_de_J_cok(i,:,:) = squeeze(G_de_J_cok(i,:,:))*input_normalize; +end +#+end_src + +#+begin_src matlab :exports none +%% Bode Plot of the decoupled plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_J_cok(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1) +plot(frf_iff.f, abs(G_de_J_cok(:,1,1)), ... + 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); +plot(frf_iff.f, abs(G_de_J_cok(:,2,2)), ... + 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); +plot(frf_iff.f, abs(G_de_J_cok(:,3,3)), ... + 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); +plot(frf_iff.f, abs(G_de_J_cok(:,4,4)), ... + 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); +plot(frf_iff.f, abs(G_de_J_cok(:,5,5)), ... + 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); +plot(frf_iff.f, abs(G_de_J_cok(:,6,6)), ... + 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); +plot(frf_iff.f, abs(G_de_J_cok(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e1]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_J_cok(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_plot_hac_iff_plant_jacobian_cok.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_bode_plot_hac_iff_plant_jacobian_cok +#+caption: Bode plot of the decoupled plant using the Jacobian evaluated at the Center of Stiffness +#+RESULTS: +[[file:figs/test_nhexa_bode_plot_hac_iff_plant_jacobian_cok.png]] + +***** SISO Controller Design +As the diagonal elements of the plant are not equal, several SISO controllers are designed and then combined to form a diagonal controller. +All the diagonal terms of the controller consists of: +- A double integrator to have high gain at low frequency +- A lead around the crossover frequency to increase stability margins +- Two second order low pass filters above the crossover frequency to increase the robustness to high frequency modes + +#+begin_src matlab :exports none +%% Controller Ry,Rz + +% Wanted crossover frequency +wc_Rxy = 2*pi*80; + +% Lead +a = 8.0; % Amount of phase lead / width of the phase lead / high frequency gain +wc = wc_Rxy; % Frequency with the maximum phase lead [rad/s] +Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); + +% Integrator +w0_int = wc_Rxy/2; % [rad/s] +xi_int = 0.3; + +Kd_int = (1 + 2*xi_int/w0_int*s + s^2/w0_int^2)/(s^2/w0_int^2); + +% Low Pass Filter (High frequency robustness) +w0_lpf = wc_Rxy*2; % Cut-off frequency [rad/s] +xi_lpf = 0.6; % Damping Ratio + +Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); + +w0_lpf_b = wc_Rxy*4; % Cut-off frequency [rad/s] +xi_lpf_b = 0.7; % Damping Ratio + +Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); + +% Unity Gain frequency +[~, i_80] = min(abs(frf_iff.f - wc_Rxy/2/pi)); + +% Combination of all the elements +Kd_Rxy = ... + -1/abs(G_de_J_cok(i_80,4,4)) * ... + Kd_lead/abs(evalfr(Kd_lead, 1j*wc_Rxy)) * ... % Lead (gain of 1 at wc) + Kd_int /abs(evalfr(Kd_int, 1j*wc_Rxy)) * ... + Kd_lpf_b/abs(evalfr(Kd_lpf_b, 1j*wc_Rxy)) * ... + Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc_Rxy)); % Low Pass Filter +#+end_src + +#+begin_src matlab :exports none +%% Controller Dx,Dy,Rz + +% Wanted crossover frequency +wc_Dxy = 2*pi*100; + +% Lead +a = 8.0; % Amount of phase lead / width of the phase lead / high frequency gain +wc = wc_Dxy; % Frequency with the maximum phase lead [rad/s] +Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); + +% Integrator +w0_int = wc_Dxy/2; % [rad/s] +xi_int = 0.3; + +Kd_int = (1 + 2*xi_int/w0_int*s + s^2/w0_int^2)/(s^2/w0_int^2); + +% Low Pass Filter (High frequency robustness) +w0_lpf = wc_Dxy*2; % Cut-off frequency [rad/s] +xi_lpf = 0.6; % Damping Ratio + +Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); + +w0_lpf_b = wc_Dxy*4; % Cut-off frequency [rad/s] +xi_lpf_b = 0.7; % Damping Ratio + +Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); + +% Unity Gain frequency +[~, i_100] = min(abs(frf_iff.f - wc_Dxy/2/pi)); + +% Combination of all the elements +Kd_Dyx_Rz = ... + -1/abs(G_de_J_cok(i_100,1,1)) * ... + Kd_int /abs(evalfr(Kd_int, 1j*wc_Dxy)) * ... % Integrator + Kd_lead/abs(evalfr(Kd_lead, 1j*wc_Dxy)) * ... % Lead (gain of 1 at wc) + Kd_lpf_b/abs(evalfr(Kd_lpf_b, 1j*wc_Dxy)) * ... % Lead (gain of 1 at wc) + Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc_Dxy)); % Low Pass Filter +#+end_src + +#+begin_src matlab :exports none +%% Controller Dz + +% Wanted crossover frequency +wc_Dz = 2*pi*100; + +% Lead +a = 8.0; % Amount of phase lead / width of the phase lead / high frequency gain +wc = wc_Dz; % Frequency with the maximum phase lead [rad/s] +Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); + +% Integrator +w0_int = wc_Dz/2; % [rad/s] +xi_int = 0.3; + +Kd_int = (1 + 2*xi_int/w0_int*s + s^2/w0_int^2)/(s^2/w0_int^2); + +% Low Pass Filter (High frequency robustness) +w0_lpf = wc_Dz*2; % Cut-off frequency [rad/s] +xi_lpf = 0.6; % Damping Ratio + +Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); + +w0_lpf_b = wc_Dz*4; % Cut-off frequency [rad/s] +xi_lpf_b = 0.7; % Damping Ratio + +Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); + +% Unity Gain frequency +[~, i_100] = min(abs(frf_iff.f - wc_Dz/2/pi)); + +% Combination of all the elements +Kd_Dz = ... + -1/abs(G_de_J_cok(i_100,3,3)) * ... + Kd_int /abs(evalfr(Kd_int, 1j*wc_Dz)) * ... % Integrator + Kd_lead/abs(evalfr(Kd_lead, 1j*wc_Dz)) * ... % Lead (gain of 1 at wc) + Kd_lpf_b/abs(evalfr(Kd_lpf_b, 1j*wc_Dz)) * ... % Lead (gain of 1 at wc) + Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc_Dz)); % Low Pass Filter +#+end_src + +#+begin_src matlab :exports none +%% Diagonal Controller +Kd_diag = blkdiag(Kd_Dyx_Rz, Kd_Dyx_Rz, Kd_Dz, Kd_Rxy, Kd_Rxy, Kd_Dyx_Rz); +#+end_src + +***** Obtained Loop Gain +#+begin_src matlab :exports none +%% Experimental Loop Gain +Lmimo = permute(pagemtimes(permute(G_de_J_cok, [2,3,1]), squeeze(freqresp(Kd_diag, frf_iff.f, 'Hz'))), [3,1,2]); +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the experimental Loop Gain +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:6 + plot(frf_iff.f, abs(Lmimo(:,i,i)), '-'); +end +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(squeeze(Lmimo(:,i,j))), 'color', [0,0,0,0.2]); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e+3]); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(frf_iff.f, 180/pi*angle(Lmimo(:,i,i)), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:45:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([1, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_plot_hac_iff_loop_gain_jacobian_cok.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_bode_plot_hac_iff_loop_gain_jacobian_cok +#+caption: Bode plot of the Loop Gain when using the Jacobian evaluated at the Center of Stiffness to decouple the system +#+RESULTS: +[[file:figs/test_nhexa_bode_plot_hac_iff_loop_gain_jacobian_cok.png]] + +#+begin_src matlab +%% Controller to be implemented +Kd = inv(J_cok')*input_normalize*ss(Kd_diag)*inv(Js_cok); +#+end_src + +***** Verification of the Stability +Now the stability of the feedback loop is verified using the generalized Nyquist criteria. + +#+begin_src matlab :exports none +%% Compute the Eigenvalues of the loop gain +Ldet = zeros(3, 6, length(frf_iff.f)); + +for i_mass = 1:3 + % Loop gain + Lmimo = pagemtimes(permute(frf_iff.G_de{i_mass}, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))); + for i_f = 2:length(frf_iff.f) + Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); + end +end +#+end_src + +#+begin_src matlab :exports none +%% Plot of the eigenvalues of L in the complex plane +figure; +hold on; +for i_mass = 2:3 + plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'DisplayName', sprintf('%i masses', i_mass)); + plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + for i = 1:6 + plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + end +end +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +legend('location', 'southeast'); +xlim([-3, 1]); ylim([-2, 2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/loci_hac_iff_loop_gain_jacobian_cok.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_loci_hac_iff_loop_gain_jacobian_cok +#+caption: Loci of $L(j\omega)$ in the complex plane. +#+RESULTS: +[[file:figs/test_nhexa_loci_hac_iff_loop_gain_jacobian_cok.png]] + +***** Save for further analysis +#+begin_src matlab :exports none :tangle no +save('matlab/data_sim/Khac_iff_struts_jacobian_cok.mat', 'Kd') +#+end_src + +#+begin_src matlab :eval no +save('data_sim/Khac_iff_struts_jacobian_cok.mat', 'Kd') +#+end_src + +***** Sensitivity transfer function from the model +#+begin_src matlab :exports none +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +open(mdl) + +Rx = zeros(1, 7); + +colors = colororder; +#+end_src + +#+begin_src matlab :exports none +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof', ... + 'controller_type', 'hac-iff-struts'); + +support.type = 1; % On top of vibration table +payload.type = 2; % Payload +#+end_src + +#+begin_src matlab :exports none +%% Load controllers +load('Kiff_opt.mat', 'Kiff'); +Kiff = c2d(Kiff, Ts, 'Tustin'); +load('Khac_iff_struts_jacobian_cok.mat', 'Kd') +Khac_iff_struts = c2d(Kd, Ts, 'Tustin'); +#+end_src + +#+begin_src matlab :exports none +%% Identify the (damped) transfer function from u to dLm +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Rx'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'output'); io_i = io_i + 1; % Plate Displacement (encoder) +#+end_src + +#+begin_src matlab :exports none +%% Identification of the dynamics +Gcl = linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Computation of the sensitivity transfer function +S = eye(6) - inv(n_hexapod.geometry.J)*Gcl; +#+end_src + +The results are shown in Figure ref:fig:test_nhexa_sensitivity_hac_jacobian_cok_3m_comp_model. + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = logspace(0, 3, 1000); + +figure; +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(S(i,i), freqs, 'Hz'))), '--', ... + 'DisplayName', sprintf('$S_{%s}$ - Model', labels{i})); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Sensitivity [-]'); +ylim([1e-4, 1e1]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); +xlim([1, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sensitivity_hac_jacobian_cok_3m_comp_model.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_sensitivity_hac_jacobian_cok_3m_comp_model +#+caption: Estimated sensitivity transfer functions for the HAC controller using the Jacobian estimated at the Center of Stiffness +#+RESULTS: +[[file:figs/test_nhexa_sensitivity_hac_jacobian_cok_3m_comp_model.png]] + +**** Using Singular Value Decomposition +***** Decoupled Plant +#+begin_src matlab +G_nom = frf_iff.G_de{2}; % Nominal Plant +#+end_src + +#+begin_src matlab :exports none +%% Take complex matrix corresponding to the plant at 100Hz +wc = 100; % Wanted crossover frequency [Hz] +[~, i_wc] = min(abs(frf_iff.f - wc)); % Indice corresponding to wc + +V = squeeze(G_nom(i_wc,:,:)); + +%% Real approximation of G(100Hz) +D = pinv(real(V'*V)); +H1 = pinv(D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2)))); + +%% Singular Value Decomposition +[U,S,V] = svd(H1); + +%% Compute the decoupled plant using SVD +G_de_svd = zeros(size(G_nom)); +for i = 1:length(frf_iff.f) + G_de_svd(i,:,:) = inv(U)*squeeze(G_nom(i,:,:))*inv(V'); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot of the decoupled plant using SVD +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(G_de_svd(:,i,j)), 'color', [0,0,0,0.2], ... + 'HandleVisibility', 'off'); + end +end +set(gca,'ColorOrderIndex',1); +for i = 1:6 + plot(frf_iff.f, abs(G_de_svd(:,i,i)), ... + 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); +end +plot(frf_iff.f, abs(G_de_svd(:,1,2)), 'color', [0,0,0,0.2], ... + 'DisplayName', 'Coupling'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-9, 1e-4]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); + +ax2 = nexttile; +hold on; +for i =1:6 + plot(frf_iff.f, 180/pi*angle(G_de_svd(:,i,i))); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 180]); + +linkaxes([ax1,ax2],'x'); +xlim([10, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_plot_hac_iff_plant_svd.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_bode_plot_hac_iff_plant_svd +#+caption: Bode plot of the decoupled plant using the SVD +#+RESULTS: +[[file:figs/test_nhexa_bode_plot_hac_iff_plant_svd.png]] + +***** Controller Design +#+begin_src matlab :exports none +%% Lead +a = 6.0; % Amount of phase lead / width of the phase lead / high frequency gain +wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s] +Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); + +%% Integrator +Kd_int = ((2*pi*50 + s)/(2*pi*0.1 + s))^2; + +%% Low Pass Filter (High frequency robustness) +w0_lpf = 2*pi*200; % Cut-off frequency [rad/s] +xi_lpf = 0.3; % Damping Ratio + +Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); + +%% Normalize Gain +Kd_norm = diag(1./abs(diag(squeeze(G_de_svd(i_wc,:,:))))); + +%% Diagonal Control +Kd_diag = ... + Kd_norm * ... % Normalize gain at 100Hz + Kd_int /abs(evalfr(Kd_int, 1j*2*pi*100)) * ... % Integrator + Kd_lead/abs(evalfr(Kd_lead, 1j*2*pi*100)) * ... % Lead (gain of 1 at wc) + Kd_lpf /abs(evalfr(Kd_lpf, 1j*2*pi*100)); % Low Pass Filter +#+end_src + +#+begin_src matlab :exports none +%% MIMO Controller +Kd = -inv(V') * ... % Output decoupling + ss(Kd_diag) * ... + inv(U); % Input decoupling +#+end_src + +***** Loop Gain +#+begin_src matlab :exports none +%% Experimental Loop Gain +Lmimo = permute(pagemtimes(permute(G_nom, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))), [3,1,2]); +#+end_src + +#+begin_src matlab :exports none +%% Loop gain when using SVD +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:6 + plot(frf_iff.f, abs(Lmimo(:,i,i)), '-'); +end +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(squeeze(Lmimo(:,i,j))), 'color', [0,0,0,0.2]); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e+3]); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(frf_iff.f, 180/pi*angle(Lmimo(:,i,i)), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:30:360); +ylim([-180, 0]); + +linkaxes([ax1,ax2],'x'); +xlim([1, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_plot_hac_iff_loop_gain_svd.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_bode_plot_hac_iff_loop_gain_svd +#+caption: Bode plot of Loop Gain when using the SVD +#+RESULTS: +[[file:figs/test_nhexa_bode_plot_hac_iff_loop_gain_svd.png]] + +***** Stability Verification +#+begin_src matlab +%% Compute the Eigenvalues of the loop gain +Ldet = zeros(3, 6, length(frf_iff.f)); + +for i = 1:3 + Lmimo = pagemtimes(permute(frf_iff.G_de{i}, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))); + for i_f = 2:length(frf_iff.f) + Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); + end +end +#+end_src + +#+begin_src matlab :exports none +%% Plot of the eigenvalues of L in the complex plane +figure; +hold on; +for i_mass = 2:3 + plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'DisplayName', sprintf('%i masses', i_mass)); + plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + for i = 1:6 + plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + end +end +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +legend('location', 'southeast'); +xlim([-3, 1]); ylim([-2, 2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/loci_hac_iff_loop_gain_svd.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_loci_hac_iff_loop_gain_svd +#+caption: Locis of $L(j\omega)$ in the complex plane. +#+RESULTS: +[[file:figs/test_nhexa_loci_hac_iff_loop_gain_svd.png]] + +***** Save for further analysis +#+begin_src matlab :exports none :tangle no +save('matlab/data_sim/Khac_iff_struts_svd.mat', 'Kd') +#+end_src + +#+begin_src matlab :eval no +save('data_sim/Khac_iff_struts_svd.mat', 'Kd') +#+end_src + +***** Measured Sensitivity Transfer Function +The sensitivity transfer function is estimated by adding a reference signal $R_x$ consisting of a low pass filtered white noise, and measuring the position error $E_x$ at the same time. + +The transfer function from $R_x$ to $E_x$ is the sensitivity transfer function. + +In order to identify the sensitivity transfer function for all directions, six reference signals are used, one for each direction. + +#+begin_src matlab :exports none +%% Tested directions +labels = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +#+end_src + +#+begin_src matlab :exports none +%% Load Identification Data +meas_hac_svd_3m = {}; + +for i = 1:6 + meas_hac_svd_3m(i) = {load(sprintf('T_S_meas_%s_3m_hac_svd_iff.mat', labels{i}), 't', 'Va', 'Vs', 'de', 'Rx')}; +end +#+end_src + +#+begin_src matlab :exports none +%% Setup useful variables +% Sampling Time [s] +Ts = (meas_hac_svd_3m{1}.t(end) - (meas_hac_svd_3m{1}.t(1)))/(length(meas_hac_svd_3m{1}.t)-1); + +% Sampling Frequency [Hz] +Fs = 1/Ts; + +% Hannning Windows +win = hanning(ceil(5*Fs)); + +% And we get the frequency vector +[~, f] = tfestimate(meas_hac_svd_3m{1}.Va, meas_hac_svd_3m{1}.de, win, Noverlap, Nfft, 1/Ts); +#+end_src + +#+begin_src matlab :exports none +%% Load Jacobian matrix +load('jacobian.mat', 'J'); + +%% Compute position error +for i = 1:6 + meas_hac_svd_3m{i}.Xm = [inv(J)*meas_hac_svd_3m{i}.de']'; + meas_hac_svd_3m{i}.Ex = meas_hac_svd_3m{i}.Rx - meas_hac_svd_3m{i}.Xm; +end +#+end_src + +An example is shown in Figure ref:fig:test_nhexa_ref_track_hac_svd_3m where both the reference signal and the measured position are shown for translations in the $x$ direction. + +#+begin_src matlab :exports none +figure; +hold on; +plot(meas_hac_svd_3m{1}.t, meas_hac_svd_3m{1}.Xm(:,1), 'DisplayName', 'Pos.') +plot(meas_hac_svd_3m{1}.t, meas_hac_svd_3m{1}.Rx(:,1), 'DisplayName', 'Ref.') +hold off; +xlabel('Time [s]'); ylabel('Dx motion [m]'); +xlim([20, 22]); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/ref_track_hac_svd_3m.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_ref_track_hac_svd_3m +#+caption: Reference position and measured position +#+RESULTS: +[[file:figs/test_nhexa_ref_track_hac_svd_3m.png]] + +#+begin_src matlab :exports none +%% Transfer function estimate of S +S_hac_svd_3m = zeros(length(f), 6, 6); + +for i = 1:6 + S_hac_svd_3m(:,:,i) = tfestimate(meas_hac_svd_3m{i}.Rx, meas_hac_svd_3m{i}.Ex, win, Noverlap, Nfft, 1/Ts); +end +#+end_src + +The sensitivity transfer functions estimated for all directions are shown in Figure ref:fig:test_nhexa_sensitivity_hac_svd_3m. + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +figure; +hold on; +for i =1:6 + plot(f, abs(S_hac_svd_3m(:,i,i)), ... + 'DisplayName', sprintf('$S_{%s}$', labels{i})); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Sensitivity [-]'); +ylim([1e-4, 1e1]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); +xlim([0.5, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sensitivity_hac_svd_3m.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_sensitivity_hac_svd_3m +#+caption: Measured diagonal elements of the sensitivity transfer function matrix. +#+RESULTS: +[[file:figs/test_nhexa_sensitivity_hac_svd_3m.png]] + +#+begin_important +From Figure ref:fig:test_nhexa_sensitivity_hac_svd_3m: +- The sensitivity transfer functions are similar for all directions +- The disturbance attenuation at 1Hz is almost a factor 1000 as wanted +- The sensitivity transfer functions for $R_x$ and $R_y$ have high peak values which indicate poor stability margins. +#+end_important + +***** Sensitivity transfer function from the model +The sensitivity transfer function is now estimated using the model and compared with the one measured. + +#+begin_src matlab :exports none +%% Open Simulink Model +mdl = 'nano_hexapod_simscape'; + +options = linearizeOptions; +options.SampleTime = 0; + +open(mdl) + +Rx = zeros(1, 7); + +colors = colororder; +#+end_src + +#+begin_src matlab :exports none +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof', ... + 'controller_type', 'hac-iff-struts'); + +support.type = 1; % On top of vibration table +payload.type = 2; % Payload +#+end_src + +#+begin_src matlab :exports none +%% Load controllers +load('Kiff_opt.mat', 'Kiff'); +Kiff = c2d(Kiff, Ts, 'Tustin'); +load('Khac_iff_struts_svd.mat', 'Kd') +Khac_iff_struts = c2d(Kd, Ts, 'Tustin'); +#+end_src + +#+begin_src matlab :exports none +%% Identify the (damped) transfer function from u to dLm +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Rx'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'output'); io_i = io_i + 1; % Plate Displacement (encoder) +#+end_src + +#+begin_src matlab :exports none +%% Identification of the dynamics +Gcl = linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Computation of the sensitivity transfer function +S = eye(6) - inv(n_hexapod.geometry.J)*Gcl; +#+end_src + +The results are shown in Figure ref:fig:test_nhexa_sensitivity_hac_svd_3m_comp_model. +The model is quite effective in estimating the sensitivity transfer functions except around 60Hz were there is a peak for the measurement. + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +freqs = logspace(0,3,1000); + +figure; +hold on; +for i =1:6 + set(gca,'ColorOrderIndex',i); + plot(f, abs(S_hac_svd_3m(:,i,i)), ... + 'DisplayName', sprintf('$S_{%s}$', labels{i})); + set(gca,'ColorOrderIndex',i); + plot(freqs, abs(squeeze(freqresp(S(i,i), freqs, 'Hz'))), '--', ... + 'DisplayName', sprintf('$S_{%s}$ - Model', labels{i})); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Sensitivity [-]'); +ylim([1e-4, 1e1]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); +xlim([0.5, 1e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/sensitivity_hac_svd_3m_comp_model.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_sensitivity_hac_svd_3m_comp_model +#+caption: Comparison of the measured sensitivity transfer functions with the model +#+RESULTS: +[[file:figs/test_nhexa_sensitivity_hac_svd_3m_comp_model.png]] + +**** Using (diagonal) Dynamical Inverse :noexport: +***** Decoupled Plant +#+begin_src matlab +G_nom = frf_iff.G_de{2}; % Nominal Plant +G_model = sim_iff.G_de{2}; % Model of the Plant +#+end_src + +#+begin_src matlab :exports none +%% Simplified model of the diagonal term +balred_opts = balredOptions('FreqIntervals', 2*pi*[0, 1000], 'StateElimMethod', 'Truncate'); + +G_red = balred(G_model(1,1), 8, balred_opts); +G_red.outputdelay = 0; % necessary for further inversion +#+end_src + +#+begin_src matlab +%% Inverse +G_inv = inv(G_red); +[G_z, G_p, G_g] = zpkdata(G_inv); +p_uns = real(G_p{1}) > 0; +G_p{1}(p_uns) = -G_p{1}(p_uns); +G_inv_stable = zpk(G_z, G_p, G_g); +#+end_src + +#+begin_src matlab :exports none +%% "Uncertainty" of inversed plant +freqs = logspace(0,3,1000); + +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i_mass = i_masses + for i = 1 + plot(freqs, abs(squeeze(freqresp(G_inv_stable*sim_iff.G_de{i_mass+1}(i,i), freqs, 'Hz'))), '-', 'color', colors(i_mass+1, :), ... + 'DisplayName', sprintf('$d\\mathcal{L}_i/u^\\prime_i$ - %i', i_mass)); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); +ylim([1e-1, 1e1]); +legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 4); + +ax2 = nexttile; +hold on; +for i_mass = i_masses + for i = 1 + plot(freqs, 180/pi*angle(squeeze(freqresp(G_inv_stable*sim_iff.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '-', 'color', colors(i_mass+1, :)); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:15:360); +ylim([-45, 45]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); +#+end_src + +***** Controller Design +#+begin_src matlab :exports none +% Wanted crossover frequency +wc = 2*pi*80; +[~, i_wc] = min(abs(frf_iff.f - wc/2/pi)); + +%% Lead +a = 20.0; % Amount of phase lead / width of the phase lead / high frequency gain +Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); + +%% Integrator +Kd_int = ((wc)/(2*pi*0.2 + s))^2; + +%% Low Pass Filter (High frequency robustness) +w0_lpf = 2*wc; % Cut-off frequency [rad/s] +xi_lpf = 0.3; % Damping Ratio + +Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); + +w0_lpf_b = wc*4; % Cut-off frequency [rad/s] +xi_lpf_b = 0.7; % Damping Ratio + +Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); + +%% Normalize Gain +Kd_norm = diag(1./abs(diag(squeeze(G_de_svd(i_wc,:,:))))); + +%% Diagonal Control +Kd_diag = ... + G_inv_stable * ... % Normalize gain at 100Hz + Kd_int /abs(evalfr(Kd_int, 1j*wc)) * ... % Integrator + Kd_lead/abs(evalfr(Kd_lead, 1j*wc)) * ... % Lead (gain of 1 at wc) + Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc)); % Low Pass Filter +#+end_src + +#+begin_src matlab :exports none +Kd = ss(Kd_diag)*eye(6); +#+end_src + +***** Loop Gain +#+begin_src matlab :exports none +%% Experimental Loop Gain +Lmimo = permute(pagemtimes(permute(G_nom, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))), [3,1,2]); +#+end_src + +#+begin_src matlab :exports none +%% Loop gain when using SVD +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:6 + plot(frf_iff.f, abs(Lmimo(:,i,i)), '-'); +end +for i = 1:5 + for j = i+1:6 + plot(frf_iff.f, abs(squeeze(Lmimo(:,i,j))), 'color', [0,0,0,0.2]); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +ylim([1e-3, 1e+3]); + +ax2 = nexttile; +hold on; +for i = 1:6 + plot(frf_iff.f, 180/pi*angle(Lmimo(:,i,i)), '-'); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:30:360); +ylim([-180, 0]); + +linkaxes([ax1,ax2],'x'); +xlim([1, 2e3]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/bode_plot_hac_iff_loop_gain_diag_inverse.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_bode_plot_hac_iff_loop_gain_diag_inverse +#+caption: Bode plot of Loop Gain when using the Diagonal inversion +#+RESULTS: +[[file:figs/test_nhexa_bode_plot_hac_iff_loop_gain_diag_inverse.png]] + +***** Stability Verification +MIMO Nyquist with eigenvalues +#+begin_src matlab +%% Compute the Eigenvalues of the loop gain +Ldet = zeros(3, 6, length(frf_iff.f)); + +for i = 1:3 + Lmimo = pagemtimes(permute(frf_iff.G_de{i}, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))); + for i_f = 2:length(frf_iff.f) + Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); + end +end +#+end_src + +#+begin_src matlab :exports none +%% Plot of the eigenvalues of L in the complex plane +figure; +hold on; +for i_mass = 2:3 + plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'DisplayName', sprintf('%i masses', i_mass)); + plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + for i = 1:6 + plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... + '.', 'color', colors(i_mass+1, :), ... + 'HandleVisibility', 'off'); + end +end +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +legend('location', 'southeast'); +xlim([-3, 1]); ylim([-2, 2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/loci_hac_iff_loop_gain_diag_inverse.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:test_nhexa_loci_hac_iff_loop_gain_diag_inverse +#+caption: Locis of $L(j\omega)$ in the complex plane. +#+RESULTS: +[[file:figs/test_nhexa_loci_hac_iff_loop_gain_diag_inverse.png]] + +#+begin_important +Even though the loop gain seems to be fine, the closed-loop system is unstable. +This might be due to the fact that there is large interaction in the plant. +We could look at the RGA-number to verify that. +#+end_important + +***** Save for further use +#+begin_src matlab :exports none :tangle no +save('matlab/data_sim/Khac_iff_struts_diag_inverse.mat', 'Kd') +#+end_src + +#+begin_src matlab :eval no +save('data_sim/Khac_iff_struts_diag_inverse.mat', 'Kd') +#+end_src + +**** Closed Loop Stability (Model) :noexport: +Verify stability using Simscape model +#+begin_src matlab +%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ... + 'flex_top_type', '3dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', '2dof', ... + 'controller_type', 'hac-iff-struts'); +#+end_src + +#+begin_src matlab +%% IFF Controller +Kiff = -g_opt*Kiff_g1*eye(6); +Khac_iff_struts = Kd*eye(6); +#+end_src + +#+begin_src matlab +%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain +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; % Plate Displacement (encoder) +#+end_src + +#+begin_src matlab +GG_cl = {}; + +for i = i_masses + payload.type = i; + GG_cl(i+1) = {exp(-s*Ts)*linearize(mdl, io, 0.0, options)}; +end +#+end_src + +#+begin_src matlab +for i = i_masses + isstable(GG_cl{i+1}) +end +#+end_src + +MIMO Nyquist +#+begin_src matlab +Kdm = Kd*eye(6); + +Ldet = zeros(3, length(fb(i_lim))); + +for i = 1:3 + Lmimo = pagemtimes(permute(G_damp_m{i}(i_lim,:,:), [2,3,1]),squeeze(freqresp(Kdm, fb(i_lim), 'Hz'))); + Ldet(i,:) = arrayfun(@(t) det(eye(6) + squeeze(Lmimo(:,:,t))), 1:size(Lmimo,3)); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +figure; +hold on; +for i_mass = 3 + for i = 1 + plot(real(Ldet(i_mass,:)), imag(Ldet(i_mass,:)), ... + '-', 'color', colors(i_mass+1, :)); + end +end +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +xlim([-10, 1]); ylim([-4, 4]); +#+end_src + +MIMO Nyquist with eigenvalues +#+begin_src matlab +Kdm = Kd*eye(6); + +Ldet = zeros(3, 6, length(fb(i_lim))); + +for i = 1:3 + Lmimo = pagemtimes(permute(G_damp_m{i}(i_lim,:,:), [2,3,1]),squeeze(freqresp(Kdm, fb(i_lim), 'Hz'))); + for i_f = 1:length(fb(i_lim)) + Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); + end +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +figure; +hold on; +for i_mass = 1 + for i = 1:6 + plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... + '-', 'color', colors(i_mass+1, :)); + end +end +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +xlim([-10, 1]); ylim([-4, 2]); +#+end_src +** Other Backups +*** Nano-Hexapod Compliance - Effect of IFF +<> + +In this section, we wish to estimate the effectiveness of the IFF strategy regarding the compliance. + +The top plate is excited vertically using the instrumented hammer two times: +1. no control loop is used +2. decentralized IFF is used + +The data are loaded. +#+begin_src matlab +frf_ol = load('Measurement_Z_axis.mat'); % Open-Loop +frf_iff = load('Measurement_Z_axis_damped.mat'); % IFF +#+end_src + +The mean vertical motion of the top platform is computed by averaging all 5 vertical accelerometers. +#+begin_src matlab +%% Multiply by 10 (gain in m/s^2/V) and divide by 5 (number of accelerometers) +d_frf_ol = 10/5*(frf_ol.FFT1_H1_4_1_RMS_Y_Mod + frf_ol.FFT1_H1_7_1_RMS_Y_Mod + frf_ol.FFT1_H1_10_1_RMS_Y_Mod + frf_ol.FFT1_H1_13_1_RMS_Y_Mod + frf_ol.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_ol.FFT1_H1_16_1_RMS_X_Val).^2; +d_frf_iff = 10/5*(frf_iff.FFT1_H1_4_1_RMS_Y_Mod + frf_iff.FFT1_H1_7_1_RMS_Y_Mod + frf_iff.FFT1_H1_10_1_RMS_Y_Mod + frf_iff.FFT1_H1_13_1_RMS_Y_Mod + frf_iff.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_iff.FFT1_H1_16_1_RMS_X_Val).^2; +#+end_src + +The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure ref:fig:test_nhexa_compliance_vertical_comp_iff. +#+begin_src matlab :exports none +%% Comparison of the vertical compliances (OL and IFF) +figure; +hold on; +plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, 'DisplayName', 'OL'); +plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, 'DisplayName', 'IFF'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]'); +xlim([20, 2e3]); ylim([2e-9, 2e-5]); +legend('location', 'northeast'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/compliance_vertical_comp_iff.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_compliance_vertical_comp_iff +#+caption: Measured vertical compliance with and without IFF +#+RESULTS: +[[file:figs/test_nhexa_compliance_vertical_comp_iff.png]] + +#+begin_important +From Figure ref:fig:test_nhexa_compliance_vertical_comp_iff, it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapod. +It also has the effect of (slightly) degrading the vertical compliance at low frequency. + +It also seems some damping can be added to the modes at around 205Hz which are flexible modes of the struts. +#+end_important + +*** Comparison with the Simscape Model +<> + +Let's initialize the Simscape model such that it corresponds to the experiment. +#+begin_src matlab +%% Nano-Hexapod is fixed on a rigid granite +support.type = 0; + +%% No Payload on top of the Nano-Hexapod +payload.type = 0; + +%% Initialize Nano-Hexapod in Open Loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', '2dof'); + +#+end_src + +And let's compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model. + +The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model. +#+begin_src matlab :exports none +%% Identify the IFF Plant (transfer function from u to taum) +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Fz_ext'], 1, 'openinput'); io_i = io_i + 1; % External - Vertical force +io(io_i) = linio([mdl, '/Z_top_plat'], 1, 'openoutput'); io_i = io_i + 1; % Absolute vertical motion of top platform +#+end_src + +#+begin_src matlab :exports none +%% Perform the identifications +G_compl_z_ol = linearize(mdl, io, 0.0, options); +#+end_src + +#+begin_src matlab :exports none +%% Initialize Nano-Hexapod with IFF +Kiff = 400*(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz) + (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain + (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances + eye(6); % Diagonal 6x6 controller + +%% Initialize the Nano-Hexapod with IFF +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'struts', ... + 'actuator_type', '2dof', ... + 'controller_type', 'iff'); + +%% Perform the identification +G_compl_z_iff = linearize(mdl, io, 0.0, options); +#+end_src + +The comparison is done in Figure ref:fig:test_nhexa_compliance_vertical_comp_model_iff. +Again, the model is quite accurate in predicting the (closed-loop) behavior of the system. + +#+begin_src matlab :exports none +%% Comparison of the measured compliance and the one obtained from the model +freqs = 2*logspace(1,3,1000); + +figure; +hold on; +plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, '-', 'DisplayName', 'OL - Meas.'); +plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, '-', 'DisplayName', 'IFF - Meas.'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(G_compl_z_ol, freqs, 'Hz'))), '--', 'DisplayName', 'OL - Model') +plot(freqs, abs(squeeze(freqresp(G_compl_z_iff, freqs, 'Hz'))), '--', 'DisplayName', 'IFF - Model') +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]'); +xlim([20, 2e3]); ylim([2e-9, 2e-5]); +legend('location', 'northeast', 'FontSize', 8); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/compliance_vertical_comp_model_iff.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_compliance_vertical_comp_model_iff +#+caption: Measured vertical compliance with and without IFF +#+RESULTS: +[[file:figs/test_nhexa_compliance_vertical_comp_model_iff.png]] + +*** Computation of the transmissibility from accelerometer data +**** Introduction :ignore: + +The goal is to compute the $6 \times 6$ transfer function matrix corresponding to the transmissibility of the Nano-Hexapod. + +To do so, several accelerometers are located both on the vibration table and on the top of the nano-hexapod. + +The vibration table is then excited using a Shaker and all the accelerometers signals are recorded. + +Using transformation (jacobian) matrices, it is then possible to compute both the motion of the top and bottom platform of the nano-hexapod. + +Finally, it is possible to compute the $6 \times 6$ transmissibility matrix. + +Such procedure is explained in cite:marneffe04_stewar_platf_activ_vibrat_isolat. + +**** Jacobian matrices + +How to compute the Jacobian matrices is explained in Section ref:sec:meas_transformation. + +#+begin_src matlab +%% Bottom Accelerometers +Opb = [-0.1875, -0.1875, -0.245; + -0.1875, -0.1875, -0.245; + 0.1875, -0.1875, -0.245; + 0.1875, -0.1875, -0.245; + 0.1875, 0.1875, -0.245; + 0.1875, 0.1875, -0.245]'; + +Osb = [0, 1, 0; + 0, 0, 1; + 1, 0, 0; + 0, 0, 1; + 1, 0, 0; + 0, 0, 1;]'; + +Jb = zeros(length(Opb), 6); + +for i = 1:length(Opb) + Ri = [0, Opb(3,i), -Opb(2,i); + -Opb(3,i), 0, Opb(1,i); + Opb(2,i), -Opb(1,i), 0]; + Jb(i, 1:3) = Osb(:,i)'; + Jb(i, 4:6) = Osb(:,i)'*Ri; +end + +Jbinv = inv(Jb); +#+end_src + +#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) +data2orgtable(Jbinv, {'$\dot{x}_x$', '$\dot{x}_y$', '$\dot{x}_z$', '$\dot{\omega}_x$', '$\dot{\omega}_y$', '$\dot{\omega}_z$'}, {'$a_1$', '$a_2$', '$a_3$', '$a_4$', '$a_5$', '$a_6$'}, ' %.1f '); +#+end_src + +#+RESULTS: +| | $a_1$ | $a_2$ | $a_3$ | $a_4$ | $a_5$ | $a_6$ | +|------------------+-------+-------+-------+-------+-------+-------| +| $\dot{x}_x$ | 0.0 | 0.7 | 0.5 | -0.7 | 0.5 | 0.0 | +| $\dot{x}_y$ | 1.0 | 0.0 | 0.5 | 0.7 | -0.5 | -0.7 | +| $\dot{x}_z$ | 0.0 | 0.5 | 0.0 | 0.0 | 0.0 | 0.5 | +| $\dot{\omega}_x$ | 0.0 | 0.0 | 0.0 | -2.7 | 0.0 | 2.7 | +| $\dot{\omega}_y$ | 0.0 | 2.7 | 0.0 | -2.7 | 0.0 | 0.0 | +| $\dot{\omega}_z$ | 0.0 | 0.0 | 2.7 | 0.0 | -2.7 | 0.0 | + +#+begin_src matlab +%% Top Accelerometers +Opt = [-0.1, 0, -0.150; + -0.1, 0, -0.150; + 0.05, 0.075, -0.150; + 0.05, 0.075, -0.150; + 0.05, -0.075, -0.150; + 0.05, -0.075, -0.150]'; + +Ost = [0, 1, 0; + 0, 0, 1; + 1, 0, 0; + 0, 0, 1; + 1, 0, 0; + 0, 0, 1;]'; + +Jt = zeros(length(Opt), 6); + +for i = 1:length(Opt) + Ri = [0, Opt(3,i), -Opt(2,i); + -Opt(3,i), 0, Opt(1,i); + Opt(2,i), -Opt(1,i), 0]; + Jt(i, 1:3) = Ost(:,i)'; + Jt(i, 4:6) = Ost(:,i)'*Ri; +end + +Jtinv = inv(Jt); +#+end_src + +#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) +data2orgtable(Jtinv, {'$\dot{x}_x$', '$\dot{x}_y$', '$\dot{x}_z$', '$\dot{\omega}_x$', '$\dot{\omega}_y$', '$\dot{\omega}_z$'}, {'$b_1$', '$b_2$', '$b_3$', '$b_4$', '$b_5$', '$b_6$'}, ' %.1f '); +#+end_src + +#+RESULTS: +| | $b_1$ | $b_2$ | $b_3$ | $b_4$ | $b_5$ | $b_6$ | +|------------------+-------+-------+-------+-------+-------+-------| +| $\dot{x}_x$ | 0.0 | 1.0 | 0.5 | -0.5 | 0.5 | -0.5 | +| $\dot{x}_y$ | 1.0 | 0.0 | -0.7 | -1.0 | 0.7 | 1.0 | +| $\dot{x}_z$ | 0.0 | 0.3 | 0.0 | 0.3 | 0.0 | 0.3 | +| $\dot{\omega}_x$ | 0.0 | 0.0 | 0.0 | 6.7 | 0.0 | -6.7 | +| $\dot{\omega}_y$ | 0.0 | 6.7 | 0.0 | -3.3 | 0.0 | -3.3 | +| $\dot{\omega}_z$ | 0.0 | 0.0 | -6.7 | 0.0 | 6.7 | 0.0 | + +**** Using =linearize= function + +#+begin_src matlab +acc_3d.type = 2; % 1: inertial mass, 2: perfect + +%% Name of the Simulink File +mdl = 'vibration_table'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/F_shaker'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/acc'], 1, 'openoutput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/acc_top'], 1, 'openoutput'); io_i = io_i + 1; + +%% Run the linearization +G = linearize(mdl, io); +G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; +G.OutputName = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6', ... + 'b1', 'b2', 'b3', 'b4', 'b5', 'b6'}; +#+end_src + +#+begin_src matlab +Gb = Jbinv*G({'a1', 'a2', 'a3', 'a4', 'a5', 'a6'}, :); +Gt = Jtinv*G({'b1', 'b2', 'b3', 'b4', 'b5', 'b6'}, :); +#+end_src + +#+begin_src matlab +T = inv(Gb)*Gt; +T = minreal(T); +T = prescale(T, {2*pi*0.1, 2*pi*1e3}); +#+end_src + +#+begin_src matlab :exports none +freqs = logspace(0, 3, 1000); + +figure; +hold on; +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(T(i, i), freqs, 'Hz')))); +end +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(T(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Transmissibility'); +ylim([1e-4, 1e2]); +xlim([freqs(1), freqs(end)]); +#+end_src + +*** Comparison with "true" transmissibility + +#+begin_src matlab +%% Name of the Simulink File +mdl = 'test_transmissibility'; + +%% Input/Output definition +clear io; io_i = 1; +io(io_i) = linio([mdl, '/d'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/acc'], 1, 'openoutput'); io_i = io_i + 1; + +%% Run the linearization +G = linearize(mdl, io); +G.InputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +G.OutputName = {'Ax', 'Ay', 'Az', 'Bx', 'By', 'Bz'}; +#+end_src + +#+begin_src matlab +Tp = G/s^2; +#+end_src + +#+begin_src matlab :exports none +freqs = logspace(0, 3, 1000); + +figure; +hold on; +for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Tp(i, i), freqs, 'Hz')))); +end +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Tp(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Transmissibility'); +ylim([1e-4, 1e2]); +xlim([freqs(1), freqs(end)]); +#+end_src + +*** Rigidification of the added payloads +- [ ] figure + +#+begin_src matlab +%% Load Identification Data +meas_added_mass = {}; + +for i_strut = 1:6 + meas_added_mass(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_1m_solid.mat', i_strut), 't', 'Va', 'Vs', 'de')}; +end +#+end_src + +Finally the $6 \times 6$ transfer function matrices from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ and from $\bm{u}$ to $\bm{\tau}_m$ are identified: +#+begin_src matlab +%% DVF Plant (transfer function from u to dLm) + +G_de = zeros(length(f), 6, 6); +for i_strut = 1:6 + G_de(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.de, win, Noverlap, Nfft, 1/Ts); +end + +%% IFF Plant (transfer function from u to taum) +G_Vs = zeros(length(f), 6, 6); +for i_strut = 1:6 + G_Vs(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.Vs, win, Noverlap, Nfft, 1/Ts); +end +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm - Several payloads +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +% Diagonal terms +for i = 1:6 + plot(frf_ol.f, abs(frf_ol.G_de{1}(:,i, i)), 'color', colors(1,:)); + plot(f, abs(G_de(:,i, i)), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +ylim([1e-8, 1e-3]); +xlim([20, 2e3]); +#+end_src + +#+begin_src matlab :exports none +%% Bode plot for the transfer function from u to dLm +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +for i = 1:6 + plot(frf_ol.f, abs(frf_ol.G_de(:,i, i)), 'color', colors(1,:)); + plot(f, abs(G_de(:,i, i)), 'color', colors(2,:)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +ylim([1e-8, 1e-3]); +xlim([10, 1e3]); +#+end_src + + +*** Table with signals + +#+name: tab:list_signals +#+caption: List of signals +#+attr_latex: :environment tabularx :width \linewidth :align Xllll +#+attr_latex: :center t :booktabs t +| | *Unit* | *Matlab* | *Vector* | *Elements* | +|------------------------------------+-----------+-----------+-----------------------+----------------------| +| Control Input (wanted DAC voltage) | =[V]= | =u= | $\bm{u}$ | $u_i$ | +| DAC Output Voltage | =[V]= | =u= | $\tilde{\bm{u}}$ | $\tilde{u}_i$ | +| PD200 Output Voltage | =[V]= | =ua= | $\bm{u}_a$ | $u_{a,i}$ | +| Actuator applied force | =[N]= | =tau= | $\bm{\tau}$ | $\tau_i$ | +|------------------------------------+-----------+-----------+-----------------------+----------------------| +| Strut motion | =[m]= | =dL= | $d\bm{\mathcal{L}}$ | $d\mathcal{L}_i$ | +| Encoder measured displacement | =[m]= | =dLm= | $d\bm{\mathcal{L}}_m$ | $d\mathcal{L}_{m,i}$ | +|------------------------------------+-----------+-----------+-----------------------+----------------------| +| Force Sensor strain | =[m]= | =epsilon= | $\bm{\epsilon}$ | $\epsilon_i$ | +| Force Sensor Generated Voltage | =[V]= | =taum= | $\tilde{\bm{\tau}}_m$ | $\tilde{\tau}_{m,i}$ | +| Measured Generated Voltage | =[V]= | =taum= | $\bm{\tau}_m$ | $\tau_{m,i}$ | +|------------------------------------+-----------+-----------+-----------------------+----------------------| +| Motion of the top platform | =[m,rad]= | =dX= | $d\bm{\mathcal{X}}$ | $d\mathcal{X}_i$ | +| Metrology measured displacement | =[m,rad]= | =dXm= | $d\bm{\mathcal{X}}_m$ | $d\mathcal{X}_{m,i}$ | + +*** RGA + + +The RGA-number, which is a measure of the interaction in the system, is computed for the transfer function matrix from $\mathbf{u}$ to $d\mathbf{\mathcal{L}}_m$ for all the payloads. +The obtained numbers are compared in Figure ref:fig:test_nhexa_rga_num_ol_masses. + +#+begin_src matlab :exports none +%% Decentralized RGA - Undamped Plant +RGA_num = zeros(length(frf_ol.f), 4); +for i_mass = [0:3] + for i = 1:length(frf_ol.f) + RGA_num(i, i_mass+1) = sum(sum(abs(eye(6) - squeeze(frf_ol.G_de{i_mass+1}(i,:,:)).*inv(squeeze(frf_ol.G_de{i_mass+1}(i,:,:))).'))); + end +end +#+end_src + +#+begin_src matlab :exports none +%% RGA for Decentralized plant +figure; +hold on; +for i_mass = [0:3] + plot(frf_ol.f, RGA_num(:,i_mass+1), '-', 'color', colors(i_mass+1,:), ... + 'DisplayName', sprintf('RGA-num - %i mass', i_mass)); +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +#+end_src + +#+begin_src matlab :exports none +%% Decentralized RGA - Undamped Plant +RGA = zeros(length(frf_ol.f), 6, 6); +for i = 1:length(frf_ol.f) +RGA(i, :, :) = squeeze(frf_ol.G_Vs{1}(i,:,:)).*inv(squeeze(frf_ol.G_Vs{1}(i,:,:))).'; +end +#+end_src + +#+begin_src matlab :exports none +%% RGA +figure; +hold on; +for i = 1:6 + plot(frf_ol.f, abs(RGA(:,i,i)), 'k-') +end +for i = 1:5 + for j = i+1:6 + plot(frf_ol.f, abs(RGA(:,i,j)), 'r-') + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('RGA Number'); +xlim([10, 1e3]); ylim([1e-2, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/rga_num_ol_masses.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:test_nhexa_rga_num_ol_masses +#+caption: RGA-number for the open-loop transfer function from $\mathbf{u}$ to $d\mathbf{\mathcal{L}}_m$ +#+RESULTS: +[[file:figs/test_nhexa_rga_num_ol_masses.png]] + +#+begin_important +From Figure ref:fig:test_nhexa_rga_num_ol_masses, it is clear that the coupling is quite large starting from the first suspension mode of the nano-hexapod. +Therefore, is the payload's mass is increase, the coupling in the system start to become unacceptably large at lower frequencies. +#+end_important diff --git a/figs/test_nhexa_nano_hexapod_signals.pdf b/figs/test_nhexa_nano_hexapod_signals.pdf index cc42f73..9d7056e 100644 Binary files a/figs/test_nhexa_nano_hexapod_signals.pdf and b/figs/test_nhexa_nano_hexapod_signals.pdf differ diff --git a/figs/test_nhexa_nano_hexapod_signals.png b/figs/test_nhexa_nano_hexapod_signals.png index 54f3cc8..acb9345 100644 Binary files a/figs/test_nhexa_nano_hexapod_signals.png and b/figs/test_nhexa_nano_hexapod_signals.png differ diff --git a/figs/test_nhexa_nano_hexapod_signals.svg b/figs/test_nhexa_nano_hexapod_signals.svg index 54eb324..0f32c9b 100644 --- a/figs/test_nhexa_nano_hexapod_signals.svg +++ b/figs/test_nhexa_nano_hexapod_signals.svg @@ -114,6 +114,9 @@ + + + @@ -132,15 +135,12 @@ - - - - + - + @@ -336,13 +336,13 @@ - + - + @@ -350,13 +350,13 @@ - + - + @@ -407,7 +407,7 @@ - + @@ -416,7 +416,7 @@ - + @@ -430,7 +430,7 @@ - + @@ -439,7 +439,7 @@ - + diff --git a/test-bench-nano-hexapod.org b/test-bench-nano-hexapod.org index d9effe6..6425a15 100644 --- a/test-bench-nano-hexapod.org +++ b/test-bench-nano-hexapod.org @@ -553,7 +553,7 @@ The effect of the payload mass on the dynamics is discussed in Section ref:ssec: \draw[->] ($(F_DAC.west)+(-0.8,0)$) node[above right]{$\mathbf{u}$} node[below right]{$[V]$} -- node[sloped]{$/$} (F_DAC.west); \draw[->] (F_DAC.east) -- node[midway, above]{$\tilde{\mathbf{u}}$}node[midway, below]{$[V]$} (PD200.west); \draw[->] (PD200.east) -- node[midway, above]{$\mathbf{V}_a$}node[midway, below]{$[V]$} (F_stack.west); - \draw[->] (F_stack.east) -- (inputF) node[above left]{$\mathbf{\tau}$}node[below left]{$[N]$}; + \draw[->] (F_stack.east) -- (inputF) node[above left]{$\mathbf{f}$}node[below left]{$[N]$}; \draw[->] (outputF) -- (Fm_stack.west) node[above left]{$\mathbf{\epsilon}$} node[below left]{$[m]$}; \draw[->] (Fm_stack.east) -- node[midway, above]{$\tilde{\mathbf{V}}_s$}node[midway, below]{$[V]$} (Fm_ADC.west); @@ -1011,7 +1011,7 @@ xlim([10, 2e3]); exportFig('figs/test_nhexa_identified_frf_Vs_masses.pdf', 'width', 'half', 'height', 600); #+end_src -#+name: fig:test_struts_mounting +#+name: fig:test_nhexa_identified_frf_masses #+caption: Measured Frequency Response Functions from $u_i$ to $d_{ei}$ (\subref{fig:test_nhexa_identified_frf_de_masses}) and from $u_i$ to $V_{si}$ (\subref{fig:test_nhexa_identified_frf_Vs_masses}) for all 4 payload conditions. Only diagonal terms are shown. #+attr_latex: :options [htbp] #+begin_figure diff --git a/test-bench-nano-hexapod.pdf b/test-bench-nano-hexapod.pdf index 52e33e5..6c40ce6 100644 Binary files a/test-bench-nano-hexapod.pdf and b/test-bench-nano-hexapod.pdf differ