From 8e08e3f3d49a142bdc750c9b9461fb67f9338e3d Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Sat, 17 Oct 2026 18:35:40 +0200 Subject: [PATCH] Update Matlab tangled files --- matlab/index.org | 88 ++-- matlab/matlab/s1_system_description.m | 282 +++++------ matlab/matlab/s2_iff_pure_int.m | 254 +++++----- matlab/matlab/s3_iff_hpf.m | 254 +++++----- matlab/matlab/s4_iff_kp.m | 470 +++++++++--------- matlab/matlab/s5_act_damp_comparison.m | 268 +++++----- .../matlab/src/computeSimultaneousDamping.m | 10 + matlab/matlab/src/rootLocusPolesSorted.m | 93 ++++ matlab/src/computeSimultaneousDamping.m | 10 - matlab/src/rootLocusPolesSorted.m | 93 ---- 10 files changed, 930 insertions(+), 892 deletions(-) create mode 100644 matlab/matlab/src/computeSimultaneousDamping.m create mode 100644 matlab/matlab/src/rootLocusPolesSorted.m delete mode 100644 matlab/src/computeSimultaneousDamping.m delete mode 100644 matlab/src/rootLocusPolesSorted.m diff --git a/matlab/index.org b/matlab/index.org index bc6fb67..0b0f1f8 100644 --- a/matlab/index.org +++ b/matlab/index.org @@ -44,10 +44,10 @@ The matlab code is accessible on [[https://zenodo.org/record/3894343][Zonodo]] a To run the Matlab code, go in the =matlab= directory and run the following Matlab files corresponding to each section. #+caption: Paper's sections and corresponding Matlab files -| Sections | Matlab File | -|------------------------------------+----------------------------| +| Sections | Matlab File | +|------------------------------------+--------------------------| | Section [[sec:system_description]] | =s1_system_description.m= | -| Section [[sec:iff_pure_int]] | =s2_iff_pure_int.m= | +| Section [[sec:iff_pure_int]] | =s2_iff_pure_int.m= | | Section [[sec:iff_pseudo_int]] | =s3_iff_hpf.m= | | Section [[sec:iff_parallel_stiffness]] | =s4_iff_kp.m= | | Section [[sec:comparison]] | =s5_act_damp_comparison.m= | @@ -60,16 +60,19 @@ To run the Matlab code, go in the =matlab= directory and run the following Matla ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> +<> #+end_src #+begin_src matlab :exports none :results silent :noweb yes - <> +<> #+end_src -#+begin_src matlab :tangle no - addpath('./matlab/'); - addpath('./src/'); +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> #+end_src ** System description @@ -440,16 +443,19 @@ Two identical controllers $K_F$ are used to feedback each of the sensed force to ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> +<> #+end_src #+begin_src matlab :exports none :results silent :noweb yes - <> +<> #+end_src -#+begin_src matlab :tangle no - addpath('./matlab/'); - addpath('./src/'); +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> #+end_src ** Plant Parameters @@ -764,16 +770,19 @@ It is shown that for non-null rotating speed, one pole is bound to the right-hal ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> +<> #+end_src #+begin_src matlab :exports none :results silent :noweb yes - <> +<> #+end_src -#+begin_src matlab :tangle no - addpath('./matlab/'); - addpath('./src/'); +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> #+end_src ** Plant Parameters @@ -1160,16 +1169,19 @@ To find the optimum, the gain that maximize the simultaneous damping of the mode ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> +<> #+end_src #+begin_src matlab :exports none :results silent :noweb yes - <> +<> #+end_src -#+begin_src matlab :tangle no - addpath('./matlab/'); - addpath('./src/'); +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> #+end_src ** Schematic @@ -1761,16 +1773,19 @@ These two methods are now compared in terms of added damping, closed-loop compli ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> +<> #+end_src #+begin_src matlab :exports none :results silent :noweb yes - <> +<> #+end_src -#+begin_src matlab :tangle no - addpath('./matlab/'); - addpath('./src/'); +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> #+end_src ** Plant Parameters @@ -2143,10 +2158,23 @@ Critical Damping corresponds to to $\xi = 1$, and thus: #+latex: \printbibliography bibliography:ref.bib +* Helping Functions :noexport: +** Initialize Path +#+NAME: m-init-path +#+BEGIN_SRC matlab +addpath('./matlab/'); % Path for scripts +addpath('./matlab/src/'); % Path for functions +#+END_SRC + +#+NAME: m-init-path-tangle +#+BEGIN_SRC matlab +addpath('./src/'); % Path for functions +#+END_SRC + * Functions :noexport: ** Sort Poles for the Root Locus :PROPERTIES: -:header-args:matlab+: :tangle src/rootLocusPolesSorted.m +:header-args:matlab+: :tangle matlab/src/rootLocusPolesSorted.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> @@ -2291,7 +2319,7 @@ This Matlab function is accessible [[file:src/rootLocusPolesSorted.m][here]]. #+end_src ** =computeSimultaneousDamping= -#+begin_src matlab :tangle src/computeSimultaneousDamping.m +#+begin_src matlab :tangle matlab/src/computeSimultaneousDamping.m function [xi_min] = computeSimultaneousDamping(g, G, K) [w, xi] = damp(minreal(feedback(G, g*K))); xi_min = 1/min(xi); diff --git a/matlab/matlab/s1_system_description.m b/matlab/matlab/s1_system_description.m index 0ea5c49..669b3b3 100644 --- a/matlab/matlab/s1_system_description.m +++ b/matlab/matlab/s1_system_description.m @@ -4,15 +4,17 @@ clear; close all; clc; %% Intialize Laplace variable s = zpk('s'); +addpath('./src/'); % Path for functions + % Numerical Values % Let's define initial values for the model. -k = 1; % Actuator Stiffness [N/m] -c = 0.05; % Actuator Damping [N/(m/s)] -m = 1; % Payload mass [kg] + k = 1; % Actuator Stiffness [N/m] + c = 0.05; % Actuator Damping [N/(m/s)] + m = 1; % Payload mass [kg] -xi = c/(2*sqrt(k*m)); -w0 = sqrt(k/m); % [rad/s] + xi = c/(2*sqrt(k*m)); + w0 = sqrt(k/m); % [rad/s] % Campbell Diagram % The Campbell Diagram displays the evolution of the real and imaginary parts of the system as a function of the rotating speed. @@ -20,19 +22,19 @@ w0 = sqrt(k/m); % [rad/s] % It is shown in Figures [[fig:campbell_diagram_real]] and [[fig:campbell_diagram_imag]], and one can see that the system becomes unstable for $\Omega > \omega_0$ (the real part of one of the poles becomes positive). -Ws = linspace(0, 2, 51); % Vector of considered rotation speed [rad/s] + Ws = linspace(0, 2, 51); % Vector of considered rotation speed [rad/s] -p_ws = zeros(4, length(Ws)); + p_ws = zeros(4, length(Ws)); -for W_i = 1:length(Ws) - W = Ws(W_i); + for W_i = 1:length(Ws) + W = Ws(W_i); - pole_G = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2)); - [~, i_sort] = sort(imag(pole_G)); - p_ws(:, W_i) = pole_G(i_sort); -end + pole_G = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2)); + [~, i_sort] = sort(imag(pole_G)); + p_ws(:, W_i) = pole_G(i_sort); + end -clear pole_G; + clear pole_G; % Simscape Model % In order to validate all the equations of motion, a Simscape model of the same system has been developed. @@ -40,152 +42,152 @@ clear pole_G; % The rotating speed for the Simscape Model is defined. -W = 0.1; % Rotation Speed [rad/s] + W = 0.1; % Rotation Speed [rad/s] -Kiff = tf(zeros(2)); + Kiff = tf(zeros(2)); -kp = 0; % Parallel Stiffness [N/m] -cp = 0; % Parallel Damping [N/(m/s)] + kp = 0; % Parallel Stiffness [N/m] + cp = 0; % Parallel Damping [N/(m/s)] -open('rotating_frame.slx'); + open('rotating_frame.slx'); % The transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is identified from the Simscape model. -%% Name of the Simulink File -mdl = 'rotating_frame'; + %% Name of the Simulink File + mdl = 'rotating_frame'; -%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1; + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1; -G = linearize(mdl, io, 0); + G = linearize(mdl, io, 0); -%% Input/Output definition -G.InputName = {'Fu', 'Fv'}; -G.OutputName = {'du', 'dv'}; + %% Input/Output definition + G.InputName = {'Fu', 'Fv'}; + G.OutputName = {'du', 'dv'}; % The same transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is written down from the analytical model. -Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... - [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ... - -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]; + Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... + [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ... + -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]; % Both transfer functions are compared in Figure [[fig:plant_simscape_analytical]] and are found to perfectly match. -freqs = logspace(-1, 1, 1000); + freqs = logspace(-1, 1, 1000); -figure; -tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(G(1,1), freqs))), '-') -plot(freqs, abs(squeeze(freqresp(Gth(1,1), freqs))), '--') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); -title('$d_u/F_u$, $d_v/F_v$'); + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(G(1,1), freqs))), '-') + plot(freqs, abs(squeeze(freqresp(Gth(1,1), freqs))), '--') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); + title('$d_u/F_u$, $d_v/F_v$'); -ax2 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(G(1,2), freqs))), '-') -plot(freqs, abs(squeeze(freqresp(Gth(1,2), freqs))), '--') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); -title('$d_u/F_v$, $d_v/F_u$'); + ax2 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(G(1,2), freqs))), '-') + plot(freqs, abs(squeeze(freqresp(Gth(1,2), freqs))), '--') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); + title('$d_u/F_v$, $d_v/F_u$'); -ax3 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,1), freqs))), '-') -plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,1), freqs))), '--') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + ax3 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,1), freqs))), '-') + plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,1), freqs))), '--') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -ax4 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,2), freqs))), '-', ... - 'DisplayName', 'Simscape') -plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,2), freqs))), '--', ... - 'DisplayName', 'Analytical') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; -legend('location', 'southwest', 'FontSize', 8); + ax4 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,2), freqs))), '-', ... + 'DisplayName', 'Simscape') + plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,2), freqs))), '--', ... + 'DisplayName', 'Analytical') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; + legend('location', 'southwest', 'FontSize', 8); -linkaxes([ax1,ax2,ax3,ax4],'x'); -xlim([freqs(1), freqs(end)]); -linkaxes([ax1,ax2],'y'); + linkaxes([ax1,ax2,ax3,ax4],'x'); + xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'y'); % Effect of the rotation speed % The transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ are identified for the following rotating speeds. -Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s] + Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s] -Gs = {zeros(2, 2, length(Ws))}; + Gs = {zeros(2, 2, length(Ws))}; -for W_i = 1:length(Ws) - W = Ws(W_i); + for W_i = 1:length(Ws) + W = Ws(W_i); - Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... - [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ... - -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]}; -end + Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... + [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ... + -2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]}; + end % They are compared in Figures [[fig:plant_compare_rotating_speed_direct]] and [[fig:plant_compare_rotating_speed_coupling]]. -freqs = logspace(-2, 1, 1000); + freqs = logspace(-2, 1, 1000); -figure; -tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -for W_i = 1:length(Ws) - plot(freqs, abs(squeeze(freqresp(Gs{W_i}(1,1), freqs))), ... - 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)) -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); -leg = legend('location', 'southwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 6; -ylim([1e-4, 1e2]); -title('$d_u/F_u$, $d_v/F_v$'); + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + for W_i = 1:length(Ws) + plot(freqs, abs(squeeze(freqresp(Gs{W_i}(1,1), freqs))), ... + 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)) + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); + leg = legend('location', 'southwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 6; + ylim([1e-4, 1e2]); + title('$d_u/F_u$, $d_v/F_v$'); -% Phase -ax2 = nexttile; -hold on; -for W_i = 1:length(Ws) - plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs)))) -end -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + % Phase + ax2 = nexttile; + hold on; + for W_i = 1:length(Ws) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs)))) + end + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -linkaxes([ax1,ax2],'x'); -xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); @@ -195,32 +197,32 @@ xlim([freqs(1), freqs(end)]); % [[file:figs/plant_compare_rotating_speed_direct.png]] -figure; -tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -for W_i = 1:length(Ws) - plot(freqs, abs(squeeze(freqresp(Gs{W_i}(2,1), freqs)))) -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); -ylim([1e-4, 1e2]); -title('$d_u/F_v$, $d_v/F_u$'); + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + for W_i = 1:length(Ws) + plot(freqs, abs(squeeze(freqresp(Gs{W_i}(2,1), freqs)))) + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]'); + ylim([1e-4, 1e2]); + title('$d_u/F_v$, $d_v/F_u$'); -% Phase -ax2 = nexttile; -hold on; -for W_i = 1:length(Ws) - plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(2,1), freqs)))) -end -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + % Phase + ax2 = nexttile; + hold on; + for W_i = 1:length(Ws) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(2,1), freqs)))) + end + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -linkaxes([ax1,ax2],'x'); -xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); diff --git a/matlab/matlab/s2_iff_pure_int.m b/matlab/matlab/s2_iff_pure_int.m index a2398d1..5c8f61c 100644 --- a/matlab/matlab/s2_iff_pure_int.m +++ b/matlab/matlab/s2_iff_pure_int.m @@ -4,160 +4,162 @@ clear; close all; clc; %% Intialize Laplace variable s = zpk('s'); +addpath('./src/'); % Path for functions + % Plant Parameters % Let's define initial values for the model. -k = 1; % Actuator Stiffness [N/m] -c = 0.05; % Actuator Damping [N/(m/s)] -m = 1; % Payload mass [kg] + k = 1; % Actuator Stiffness [N/m] + c = 0.05; % Actuator Damping [N/(m/s)] + m = 1; % Payload mass [kg] -xi = c/(2*sqrt(k*m)); -w0 = sqrt(k/m); % [rad/s] + xi = c/(2*sqrt(k*m)); + w0 = sqrt(k/m); % [rad/s] -kp = 0; % [N/m] -cp = 0; % [N/(m/s)] + kp = 0; % [N/m] + cp = 0; % [N/(m/s)] % Comparison of the Analytical Model and the Simscape Model % The rotation speed is set to $\Omega = 0.1 \omega_0$. -W = 0.1*w0; % [rad/s] + W = 0.1*w0; % [rad/s] -Kiff = tf(zeros(2)); + Kiff = tf(zeros(2)); -open('rotating_frame.slx'); + open('rotating_frame.slx'); % And the transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is identified using the Simscape model. -%% Name of the Simulink File -mdl = 'rotating_frame'; + %% Name of the Simulink File + mdl = 'rotating_frame'; -%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1; + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1; -Giff = linearize(mdl, io, 0); + Giff = linearize(mdl, io, 0); -%% Input/Output definition -Giff.InputName = {'Fu', 'Fv'}; -Giff.OutputName = {'fu', 'fv'}; + %% Input/Output definition + Giff.InputName = {'Fu', 'Fv'}; + Giff.OutputName = {'fu', 'fv'}; % The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model. -Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... - [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... - (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; + Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... + [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... + (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; % The two are compared in Figure [[fig:plant_iff_comp_simscape_analytical]] and found to perfectly match. -freqs = logspace(-1, 1, 1000); + freqs = logspace(-1, 1, 1000); -figure; -tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-') -plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); -title('$f_u/F_u$, $f_v/F_v$'); + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-') + plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); + title('$f_u/F_u$, $f_v/F_v$'); -ax2 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-') -plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); -title('$f_u/F_v$, $f_v/F_u$'); + ax2 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-') + plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); + title('$f_u/F_v$, $f_v/F_u$'); -ax3 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + ax3 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -ax4 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ... - 'DisplayName', 'Simscape') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ... - 'DisplayName', 'Analytical') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; -legend('location', 'northeast', 'FontSize', 8); + ax4 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ... + 'DisplayName', 'Simscape') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ... + 'DisplayName', 'Analytical') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; + legend('location', 'northeast', 'FontSize', 8); -linkaxes([ax1,ax2,ax3,ax4],'x'); -xlim([freqs(1), freqs(end)]); -linkaxes([ax1,ax2],'y'); + linkaxes([ax1,ax2,ax3,ax4],'x'); + xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'y'); % Effect of the rotation speed % The transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ are identified for the following rotating speeds. -Ws = [0, 0.2, 0.7]*w0; % Rotating Speeds [rad/s] + Ws = [0, 0.2, 0.7]*w0; % Rotating Speeds [rad/s] -Gsiff = {zeros(2, 2, length(Ws))}; + Gsiff = {zeros(2, 2, length(Ws))}; -for W_i = 1:length(Ws) - W = Ws(W_i); + for W_i = 1:length(Ws) + W = Ws(W_i); - Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... - [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... - (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]}; -end + Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... + [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... + (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]}; + end % The obtained transfer functions are shown in Figure [[fig:plant_iff_compare_rotating_speed]]. -freqs = logspace(-2, 1, 1000); + freqs = logspace(-2, 1, 1000); -figure; -tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -for W_i = 1:length(Ws) - plot(freqs, abs(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))), ... - 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)) -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); -leg = legend('location', 'southeast', 'FontSize', 8); -leg.ItemTokenSize(1) = 6; + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + for W_i = 1:length(Ws) + plot(freqs, abs(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))), ... + 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)) + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); + leg = legend('location', 'southeast', 'FontSize', 8); + leg.ItemTokenSize(1) = 6; -% Phase -ax2 = nexttile; -hold on; -for W_i = 1:length(Ws) - plot(freqs, 180/pi*angle(squeeze(freqresp(Gsiff{W_i}(1,1), freqs)))) -end -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + % Phase + ax2 = nexttile; + hold on; + for W_i = 1:length(Ws) + plot(freqs, 180/pi*angle(squeeze(freqresp(Gsiff{W_i}(1,1), freqs)))) + end + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -linkaxes([ax1,ax2],'x'); -xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); % Decentralized Integral Force Feedback % The decentralized IFF controller consists of pure integrators: @@ -172,29 +174,29 @@ xlim([freqs(1), freqs(end)]); % It is shown that for non-null rotating speed, one pole is bound to the right-half plane, and thus the closed loop system is unstable. -figure; + figure; -gains = logspace(-2, 4, 100); + gains = logspace(-2, 4, 100); -hold on; -for W_i = 1:length(Ws) - set(gca,'ColorOrderIndex',W_i); - plot(real(pole(Gsiff{W_i})), imag(pole(Gsiff{W_i})), 'x', ... - 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)); - set(gca,'ColorOrderIndex',W_i); - plot(real(tzero(Gsiff{W_i})), imag(tzero(Gsiff{W_i})), 'o', ... - 'HandleVisibility', 'off'); - for g = gains - set(gca,'ColorOrderIndex',W_i); - cl_poles = pole(feedback(Gsiff{W_i}, g/s*eye(2))); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); - end -end -hold off; -axis square; -xlim([-2, 0.5]); ylim([0, 2.5]); + hold on; + for W_i = 1:length(Ws) + set(gca,'ColorOrderIndex',W_i); + plot(real(pole(Gsiff{W_i})), imag(pole(Gsiff{W_i})), 'x', ... + 'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0)); + set(gca,'ColorOrderIndex',W_i); + plot(real(tzero(Gsiff{W_i})), imag(tzero(Gsiff{W_i})), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + set(gca,'ColorOrderIndex',W_i); + cl_poles = pole(feedback(Gsiff{W_i}, g/s*eye(2))); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end + end + hold off; + axis square; + xlim([-2, 0.5]); ylim([0, 2.5]); -xlabel('Real Part'); ylabel('Imaginary Part'); -leg = legend('location', 'northwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 8; + xlabel('Real Part'); ylabel('Imaginary Part'); + leg = legend('location', 'northwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 8; diff --git a/matlab/matlab/s3_iff_hpf.m b/matlab/matlab/s3_iff_hpf.m index e1f507c..3bde98e 100644 --- a/matlab/matlab/s3_iff_hpf.m +++ b/matlab/matlab/s3_iff_hpf.m @@ -4,18 +4,20 @@ clear; close all; clc; %% Intialize Laplace variable s = zpk('s'); +addpath('./src/'); % Path for functions + % Plant Parameters % Let's define initial values for the model. -k = 1; % Actuator Stiffness [N/m] -c = 0.05; % Actuator Damping [N/(m/s)] -m = 1; % Payload mass [kg] + k = 1; % Actuator Stiffness [N/m] + c = 0.05; % Actuator Damping [N/(m/s)] + m = 1; % Payload mass [kg] -xi = c/(2*sqrt(k*m)); -w0 = sqrt(k/m); % [rad/s] + xi = c/(2*sqrt(k*m)); + w0 = sqrt(k/m); % [rad/s] -kp = 0; % [N/m] -cp = 0; % [N/(m/s)] + kp = 0; % [N/m] + cp = 0; % [N/(m/s)] % Modified Integral Force Feedback Controller % Let's modify the initial Integral Force Feedback Controller ; instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used: @@ -29,93 +31,93 @@ cp = 0; % [N/(m/s)] % Let's arbitrary choose the following control parameters: -g = 2; -wi = 0.1*w0; + g = 2; + wi = 0.1*w0; -Kiff = (g/(wi+s))*eye(2); + Kiff = (g/(wi+s))*eye(2); % And the following rotating speed. -W = 0.1*w0; + W = 0.1*w0; -Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... - [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... - (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; + Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... + [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... + (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; % The obtained Loop Gain is shown in Figure [[fig:loop_gain_modified_iff]]. -freqs = logspace(-2, 1, 1000); + freqs = logspace(-2, 1, 1000); -figure; -tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Giff(1,1)*(g/s), freqs)))) -plot(freqs, abs(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs)))) -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Loop Gain'); + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(Giff(1,1)*(g/s), freqs)))) + plot(freqs, abs(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs)))) + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Loop Gain'); -% Phase -ax2 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*(g/s), freqs))), ... - 'DisplayName', 'IFF') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))), ... - 'DisplayName', 'IFF + HPF') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -leg = legend('location', 'southwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 6; + % Phase + ax2 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*(g/s), freqs))), ... + 'DisplayName', 'IFF') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))), ... + 'DisplayName', 'IFF + HPF') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + leg = legend('location', 'southwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 6; -hold off; + hold off; -linkaxes([ax1,ax2],'x'); -xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); % Root Locus % As shown in the Root Locus plot (Figure [[fig:root_locus_modified_iff]]), for some value of the gain, the system remains stable. -figure; + figure; -gains = logspace(-2, 4, 100); + gains = logspace(-2, 4, 100); -hold on; -% Pure Integrator -set(gca,'ColorOrderIndex',1); -plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF'); -set(gca,'ColorOrderIndex',1); -plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off'); -for g = gains - clpoles = pole(feedback(Giff, (g/s)*eye(2))); - set(gca,'ColorOrderIndex',1); - plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off'); -end -% Modified IFF -set(gca,'ColorOrderIndex',2); -plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF'); -set(gca,'ColorOrderIndex',2); -plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off'); -for g = gains - clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2))); - set(gca,'ColorOrderIndex',2); - plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off'); -end -hold off; -axis square; -xlim([-2, 0.5]); ylim([-1.25, 1.25]); -leg = legend('location', 'northwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 8; -xlabel('Real Part'); ylabel('Imaginary Part'); + hold on; + % Pure Integrator + set(gca,'ColorOrderIndex',1); + plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF'); + set(gca,'ColorOrderIndex',1); + plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off'); + for g = gains + clpoles = pole(feedback(Giff, (g/s)*eye(2))); + set(gca,'ColorOrderIndex',1); + plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off'); + end + % Modified IFF + set(gca,'ColorOrderIndex',2); + plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF'); + set(gca,'ColorOrderIndex',2); + plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off'); + for g = gains + clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2))); + set(gca,'ColorOrderIndex',2); + plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off'); + end + hold off; + axis square; + xlim([-2, 0.5]); ylim([-1.25, 1.25]); + leg = legend('location', 'northwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 8; + xlabel('Real Part'); ylabel('Imaginary Part'); @@ -125,40 +127,40 @@ xlabel('Real Part'); ylabel('Imaginary Part'); % [[file:figs/root_locus_modified_iff.png]] -xlim([-0.2, 0.1]); ylim([-0.15, 0.15]); -legend('hide'); + xlim([-0.2, 0.1]); ylim([-0.15, 0.15]); + legend('hide'); % What is the optimal $\omega_i$ and $g$? % In order to visualize the effect of $\omega_i$ on the attainable damping, the Root Locus is displayed in Figure [[fig:root_locus_wi_modified_iff]] for the following $\omega_i$: -wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s] + wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s] -figure; + figure; -gains = logspace(-2, 4, 100); + gains = logspace(-2, 4, 100); -hold on; -for wi_i = 1:length(wis) - set(gca,'ColorOrderIndex',wi_i); - wi = wis(wi_i); - L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0)); - for g = gains - clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2))); - set(gca,'ColorOrderIndex',wi_i); - plot(real(clpoles), imag(clpoles), '.'); - end -end -plot(real(pole(Giff)), imag(pole(Giff)), 'kx'); -plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko'); -hold off; -axis square; -xlim([-2.3, 0.1]); ylim([-1.2, 1.2]); -xticks([-2:1:2]); yticks([-2:1:2]); -leg = legend(L, 'location', 'northwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 8; -xlabel('Real Part'); ylabel('Imaginary Part'); + hold on; + for wi_i = 1:length(wis) + set(gca,'ColorOrderIndex',wi_i); + wi = wis(wi_i); + L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0)); + for g = gains + clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2))); + set(gca,'ColorOrderIndex',wi_i); + plot(real(clpoles), imag(clpoles), '.'); + end + end + plot(real(pole(Giff)), imag(pole(Giff)), 'kx'); + plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko'); + hold off; + axis square; + xlim([-2.3, 0.1]); ylim([-1.2, 1.2]); + xticks([-2:1:2]); yticks([-2:1:2]); + leg = legend(L, 'location', 'northwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 8; + xlabel('Real Part'); ylabel('Imaginary Part'); -clear L + clear L @@ -168,9 +170,9 @@ clear L % [[file:figs/root_locus_wi_modified_iff.png]] -xlim([-0.2, 0.1]); ylim([-0.15, 0.15]); -xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]); -legend('hide'); + xlim([-0.2, 0.1]); ylim([-0.15, 0.15]); + xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]); + legend('hide'); @@ -192,37 +194,37 @@ legend('hide'); % To find the optimum, the gain that maximize the simultaneous damping of the mode is identified for a wide range of $\omega_i$ (Figure [[fig:mod_iff_damping_wi]]). -wis = logspace(-2, 1, 100)*w0; % [rad/s] + wis = logspace(-2, 1, 100)*w0; % [rad/s] -opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping -opt_gain = zeros(1, length(wis)); % Corresponding optimal gain + opt_xi = zeros(1, length(wis)); % Optimal simultaneous damping + opt_gain = zeros(1, length(wis)); % Corresponding optimal gain -for wi_i = 1:length(wis) - wi = wis(wi_i); - Kiff = 1/(s + wi)*eye(2); + for wi_i = 1:length(wis) + wi = wis(wi_i); + Kiff = 1/(s + wi)*eye(2); - fun = @(g)computeSimultaneousDamping(g, Giff, Kiff); + fun = @(g)computeSimultaneousDamping(g, Giff, Kiff); - [g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((w0/W)^2 - 1)); - opt_xi(wi_i) = 1/xi_opt; - opt_gain(wi_i) = g_opt; -end + [g_opt, xi_opt] = fminsearch(fun, 0.5*wi*((w0/W)^2 - 1)); + opt_xi(wi_i) = 1/xi_opt; + opt_gain(wi_i) = g_opt; + end -figure; -yyaxis left -plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$'); -set(gca, 'YScale', 'lin'); -ylim([0,1]); -ylabel('Damping Ratio $\xi$'); + figure; + yyaxis left + plot(wis, opt_xi, '-', 'DisplayName', '$\xi_{cl}$'); + set(gca, 'YScale', 'lin'); + ylim([0,1]); + ylabel('Damping Ratio $\xi$'); -yyaxis right -hold on; -plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$'); -plot(wis, wis*((w0/W)^2 - 1), '--', 'DisplayName', '$g_{max}$'); -set(gca, 'YScale', 'lin'); -ylim([0,10]); -ylabel('Controller gain $g$'); + yyaxis right + hold on; + plot(wis, opt_gain, '-', 'DisplayName', '$g_{opt}$'); + plot(wis, wis*((w0/W)^2 - 1), '--', 'DisplayName', '$g_{max}$'); + set(gca, 'YScale', 'lin'); + ylim([0,10]); + ylabel('Controller gain $g$'); -xlabel('$\omega_i/\omega_0$'); -set(gca, 'XScale', 'log'); -legend('location', 'northeast', 'FontSize', 8); + xlabel('$\omega_i/\omega_0$'); + set(gca, 'XScale', 'log'); + legend('location', 'northeast', 'FontSize', 8); diff --git a/matlab/matlab/s4_iff_kp.m b/matlab/matlab/s4_iff_kp.m index 3b655f1..90fea32 100644 --- a/matlab/matlab/s4_iff_kp.m +++ b/matlab/matlab/s4_iff_kp.m @@ -4,108 +4,110 @@ clear; close all; clc; %% Intialize Laplace variable s = zpk('s'); +addpath('./src/'); % Path for functions + % Plant Parameters % Let's define initial values for the model. -k = 1; % Actuator Stiffness [N/m] -c = 0.05; % Actuator Damping [N/(m/s)] -m = 1; % Payload mass [kg] + k = 1; % Actuator Stiffness [N/m] + c = 0.05; % Actuator Damping [N/(m/s)] + m = 1; % Payload mass [kg] -xi = c/(2*sqrt(k*m)); -w0 = sqrt(k/m); % [rad/s] + xi = c/(2*sqrt(k*m)); + w0 = sqrt(k/m); % [rad/s] -kp = 0; % [N/m] -cp = 0; % [N/(m/s)] + kp = 0; % [N/m] + cp = 0; % [N/(m/s)] % Comparison of the Analytical Model and the Simscape Model % The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model. -W = 0.1*w0; % [rad/s] + W = 0.1*w0; % [rad/s] -kp = 1.5*m*W^2; -cp = 0; + kp = 1.5*m*W^2; + cp = 0; -Kiff = tf(zeros(2)); + Kiff = tf(zeros(2)); -open('rotating_frame.slx'); + open('rotating_frame.slx'); -%% Name of the Simulink File -mdl = 'rotating_frame'; + %% Name of the Simulink File + mdl = 'rotating_frame'; -%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1; + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1; -Giff = linearize(mdl, io, 0); + Giff = linearize(mdl, io, 0); -%% Input/Output definition -Giff.InputName = {'Fu', 'Fv'}; -Giff.OutputName = {'fu', 'fv'}; + %% Input/Output definition + Giff.InputName = {'Fu', 'Fv'}; + Giff.OutputName = {'fu', 'fv'}; -w0p = sqrt((k + kp)/m); -xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); -Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; -Giff_th.InputName = {'Fu', 'Fv'}; -Giff_th.OutputName = {'fu', 'fv'}; + Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; + Giff_th.InputName = {'Fu', 'Fv'}; + Giff_th.OutputName = {'fu', 'fv'}; -freqs = logspace(-1, 1, 1000); + freqs = logspace(-1, 1, 1000); -figure; -tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 2, 'TileSpacing', 'None', 'Padding', 'None'); -ax1 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-') -plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); -title('$f_u/F_u$, $f_v/F_v$'); + ax1 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-') + plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); + title('$f_u/F_u$, $f_v/F_v$'); -ax2 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-') -plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); -title('$f_u/F_v$, $f_v/F_u$'); + ax2 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-') + plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); + title('$f_u/F_v$, $f_v/F_u$'); -ax3 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-', ... - 'DisplayName', 'Simscape') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--', ... - 'DisplayName', 'Analytical') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; -legend('location', 'southwest', 'FontSize', 8); + ax3 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-', ... + 'DisplayName', 'Simscape') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--', ... + 'DisplayName', 'Analytical') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; + legend('location', 'southwest', 'FontSize', 8); -ax4 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + ax4 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -linkaxes([ax1,ax2,ax3,ax4],'x'); -xlim([freqs(1), freqs(end)]); -linkaxes([ax1,ax2],'y'); + linkaxes([ax1,ax2,ax3,ax4],'x'); + xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'y'); % Effect of the parallel stiffness on the IFF plant % The rotation speed is set to $\Omega = 0.1 \omega_0$. -W = 0.1*w0; % [rad/s] + W = 0.1*w0; % [rad/s] @@ -119,69 +121,69 @@ W = 0.1*w0; % [rad/s] % One can see that for $k_p > m \Omega^2$, the systems shows alternating complex conjugate poles and zeros. -kp = 0; + kp = 0; -w0p = sqrt((k + kp)/m); -xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); -Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; + Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; -kp = 0.5*m*W^2; -k = 1 - kp; + kp = 0.5*m*W^2; + k = 1 - kp; -w0p = sqrt((k + kp)/m); -xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); -Giff_s = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; + Giff_s = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; -kp = 1.5*m*W^2; -k = 1 - kp; + kp = 1.5*m*W^2; + k = 1 - kp; -w0p = sqrt((k + kp)/m); -xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); -Giff_l = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; + Giff_l = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; -freqs = logspace(-2, 1, 1000); + freqs = logspace(-2, 1, 1000); -figure; -tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); + figure; + tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); -% Magnitude -ax1 = nexttile([2, 1]); -hold on; -plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), 'k-', ... - 'DisplayName', '$k_p = 0$') -plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ... - 'DisplayName', '$k_p < m\Omega^2$') -plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ... - 'DisplayName', '$k_p > m\Omega^2$') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); -ylim([1e-4, 2e1]); -legend('location', 'southeast', 'FontSize', 8); + % Magnitude + ax1 = nexttile([2, 1]); + hold on; + plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), 'k-', ... + 'DisplayName', '$k_p = 0$') + plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ... + 'DisplayName', '$k_p < m\Omega^2$') + plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ... + 'DisplayName', '$k_p > m\Omega^2$') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]'); + ylim([1e-4, 2e1]); + legend('location', 'southeast', 'FontSize', 8); -% Phase -ax2 = nexttile; -hold on; -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), 'k-') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--') -plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:') -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); -yticks(-180:90:180); -ylim([-180 180]); -hold off; + % Phase + ax2 = nexttile; + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), 'k-') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--') + plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:') + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + xlabel('Frequency [rad/s]'); ylabel('Phase [deg]'); + yticks(-180:90:180); + ylim([-180 180]); + hold off; -linkaxes([ax1,ax2],'x'); -xlim([freqs(1), freqs(end)]); + linkaxes([ax1,ax2],'x'); + xlim([freqs(1), freqs(end)]); % IFF when adding a spring in parallel % In Figure [[fig:root_locus_iff_kp]] is displayed the Root Locus in the three considered cases with @@ -200,55 +202,55 @@ xlim([freqs(1), freqs(end)]); % \end{equation} -gains = logspace(-2, 2, 100); + gains = logspace(-2, 2, 100); -figure; -hold on; -set(gca,'ColorOrderIndex',1); -plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... - 'DisplayName', '$k_p = 0$'); -set(gca,'ColorOrderIndex',1); -plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... - 'HandleVisibility', 'off'); -for g = gains - cl_poles = pole(feedback(Giff, (g/s)*eye(2))); - set(gca,'ColorOrderIndex',1); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); -end + figure; + hold on; + set(gca,'ColorOrderIndex',1); + plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... + 'DisplayName', '$k_p = 0$'); + set(gca,'ColorOrderIndex',1); + plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + cl_poles = pole(feedback(Giff, (g/s)*eye(2))); + set(gca,'ColorOrderIndex',1); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end -set(gca,'ColorOrderIndex',2); -plot(real(pole(Giff_s)), imag(pole(Giff_s)), 'x', ... - 'DisplayName', '$k_p < m\Omega^2$'); -set(gca,'ColorOrderIndex',2); -plot(real(tzero(Giff_s)), imag(tzero(Giff_s)), 'o', ... - 'HandleVisibility', 'off'); -for g = gains - cl_poles = pole(feedback(Giff_s, (g/s)*eye(2))); - set(gca,'ColorOrderIndex',2); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); -end + set(gca,'ColorOrderIndex',2); + plot(real(pole(Giff_s)), imag(pole(Giff_s)), 'x', ... + 'DisplayName', '$k_p < m\Omega^2$'); + set(gca,'ColorOrderIndex',2); + plot(real(tzero(Giff_s)), imag(tzero(Giff_s)), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + cl_poles = pole(feedback(Giff_s, (g/s)*eye(2))); + set(gca,'ColorOrderIndex',2); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end -set(gca,'ColorOrderIndex',3); -plot(real(pole(Giff_l)), imag(pole(Giff_l)), 'x', ... - 'DisplayName', '$k_p > m\Omega^2$'); -set(gca,'ColorOrderIndex',3); -plot(real(tzero(Giff_l)), imag(tzero(Giff_l)), 'o', ... - 'HandleVisibility', 'off'); -for g = gains - set(gca,'ColorOrderIndex',3); - cl_poles = pole(feedback(Giff_l, (g/s)*eye(2))); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); -end -hold off; -axis square; -xlim([-1, 0.2]); ylim([0, 1.2]); + set(gca,'ColorOrderIndex',3); + plot(real(pole(Giff_l)), imag(pole(Giff_l)), 'x', ... + 'DisplayName', '$k_p > m\Omega^2$'); + set(gca,'ColorOrderIndex',3); + plot(real(tzero(Giff_l)), imag(tzero(Giff_l)), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + set(gca,'ColorOrderIndex',3); + cl_poles = pole(feedback(Giff_l, (g/s)*eye(2))); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end + hold off; + axis square; + xlim([-1, 0.2]); ylim([0, 1.2]); -xlabel('Real Part'); ylabel('Imaginary Part'); -leg = legend('location', 'northwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 8; + xlabel('Real Part'); ylabel('Imaginary Part'); + leg = legend('location', 'northwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 8; @@ -258,97 +260,97 @@ leg.ItemTokenSize(1) = 8; % [[file:figs/root_locus_iff_kp.png]] -xlim([-0.04, 0.06]); ylim([0, 0.1]); -legend('hide'); + xlim([-0.04, 0.06]); ylim([0, 0.1]); + legend('hide'); % Effect of $k_p$ on the attainable damping % However, having large values of $k_p$ may decrease the attainable damping. % To study the second point, Root Locus plots for the following values of $k_p$ are shown in Figure [[fig:root_locus_iff_kps]]. -kps = [2, 20, 40]*m*W^2; + kps = [2, 20, 40]*m*W^2; % It is shown that large values of $k_p$ decreases the attainable damping. -figure; + figure; -gains = logspace(-2, 4, 500); + gains = logspace(-2, 4, 500); -hold on; -for kp_i = 1:length(kps) - kp = kps(kp_i); - k = 1 - kp; + hold on; + for kp_i = 1:length(kps) + kp = kps(kp_i); + k = 1 - kp; - w0p = sqrt((k + kp)/m); - xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); - Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; + Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; - set(gca,'ColorOrderIndex',kp_i); - plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... - 'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/(m*W^2))); - set(gca,'ColorOrderIndex',kp_i); - plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... - 'HandleVisibility', 'off'); - for g = gains - Kiffa = (g/s)*eye(2); - cl_poles = pole(feedback(Giff, Kiffa)); - set(gca,'ColorOrderIndex',kp_i); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); - end -end -hold off; -axis square; -xlim([-1.2, 0.2]); ylim([0, 1.4]); + set(gca,'ColorOrderIndex',kp_i); + plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... + 'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/(m*W^2))); + set(gca,'ColorOrderIndex',kp_i); + plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + Kiffa = (g/s)*eye(2); + cl_poles = pole(feedback(Giff, Kiffa)); + set(gca,'ColorOrderIndex',kp_i); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end + end + hold off; + axis square; + xlim([-1.2, 0.2]); ylim([0, 1.4]); -xlabel('Real Part'); ylabel('Imaginary Part'); -leg = legend('location', 'northwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 8; + xlabel('Real Part'); ylabel('Imaginary Part'); + leg = legend('location', 'northwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 8; -alphas = logspace(-2, 0, 100); + alphas = logspace(-2, 0, 100); -opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping -opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain + opt_xi = zeros(1, length(alphas)); % Optimal simultaneous damping + opt_gain = zeros(1, length(alphas)); % Corresponding optimal gain -Kiff = 1/s*eye(2); + Kiff = 1/s*eye(2); -for alpha_i = 1:length(alphas) - kp = alphas(alpha_i); - k = 1 - alphas(alpha_i); + for alpha_i = 1:length(alphas) + kp = alphas(alpha_i); + k = 1 - alphas(alpha_i); - w0p = sqrt((k + kp)/m); - xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); - Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; + Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2]; - fun = @(g)computeSimultaneousDamping(g, Giff, Kiff); + fun = @(g)computeSimultaneousDamping(g, Giff, Kiff); - [g_opt, xi_opt] = fminsearch(fun, 2); - opt_xi(alpha_i) = 1/xi_opt; - opt_gain(alpha_i) = g_opt; -end + [g_opt, xi_opt] = fminsearch(fun, 2); + opt_xi(alpha_i) = 1/xi_opt; + opt_gain(alpha_i) = g_opt; + end -figure; -yyaxis left -plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$'); -set(gca, 'YScale', 'lin'); -ylim([0,1]); -ylabel('Damping Ratio $\xi$'); + figure; + yyaxis left + plot(alphas, opt_xi, '-', 'DisplayName', '$\xi_{cl}$'); + set(gca, 'YScale', 'lin'); + ylim([0,1]); + ylabel('Damping Ratio $\xi$'); -yyaxis right -hold on; -plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$'); -set(gca, 'YScale', 'lin'); -ylim([0,2.5]); -ylabel('Controller gain $g$'); + yyaxis right + hold on; + plot(alphas, opt_gain, '-', 'DisplayName', '$g_{opt}$'); + set(gca, 'YScale', 'lin'); + ylim([0,2.5]); + ylabel('Controller gain $g$'); -xlabel('$\alpha$'); -set(gca, 'XScale', 'log'); -legend('location', 'northeast', 'FontSize', 8); + xlabel('$\alpha$'); + set(gca, 'XScale', 'log'); + legend('location', 'northeast', 'FontSize', 8); diff --git a/matlab/matlab/s5_act_damp_comparison.m b/matlab/matlab/s5_act_damp_comparison.m index 9a7a69e..c8b091e 100644 --- a/matlab/matlab/s5_act_damp_comparison.m +++ b/matlab/matlab/s5_act_damp_comparison.m @@ -4,103 +4,105 @@ clear; close all; clc; %% Intialize Laplace variable s = zpk('s'); +addpath('./src/'); % Path for functions + % Plant Parameters % Let's define initial values for the model. -k = 1; % Actuator Stiffness [N/m] -c = 0.05; % Actuator Damping [N/(m/s)] -m = 1; % Payload mass [kg] + k = 1; % Actuator Stiffness [N/m] + c = 0.05; % Actuator Damping [N/(m/s)] + m = 1; % Payload mass [kg] -xi = c/(2*sqrt(k*m)); -w0 = sqrt(k/m); % [rad/s] + xi = c/(2*sqrt(k*m)); + w0 = sqrt(k/m); % [rad/s] -kp = 0; % [N/m] -cp = 0; % [N/(m/s)] + kp = 0; % [N/m] + cp = 0; % [N/(m/s)] % The rotating speed is set to $\Omega = 0.1 \omega_0$. -W = 0.1*w0; + W = 0.1*w0; % Root Locus % IFF with High Pass Filter -wi = 0.1*w0; % [rad/s] + wi = 0.1*w0; % [rad/s] -Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... - [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... - (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; + Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ... + [(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ... + (2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]; % IFF With parallel Stiffness -kp = 5*m*W^2; -k = k - kp; + kp = 5*m*W^2; + k = k - kp; -w0p = sqrt((k + kp)/m); -xip = c/(2*sqrt((k+kp)*m)); + w0p = sqrt((k + kp)/m); + xip = c/(2*sqrt((k+kp)*m)); -Giff_kp = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... - (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); - (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; + Giff_kp = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ... + (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)); + (2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ]; -k = k + kp; + k = k + kp; -figure; + figure; -gains = logspace(-2, 2, 100); + gains = logspace(-2, 2, 100); -hold on; -set(gca,'ColorOrderIndex',1); -plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... - 'DisplayName', 'IFF + LFP'); -set(gca,'ColorOrderIndex',1); -plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... - 'HandleVisibility', 'off'); -for g = gains - Kiff = (g/(wi + s))*eye(2); - cl_poles = pole(feedback(Giff, Kiff)); - set(gca,'ColorOrderIndex',1); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); -end + hold on; + set(gca,'ColorOrderIndex',1); + plot(real(pole(Giff)), imag(pole(Giff)), 'x', ... + 'DisplayName', 'IFF + LFP'); + set(gca,'ColorOrderIndex',1); + plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + Kiff = (g/(wi + s))*eye(2); + cl_poles = pole(feedback(Giff, Kiff)); + set(gca,'ColorOrderIndex',1); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end -set(gca,'ColorOrderIndex',2); -plot(real(pole(Giff_kp)), imag(pole(Giff_kp)), 'x', ... - 'DisplayName', 'IFF + $k_p$'); -set(gca,'ColorOrderIndex',2); -plot(real(tzero(Giff_kp)), imag(tzero(Giff_kp)), 'o', ... - 'HandleVisibility', 'off'); -for g = gains - Kiffa = (g/s)*eye(2); - cl_poles = pole(feedback(Giff_kp, Kiffa)); - set(gca,'ColorOrderIndex',2); - plot(real(cl_poles), imag(cl_poles), '.', ... - 'HandleVisibility', 'off'); -end -hold off; -axis square; -xlim([-1.2, 0.05]); ylim([0, 1.25]); + set(gca,'ColorOrderIndex',2); + plot(real(pole(Giff_kp)), imag(pole(Giff_kp)), 'x', ... + 'DisplayName', 'IFF + $k_p$'); + set(gca,'ColorOrderIndex',2); + plot(real(tzero(Giff_kp)), imag(tzero(Giff_kp)), 'o', ... + 'HandleVisibility', 'off'); + for g = gains + Kiffa = (g/s)*eye(2); + cl_poles = pole(feedback(Giff_kp, Kiffa)); + set(gca,'ColorOrderIndex',2); + plot(real(cl_poles), imag(cl_poles), '.', ... + 'HandleVisibility', 'off'); + end + hold off; + axis square; + xlim([-1.2, 0.05]); ylim([0, 1.25]); -xlabel('Real Part'); ylabel('Imaginary Part'); -leg = legend('location', 'northwest', 'FontSize', 8); -leg.ItemTokenSize(1) = 8; + xlabel('Real Part'); ylabel('Imaginary Part'); + leg = legend('location', 'northwest', 'FontSize', 8); + leg.ItemTokenSize(1) = 8; % Controllers - Optimal Gains % In order to compare to three considered Active Damping techniques, gains that yield maximum damping of all the modes are computed for each case. -fun = @(g)computeSimultaneousDamping(g, Giff, (1/(wi+s))*eye(2)); + fun = @(g)computeSimultaneousDamping(g, Giff, (1/(wi+s))*eye(2)); -[opt_gain_iff, opt_xi_iff] = fminsearch(fun, 0.5*(w0^2/W^2 - 1)*wi); -opt_xi_iff = 1/opt_xi_iff; + [opt_gain_iff, opt_xi_iff] = fminsearch(fun, 0.5*(w0^2/W^2 - 1)*wi); + opt_xi_iff = 1/opt_xi_iff; -fun = @(g)computeSimultaneousDamping(g, Giff_kp, 1/s*eye(2)); + fun = @(g)computeSimultaneousDamping(g, Giff_kp, 1/s*eye(2)); -[opt_gain_kp, opt_xi_kp] = fminsearch(fun, 2); -opt_xi_kp = 1/opt_xi_kp; + [opt_gain_kp, opt_xi_kp] = fminsearch(fun, 2); + opt_xi_kp = 1/opt_xi_kp; % Passive Damping - Critical Damping % \begin{equation} @@ -113,116 +115,116 @@ opt_xi_kp = 1/opt_xi_kp; % \end{equation} -c_opt = 2*sqrt(k*m); + c_opt = 2*sqrt(k*m); % Transmissibility And Compliance % <> -open('rotating_frame.slx'); + open('rotating_frame.slx'); -%% Name of the Simulink File -mdl = 'rotating_frame'; + %% Name of the Simulink File + mdl = 'rotating_frame'; -%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/fd'], 1, 'input'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1; + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/fd'], 1, 'input'); io_i = io_i + 1; + io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1; % Open Loop :ignore: -Kiff = tf(zeros(2)); + Kiff = tf(zeros(2)); -kp = 0; -cp = 0; + kp = 0; + cp = 0; -G_ol = linearize(mdl, io, 0); + G_ol = linearize(mdl, io, 0); -%% Input/Output definition -G_ol.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; -G_ol.OutputName = {'Dx', 'Dy'}; + %% Input/Output definition + G_ol.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; + G_ol.OutputName = {'Dx', 'Dy'}; % Passive Damping -kp = 0; -cp = 0; + kp = 0; + cp = 0; -c_old = c; -c = c_opt; + c_old = c; + c = c_opt; -Kiff = tf(zeros(2)); + Kiff = tf(zeros(2)); -G_pas = linearize(mdl, io, 0); + G_pas = linearize(mdl, io, 0); -%% Input/Output definition -G_pas.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; -G_pas.OutputName = {'Dx', 'Dy'}; + %% Input/Output definition + G_pas.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; + G_pas.OutputName = {'Dx', 'Dy'}; -c = c_old; + c = c_old; % Pseudo Integrator IFF :ignore: -kp = 0; -cp = 0; + kp = 0; + cp = 0; -Kiff = opt_gain_iff/(wi + s)*tf(eye(2)); + Kiff = opt_gain_iff/(wi + s)*tf(eye(2)); -G_iff = linearize(mdl, io, 0); + G_iff = linearize(mdl, io, 0); -%% Input/Output definition -G_iff.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; -G_iff.OutputName = {'Dx', 'Dy'}; + %% Input/Output definition + G_iff.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; + G_iff.OutputName = {'Dx', 'Dy'}; % IFF With parallel Stiffness :ignore: -kp = 5*m*W^2; -cp = 0.01; + kp = 5*m*W^2; + cp = 0.01; -Kiff = opt_gain_kp/s*tf(eye(2)); + Kiff = opt_gain_kp/s*tf(eye(2)); -G_kp = linearize(mdl, io, 0); + G_kp = linearize(mdl, io, 0); -%% Input/Output definition -G_kp.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; -G_kp.OutputName = {'Dx', 'Dy'}; + %% Input/Output definition + G_kp.InputName = {'Dwx', 'Dwy', 'Fdx', 'Fdy'}; + G_kp.OutputName = {'Dx', 'Dy'}; % Transmissibility :ignore: -freqs = logspace(-2, 1, 1000); + freqs = logspace(-2, 1, 1000); -figure; -hold on; -plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Dwx'}), freqs))), ... - 'DisplayName', 'IFF + HPF') -plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Dwx'}), freqs))), ... - 'DisplayName', 'IFF + $k_p$') -plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Dwx'}), freqs))), ... - 'DisplayName', 'Passive') -plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Dwx'}), freqs))), 'k-', ... - 'DisplayName', 'Open-Loop') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylim([1e-2, 3e1]); -xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]'); -legend('location', 'southwest', 'FontSize', 8); + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Dwx'}), freqs))), ... + 'DisplayName', 'IFF + HPF') + plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Dwx'}), freqs))), ... + 'DisplayName', 'IFF + $k_p$') + plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Dwx'}), freqs))), ... + 'DisplayName', 'Passive') + plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Dwx'}), freqs))), 'k-', ... + 'DisplayName', 'Open-Loop') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-2, 3e1]); + xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]'); + legend('location', 'southwest', 'FontSize', 8); % Compliance :ignore: -freqs = logspace(-2, 1, 1000); + freqs = logspace(-2, 1, 1000); -figure; -hold on; -plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Fdx'}), freqs))), ... - 'DisplayName', 'IFF + HPF') -plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Fdx'}), freqs))), ... - 'DisplayName', 'IFF + $k_p$') -plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Fdx'}), freqs))), ... - 'DisplayName', 'Passive') -plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Fdx'}), freqs))), 'k-', ... - 'DisplayName', 'Open-Loop') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylim([1e-2, 3e1]); -xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]'); -legend('location', 'southwest', 'FontSize', 8); + figure; + hold on; + plot(freqs, abs(squeeze(freqresp(G_iff({'Dx'}, {'Fdx'}), freqs))), ... + 'DisplayName', 'IFF + HPF') + plot(freqs, abs(squeeze(freqresp(G_kp( {'Dx'}, {'Fdx'}), freqs))), ... + 'DisplayName', 'IFF + $k_p$') + plot(freqs, abs(squeeze(freqresp(G_pas({'Dx'}, {'Fdx'}), freqs))), ... + 'DisplayName', 'Passive') + plot(freqs, abs(squeeze(freqresp(G_ol( {'Dx'}, {'Fdx'}), freqs))), 'k-', ... + 'DisplayName', 'Open-Loop') + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylim([1e-2, 3e1]); + xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]'); + legend('location', 'southwest', 'FontSize', 8); diff --git a/matlab/matlab/src/computeSimultaneousDamping.m b/matlab/matlab/src/computeSimultaneousDamping.m new file mode 100644 index 0000000..c56c804 --- /dev/null +++ b/matlab/matlab/src/computeSimultaneousDamping.m @@ -0,0 +1,10 @@ +% =computeSimultaneousDamping= + + function [xi_min] = computeSimultaneousDamping(g, G, K) + [w, xi] = damp(minreal(feedback(G, g*K))); + xi_min = 1/min(xi); + + if xi_min < 0 + xi_min = 1e8; + end + end diff --git a/matlab/matlab/src/rootLocusPolesSorted.m b/matlab/matlab/src/rootLocusPolesSorted.m new file mode 100644 index 0000000..8df34ec --- /dev/null +++ b/matlab/matlab/src/rootLocusPolesSorted.m @@ -0,0 +1,93 @@ + function [poles] = rootLocusPolesSorted(G, K, gains, args) + % rootLocusPolesSorted - + % + % Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args) + % + % Inputs: + % - G, K, gains, args - + % + % Outputs: + % - poles - + + arguments + G + K + gains + args.minreal double {mustBeNumericOrLogical} = false + args.p_half double {mustBeNumericOrLogical} = false + args.d_max double {mustBeNumeric} = -1 + end + + if args.minreal + p1 = pole(minreal(feedback(G, gains(1)*K))); + [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true); + p1 = p1(i_uniq); + + poles = zeros(length(p1), length(gains)); + poles(:, 1) = p1; + else + p1 = pole(feedback(G, gains(1)*K)); + [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true); + p1 = p1(i_uniq); + + poles = zeros(length(p1), length(gains)); + poles(:, 1) = p1; + end + + if args.minreal + p2 = pole(minreal(feedback(G, gains(2)*K))); + [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true); + p2 = p2(i_uniq); + poles(:, 2) = p2; + else + p2 = pole(feedback(G, gains(2)*K)); + [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true); + p2 = p2(i_uniq); + poles(:, 2) = p2; + end + + for g_i = 3:length(gains) + % Estimated value of the poles + poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2)); + + % New values for the poles + poles_gi = pole(feedback(G, gains(g_i)*K)); + [~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true); + poles_gi = poles_gi(i_uniq); + + % Array of distances between all the poles + poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.')); + + % Get indices corresponding to distances from lowest to highest + [~, c] = sort(min(poles_dist)); + + as = 1:length(poles_gi); + + % for each column of poles_dist corresponding to the i'th pole + % with closest previous poles + for p_i = c + % Get the indice a_i of the previous pole that is the closest + % to pole c(p_i) + [~, a_i] = min(poles_dist(:, p_i)); + + poles(as(a_i), g_i) = poles_gi(p_i); + + % Remove old poles that are already matched + % poles_gi(as(a_i), :) = []; + poles_dist(a_i, :) = []; + as(a_i) = []; + end + end + + if args.d_max > 0 + poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :); + end + + if args.p_half + poles = poles(1:round(end/2), :); + end + + [~, s_p] = sort(imag(poles(:,1)), 'descend'); + poles = poles(s_p, :); + + poles = poles.'; diff --git a/matlab/src/computeSimultaneousDamping.m b/matlab/src/computeSimultaneousDamping.m deleted file mode 100644 index d3fccc6..0000000 --- a/matlab/src/computeSimultaneousDamping.m +++ /dev/null @@ -1,10 +0,0 @@ -% =computeSimultaneousDamping= - -function [xi_min] = computeSimultaneousDamping(g, G, K) - [w, xi] = damp(minreal(feedback(G, g*K))); - xi_min = 1/min(xi); - - if xi_min < 0 - xi_min = 1e8; - end -end diff --git a/matlab/src/rootLocusPolesSorted.m b/matlab/src/rootLocusPolesSorted.m deleted file mode 100644 index 04beb9f..0000000 --- a/matlab/src/rootLocusPolesSorted.m +++ /dev/null @@ -1,93 +0,0 @@ -function [poles] = rootLocusPolesSorted(G, K, gains, args) -% rootLocusPolesSorted - -% -% Syntax: [poles] = rootLocusPolesSorted(G, K, gains, args) -% -% Inputs: -% - G, K, gains, args - -% -% Outputs: -% - poles - - -arguments - G - K - gains - args.minreal double {mustBeNumericOrLogical} = false - args.p_half double {mustBeNumericOrLogical} = false - args.d_max double {mustBeNumeric} = -1 -end - -if args.minreal - p1 = pole(minreal(feedback(G, gains(1)*K))); - [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true); - p1 = p1(i_uniq); - - poles = zeros(length(p1), length(gains)); - poles(:, 1) = p1; -else - p1 = pole(feedback(G, gains(1)*K)); - [~, i_uniq] = uniquetol([real(p1), imag(p1)], 1e-10, 'ByRows', true); - p1 = p1(i_uniq); - - poles = zeros(length(p1), length(gains)); - poles(:, 1) = p1; -end - -if args.minreal - p2 = pole(minreal(feedback(G, gains(2)*K))); - [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true); - p2 = p2(i_uniq); - poles(:, 2) = p2; -else - p2 = pole(feedback(G, gains(2)*K)); - [~, i_uniq] = uniquetol([real(p2), imag(p2)], 1e-10, 'ByRows', true); - p2 = p2(i_uniq); - poles(:, 2) = p2; -end - -for g_i = 3:length(gains) - % Estimated value of the poles - poles_est = poles(:, g_i-1) + (poles(:, g_i-1) - poles(:, g_i-2))*(gains(g_i) - gains(g_i-1))/(gains(g_i-1) - gains(g_i - 2)); - - % New values for the poles - poles_gi = pole(feedback(G, gains(g_i)*K)); - [~, i_uniq] = uniquetol([real(poles_gi), imag(poles_gi)], 1e-10, 'ByRows', true); - poles_gi = poles_gi(i_uniq); - - % Array of distances between all the poles - poles_dist = sqrt((poles_est-poles_gi.').*conj(poles_est-poles_gi.')); - - % Get indices corresponding to distances from lowest to highest - [~, c] = sort(min(poles_dist)); - - as = 1:length(poles_gi); - - % for each column of poles_dist corresponding to the i'th pole - % with closest previous poles - for p_i = c - % Get the indice a_i of the previous pole that is the closest - % to pole c(p_i) - [~, a_i] = min(poles_dist(:, p_i)); - - poles(as(a_i), g_i) = poles_gi(p_i); - - % Remove old poles that are already matched - % poles_gi(as(a_i), :) = []; - poles_dist(a_i, :) = []; - as(a_i) = []; - end -end - -if args.d_max > 0 - poles = poles(max(abs(poles(:, 2:end) - poles(:, 1:end-1))') > args.d_max, :); -end - -if args.p_half - poles = poles(1:round(end/2), :); -end - -[~, s_p] = sort(imag(poles(:,1)), 'descend'); -poles = poles(s_p, :); - -poles = poles.';