#+TITLE: Nano-Hexapod #+SETUPFILE: ./setup/org-setup-file.org * Introduction :ignore: * Nano-Hexapod ** Introduction :ignore: ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no simulinkproject('../'); #+end_src #+begin_src matlab :eval no addpath('nano_hexapod'); open('nano_hexapod/nano_hexapod.slx') #+end_src #+begin_src matlab :tangle no addpath('matlab/nano_hexapod'); open('matlab/nano_hexapod/nano_hexapod.slx') #+end_src ** Nano Hexapod - Configuration <> The nano-hexapod can be initialized and configured using the =initializeNanoHexapodFinal= function ([[sec:initializeNanoHexapodFinal][link]]). #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof', ... 'MO_B', 150e-3); #+end_src We initialize the identification parameters. #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'nano_hexapod'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs #+end_src ** Effect of encoders on the decentralized plant <> We here wish to compare the plant from actuators to the encoders when the encoders are either fixed on the struts or on the plates. Identify the plant when the encoders are on the struts: #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); Gs = linearize(mdl, io, 0.0, options); Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; #+end_src And identify the plant when the encoders are fixed on the plates: #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'plates', ... 'actuator_type', '2dof'); Gp = linearize(mdl, io, 0.0, options); Gp.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; Gp.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; #+end_src The obtained plants are compared in Figure [[fig:nano_hexapod_effect_encoder]]. #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gs('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', 'Struts'); for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ... 'HandleVisibility', 'off'); end set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(Gp('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', 'Plates'); for i = 1:6 set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(Gp(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); legend('location', 'northeast'); ylim([1e-11, 1e-6]); ax2 = nexttile; hold on; for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-'); end for i = 1:6 set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(Gp(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_effect_encoder.pdf', 'width', 'full', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_effect_encoder #+caption: Comparison of the plants from actuator to associated encoder when the encoders are either fixed to the struts or to the plates #+RESULTS: [[file:figs/nano_hexapod_effect_encoder.png]] #+begin_question Why do we have zeros at 400Hz and 800Hz when the encoders are fixed on the struts? #+end_question ** Effect of APA flexibility <> First identify the plant for APA represented by 2DoF system: #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof', ... 'actuator_Ga', 2); Gs = linearize(mdl, io, 0.0, options); Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; #+end_src First identify the plant for APA represented by a flexible element: #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', 'flexible'); Gf = linearize(mdl, io, 0.0, options); Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; Gf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; #+end_src #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gs('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', '2DoF'); for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ... 'HandleVisibility', 'off'); end set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(Gf('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', 'Flexible'); for i = 1:6 set(gca,'ColorOrderIndex',2); plot(freqs, abs(squeeze(freqresp(Gf(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); legend('location', 'northeast'); ylim([1e-11, 1e-6]); ax2 = nexttile; hold on; for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-'); end for i = 1:6 set(gca,'ColorOrderIndex',2); plot(freqs, 180/pi*angle(squeeze(freqresp(Gf(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_effect_flexible_apa.pdf', 'width', 'full', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_effect_flexible_apa #+caption: Comparison of the plants from actuator to associated strut encoder when the APA are modelled with a 2DoF system of with a flexible one #+RESULTS: [[file:figs/nano_hexapod_effect_flexible_apa.png]] #+begin_important The first resonance is strange when using the flexible APA model (Figure [[fig:nano_hexapod_effect_flexible_apa]]). Otherwise, the 2DoF model matches quite well the flexible model considering its simplicity. #+end_important ** Nano Hexapod - Number of DoF <> In this section, we wish to see how the configuration of each element changes the number of the states of the obtained system. The most minimalist model is the following: #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ... 'flex_top_type', '2dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); G = linearize(mdl, io, 0.0, options); #+end_src #+begin_src matlab :results value replace :exports results sprintf('There are %i states.', length(G.statename)) #+end_src #+RESULTS: : There are 24 states. These states are summarized on table [[tab:num_states_nano_hexapod]]. #+name: tab:num_states_nano_hexapod #+caption: Number of states for the minimalist model #+attr_latex: :environment tabularx :width 0.3\linewidth :align cc #+attr_latex: :center t :booktabs t :float t | Element | States | |-----------+--------| | Struts | 2*6 | | Top Plate | 12 | |-----------+--------| | Total: | 24 | #+TBLFM: @>$2=vsum(@I..@II) If we add axial stiffness on the top joints, we should add 2 states for each struts. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '2dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); G = linearize(mdl, io, 0.0, options); #+end_src #+begin_src matlab :results value replace :exports results sprintf('There are %i states.', length(G.statename)) #+end_src #+RESULTS: : There are 36 states. If we add torsional stiffness on the bottom joints, we should again add 2 states for each struts. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); G = linearize(mdl, io, 0.0, options); #+end_src #+begin_src matlab :results value replace :exports results sprintf('There are %i states.', length(G.statename)) #+end_src #+RESULTS: : There are 48 states. Finally, if we add axial stiffness on the bottom joint, we should add 2 states for each struts. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '4dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); G = linearize(mdl, io, 0.0, options); #+end_src #+begin_src matlab :results value replace :exports results sprintf('There are %i states.', length(G.statename)) #+end_src #+RESULTS: : There are 60 states. #+begin_important Obtained number of states is very comprehensible. Depending on the physical effects we want to model, we therefore know how many states are added when configuring the model. #+end_important ** Direct Velocity Feedback Plant <> The transfer function from actuator forces $\tau_i$ to the encoder measurements $\mathcal{L}_i$ is now identified both when the encoders are fixed to the struts. #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'nano_hexapod'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); Gdvf = linearize(mdl, io, 0.0, options); #+end_src The DC gain from actuator to relative motion sensor should be equal to (for the 2dof APA): \[ \frac{1}{k + k_a + kk_a/k_e} \] Which is equal to: #+begin_src matlab :results value replace :exports results :tangle no sprintf('DCgain = %.2e [m/N]', 1/(n_hexapod.actuator.k(1) + n_hexapod.actuator.ka(1) + n_hexapod.actuator.k(1)*n_hexapod.actuator.ka(1)/n_hexapod.actuator.ke(1))) #+end_src #+RESULTS: : DCgain = 1.87e-08 [m/N] Let's verify that by looking at the DC gain of the $6 \times 6$ DVF plant in Table [[tab:dvf_dc_gain]]. #+begin_src matlab :results value replace :exports results :tangle no dcgain(Gdvf) #+end_src #+name: tab:dvf_dc_gain #+caption: DC gain of the DVF plant #+attr_latex: :environment tabularx :width 0.9\linewidth :align cccccc #+attr_latex: :center t :booktabs t :float t #+RESULTS: | 1.8617e-08 | -1.0408e-10 | 1.3034e-10 | 3.2559e-11 | -1.1188e-10 | 9.0385e-11 | | -5.1839e-11 | 1.8593e-08 | -4.4868e-11 | 8.016e-11 | 4.3558e-11 | -1.1164e-10 | | 5.1963e-12 | -6.9001e-12 | 1.8564e-08 | 3.0844e-11 | 4.0097e-11 | -3.4387e-11 | | 1.9359e-11 | 1.7432e-10 | -5.0928e-11 | 1.855e-08 | 1.6406e-10 | 4.5757e-12 | | -2.1185e-11 | 2.1724e-10 | 1.5333e-12 | -8.802e-11 | 1.8803e-08 | -4.6946e-11 | | -1.1728e-11 | -5.7682e-11 | 1.6213e-10 | 2.1934e-12 | -1.6237e-10 | 1.8715e-08 | And the bode plot of the DVF plant is shown in Figure [[fig:nano_hexapod_struts_2dof_dvf_plant]]. #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ... 'DisplayName', '$\mathcal{L}_{i}/\tau_i$') for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ... 'HandleVisibility', 'off'); end % Off diagonal terms plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$F_{m,i}/\tau_j \quad i \neq j$'); for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); legend('location', 'southeast'); ax2 = nexttile; hold on; for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_struts_2dof_dvf_plant.pdf', 'width', 'full', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_struts_2dof_dvf_plant #+caption: Bode plot of the transfer functions from actuator forces $\tau_i$ to relative motion sensors attached to the struts $\mathcal{L}_i$. Diagonal terms are shown in blue, and off-diagonal terms in black. #+RESULTS: [[file:figs/nano_hexapod_struts_2dof_dvf_plant.png]] ** Integral Force Feedback Plant <> The transfer function from actuators to force sensors is identified. #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'nano_hexapod'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); Giff = linearize(mdl, io, 0.0, options); #+end_src This is corresponding to the dynamics for the Integral Force Feedback (IFF) control strategy. The bode plot is shown in Figure [[fig:nano_hexapod_struts_2dof_iff_plant]]. #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ... 'DisplayName', '$F_{m,i}/\tau_i$') for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ... 'HandleVisibility', 'off'); end % Off diagonal terms plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', '$F_{m,i}/\tau_j \quad i \neq j$'); for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); legend('location', 'southeast'); ax2 = nexttile; hold on; for i = 1:6 set(gca,'ColorOrderIndex',1); plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_struts_2dof_iff_plant.pdf', 'width', 'full', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_struts_2dof_iff_plant #+caption: Bode plot of the transfer functions from actuator forces $\tau_i$ to force sensors $F_{m,i}$. Diagonal terms are shown in blue, and off-diagonal terms in black. #+RESULTS: [[file:figs/nano_hexapod_struts_2dof_iff_plant.png]] ** Decentralized Plant - Cartesian coordinates <> *** Introduction :ignore: Consider the plant shown in Figure [[fig:nano_hexapod_decentralized_schematic]] with: - $\tau$ the 6 input forces (APA) - $d\mathcal{L}$ the relative motion sensor outputs (encoders) - $\mathcal{X}$ the motion of the top platform measured with "perfect" 6-dof sensor - $J_a$ and $J_s$ the Jacobians for the actuators and sensors #+begin_src latex :file nano_hexapod_decentralized_schematic.pdf \begin{tikzpicture} % Blocs \node[block={2.0cm}{2.0cm}] (P) {Plant}; \coordinate[] (inputF) at (P.west); \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$); \coordinate[] (outputX) at ($(P.south east)!0.2!(P.north east)$); \node[block, left= of inputF] (Ja) {$\bm{J}^{-T}_a$}; \node[block, right= of outputL] (Js) {$\bm{J}^{-1}_s$}; % Connections and labels \draw[->] ($(Ja.west)+(-1,0)$) -- (Ja.west) node[above left]{$\bm{\mathcal{F}}$}; \draw[->] (Ja.east) -- (inputF) node[above left]{$\bm{\tau}$}; \draw[->] (outputL) -- (Js.west) node[above left]{$d\bm{\mathcal{L}}$}; \draw[->] (Js.east) -- ++(1, 0) node[above left]{$d\bm{\mathcal{X}}$}; \draw[->] (outputX.east) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$}; \end{tikzpicture} #+end_src #+name: fig:nano_hexapod_decentralized_schematic #+caption: Plant in the cartesian Frame #+RESULTS: [[file:figs/nano_hexapod_decentralized_schematic.png]] *** Verification of the Sensor Jacobian The "perfect" sensor output $\mathcal{X}$ is used to verify that the sensor Jacobian is working correctly both when the encoders are fixed to the struts and to the plates. Let's then identify the plant for both configuration, and compare the transfer functions from $\mathcal{F}$ to $d\mathcal{X}$ and to $\mathcal{X}$. #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'nano_hexapod'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs #+end_src Start when the encoders are fixed on the struts. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof', ... 'MO_B', 150e-3); Gs = linearize(mdl, io, 0.0, options); Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ... 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; % Cartesian plant using the Jacobians Gsc = inv(n_hexapod.geometry.Js)*Gs({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)'; % Cartesian plant using the perfect sensor Gsp = -Gs({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)'; #+end_src The diagonal elements of the plant are shown in Figure [[fig:nano_hexapod_comp_cartesian_plants_struts]]. #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ... '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-', ... 'DisplayName', labels{i}); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gsp(i,i), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); legend('location', 'southwest'); ylim([1e-11, 1e-4]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gsp(i,i), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_comp_cartesian_plants_struts.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_comp_cartesian_plants_struts #+caption: Bode plot of the diagonal elements of the decentralized (cartesian) plant when using the sensor Jacobian (solid) and when using "perfect" 6dof sensor (dashed). The encoders are fixed on the struts. #+RESULTS: [[file:figs/nano_hexapod_comp_cartesian_plants_struts.png]] The same if performed when the encoders are fixed to the plates. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'plates', ... 'actuator_type', '2dof', ... 'MO_B', 150e-3); Gp = linearize(mdl, io, 0.0, options); Gp.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; Gp.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ... 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; % Cartesian plant using the Jacobians Gpc = inv(n_hexapod.geometry.Js)*Gp({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)'; % Cartesian plant using the perfect sensor Gpp = -Gp({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)'; #+end_src The obtained bode plots are shown in Figure [[fig:nano_hexapod_comp_cartesian_plants_plates]]. #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ... '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '-', ... 'DisplayName', labels{i}); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gpp(i,i), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); legend('location', 'southwest'); ylim([1e-11, 1e-4]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gpp(i,i), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_comp_cartesian_plants_plates.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_comp_cartesian_plants_plates #+caption: Bode plot of the diagonal elements of the decentralized (cartesian) plant when using the sensor Jacobian (solid) and when using "perfect" 6dof sensor (dashed). The encoders are fixed on the plates. #+RESULTS: [[file:figs/nano_hexapod_comp_cartesian_plants_plates.png]] #+begin_important The Jacobian for the encoders is working properly both when the encoders are fixed to the plates or to the struts. However, then the encoders are fixed to the struts, there is a mismatch between the estimated motion and the measured motion above 100Hz due to a complex conjugate zero. #+end_important *** Comparison of the decentralized plants The decentralized plants are now compared whether the encoders are fixed on the struts or on the plates in Figure [[fig:nano_hexapod_cartesian_plant_encoder_comp]]. #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ... '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-', ... 'DisplayName', labels{i}); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '--', ... 'HandleVisibility', 'off'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); legend('location', 'southwest'); ylim([1e-11, 1e-4]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_cartesian_plant_encoder_comp.pdf', 'width', 'full', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_cartesian_plant_encoder_comp #+caption: Bode plot of the "cartesian" plant (transfer function from $\mathcal{F}$ to $d\mathcal{X}$) when the encoders are fixed on the struts (solid) and on the plates (dashed) #+RESULTS: [[file:figs/nano_hexapod_cartesian_plant_encoder_comp.png]] ** Decentralized Plant - Decoupling at the Center of Stiffness <> *** Center of Stiffness <> Let's define some parameters: #+begin_src matlab si = n_hexapod.geometry.si; % Orientation of struts bi = n_hexapod.geometry.Fb; % Location of bi w.r.t. {F} ki = ones(1,6); % Normalized strut stiffness #+end_src In order to find is the Center of Stiffness (CoK) exists, we have to verify is the following is diagonal: #+begin_src matlab :results value replace :exports both ki.*si*si' #+end_src #+RESULTS: | 1.8977 | 2.4659e-17 | 5.1838e-19 | | 2.4659e-17 | 1.8977 | -2.3143e-05 | | 5.1838e-19 | -2.3143e-05 | 2.2046 | And we can find the location of the CoK with respect to {F}: #+begin_src matlab OkX = (ki.*cross(bi, si)*si')/(ki.*si*si'); Ok = [OkX(3,2);OkX(1,3);OkX(2,1)] #+end_src #+begin_src matlab :results value replace :exports results :tangle no ans = Ok #+end_src #+RESULTS: | -1.7444e-18 | | 2.1511e-06 | | 0.052707 | The center of the cube is therefore 52.7mm above the bottom platform {F} frame. Let's initialize the hexapod with frame {A} and {B} at the CoK: #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof', ... 'MO_B', Ok(3)-95e-3); #+end_src And the (normalized) stiffness matrix is computed as follows: #+begin_src matlab :results value replace n_hexapod.geometry.J'*diag(ki)*n_hexapod.geometry.J #+end_src #+name: tab:stiffness_matrix_cok #+caption: Normalized Stiffness Matrix - Center of Stiffness #+attr_latex: :environment tabularx :width 0.9\linewidth :align cccccc #+attr_latex: :center t :booktabs t :float t #+RESULTS: | 1.8977 | 0 | 0 | 0 | -2.0817e-17 | -1.5311e-06 | | 0 | 1.8977 | -2.3143e-05 | 4.175e-06 | 0 | 0 | | 0 | -2.3143e-05 | 2.2046 | 4.7422e-06 | 0 | 0 | | 0 | 4.175e-06 | 4.7422e-06 | 0.012594 | 2.1684e-19 | -8.6736e-19 | | -1.8521e-17 | 0 | 0 | 0 | 0.012594 | -9.3183e-08 | | -1.5311e-06 | -6.9389e-18 | 2.7756e-17 | -8.6736e-19 | -9.3183e-08 | 0.043362 | And we indeed obtain a diagonal stiffness matrix. *** Obtained plant #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'nano_hexapod'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs #+end_src #+begin_src matlab G = linearize(mdl, io, 0.0, options); G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ... 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; % % Cartesian plant using the Jacobians % Gsc = inv(n_hexapod.geometry.Js)*Gs({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)'; % % Cartesian plant using the perfect sensor % Gsp = -Gs({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)'; #+end_src #+begin_src matlab Gc = inv(n_hexapod.geometry.J)*G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J'); #+end_src #+begin_src matlab :results value replace :exports results :tangle no dcgain(Gc) #+end_src #+RESULTS: | 9.8602e-09 | 7.8692e-11 | 1.4426e-11 | -2.663e-10 | 2.8e-10 | -4.7559e-11 | | 8.457e-11 | 9.8788e-09 | -2.4002e-11 | -2.9502e-11 | -1.3262e-10 | -8.7346e-11 | | -4.3244e-11 | 2.4075e-13 | 8.4775e-09 | 1.1442e-11 | -2.5809e-10 | 2.8796e-11 | | -1.8326e-09 | -9.318e-10 | 6.8188e-10 | 1.4697e-06 | 5.5936e-09 | 8.7632e-10 | | 4.6906e-10 | 1.5911e-09 | 1.6989e-10 | -5.223e-09 | 1.4729e-06 | -2.6059e-10 | | -6.5754e-11 | -3.0408e-12 | 5.394e-11 | -1.0917e-10 | 6.9479e-10 | 4.2979e-07 | As the rotations and translations have very different gains, we normalize each motion to one. #+begin_src matlab Gc = diag(1./diag(dcgain(Gc)))*Gc; #+end_src The diagonal and off-diagonal elements are shown in Figure [[fig:nano_hexapod_diagonal_plant_cok]], and we can see good decoupling at low frequency. #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ... '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; figure; hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gc(i,i), freqs, 'Hz'))), '-', ... 'DisplayName', labels{i}); end plot(freqs, abs(squeeze(freqresp(Gc(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'DisplayName', 'Off-Diagonal'); for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... 'HandleVisibility', 'off'); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('Amplitude'); xlim([freqs(1), freqs(end)]); ylim([1e-8, 1e3]) legend('location', 'southwest'); #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/nano_hexapod_diagonal_plant_cok.pdf', 'width', 'wide', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_diagonal_plant_cok #+caption: Diagonal and off-diagonal elements of the (normalized) decentralized plant with the Jacobians estimated at the "center of stiffness" #+RESULTS: [[file:figs/nano_hexapod_diagonal_plant_cok.png]] #+begin_important The Jacobian matrices can be used to decoupled the plant at low frequency. #+end_important ** Stiffness matrix *** Introduction :ignore: The stiffness matrix of the nano-hexapod describes its induced static displacement/rotation when a force/torque is applied on its top platform. The location of the applied force/torque and the expressed displacement/rotation can be defined as wanted. Such location (or frame) is then used for the computation of the Jacobian which in turns is used to compute the stiffness matrix. *** Compute the theoretical stiffness of the nano-hexapod Neglecting stiffness of the joints, we have: \[ K = J^t \mathcal{K} J \] where $\mathcal{K}$ is a diagonal 6x6 matrix with axial stiffness of the struts on the diagonal. Let's note the axial stiffness of the APA: \[ k_{\text{APA}} = k + \frac{k_e k_a}{k_e + k_a} \] Them axial stiffness of the struts $k_s$: \[ k_s = \frac{k_z k_{\text{APA}}}{k_z + 2 k_{\text{APA}}} \] with $k_z$ the axial stiffness of the flexible joints. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '4dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); #+end_src #+begin_src matlab k = n_hexapod.actuator.k(1); ke = n_hexapod.actuator.ke(1); ka = n_hexapod.actuator.ka(1); kz = n_hexapod.flex_top.kz(1); #+end_src #+begin_src matlab kAPA = k + ke*ka/(ke + ka); #+end_src #+begin_src matlab :results value replace :exports results :tangle no sprintf('kAPA = %.3e [N/m]', kAPA) #+end_src #+RESULTS: : kAPA = 1.799e+06 [N/m] #+begin_src matlab ks = kz*kAPA/(kz + 2*kAPA); #+end_src #+begin_src matlab :results value replace :exports results :tangle no sprintf('ks = %.3e [N/m]', ks) #+end_src #+RESULTS: : ks = 1.737e+06 [N/m] We can see that the axial stiffness of the flexible joint as little impact on the total axial stiffness of the struts. Let's now compute the stiffness matrix corresponding to an hexapod with perfect joints and the above computed axial stiffness: #+begin_src matlab Ks = n_hexapod.geometry.J'*(ks*eye(6))*n_hexapod.geometry.J; #+end_src And the compliance matrix can be computed as the inverse of the stiffness matrix: #+begin_src matlab C = inv(Ks); #+end_src #+begin_src matlab :results value replace :exports results :tangle no ans = C #+end_src #+name: tab:compliance_matrix_perfect_joints #+caption: Compliance Matrix - Perfect Joints #+attr_latex: :environment tabularx :width 0.9\linewidth :align cccccc #+attr_latex: :center t :booktabs t :float t #+RESULTS: | 1.9938e-06 | -2.3138e-22 | 3.3403e-23 | 1.0202e-21 | 8.7906e-06 | 2.9603e-11 | | -3.1875e-23 | 1.9938e-06 | 2.2094e-11 | -8.7909e-06 | -1.6576e-22 | -3.5622e-28 | | 6.6811e-23 | 2.2094e-11 | 2.6115e-07 | -9.8337e-11 | 3.4744e-22 | 7.4663e-28 | | 1.4054e-22 | -8.7909e-06 | -9.8337e-11 | 4.5715e-05 | 7.3086e-22 | 1.5706e-27 | | 8.7906e-06 | -1.0202e-21 | 1.7371e-22 | 4.498e-21 | 4.5714e-05 | 9.8237e-11 | | 2.9603e-11 | -1.9261e-22 | -1.7611e-27 | 8.4925e-22 | 9.8237e-11 | 1.3277e-05 | *** Comparison with Simscape Model #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'nano_hexapod'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; G = linearize(mdl, io, 0.0, options); #+end_src #+begin_src matlab :results value replace :exports results :tangle no dcgain(G) #+end_src #+name: tab:compliance_matrix_simscape_joints #+caption: Compliance Matrix - Estimated from Simscape #+attr_latex: :environment tabularx :width 0.9\linewidth :align cccccc #+attr_latex: :center t :booktabs t :float t #+RESULTS: | 1.9863e-06 | 2.4683e-08 | 2.2859e-09 | -6.4768e-08 | 8.7307e-06 | 3.2754e-10 | | -3.693e-09 | 1.9663e-06 | -7.4503e-09 | -8.615e-06 | -6.7926e-08 | -2.0193e-08 | | -3.5525e-09 | -8.8671e-10 | 2.6042e-07 | 4.7504e-09 | -9.9677e-09 | 1.7242e-10 | | 2.1133e-08 | -8.6554e-06 | 3.2841e-08 | 4.4849e-05 | 3.0873e-07 | 8.1525e-08 | | 8.7375e-06 | 9.9165e-08 | 8.135e-09 | -2.5208e-07 | 4.5331e-05 | 3.0602e-09 | | 6.1611e-09 | 6.1733e-09 | 2.6778e-09 | -3.3188e-08 | 3.3887e-08 | 1.3212e-05 | * To-order :noexport: ** Verify why unstable strut #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ... 'flex_top_type', '2dof', ... 'motion_sensor_type', 'plates', ... 'actuator_type', 'flexible'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 1 #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ... 'flex_top_type', '2dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', 'flexible'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 0 #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'plates', ... 'actuator_type', 'flexible'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 0 #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', 'flexible'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 0 ** Verify why unstable strut #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ... 'flex_top_type', '2dof', ... 'motion_sensor_type', 'plates', ... 'actuator_type', '2dof'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 1 #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ... 'flex_top_type', '2dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 1 #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'plates', ... 'actuator_type', '2dof'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 1 #+begin_src matlab :results value replace n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); isstable(linearize(mdl, io, 0.0, options)) #+end_src #+RESULTS: : 1 ** APA300ML structure #+begin_src matlab open('matlab/nano_hexapod/apa300ml.slx') #+end_src #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml_structure'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'F', 'Fa'}; G.OutputName = {'D', 'x'}; #+end_src #+begin_src matlab dcgain(G('D', 'Fa'))/dcgain(G('x', 'Fa')) #+end_src #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(G('x', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(G('x', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(G('x', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(G('x', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src ** Full APA300ML *** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no simulinkproject('../'); #+end_src *** APA #+begin_src matlab K = readmatrix('APA300ML_full_mat_K.CSV'); M = readmatrix('APA300ML_full_mat_M.CSV'); [int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA300ML_full_out_nodes_3D.txt'); #+end_src #+begin_src matlab open('matlab/nano_hexapod/apa300ml.slx') #+end_src *** APA Stiffness #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1; %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'Fe'}; G.OutputName = {'L'}; #+end_src #+begin_src matlab :results value replace 1./dcgain(G) #+end_src #+RESULTS: : 1796000.0 Same as documentation *** Amplification Factor #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Ls'], 1, 'openoutput'); io_i = io_i + 1; % io(io_i) = linio([mdl, '/Sensor Stack'], 1, 'openoutput'); io_i = io_i + 1; % io(io_i) = linio([mdl, '/Actuator Stack'], 1, 'openoutput'); io_i = io_i + 1; %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'F'}; G.OutputName = {'L', 'Ls'}; #+end_src #+begin_src matlab :results value replace abs(dcgain(G('L', 'F'))./dcgain(G('Ls', 'F'))) #+end_src #+RESULTS: : 5.1155 Same as estimated. *** Plant Identification #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs io(io_i) = linio([mdl, '/Sensor Stack'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'F'}; G.OutputName = {'L', 'dL'}; #+end_src IFF Plant #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G('dL', 'F'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G('dL', 'F'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src DVF Plant #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G('L', 'F'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G('L', 'F'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src *** Reduction from Full APA to 2DoF system #+begin_src matlab m = 1; % [kg] #+end_src Identification of the full APA model: #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'Fa', 'Fe'}; G.OutputName = {'Fm', 'dL'}; #+end_src #+begin_src matlab :results output replace :exports results :tangle no size(G) #+end_src #+RESULTS: : size(G) : State-space model with 2 outputs, 2 inputs, and 50 states. #+begin_src matlab k = 0.35e6; c = 3e1; ka = 43e6; ke = 1.5e6; Leq = 0.056; % [m] #+end_src Identification of the 2-DoF APA model: #+begin_src matlab open('matlab/nano_hexapod/apa300ml_2dof.slx') %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml_2dof'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor %% Run the linearization Gb = linearize(mdl, io, 0.0, options); Gb.InputName = {'Fa', 'Fe'}; Gb.OutputName = {'Fm', 'dL'}; #+end_src #+begin_src matlab :results output replace :exports results :tangle no size(Gb) #+end_src #+RESULTS: : size(Gb) : State-space model with 2 outputs, 2 inputs, and 4 states. From external force to displacement #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src From external force to force sensor #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src From Actuator to displacement #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src From Actuator to Force Sensor #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src #+begin_src matlab tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('To vertical motion [m]'); title('From Actuator $F_a$ [N]'); ax2 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); title('From External $F_e$ [N]'); ax3 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('To Force Sensor [N]'); ax4 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); linkaxes([ax1,ax2,ax3,ax4],'x'); linkaxes([ax1,ax2],'y'); linkaxes([ax3,ax4],'y'); #+end_src *** Reduction from Full APA to 2DoF system #+begin_src matlab Ms = [0.1, 1, 5, 10]; % [kg] #+end_src Identification of the full APA model: #+begin_src matlab %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'Fa', 'Fe'}; G.OutputName = {'Fm', 'dL'}; #+end_src #+begin_src matlab :results output replace :exports results :tangle no size(G) #+end_src #+RESULTS: : size(G) : State-space model with 2 outputs, 2 inputs, and 50 states. #+begin_src matlab k = 0.35e6; c = 3e1; ka = 43e6; ke = 1.5e6; Leq = 0.056; % [m] #+end_src Identification of the 2-DoF APA model: #+begin_src matlab open('matlab/nano_hexapod/apa300ml_2dof.slx') %% Options for Linearized options = linearizeOptions; options.SampleTime = 0; %% Name of the Simulink File mdl = 'apa300ml_2dof'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor %% Run the linearization Gb = linearize(mdl, io, 0.0, options); Gb.InputName = {'Fa', 'Fe'}; Gb.OutputName = {'Fm', 'dL'}; #+end_src #+begin_src matlab :results output replace :exports results :tangle no size(Gb) #+end_src #+RESULTS: : size(Gb) : State-space model with 2 outputs, 2 inputs, and 4 states. From external force to displacement #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src From external force to force sensor #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src From Actuator to displacement #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src From Actuator to Force Sensor #+begin_src matlab :exports none freqs = logspace(1, 4, 1000); figure; ax1 = subplot(2, 1, 1); hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ax2 = subplot(2, 1, 2); hold on; plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylim([-180, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); #+end_src #+begin_src matlab tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); ylabel('To vertical motion [m]'); title('From Actuator $F_a$ [N]'); ax2 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); title('From External $F_e$ [N]'); ax3 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); ylabel('To Force Sensor [N]'); ax4 = nexttile; hold on; plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz')))); hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); linkaxes([ax1,ax2,ax3,ax4],'x'); linkaxes([ax1,ax2],'y'); linkaxes([ax3,ax4],'y'); #+end_src * Function - Initialize Nano Hexapod :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeNanoHexapodFinal.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> ** Function description :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab function [nano_hexapod] = initializeNanoHexapodFinal(args) #+end_src ** Optional Parameters :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab arguments %% Bottom Flexible Joints args.flex_bot_type char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof' args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad] args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad] args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad] args.flex_bot_kz (6,1) double {mustBeNumeric} = ones(6,1)*1e8 % Axial Stiffness [N/m] args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % X bending Damping [Nm/(rad/s)] args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Y bending Damping [Nm/(rad/s)] args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Torsionnal Damping [Nm/(rad/s)] args.flex_bot_cz (6,1) double {mustBeNumeric} = ones(6,1)*1e2 % Axial Damping [N/(m/s)] %% Top Flexible Joints args.flex_top_type char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof' args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad] args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad] args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad] args.flex_top_kz (6,1) double {mustBeNumeric} = ones(6,1)*1e8 % Axial Stiffness [N/m] args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % X bending Damping [Nm/(rad/s)] args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Y bending Damping [Nm/(rad/s)] args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Torsionnal Damping [Nm/(rad/s)] args.flex_top_cz (6,1) double {mustBeNumeric} = ones(6,1)*1e2 % Axial Damping [N/(m/s)] %% Jacobian - Location of frame {A} and {B} args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m] %% Relative Motion Sensor args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts' %% Actuators args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible' args.actuator_Ga (6,1) double {mustBeNumeric} = ones(6,1)*1 % Actuator gain [N/V] args.actuator_Gs (6,1) double {mustBeNumeric} = ones(6,1)*1 % Sensor gain [V/m] % For 2DoF args.actuator_k (6,1) double {mustBeNumeric} = ones(6,1)*0.35e6 % [N/m] args.actuator_ke (6,1) double {mustBeNumeric} = ones(6,1)*1.5e6 % [N/m] args.actuator_ka (6,1) double {mustBeNumeric} = ones(6,1)*43e6 % [N/m] args.actuator_c (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)] args.actuator_ce (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)] args.actuator_ca (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)] args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m] % For Flexible Frame args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m] args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m] % For Flexible args.actuator_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio end #+end_src ** Nano Hexapod Object :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab nano_hexapod = struct(); #+end_src ** Flexible Joints - Bot :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab nano_hexapod.flex_bot = struct(); switch args.flex_bot_type case '2dof' nano_hexapod.flex_bot.type = 1; case '3dof' nano_hexapod.flex_bot.type = 2; case '4dof' nano_hexapod.flex_bot.type = 3; case 'flexible' nano_hexapod.flex_bot.type = 4; end nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad] nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad] nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad] nano_hexapod.flex_bot.kz = args.flex_bot_kz; % Axial stiffness [N/m] nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)] nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)] nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)] nano_hexapod.flex_bot.cz = args.flex_bot_cz; %[N/(m/s)] #+end_src ** Flexible Joints - Top :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab nano_hexapod.flex_top = struct(); switch args.flex_top_type case '2dof' nano_hexapod.flex_top.type = 1; case '3dof' nano_hexapod.flex_top.type = 2; case '4dof' nano_hexapod.flex_top.type = 3; case 'flexible' nano_hexapod.flex_top.type = 4; end nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad] nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad] nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad] nano_hexapod.flex_top.kz = args.flex_top_kz; % Axial stiffness [N/m] nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)] nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)] nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)] nano_hexapod.flex_top.cz = args.flex_top_cz; %[N/(m/s)] #+end_src ** Relative Motion Sensor :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab nano_hexapod.motion_sensor = struct(); switch args.motion_sensor_type case 'struts' nano_hexapod.motion_sensor.type = 1; case 'plates' nano_hexapod.motion_sensor.type = 2; end #+end_src ** Amplified Piezoelectric Actuator :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab nano_hexapod.actuator = struct(); switch args.actuator_type case '2dof' nano_hexapod.actuator.type = 1; case 'flexible frame' nano_hexapod.actuator.type = 2; case 'flexible' nano_hexapod.actuator.type = 3; end #+end_src #+begin_src matlab nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V] nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m] #+end_src 2dof #+begin_src matlab nano_hexapod.actuator.k = args.actuator_k; % [N/m] nano_hexapod.actuator.ke = args.actuator_ke; % [N/m] nano_hexapod.actuator.ka = args.actuator_ka; % [N/m] nano_hexapod.actuator.c = args.actuator_c; % [N/(m/s)] nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)] nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)] nano_hexapod.actuator.Leq = args.actuator_Leq; % [m] #+end_src Flexible frame and fully flexible #+begin_src matlab switch args.actuator_type case 'flexible frame' nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m] case 'flexible' nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m] end nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m] nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m] #+end_src ** Geometry :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab nano_hexapod.geometry = struct(); #+end_src Center of joints $a_i$ with respect to {F}: #+begin_src matlab Fa = [[-86.05, -74.78, 22.49], [ 86.05, -74.78, 22.49], [ 107.79, -37.13, 22.49], [ 21.74, 111.91, 22.49], [-21.74, 111.91, 22.49], [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m] #+end_src Center of joints $b_i$ with respect to {M}: #+begin_src matlab Mb = [[-28.47, -106.25, -22.50], [ 28.47, -106.25, -22.50], [ 106.25, 28.47, -22.50], [ 77.78, 77.78, -22.50], [-77.78, 77.78, -22.50], [-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m] #+end_src Now compute the positions $b_i$ with respect to {F}: #+begin_src matlab Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m] #+end_src The unit vector representing the orientation of the struts can then be computed: #+begin_src matlab si = Fb - Fa; si = si./vecnorm(si); % Normalize #+end_src Location of encoder measurement points when fixed on the plates: #+begin_src matlab Fc = [[-29.362, -105.765, 52.605] [ 29.362, -105.765, 52.605] [ 106.276, 27.454, 52.605] [ 76.914, 78.31, 52.605] [-76.914, 78.31, 52.605] [-106.276, 27.454, 52.605]]'*1e-3; % Meas pos w.r.t. {F} Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M} #+end_src #+begin_src matlab nano_hexapod.geometry.Fa = Fa; nano_hexapod.geometry.Fb = Fb; nano_hexapod.geometry.Fc = Fc; nano_hexapod.geometry.Mb = Mb; nano_hexapod.geometry.Mc = Mc; nano_hexapod.geometry.si = si; nano_hexapod.geometry.MO_B = args.MO_B; #+end_src ** Jacobian for Actuators :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab Bb = Mb - [0; 0; args.MO_B]; nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)']; #+end_src ** Jacobian for Sensors :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab switch args.motion_sensor_type case 'struts' nano_hexapod.geometry.Js = nano_hexapod.geometry.J; case 'plates' Bc = Mc - [0; 0; args.MO_B]; nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)']; end #+end_src ** Save the Structure :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab if nargout == 0 save('./mat/stages.mat', 'nano_hexapod', '-append'); end #+end_src