1804 lines
52 KiB
Org Mode
1804 lines
52 KiB
Org Mode
#+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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+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:
|
|
<<sec:initializeNanoHexapodFinal>>
|
|
|
|
** 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
|