Start the study about flexible joints
This commit is contained in:
parent
31ce8a417e
commit
4e531a6673
BIN
docs/figs/flex_joint_rot_dvf.pdf
Normal file
BIN
docs/figs/flex_joint_rot_dvf.pdf
Normal file
Binary file not shown.
BIN
docs/figs/flex_joint_rot_dvf.png
Normal file
BIN
docs/figs/flex_joint_rot_dvf.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 131 KiB |
BIN
docs/figs/flex_joints_rot_primary_plant_L.pdf
Normal file
BIN
docs/figs/flex_joints_rot_primary_plant_L.pdf
Normal file
Binary file not shown.
BIN
docs/figs/flex_joints_rot_primary_plant_L.png
Normal file
BIN
docs/figs/flex_joints_rot_primary_plant_L.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 103 KiB |
BIN
docs/figs/flex_joints_rot_study_dvf.pdf
Normal file
BIN
docs/figs/flex_joints_rot_study_dvf.pdf
Normal file
Binary file not shown.
BIN
docs/figs/flex_joints_rot_study_dvf.png
Normal file
BIN
docs/figs/flex_joints_rot_study_dvf.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 169 KiB |
BIN
docs/figs/flex_joints_rot_study_dvf_root_locus.pdf
Normal file
BIN
docs/figs/flex_joints_rot_study_dvf_root_locus.pdf
Normal file
Binary file not shown.
BIN
docs/figs/flex_joints_rot_study_dvf_root_locus.png
Normal file
BIN
docs/figs/flex_joints_rot_study_dvf_root_locus.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 91 KiB |
891
org/flexible_joints_study.org
Normal file
891
org/flexible_joints_study.org
Normal file
@ -0,0 +1,891 @@
|
|||||||
|
#+TITLE: Study of the Flexible Joints
|
||||||
|
#+SETUPFILE: ./setup/org-setup-file.org
|
||||||
|
|
||||||
|
* Introduction :ignore:
|
||||||
|
In this document is studied the effect of the mechanical behavior of the flexible joints that are located the extremities of each nano-hexapod's legs.
|
||||||
|
|
||||||
|
Ideally, we want the x and y rotations to be free and all the translations to be blocked.
|
||||||
|
However, this is never the case and be have to consider:
|
||||||
|
- Finite x and y rotational stiffnesses (Section [[sec:rot_stiffness]])
|
||||||
|
- Translation stiffness in the direction of the legs (Section [[sec:trans_stiffness]])
|
||||||
|
|
||||||
|
This may impose some limitations, also, the goal is to specify the required joints stiffnesses.
|
||||||
|
|
||||||
|
* Rotational Stiffness
|
||||||
|
<<sec:rot_stiffness>>
|
||||||
|
|
||||||
|
** Introduction :ignore:
|
||||||
|
In this section, we wish to study the effect of the rotation flexibility of the nano-hexapod joints.
|
||||||
|
|
||||||
|
** 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
|
||||||
|
load('mat/conf_simulink.mat');
|
||||||
|
open('nass_model.slx')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Initialization
|
||||||
|
Let's initialize all the stages with default parameters.
|
||||||
|
#+begin_src matlab
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeSimscapeConfiguration();
|
||||||
|
initializeDisturbances('enable', false);
|
||||||
|
initializeLoggingConfiguration('log', 'none');
|
||||||
|
|
||||||
|
initializeController('type', 'hac-dvf');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Let's consider the heaviest mass which should we the most problematic with it comes to the flexible joints.
|
||||||
|
#+begin_src matlab
|
||||||
|
initializeSample('mass', 50, 'freq', 200*ones(6,1));
|
||||||
|
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 60);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
K = tf(zeros(6));
|
||||||
|
Kdvf = tf(zeros(6));
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Realistic Rotational Stiffness Values
|
||||||
|
*** Introduction :ignore:
|
||||||
|
Let's compare the ideal case (zero stiffness in rotation and infinite stiffness in translation) with a more realistic case:
|
||||||
|
- $K_{\theta, \phi} = 15\,[Nm/rad]$ stiffness in flexion
|
||||||
|
- $K_{\psi} = 20\,[Nm/rad]$ stiffness in torsion
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
Kf_M = 15*ones(6,1);
|
||||||
|
Kt_M = 20*ones(6,1);
|
||||||
|
Kf_F = 15*ones(6,1);
|
||||||
|
Kt_F = 20*ones(6,1);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
The stiffness and damping of the nano-hexapod's legs are:
|
||||||
|
#+begin_src matlab
|
||||||
|
k = 1e5; % [N/m]
|
||||||
|
c = 2e2; % [N/(m/s)]
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
*** Direct Velocity Feedback
|
||||||
|
We identify the dynamics from actuators force $\tau_i$ to relative motion sensors $d\mathcal{L}_i$ with and without considering the flexible joint stiffness.
|
||||||
|
|
||||||
|
The obtained dynamics are shown in Figure [[fig:flex_joint_rot_dvf]].
|
||||||
|
It is shown that the adding of stiffness for the flexible joints does increase a little bit the frequencies of the mass suspension modes. It stiffen the structure.
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', k, 'c', c, ...
|
||||||
|
'type_F', 'universal_p', ...
|
||||||
|
'type_M', 'spherical_p');
|
||||||
|
|
||||||
|
G_dvf_p = linearize(mdl, io);
|
||||||
|
G_dvf_p.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_dvf_p.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', k, 'c', c, ...
|
||||||
|
'type_F', 'universal', ...
|
||||||
|
'type_M', 'spherical', ...
|
||||||
|
'Kf_M', Kf_M, ...
|
||||||
|
'Kt_M', Kt_M, ...
|
||||||
|
'Kf_F', Kf_F, ...
|
||||||
|
'Kt_F', Kt_F);
|
||||||
|
|
||||||
|
G_dvf = linearize(mdl, io);
|
||||||
|
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(-1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf_p(1, 1), 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*unwrap(angle(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', 'Flexible Joints');
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', 'Perfect Joints');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joint_rot_dvf.pdf', 'width', 'full', 'height', 'full')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joint_rot_dvf
|
||||||
|
#+caption: Dynamics from actuators force $\tau_i$ to relative motion sensors $d\mathcal{L}_i$ with (blue) and without (red) considering the flexible joint stiffness
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joint_rot_dvf.png]]
|
||||||
|
|
||||||
|
*** Primary Plant
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Let's now identify the dynamics from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ (for the primary controller designed in the frame of the legs).
|
||||||
|
|
||||||
|
The dynamics is compare with and without the joint flexibility in Figure [[fig:flex_joints_rot_primary_plant_L]].
|
||||||
|
The plant dynamics is not found to be changing significantly.
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||||
|
|
||||||
|
load('mat/stages.mat', 'nano_hexapod');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', k, 'c', c, ...
|
||||||
|
'type_F', 'universal_p', ...
|
||||||
|
'type_M', 'spherical_p');
|
||||||
|
|
||||||
|
G_p = linearize(mdl, io);
|
||||||
|
G_p.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_p.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||||
|
|
||||||
|
Gx_p = -G_p*inv(nano_hexapod.kinematics.J');
|
||||||
|
Gx_p.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
|
||||||
|
Gl_p = -nano_hexapod.kinematics.J*G_p;
|
||||||
|
Gl_p.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', k, 'c', c, ...
|
||||||
|
'type_F', 'universal', ...
|
||||||
|
'type_M', 'spherical', ...
|
||||||
|
'Kf_M', Kf_M, ...
|
||||||
|
'Kt_M', Kt_M, ...
|
||||||
|
'Kf_F', Kf_F, ...
|
||||||
|
'Kt_F', Kt_F);
|
||||||
|
|
||||||
|
G = linearize(mdl, io);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||||
|
|
||||||
|
Gx = -G*inv(nano_hexapod.kinematics.J');
|
||||||
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
|
||||||
|
Gl = -nano_hexapod.kinematics.J*G;
|
||||||
|
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl(j, j), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl_p(j, j), freqs, 'Hz'))), '--');
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl(j, j), freqs, 'Hz')))));
|
||||||
|
end
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_p(j, j), freqs, 'Hz')))), '--');
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joints_rot_primary_plant_L.pdf', 'width', 'full', 'height', 'full');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joints_rot_primary_plant_L
|
||||||
|
#+caption: Dynamics from $\bm{\tau}^\prime_i$ to $\bm{\epsilon}_{\mathcal{X}_n,i}$ with perfect joints (dashed) and with flexible joints (solid)
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joints_rot_primary_plant_L.png]]
|
||||||
|
|
||||||
|
*** Conclusion
|
||||||
|
#+begin_important
|
||||||
|
Considering realistic flexible joint rotational stiffness for the nano-hexapod does not seems to impose any limitation on the DVF control nor on the primary control.
|
||||||
|
|
||||||
|
It only increases a little bit the suspension modes of the sample on top of the nano-hexapod.
|
||||||
|
#+end_important
|
||||||
|
|
||||||
|
** Parametric Study
|
||||||
|
*** Introduction :ignore:
|
||||||
|
We wish now to see what is the impact of the rotation stiffness of the flexible joints on the dynamics.
|
||||||
|
This will help to determine the requirements on the joint's stiffness.
|
||||||
|
|
||||||
|
Let's consider the following rotational stiffness of the flexible joints:
|
||||||
|
#+begin_src matlab
|
||||||
|
Ks = [1, 10, 100]; % [Nm/rad]
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
We also consider here a nano-hexapod with the identified optimal actuator stiffness.
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
K = tf(zeros(6));
|
||||||
|
Kdvf = tf(zeros(6));
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
*** Direct Velocity Feedback
|
||||||
|
The dynamics from the actuators to the relative displacement sensor in each leg is identified and shown in Figure [[fig:flex_joints_rot_study_dvf]].
|
||||||
|
|
||||||
|
The corresponding Root Locus plot is shown in Figure [[fig:flex_joints_rot_study_dvf_root_locus]].
|
||||||
|
|
||||||
|
It is shown that the rotational stiffness of the flexible joints does indeed change a little the dynamics, but critical damping is stiff achievable with Direct Velocity Feedback.
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
|
||||||
|
|
||||||
|
Gdvf_s = {zeros(length(Ks), 1)};
|
||||||
|
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
initializeNanoHexapod('k', k, 'c', c, ...
|
||||||
|
'type_F', 'universal', ...
|
||||||
|
'type_M', 'spherical', ...
|
||||||
|
'Kf_M', Ks(i), ...
|
||||||
|
'Kt_M', Ks(i), ...
|
||||||
|
'Kf_F', Ks(i), ...
|
||||||
|
'Kt_F', Ks(i), ...
|
||||||
|
'Cf_M', 0.2*sqrt(Ks(i)*1), ...
|
||||||
|
'Ct_M', 0.2*sqrt(Ks(i)*1), ...
|
||||||
|
'Cf_F', 0.2*sqrt(Ks(i)*1), ...
|
||||||
|
'Ct_F', 0.2*sqrt(Ks(i)*1));
|
||||||
|
|
||||||
|
Gdvf = linearize(mdl, io);
|
||||||
|
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||||
|
|
||||||
|
Gdvf_s(i) = {Gdvf};
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(-1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gdvf_s{i}(1, 1), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz'))), 'k--');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdvf_s{i}(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
|
||||||
|
end
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_p(1, 1), freqs, 'Hz')))), 'k--', ...
|
||||||
|
'DisplayName', 'Ideal Joint');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
legend('location', 'southwest');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joints_rot_study_dvf.pdf', 'width', 'full', 'height', 'full')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joints_rot_study_dvf
|
||||||
|
#+caption: Dynamics from $\tau_i$ to $d\mathcal{L}_i$ for all the considered Rotation Stiffnesses
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joints_rot_study_dvf.png]]
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
figure;
|
||||||
|
|
||||||
|
gains = logspace(2, 5, 300);
|
||||||
|
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
set(gca,'ColorOrderIndex',i);
|
||||||
|
plot(real(pole(Gdvf_s{i})), imag(pole(Gdvf_s{i})), 'x', ...
|
||||||
|
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
|
||||||
|
set(gca,'ColorOrderIndex',i);
|
||||||
|
plot(real(tzero(Gdvf_s{i})), imag(tzero(Gdvf_s{i})), 'o', ...
|
||||||
|
'HandleVisibility', 'off');
|
||||||
|
for k = 1:length(gains)
|
||||||
|
set(gca,'ColorOrderIndex',i);
|
||||||
|
cl_poles = pole(feedback(Gdvf_s{i}, (gains(k)*s)*eye(6)));
|
||||||
|
plot(real(cl_poles), imag(cl_poles), '.', ...
|
||||||
|
'HandleVisibility', 'off');
|
||||||
|
end
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
axis square;
|
||||||
|
xlim([-140, 10]); ylim([0, 150]);
|
||||||
|
|
||||||
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||||
|
legend('location', 'northwest');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joints_rot_study_dvf_root_locus.pdf', 'width', 'wide', 'height', 'tall')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joints_rot_study_dvf_root_locus
|
||||||
|
#+caption: Root Locus for all the considered Rotation Stiffnesses
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joints_rot_study_dvf_root_locus.png]]
|
||||||
|
|
||||||
|
*** Primary Control
|
||||||
|
#+begin_src matlab
|
||||||
|
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||||
|
|
||||||
|
load('mat/stages.mat', 'nano_hexapod');
|
||||||
|
|
||||||
|
Gx_3dof_s = {zeros(length(Ks), 1)};
|
||||||
|
Gl_3dof_s = {zeros(length(Ks), 1)};
|
||||||
|
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
initializeNanoHexapod('k', k, 'c', c, ...
|
||||||
|
'type_F', 'universal', ...
|
||||||
|
'type_M', 'spherical', ...
|
||||||
|
'Kf_M', Ks(i), ...
|
||||||
|
'Kt_M', 20, ...
|
||||||
|
'Kf_F', Ks(i), ...
|
||||||
|
'Kt_F', 20, ...
|
||||||
|
'Cf_M', 0, ...
|
||||||
|
'Ct_M', 0, ...
|
||||||
|
'Cf_F', 0, ...
|
||||||
|
'Ct_F', 0);
|
||||||
|
|
||||||
|
G = linearize(mdl, io);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||||
|
|
||||||
|
Gx_3dof = -G*inv(nano_hexapod.kinematics.J');
|
||||||
|
Gx_3dof.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
Gx_3dof_s(i) = {Gx_3dof};
|
||||||
|
|
||||||
|
Gl_3dof = -nano_hexapod.kinematics.J*G;
|
||||||
|
Gl_3dof.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||||
|
Gl_3dof_s(i) = {Gl_3dof};
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(-1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl_3dof_s{i}(1, 1), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl_p(1, 1), freqs, 'Hz'))), 'k--');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Ks)
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_3dof_s{i}(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', sprintf('$k = %.0g$ [N/m]', Ks(i)));
|
||||||
|
end
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_p(1, 1), freqs, 'Hz')))), 'k--', ...
|
||||||
|
'DisplayName', 'Ideal Joint');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
legend('location', 'southwest');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joints_rot_study_primary_plant.pdf', 'width', 'full', 'height', 'full');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joints_rot_study_primary_plant
|
||||||
|
#+caption: Diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the considered rotational stiffnesses
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joints_rot_study_primary_plant.png]]
|
||||||
|
|
||||||
|
*** Conclusion
|
||||||
|
#+begin_important
|
||||||
|
|
||||||
|
#+end_important
|
||||||
|
|
||||||
|
* Translation Stiffness
|
||||||
|
<<sec:trans_stiffness>>
|
||||||
|
|
||||||
|
** Introduction :ignore:
|
||||||
|
Let's know consider a flexibility in translation of the flexible joint.
|
||||||
|
|
||||||
|
** 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
|
||||||
|
load('mat/conf_simulink.mat');
|
||||||
|
open('nass_model.slx')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Initialization
|
||||||
|
Let's initialize all the stages with default parameters.
|
||||||
|
#+begin_src matlab
|
||||||
|
initializeGround();
|
||||||
|
initializeGranite();
|
||||||
|
initializeTy();
|
||||||
|
initializeRy();
|
||||||
|
initializeRz();
|
||||||
|
initializeMicroHexapod();
|
||||||
|
initializeAxisc();
|
||||||
|
initializeMirror();
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeSimscapeConfiguration();
|
||||||
|
initializeDisturbances('enable', false);
|
||||||
|
initializeLoggingConfiguration('log', 'none');
|
||||||
|
|
||||||
|
initializeController('type', 'hac-dvf');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Let's consider the heaviest mass which should we the most problematic with it comes to the flexible joints.
|
||||||
|
#+begin_src matlab
|
||||||
|
initializeSample('mass', 50, 'freq', 200*ones(6,1));
|
||||||
|
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', 60);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
K = tf(zeros(6));
|
||||||
|
Kdvf = tf(zeros(6));
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Direct Velocity Feedback
|
||||||
|
#+begin_src matlab
|
||||||
|
Kz_F = 60e6*ones(6,1); % [N/m]
|
||||||
|
Kz_M = 60e6*ones(6,1); % [N/m]
|
||||||
|
Cz_F = 1e2*ones(6,1); % [N/m]
|
||||||
|
Cz_M = 1e2*ones(6,1); % [N/m]
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
|
||||||
|
'type_F', 'universal_3dof', ...
|
||||||
|
'type_M', 'spherical_3dof', ...
|
||||||
|
'Kz_F', Kz_F, ...
|
||||||
|
'Kz_M', Kz_M, ...
|
||||||
|
'Cz_F', Cz_F, ...
|
||||||
|
'Cz_M', Cz_M);
|
||||||
|
|
||||||
|
G_dvf_3dof = linearize(mdl, io);
|
||||||
|
G_dvf_3dof.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_dvf_3dof.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
|
||||||
|
'type_F', 'universal', ...
|
||||||
|
'type_M', 'spherical');
|
||||||
|
|
||||||
|
G_dvf = linearize(mdl, io);
|
||||||
|
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(-1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G_dvf_3dof(1, 1), 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*unwrap(angle(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', '2dof Joints');
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_dvf_3dof(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', '3dof Joints');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joint_trans_dvf.pdf', 'width', 'full', 'height', 'full')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joint_trans_dvf
|
||||||
|
#+caption:
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joint_trans_dvf.png]]
|
||||||
|
|
||||||
|
** Primary Plant
|
||||||
|
#+begin_src matlab
|
||||||
|
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||||
|
|
||||||
|
load('mat/stages.mat', 'nano_hexapod');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
|
||||||
|
'type_F', 'universal_3dof', ...
|
||||||
|
'type_M', 'spherical_3dof', ...
|
||||||
|
'Kz_F', Kz_F, ...
|
||||||
|
'Kz_M', Kz_M, ...
|
||||||
|
'Cz_F', Cz_F, ...
|
||||||
|
'Cz_M', Cz_M);
|
||||||
|
|
||||||
|
G_3dof = linearize(mdl, io);
|
||||||
|
G_3dof.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G_3dof.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||||
|
|
||||||
|
Gx_3dof = -G_3dof*inv(nano_hexapod.kinematics.J');
|
||||||
|
Gx_3dof.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
|
||||||
|
Gl_3dof = -nano_hexapod.kinematics.J*G_3dof;
|
||||||
|
Gl_3dof.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
|
||||||
|
'type_F', 'universal', ...
|
||||||
|
'type_M', 'spherical');
|
||||||
|
|
||||||
|
G = linearize(mdl, io);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||||
|
|
||||||
|
Gx = -G*inv(nano_hexapod.kinematics.J');
|
||||||
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
|
||||||
|
Gl = -nano_hexapod.kinematics.J*G;
|
||||||
|
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl(j, j), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl_3dof(j, j), freqs, 'Hz'))), '--');
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
title('Diagonal elements of the Plant');
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl(j, j), freqs, 'Hz')))));
|
||||||
|
end
|
||||||
|
for j = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_3dof(j, j), freqs, 'Hz')))), '--');
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/flex_joints_trans_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:flex_joints_trans_primary_plant_L
|
||||||
|
#+caption:
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/flex_joints_trans_primary_plant_L.png]]
|
||||||
|
|
||||||
|
** Parametric study
|
||||||
|
*** Introduction :ignore:
|
||||||
|
#+begin_src matlab
|
||||||
|
Kzs = [1e4, 1e5, 1e6, 1e7, 1e8, 1e9]; % [N/m]
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
*** Direct Velocity Feedback
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
Gdvf_3dof_s = {zeros(length(Kzs), 1)};
|
||||||
|
|
||||||
|
for i = 1:length(Kzs)
|
||||||
|
initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
|
||||||
|
'type_F', 'universal_3dof', ...
|
||||||
|
'type_M', 'spherical_3dof', ...
|
||||||
|
'Kz_F', Kzs(i), ...
|
||||||
|
'Kz_M', Kzs(i), ...
|
||||||
|
'Cz_F', 0.2*sqrt(Kzs(i)*10), ...
|
||||||
|
'Cz_M', 0.2*sqrt(Kzs(i)*10));
|
||||||
|
|
||||||
|
G = linearize(mdl, io);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||||
|
|
||||||
|
Gdvf_3dof_s(i) = {G};
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(-1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Kzs)
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gdvf_3dof_s{i}(1, 1), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 1), freqs, 'Hz'))), 'k--');
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Kzs)
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdvf_3dof_s{i}(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', sprintf('$k = %.0g$ [N/m]', Kzs(i)));
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gdvf(1, 1), freqs, 'Hz')))), 'k--', ...
|
||||||
|
'DisplayName', 'Perfect Joint');
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
*** Primary Control
|
||||||
|
#+begin_src matlab
|
||||||
|
Kdvf = 5e3*s/(1+s/2/pi/1e3)*eye(6);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'nass_model';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||||
|
|
||||||
|
load('mat/stages.mat', 'nano_hexapod');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
Gx_3dof_s = {zeros(length(Kzs), 1)};
|
||||||
|
Gl_3dof_s = {zeros(length(Kzs), 1)};
|
||||||
|
|
||||||
|
for i = 1:length(Kzs)
|
||||||
|
initializeNanoHexapod('k', 1e5, 'c', 2e2, ...
|
||||||
|
'type_F', 'universal_3dof', ...
|
||||||
|
'type_M', 'spherical_3dof', ...
|
||||||
|
'Kz_F', Kzs(i), ...
|
||||||
|
'Kz_M', Kzs(i), ...
|
||||||
|
'Cz_F', 0.2*sqrt(Kzs(i)*10), ...
|
||||||
|
'Cz_M', 0.2*sqrt(Kzs(i)*10));
|
||||||
|
|
||||||
|
G = linearize(mdl, io);
|
||||||
|
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||||
|
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||||
|
|
||||||
|
Gx_3dof = -G*inv(nano_hexapod.kinematics.J');
|
||||||
|
Gx_3dof.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
Gx_3dof_s(i) = {Gx_3dof};
|
||||||
|
|
||||||
|
Gl_3dof = -nano_hexapod.kinematics.J*G;
|
||||||
|
Gl_3dof.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||||
|
Gl_3dof_s(i) = {Gl_3dof};
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(-1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Kzs)
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gl_3dof_s{i}(1, 1), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(Kzs)
|
||||||
|
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gl_3dof_s{i}(1, 1), freqs, 'Hz')))), ...
|
||||||
|
'DisplayName', sprintf('$k = %.0g$ [N/m]', Kzs(i)));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-270, 90]);
|
||||||
|
yticks([-360:90:360]);
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Conclusion
|
||||||
|
#+begin_important
|
||||||
|
|
||||||
|
#+end_important
|
||||||
Loading…
Reference in New Issue
Block a user