#+TITLE: Nano-Hexapod #+SETUPFILE: ./setup/org-setup-file.org * Nano-Hexapod ** 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 addpath('matlab/nano_hexapod'); open('matlab/nano_hexapod/nano_hexapod.slx') #+end_src ** Nano Hexapod object The nano-hexapod can be initialized and configured using the =initializeNanoHexapodFinal= function. #+begin_src matlab n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '3dof', ... 'motion_sensor_type', 'struts', ... 'actuator_type', '2dof'); #+end_src ** Jacobian from Solidworks Model 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; #+end_src Center of joints a_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; #+end_src #+begin_src matlab Fb = Mb + [0; 0; 95e-3]; si = Fb - Fa; si = si./vecnorm(si); #+end_src #+begin_src matlab ki = ones(1,6); bi = Fb; #+end_src #+begin_src matlab :results value replace :exports both ki.*si*si' #+end_src #+RESULTS: | 1.8977 | 2.4659e-17 | 1.3383e-18 | | 2.4659e-17 | 1.8977 | -2.3143e-05 | | 1.3383e-18 | -2.3143e-05 | 2.2046 | #+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 compute the Jacobian at this location: #+begin_src matlab Jk = [si', cross(bi - Ok, si)']; #+end_src And the stiffness matrix: #+begin_src matlab :results value replace Jk'*diag(ki)*Jk #+end_src #+RESULTS: | 1.8977 | 0 | 0 | 0 | -3.4694e-18 | 2.5509e-06 | | 0 | 1.8977 | -2.3143e-05 | 4.1751e-06 | 1.3878e-17 | 0 | | 0 | -2.3143e-05 | 2.2046 | -5.0916e-11 | -3.4694e-18 | 0 | | 0 | 4.1751e-06 | -5.0916e-11 | 0.012594 | 2.1684e-19 | 8.6736e-19 | | -3.2704e-18 | 0 | -4.206e-18 | 3.9451e-19 | 0.012594 | -9.3183e-08 | | 2.5509e-06 | 0 | 0 | 8.6736e-19 | -9.3183e-08 | 0.043362 | ** Jacobian for encoders on the plates *Goal*: Compute the 6-DoF motion of the top platform (relative to the bottom platform) from the 6 encoder measurements. The main differences with the case where the encoders are parallel with the strut are: - the encoder and ruler may not be parallel anymore as the top platform is moving - the derivation of the Jacobian (if possible) will not be the same ** Compare encoders on the struts and encoders on the platforms #+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 #+begin_src matlab n_hexapod.motion_sensor = struct(); n_hexapod.motion_sensor.type = 1; % 1: Leg / 2: Plate #+end_src #+begin_src matlab 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 #+begin_src matlab n_hexapod.motion_sensor = struct(); n_hexapod.motion_sensor.type = 2; % 1: Leg / 2: Plate #+end_src #+begin_src matlab 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 #+begin_src matlab :exports none freqs = logspace(0, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz')))); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(j)]), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ylim([1e-10, 1e-6]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gs(['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([0, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+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; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gp(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz')))); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gp(['D', num2str(i)], ['F', num2str(j)]), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ylim([1e-10, 1e-6]); ax2 = nexttile; hold on; for i = 1:6 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([0, 180]); yticks([-180, -90, 0, 90, 180]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); #+end_src ** Nano Hexapod #+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, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion of top platform %% Run the linearization G = linearize(mdl, io, 0.0, options); G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ... 'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ... 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6' ... 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; #+end_src #+begin_src matlab :results output replace :exports results :tangle no size(G) #+end_src #+RESULTS: : size(G) : State-space model with 18 outputs, 12 inputs, and 60 states. Verify the number of DOF: | Element | DoF | |--------------------+-----| | Bot Plate | 0 | | Struts | 6*4 | | Top Plate + Sample | 12 | |--------------------+-----| | Total: | 36 | #+TBLFM: @>$2=vsum(@I..@II) *** DVF Plant 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} \] #+begin_src matlab :results value replace :exports results :tangle no 1/(k + ka + k*ka/ke) #+end_src #+RESULTS: : 1.8732e-08 Which is almost the case: #+begin_src matlab :results value replace :exports results :tangle no dcgain(G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})) #+end_src #+RESULTS: | -1.8676e-08 | 5.2396e-11 | -8.8935e-11 | 2.6392e-11 | 9.7629e-11 | -1.5609e-10 | | -1.4754e-11 | -1.8725e-08 | 1.4348e-11 | -5.7189e-11 | -3.3327e-11 | 7.0799e-11 | | -3.4924e-11 | -7.7445e-11 | -1.8607e-08 | -4.6578e-11 | -1.9592e-11 | 4.0299e-11 | | 6.1699e-11 | -7.1962e-11 | 1.3374e-11 | -1.8623e-08 | -1.7898e-10 | 5.4541e-11 | | 9.735e-11 | -1.0698e-10 | -1.6424e-11 | 2.3718e-11 | -1.8826e-08 | 8.387e-11 | | -3.94e-11 | 2.6436e-11 | -1.1338e-10 | 5.0793e-11 | 1.2358e-10 | -1.8792e-08 | Other (off-diagonal) elements should be close to zero (probably limited to joint's stiffness). *However, when setting joint's axial stiffness to infinity, I obtain better diagonal*. #+begin_src matlab :exports none Gdvf = minreal(G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})); freqs = 2*logspace(0, 2, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gdvf(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz')))); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gdvf(['D', num2str(i)], ['F', num2str(j)]), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ylim([1e-10, 1e-6]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(['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([0, 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', 'wide', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_struts_2dof_dvf_plant #+caption: IFF Plant #+RESULTS: [[file:figs/nano_hexapod_struts_2dof_dvf_plant.png]] *** IFF Plant #+begin_src matlab :exports none freqs = 2*logspace(0, 2, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(G(['Fm', num2str(i)], ['F', num2str(i)]), freqs, 'Hz')))); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(G(['Fm', num2str(i)], ['F', num2str(j)]), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]); ylim([1e-4, 1e1]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fm', 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, 0]); 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', 'wide', 'height', 'tall'); #+end_src #+name: fig:nano_hexapod_struts_2dof_iff_plant #+caption: DVF Plant #+RESULTS: [[file:figs/nano_hexapod_struts_2dof_iff_plant.png]] ** Decoupling at the CoK #+begin_src matlab Gx = inv(Jk)*G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(Jk'); Gx.inputname = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; Gx.outputname = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; #+end_src #+begin_src matlab :results value replace :exports results :tangle no dcgain(Gx) #+end_src #+RESULTS: | -9.8399e-09 | -8.0785e-11 | -1.5723e-11 | 2.6356e-10 | -4.9857e-10 | 5.2177e-11 | | -8.7554e-11 | -9.8585e-09 | 2.6292e-11 | 2.0485e-10 | 2.0385e-10 | 8.2942e-11 | | 4.3837e-11 | -5.5862e-14 | -8.4859e-09 | -9.0036e-12 | 2.9325e-10 | -3.104e-11 | | 1.901e-09 | 5.3078e-10 | -7.5477e-10 | -1.4759e-06 | -7.7918e-09 | -7.491e-10 | | -7.1795e-11 | -1.6333e-09 | -1.8359e-10 | 5.2707e-09 | -1.4794e-06 | 3.2304e-10 | | 7.0322e-11 | 1.7713e-12 | -5.8019e-11 | 7.4838e-11 | -8.9677e-10 | -4.2938e-07 | #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); figure; tiledlayout(6, 6, 'TileSpacing', 'None', 'Padding', 'None'); for i = 1:6 for j = 1:6 nexttile; plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'k-'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); end end axs = findobj(gcf,'Type','axes','Visible','on'); linkaxes(axs, 'xy'); xlim([freqs(1), freqs(end)]); ylim([1e-10, 1e-4]); #+end_src #+begin_src matlab :exports none freqs = logspace(0, 2, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gx(i,i), freqs, 'Hz'))), '-'); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(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 ** Jacobian *** Compute Jacobian #+begin_src matlab Ai = [out.b1.Data(1,:) out.b2.Data(1,:) out.b3.Data(1,:) out.b4.Data(1,:) out.b5.Data(1,:) out.b6.Data(1,:)]'; Bi = [out.a1.Data(1,:) out.a2.Data(1,:) out.a3.Data(1,:) out.a4.Data(1,:) out.a5.Data(1,:) out.a6.Data(1,:)]'; #+end_src #+begin_src matlab stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, 'H', 95e-3, 'MO_B', 150e-3); stewart = generateGeneralConfiguration(stewart); stewart.platform_F.Fa = Ai; stewart.platform_M.Mb = Bi - [0;0;1]*stewart.geometry.H; stewart = computeJointsPose(stewart); stewart = initializeStewartPose(stewart, 'AP', [0;0;0], 'ARB', eye(3)); #+end_src #+begin_src matlab stewart = initializeCylindricalPlatforms(stewart); stewart = initializeCylindricalStruts(stewart); #+end_src #+begin_src matlab stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e2*ones(6,1)); stewart = initializeAmplifiedStrutDynamics(stewart); stewart = initializeJointDynamics(stewart); #+end_src #+begin_src matlab displayArchitecture(stewart, 'views', 'all'); #+end_src #+begin_src matlab :results output replace :exports results describeStewartPlatform(stewart) #+end_src #+RESULTS: #+begin_example describeStewartPlatform(stewart) GEOMETRY: - The height between the fixed based and the top platform is 95 [mm]. - Frame {A} is located 150 [mm] above the top platform. - The initial length of the struts are: 82.8, 82.8, 82.8, 82.8, 82.8, 82.8 [mm] ACTUATORS: - The actuators are mechanicaly amplified. - The vertical stiffness and damping contribution of the piezoelectric stack is: ka = 2e+07 [N/m] ca = 1e+01 [N/(m/s)] - Vertical stiffness when the piezoelectric stack is removed is: kr = 5e+06 [N/m] cr = 1e+01 [N/(m/s)] JOINTS: - The joints on the fixed based are universal joints - The joints on the mobile based are spherical joints - The position of the joints on the fixed based with respect to {F} are (in [mm]): -86.2 -74.7 22.3 86.3 -74.6 22.3 108 -37.3 22.3 21.5 112 22.3 -21.5 112 22.3 -108 -37.5 22.2 - The position of the joints on the mobile based with respect to {M} are (in [mm]): -28.4 -106 -22.4 28.5 -106 -22.5 106 28.5 -22.5 77.8 77.8 -22.5 -77.8 77.8 -22.5 -106 28.4 -22.5 #+end_example #+begin_src matlab stewart = computeJacobian(stewart); #+end_src *** Verify Jacobian #+begin_src matlab load('./mat/nano_hexapod_object.mat', 'stewart'); J = stewart.kinematics.J; #+end_src Transformation of motion: #+begin_src matlab G1 = -inv(J)*G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}); G2 = G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}); #+end_src #+begin_src matlab :exports none freqs = logspace(1, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(G1(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, abs(squeeze(freqresp(G2(i,i), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G1(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G2(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'); #+end_src Transformation of Forces and Torques #+begin_src matlab G1 = G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}); G2 = G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(J'); #+end_src *Not working because the forces applied on the APA are not really forces applied on the top platform (reduced by a factor ~ka/ke)* #+begin_src matlab :exports none freqs = logspace(1, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(G1(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, abs(squeeze(freqresp(G2(i,i), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G1(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G2(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'); #+end_src *** Plant from APA forces to sample's displacement #+begin_src matlab Gx = G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(J'); Gx.inputname = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; #+end_src #+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; for i = 1:6 plot(freqs, abs(squeeze(freqresp(Gx(i,i), freqs, 'Hz'))), '-'); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(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 :exports none freqs = 5*logspace(0, 2, 1000); figure; tiledlayout(6, 6, 'TileSpacing', 'None', 'Padding', 'None'); for i = 1:6 for j = 1:6 nexttile; plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'k-'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); end end axs = findobj(gcf,'Type','axes','Visible','on'); linkaxes(axs, 'xy'); xlim([freqs(1), freqs(end)]); ylim([1e-10, 1e-4]); #+end_src #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); figure; hold on; for i = 1:3 plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'k-'); end for i = 1:3 for j = i+1:3 plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'r-'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); end end hold off; xlim([freqs(1), freqs(end)]); ylim([1e-10, 1e-4]); #+end_src #+begin_src matlab :exports none freqs = 5*logspace(0, 2, 1000); figure; hold on; for i = 4:6 plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'k-'); end for i = 4:6 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'r-'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); end end hold off; xlim([freqs(1), freqs(end)]); ylim([1e-10, 1e-4]); #+end_src *** Plant in the Cartesian frame Transfer function from forces/torque to displacement/rotation: #+begin_src matlab GJ = inv(J)*G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(J'); GJ.inputname = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; GJ.outputname = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; #+end_src #+begin_src matlab :exports none freqs = logspace(1, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(GJ(i,i), freqs, 'Hz'))), '-'); end for i = 1:5 for j = i+1:6 plot(freqs, abs(squeeze(freqresp(GJ(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); end end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(GJ(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'); #+end_src *** Plant in the frame of the legs => Verification of Jacobian for displacements Transfer Function in the frame of the legs #+begin_src matlab G1 = -J*G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}); G1.outputname = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; G2 = G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}); #+end_src #+begin_important It is working fine (until internal resonance of strut). #+end_important #+begin_src matlab :exports none freqs = logspace(1, 3, 1000); figure; tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); ax1 = nexttile([2,1]); hold on; for i = 1:6 plot(freqs, abs(squeeze(freqresp(G1(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, abs(squeeze(freqresp(G2(i,i), freqs, 'Hz'))), '--'); end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); ax2 = nexttile; hold on; for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G1(i,i), freqs, 'Hz'))), '-'); end set(gca,'ColorOrderIndex',1); for i = 1:6 plot(freqs, 180/pi*angle(squeeze(freqresp(G2(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'); #+end_src ** Stiffness matrix 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}}} \] #+begin_src matlab kAPA = k + ke*ka/(ke + ka); ks = kz*kAPA/(kz + 2*kAPA); #+end_src #+begin_src matlab Ks = J'*(ks*eye(6))*J #+end_src #+begin_src matlab :results value replace :exports results :tangle no inv(Ks) #+end_src #+RESULTS: | 1.9937e-06 | 7.7027e-12 | -1.5885e-11 | -1.6811e-10 | 8.7902e-06 | 3.0476e-11 | | 7.7027e-12 | 1.9937e-06 | 2.7221e-11 | -8.7901e-06 | 6.5081e-12 | 2.8619e-11 | | -1.5885e-11 | 2.7221e-11 | 2.6114e-07 | -1.3209e-10 | -6.1725e-11 | 4.0402e-11 | | -1.6811e-10 | -8.7901e-06 | -1.3209e-10 | 4.5712e-05 | -5.7781e-10 | -4.5817e-10 | | 8.7902e-06 | 6.5081e-12 | -6.1725e-11 | -5.7781e-10 | 4.5712e-05 | 5.1628e-10 | | 3.0476e-11 | 2.8619e-11 | 4.0402e-11 | -4.5817e-10 | 5.1628e-10 | 1.3277e-05 | #+begin_src matlab :results value replace :exports results :tangle no dcgain(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'})) #+end_src #+RESULTS: | 1.9869e-06 | 2.043e-08 | 1.6444e-09 | -4.094e-08 | 8.7579e-06 | -3.5002e-09 | | 3.746e-09 | 1.9841e-06 | -5.5274e-09 | -8.7257e-06 | -5.5082e-08 | -7.6978e-09 | | -3.2056e-09 | -6.4295e-11 | 2.6079e-07 | 3.4103e-10 | -9.3873e-09 | 9.6146e-10 | | -1.1677e-08 | -8.736e-06 | 2.4322e-08 | 4.5343e-05 | 2.52e-07 | 2.5932e-08 | | 8.7439e-06 | 8.441e-08 | 5.9144e-09 | -1.6879e-07 | 4.546e-05 | -9.8986e-09 | | 3.3453e-09 | 4.508e-10 | 1.8718e-09 | -2.7294e-09 | 2.8776e-08 | 1.3193e-05 | We can see that we have almost the same stiffness: #+begin_src matlab :results value replace :exports results :tangle no inv(Ks)./dcgain(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'})) #+end_src #+RESULTS: | 1.0034 | 0.00037703 | -0.0096597 | 0.0041063 | 1.0037 | -0.0087071 | | 0.0020562 | 1.0048 | -0.0049247 | 1.0074 | -0.00011815 | -0.0037178 | | 0.0049552 | -0.42338 | 1.0013 | -0.38734 | 0.0065754 | 0.042021 | | 0.014397 | 1.0062 | -0.005431 | 1.0081 | -0.0022929 | -0.017668 | | 1.0053 | 7.7101e-05 | -0.010436 | 0.0034233 | 1.0055 | -0.052157 | | 0.0091102 | 0.063485 | 0.021584 | 0.16786 | 0.017941 | 1.0063 | * To-order :noexport: ** 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)] %% 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 % Sensor gain [V/m] 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('APA300ML_full_mat_K.CSV'); % Stiffness Matrix nano_hexapod.actuator.M = readmatrix('APA300ML_full_mat_M.CSV'); % Mass Matrix nano_hexapod.actuator.P = extractNodes('APA300ML_full_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 ** Save the Structure :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab if nargout == 0 save('./mat/stages.mat', 'nano_hexapod', '-append'); end #+end_src