From 9b29c73ffff55e77f9ebe5b0c48dbab2780879db Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 25 Nov 2020 09:17:34 +0100 Subject: [PATCH] Delete unused sections (analytical models) --- index.org | 782 ------------------------------------------------------ 1 file changed, 782 deletions(-) diff --git a/index.org b/index.org index a5f5be7..c9f95b9 100644 --- a/index.org +++ b/index.org @@ -818,533 +818,6 @@ The obtained transmissibility in Open-loop, for the centralized control as well #+RESULTS: [[file:figs/gravimeter_platform_simscape_cl_transmissibility.png]] -* Gravimeter - Analytical Model :noexport: -** System Identification - With Gravity :noexport: -#+begin_src matlab - g = 9.80665; % Gravity [m/s2] -#+end_src - -#+begin_src matlab - Gg = linearize(mdl, io); - Gg.InputName = {'F1', 'F2', 'F3'}; - Gg.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'}; -#+end_src - -We can now see that the system is unstable due to gravity. -#+begin_src matlab :results output replace :exports results - pole(Gg) -#+end_src - -#+RESULTS: -#+begin_example - -7.49865861504606e-05 + 8.65948534948982i - -7.49865861504606e-05 - 8.65948534948982i - -4.76450798645977 + 0i - 4.7642612321107 + 0i - -7.34348883628024e-05 + 4.29133825321225i - -7.34348883628024e-05 - 4.29133825321225i -#+end_example - -#+begin_src matlab :exports none - freqs = logspace(-2, 2, 1000); - - figure; - for in_i = 1:3 - for out_i = 1:4 - subplot(4, 3, 3*(out_i-1)+in_i); - hold on; - plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-'); - plot(freqs, abs(squeeze(freqresp(Gg(out_i,in_i), freqs, 'Hz'))), '-'); - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - end - end -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/open_loop_tf_g.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:open_loop_tf_g -#+caption: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity -#+RESULTS: -[[file:figs/open_loop_tf_g.png]] - -** Parameters -Bode options. -#+begin_src matlab - P = bodeoptions; - P.FreqUnits = 'Hz'; - P.MagUnits = 'abs'; - P.MagScale = 'log'; - P.Grid = 'on'; - P.PhaseWrapping = 'on'; - P.Title.FontSize = 14; - P.XLabel.FontSize = 14; - P.YLabel.FontSize = 14; - P.TickLabel.FontSize = 12; - P.Xlim = [1e-1,1e2]; - P.MagLowerLimMode = 'manual'; - P.MagLowerLim= 1e-3; -#+end_src - -Frequency vector. -#+begin_src matlab - w = 2*pi*logspace(-1,2,1000); % [rad/s] -#+end_src - -** Generation of the State Space Model -Mass matrix -#+begin_src matlab - M = [m 0 0 - 0 m 0 - 0 0 I]; -#+end_src - -Jacobian of the bottom sensor -#+begin_src matlab - Js1 = [1 0 h/2 - 0 1 -l/2]; -#+end_src - -Jacobian of the top sensor -#+begin_src matlab - Js2 = [1 0 -h/2 - 0 1 0]; -#+end_src - -Jacobian of the actuators -#+begin_src matlab - Ja = [1 0 ha % Left horizontal actuator - 0 1 -la % Left vertical actuator - 0 1 la]; % Right vertical actuator - Jta = Ja'; -#+end_src - -Stiffness and Damping matrices -#+begin_src matlab - K = k*Jta*Ja; - C = c*Jta*Ja; -#+end_src - -State Space Matrices -#+begin_src matlab - E = [1 0 0 - 0 1 0 - 0 0 1]; %projecting ground motion in the directions of the legs - - AA = [zeros(3) eye(3) - -M\K -M\C]; - - BB = [zeros(3,6) - M\Jta M\(k*Jta*E)]; - - CC = [[Js1;Js2] zeros(4,3); - zeros(2,6) - (Js1+Js2)./2 zeros(2,3) - (Js1-Js2)./2 zeros(2,3) - (Js1-Js2)./(2*h) zeros(2,3)]; - - DD = [zeros(4,6) - zeros(2,3) eye(2,3) - zeros(6,6)]; -#+end_src - -State Space model: -- Input = three actuators and three ground motions -- Output = the bottom sensor; the top sensor; the ground motion; the half sum; the half difference; the rotation - -#+begin_src matlab - system_dec = ss(AA,BB,CC,DD); -#+end_src - - -#+begin_src matlab :results output replace - size(system_dec) -#+end_src - -#+RESULTS: -: State-space model with 12 outputs, 6 inputs, and 6 states. - -** Comparison with the Simscape Model -#+begin_src matlab :exports none - freqs = logspace(-2, 2, 1000); - - figure; - for in_i = 1:3 - for out_i = 1:4 - subplot(4, 3, 3*(out_i-1)+in_i); - hold on; - plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-'); - plot(freqs, abs(squeeze(freqresp(system_dec(out_i,in_i)*s^2, freqs, 'Hz'))), '-'); - hold off; - set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); - end - end -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/gravimeter_analytical_system_open_loop_models.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:gravimeter_analytical_system_open_loop_models -#+caption: Comparison of the analytical and the Simscape models -#+RESULTS: -[[file:figs/gravimeter_analytical_system_open_loop_models.png]] - -** Analysis -#+begin_src matlab - % figure - % bode(system_dec,P); - % return -#+end_src - -#+begin_src matlab - %% svd decomposition - % system_dec_freq = freqresp(system_dec,w); - % S = zeros(3,length(w)); - % for m = 1:length(w) - % S(:,m) = svd(system_dec_freq(1:4,1:3,m)); - % end - % figure - % loglog(w./(2*pi), S);hold on; - % % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:))); - % xlabel('Frequency [Hz]');ylabel('Singular Value [-]'); - % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6'); - % ylim([1e-8 1e-2]); - % - % %condition number - % figure - % loglog(w./(2*pi), S(1,:)./S(3,:));hold on; - % % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:))); - % xlabel('Frequency [Hz]');ylabel('Condition number [-]'); - % % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6'); - % - % %performance indicator - % system_dec_svd = freqresp(system_dec(1:4,1:3),2*pi*10); - % [U,S,V] = svd(system_dec_svd); - % H_svd_OL = -eye(3,4);%-[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*40,-2*pi*200,40/200) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);% - % H_svd = pinv(V')*H_svd_OL*pinv(U); - % % system_dec_control_svd_ = feedback(system_dec,g*pinv(V')*H*pinv(U)); - % - % OL_dec = g_svd*H_svd*system_dec(1:4,1:3); - % OL_freq = freqresp(OL_dec,w); % OL = G*H - % CL_system = feedback(eye(3),-g_svd*H_svd*system_dec(1:4,1:3)); - % CL_freq = freqresp(CL_system,w); % CL = (1+G*H)^-1 - % % CL_system_2 = feedback(system_dec,H); - % % CL_freq_2 = freqresp(CL_system_2,w); % CL = G/(1+G*H) - % for i = 1:size(w,2) - % OL(:,i) = svd(OL_freq(:,:,i)); - % CL (:,i) = svd(CL_freq(:,:,i)); - % %CL2 (:,i) = svd(CL_freq_2(:,:,i)); - % end - % - % un = ones(1,length(w)); - % figure - % loglog(w./(2*pi),OL(3,:)+1,'k',w./(2*pi),OL(3,:)-1,'b',w./(2*pi),1./CL(1,:),'r--',w./(2*pi),un,'k:');hold on;% - % % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:))); - % % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:))); - % xlabel('Frequency [Hz]');ylabel('Singular Value [-]'); - % legend('GH \sigma_{inf} +1 ','GH \sigma_{inf} -1','S 1/\sigma_{sup}');%,'\lambda_1','\lambda_2','\lambda_3'); - % - % figure - % loglog(w./(2*pi),OL(1,:)+1,'k',w./(2*pi),OL(1,:)-1,'b',w./(2*pi),1./CL(3,:),'r--',w./(2*pi),un,'k:');hold on;% - % % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:))); - % % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:))); - % xlabel('Frequency [Hz]');ylabel('Singular Value [-]'); - % legend('GH \sigma_{sup} +1 ','GH \sigma_{sup} -1','S 1/\sigma_{inf}');%,'\lambda_1','\lambda_2','\lambda_3'); -#+end_src - -** Control Section -#+begin_src matlab - system_dec_10Hz = freqresp(system_dec,2*pi*10); - system_dec_0Hz = freqresp(system_dec,0); - - system_decReal_10Hz = pinv(align(system_dec_10Hz)); - [Ureal,Sreal,Vreal] = svd(system_decReal_10Hz(1:4,1:3)); - normalizationMatrixReal = abs(pinv(Ureal)*system_dec_0Hz(1:4,1:3)*pinv(Vreal')); - - [U,S,V] = svd(system_dec_10Hz(1:4,1:3)); - normalizationMatrix = abs(pinv(U)*system_dec_0Hz(1:4,1:3)*pinv(V')); - - H_dec = ([zpk(-2*pi*5,-2*pi*30,30/5) 0 0 0 - 0 zpk(-2*pi*4,-2*pi*20,20/4) 0 0 - 0 0 0 zpk(-2*pi,-2*pi*10,10)]); - H_cen_OL = [zpk(-2*pi,-2*pi*10,10) 0 0; 0 zpk(-2*pi,-2*pi*10,10) 0; - 0 0 zpk(-2*pi*5,-2*pi*30,30/5)]; - H_cen = pinv(Jta)*H_cen_OL*pinv([Js1; Js2]); - % H_svd_OL = -[1/normalizationMatrix(1,1) 0 0 0 - % 0 1/normalizationMatrix(2,2) 0 0 - % 0 0 1/normalizationMatrix(3,3) 0]; - % H_svd_OL_real = -[1/normalizationMatrixReal(1,1) 0 0 0 - % 0 1/normalizationMatrixReal(2,2) 0 0 - % 0 0 1/normalizationMatrixReal(3,3) 0]; - H_svd_OL = -[1/normalizationMatrix(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0 - 0 1/normalizationMatrix(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0 - 0 0 1/normalizationMatrix(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0]; - H_svd_OL_real = -[1/normalizationMatrixReal(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0 - 0 1/normalizationMatrixReal(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0 - 0 0 1/normalizationMatrixReal(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0]; - % H_svd_OL_real = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*10,-2*pi*100,100/10) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];%-eye(3,4); - % H_svd_OL = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 zpk(-2*pi*4,-2*pi*20,4/20) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);% - H_svd = pinv(V')*H_svd_OL*pinv(U); - H_svd_real = pinv(Vreal')*H_svd_OL_real*pinv(Ureal); - - OL_dec = g*H_dec*system_dec(1:4,1:3); - OL_cen = g*H_cen_OL*pinv([Js1; Js2])*system_dec(1:4,1:3)*pinv(Jta); - OL_svd = 100*H_svd_OL*pinv(U)*system_dec(1:4,1:3)*pinv(V'); - OL_svd_real = 100*H_svd_OL_real*pinv(Ureal)*system_dec(1:4,1:3)*pinv(Vreal'); -#+end_src - -#+begin_src matlab - % figure - % bode(OL_dec,w,P);title('OL Decentralized'); - % figure - % bode(OL_cen,w,P);title('OL Centralized'); -#+end_src - -#+begin_src matlab - figure - bode(g*system_dec(1:4,1:3),w,P); - title('gain * Plant'); -#+end_src - -#+begin_src matlab - figure - bode(OL_svd,OL_svd_real,w,P); - title('OL SVD'); - legend('SVD of Complex plant','SVD of real approximation of the complex plant') -#+end_src - -#+begin_src matlab - figure - bode(system_dec(1:4,1:3),pinv(U)*system_dec(1:4,1:3)*pinv(V'),P); -#+end_src - -#+begin_src matlab - CL_dec = feedback(system_dec,g*H_dec,[1 2 3],[1 2 3 4]); - CL_cen = feedback(system_dec,g*H_cen,[1 2 3],[1 2 3 4]); - CL_svd = feedback(system_dec,100*H_svd,[1 2 3],[1 2 3 4]); - CL_svd_real = feedback(system_dec,100*H_svd_real,[1 2 3],[1 2 3 4]); -#+end_src - -#+begin_src matlab - pzmap_testCL(system_dec,H_dec,g,[1 2 3],[1 2 3 4]) - title('Decentralized control'); -#+end_src - -#+begin_src matlab - pzmap_testCL(system_dec,H_cen,g,[1 2 3],[1 2 3 4]) - title('Centralized control'); -#+end_src - -#+begin_src matlab - pzmap_testCL(system_dec,H_svd,100,[1 2 3],[1 2 3 4]) - title('SVD control'); -#+end_src - -#+begin_src matlab - pzmap_testCL(system_dec,H_svd_real,100,[1 2 3],[1 2 3 4]) - title('Real approximation SVD control'); -#+end_src - -#+begin_src matlab - P.Ylim = [1e-8 1e-3]; - figure - bodemag(system_dec(1:4,1:3),CL_dec(1:4,1:3),CL_cen(1:4,1:3),CL_svd(1:4,1:3),CL_svd_real(1:4,1:3),P); - title('Motion/actuator') - legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); -#+end_src - -#+begin_src matlab - P.Ylim = [1e-5 1e1]; - figure - bodemag(system_dec(1:4,4:6),CL_dec(1:4,4:6),CL_cen(1:4,4:6),CL_svd(1:4,4:6),CL_svd_real(1:4,4:6),P); - title('Transmissibility'); - legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); -#+end_src - -#+begin_src matlab - figure - bodemag(system_dec([7 9],4:6),CL_dec([7 9],4:6),CL_cen([7 9],4:6),CL_svd([7 9],4:6),CL_svd_real([7 9],4:6),P); - title('Transmissibility from half sum and half difference in the X direction'); - legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); -#+end_src - -#+begin_src matlab - figure - bodemag(system_dec([8 10],4:6),CL_dec([8 10],4:6),CL_cen([8 10],4:6),CL_svd([8 10],4:6),CL_svd_real([8 10],4:6),P); - title('Transmissibility from half sum and half difference in the Z direction'); - legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.'); -#+end_src - -** Greshgorin radius -#+begin_src matlab - system_dec_freq = freqresp(system_dec,w); - x1 = zeros(1,length(w)); - z1 = zeros(1,length(w)); - x2 = zeros(1,length(w)); - S1 = zeros(1,length(w)); - S2 = zeros(1,length(w)); - S3 = zeros(1,length(w)); - - for t = 1:length(w) - x1(t) = (abs(system_dec_freq(1,2,t))+abs(system_dec_freq(1,3,t)))/abs(system_dec_freq(1,1,t)); - z1(t) = (abs(system_dec_freq(2,1,t))+abs(system_dec_freq(2,3,t)))/abs(system_dec_freq(2,2,t)); - x2(t) = (abs(system_dec_freq(3,1,t))+abs(system_dec_freq(3,2,t)))/abs(system_dec_freq(3,3,t)); - system_svd = pinv(Ureal)*system_dec_freq(1:4,1:3,t)*pinv(Vreal'); - S1(t) = (abs(system_svd(1,2))+abs(system_svd(1,3)))/abs(system_svd(1,1)); - S2(t) = (abs(system_svd(2,1))+abs(system_svd(2,3)))/abs(system_svd(2,2)); - S2(t) = (abs(system_svd(3,1))+abs(system_svd(3,2)))/abs(system_svd(3,3)); - end - - limit = 0.5*ones(1,length(w)); -#+end_src - -#+begin_src matlab - figure - loglog(w./(2*pi),x1,w./(2*pi),z1,w./(2*pi),x2,w./(2*pi),limit,'--'); - legend('x_1','z_1','x_2','Limit'); - xlabel('Frequency [Hz]'); - ylabel('Greshgorin radius [-]'); -#+end_src - -#+begin_src matlab - figure - loglog(w./(2*pi),S1,w./(2*pi),S2,w./(2*pi),S3,w./(2*pi),limit,'--'); - legend('S1','S2','S3','Limit'); - xlabel('Frequency [Hz]'); - ylabel('Greshgorin radius [-]'); - % set(gcf,'color','w') -#+end_src - -** Injecting ground motion in the system to have the output -#+begin_src matlab - Fr = logspace(-2,3,1e3); - w=2*pi*Fr*1i; - %fit of the ground motion data in m/s^2/rtHz - Fr_ground_x = [0.07 0.1 0.15 0.3 0.7 0.8 0.9 1.2 5 10]; - n_ground_x1 = [4e-7 4e-7 2e-6 1e-6 5e-7 5e-7 5e-7 1e-6 1e-5 3.5e-5]; - Fr_ground_v = [0.07 0.08 0.1 0.11 0.12 0.15 0.25 0.6 0.8 1 1.2 1.6 2 6 10]; - n_ground_v1 = [7e-7 7e-7 7e-7 1e-6 1.2e-6 1.5e-6 1e-6 9e-7 7e-7 7e-7 7e-7 1e-6 2e-6 1e-5 3e-5]; - - n_ground_x = interp1(Fr_ground_x,n_ground_x1,Fr,'linear'); - n_ground_v = interp1(Fr_ground_v,n_ground_v1,Fr,'linear'); - % figure - % loglog(Fr,abs(n_ground_v),Fr_ground_v,n_ground_v1,'*'); - % xlabel('Frequency [Hz]');ylabel('ASD [m/s^2 /rtHz]'); - % return - - %converting into PSD - n_ground_x = (n_ground_x).^2; - n_ground_v = (n_ground_v).^2; - - %Injecting ground motion in the system and getting the outputs - system_dec_f = (freqresp(system_dec,abs(w))); - PHI = zeros(size(Fr,2),12,12); - for p = 1:size(Fr,2) - Sw=zeros(6,6); - Iact = zeros(3,3); - Sw(4,4) = n_ground_x(p); - Sw(5,5) = n_ground_v(p); - Sw(6,6) = n_ground_v(p); - Sw(1:3,1:3) = Iact; - PHI(p,:,:) = (system_dec_f(:,:,p))*Sw(:,:)*(system_dec_f(:,:,p))'; - end - x1 = PHI(:,1,1); - z1 = PHI(:,2,2); - x2 = PHI(:,3,3); - z2 = PHI(:,4,4); - wx = PHI(:,5,5); - wz = PHI(:,6,6); - x12 = PHI(:,1,3); - z12 = PHI(:,2,4); - PHIwx = PHI(:,1,5); - PHIwz = PHI(:,2,6); - xsum = PHI(:,7,7); - zsum = PHI(:,8,8); - xdelta = PHI(:,9,9); - zdelta = PHI(:,10,10); - rot = PHI(:,11,11); -#+end_src - -* Gravimeter - Functions :noexport: -:PROPERTIES: -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -** =align= -:PROPERTIES: -:header-args:matlab+: :tangle gravimeter/align.m -:END: -<> - -This Matlab function is accessible [[file:gravimeter/align.m][here]]. - -#+begin_src matlab - function [A] = align(V) - %A!ALIGN(V) returns a constat matrix A which is the real alignment of the - %INVERSE of the complex input matrix V - %from Mohit slides - - if (nargin ==0) || (nargin > 1) - disp('usage: mat_inv_real = align(mat)') - return - end - - D = pinv(real(V'*V)); - A = D*real(V'*diag(exp(1i * angle(diag(V*D*V.'))/2))); - - - end -#+end_src - - -** =pzmap_testCL= -:PROPERTIES: -:header-args:matlab+: :tangle gravimeter/pzmap_testCL.m -:END: -<> - -This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]]. - -#+begin_src matlab - function [] = pzmap_testCL(system,H,gain,feedin,feedout) - % evaluate and plot the pole-zero map for the closed loop system for - % different values of the gain - - [~, n] = size(gain); - [m1, n1, ~] = size(H); - [~,n2] = size(feedin); - - figure - for i = 1:n - % if n1 == n2 - system_CL = feedback(system,gain(i)*H,feedin,feedout); - - [P,Z] = pzmap(system_CL); - plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on - xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})'); - % clear P Z - % else - % system_CL = feedback(system,gain(i)*H(:,1+(i-1)*m1:m1+(i-1)*m1),feedin,feedout); - % - % [P,Z] = pzmap(system_CL); - % plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on - % xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})'); - % clear P Z - % end - end - str = {strcat('gain = ' , num2str(gain(1)))}; % at the end of first loop, z being loop output - str = [str , strcat('gain = ' , num2str(gain(1)))]; % after 2nd loop - for i = 2:n - str = [str , strcat('gain = ' , num2str(gain(i)))]; % after 2nd loop - str = [str , strcat('gain = ' , num2str(gain(i)))]; % after 2nd loop - end - legend(str{:}) - end - -#+end_src - * Stewart Platform - Simscape Model :PROPERTIES: :header-args:matlab+: :tangle stewart_platform/simscape_model.m @@ -2393,258 +1866,3 @@ The system is identified again: ylim([1e-3, 1e2]); #+end_src -* Stewart Platform - Analytical Model :noexport: -:PROPERTIES: -:header-args:matlab+: :tangle stewart_platform/analytical_model.m -:END: -** 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 - %% Bode plot options - opts = bodeoptions('cstprefs'); - opts.FreqUnits = 'Hz'; - opts.MagUnits = 'abs'; - opts.MagScale = 'log'; - opts.PhaseWrapping = 'on'; - opts.xlim = [1 1000]; -#+end_src - -** Characteristics -#+begin_src matlab - L = 0.055; % Leg length [m] - Zc = 0; % ? - m = 0.2; % Top platform mass [m] - k = 1e3; % Total vertical stiffness [N/m] - c = 2*0.1*sqrt(k*m); % Damping ? [N/(m/s)] - - Rx = 0.04; % ? - Rz = 0.04; % ? - Ix = m*Rx^2; % ? - Iy = m*Rx^2; % ? - Iz = m*Rz^2; % ? -#+end_src - -** Mass Matrix -#+begin_src matlab - M = m*[1 0 0 0 Zc 0; - 0 1 0 -Zc 0 0; - 0 0 1 0 0 0; - 0 -Zc 0 Rx^2+Zc^2 0 0; - Zc 0 0 0 Rx^2+Zc^2 0; - 0 0 0 0 0 Rz^2]; -#+end_src - -** Jacobian Matrix -#+begin_src matlab - Bj=1/sqrt(6)*[ 1 1 -2 1 1 -2; - sqrt(3) -sqrt(3) 0 sqrt(3) -sqrt(3) 0; - sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2); - 0 0 L L -L -L; - -L*2/sqrt(3) -L*2/sqrt(3) L/sqrt(3) L/sqrt(3) L/sqrt(3) L/sqrt(3); - L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2)]; -#+end_src - -** Stifnness and Damping matrices -#+begin_src matlab - kv = k/3; % Vertical Stiffness of the springs [N/m] - kh = 0.5*k/3; % Horizontal Stiffness of the springs [N/m] - - K = diag([3*kh, 3*kh, 3*kv, 3*kv*Rx^2/2, 3*kv*Rx^2/2, 3*kh*Rx^2]); % Stiffness Matrix - C = c*K/100000; % Damping Matrix -#+end_src - -** State Space System -#+begin_src matlab - A = [ zeros(6) eye(6); ... - -M\K -M\C]; - Bw = [zeros(6); -eye(6)]; - Bu = [zeros(6); M\Bj]; - - Co = [-M\K -M\C]; - - D = [zeros(6) M\Bj]; - - ST = ss(A,[Bw Bu],Co,D); -#+end_src - -- OUT 1-6: 6 dof -- IN 1-6 : ground displacement in the directions of the legs -- IN 7-12: forces in the actuators. -#+begin_src matlab - ST.StateName = {'x';'y';'z';'theta_x';'theta_y';'theta_z';... - 'dx';'dy';'dz';'dtheta_x';'dtheta_y';'dtheta_z'}; - - ST.InputName = {'w1';'w2';'w3';'w4';'w5';'w6';... - 'u1';'u2';'u3';'u4';'u5';'u6'}; - - ST.OutputName = {'ax';'ay';'az';'atheta_x';'atheta_y';'atheta_z'}; -#+end_src - -** Transmissibility -#+begin_src matlab - TR=ST*[eye(6); zeros(6)]; -#+end_src - -#+begin_src matlab - figure - subplot(231) - bodemag(TR(1,1)); - subplot(232) - bodemag(TR(2,2)); - subplot(233) - bodemag(TR(3,3)); - subplot(234) - bodemag(TR(4,4)); - subplot(235) - bodemag(TR(5,5)); - subplot(236) - bodemag(TR(6,6)); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/stewart_platform_analytical_transmissibility.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:stewart_platform_analytical_transmissibility -#+caption: Transmissibility -#+RESULTS: -[[file:figs/stewart_platform_analytical_transmissibility.png]] - -** Real approximation of $G(j\omega)$ at decoupling frequency -#+begin_src matlab - sys1 = ST*[zeros(6); eye(6)]; % take only the forces inputs - - dec_fr = 20; - H1 = evalfr(sys1,j*2*pi*dec_fr); - H2 = H1; - D = pinv(real(H2'*H2)); - H1 = inv(D*real(H2'*diag(exp(j*angle(diag(H2*D*H2.'))/2)))) ; - [U,S,V] = svd(H1); - - wf = logspace(-1,2,1000); - for i = 1:length(wf) - H = abs(evalfr(sys1,j*2*pi*wf(i))); - H_dec = abs(evalfr(U'*sys1*V,j*2*pi*wf(i))); - for j = 1:size(H,2) - g_r1(i,j) = (sum(H(j,:))-H(j,j))/H(j,j); - g_r2(i,j) = (sum(H_dec(j,:))-H_dec(j,j))/H_dec(j,j); - % keyboard - end - g_lim(i) = 0.5; - end -#+end_src - -** Coupled and Decoupled Plant "Gershgorin Radii" -#+begin_src matlab - figure; - title('Coupled plant') - loglog(wf,g_r1(:,1),wf,g_r1(:,2),wf,g_r1(:,3),wf,g_r1(:,4),wf,g_r1(:,5),wf,g_r1(:,6),wf,g_lim,'--'); - legend('$a_x$','$a_y$','$a_z$','$\theta_x$','$\theta_y$','$\theta_z$','Limit'); - xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii') -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/gershorin_raddii_coupled_analytical.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:gershorin_raddii_coupled_analytical -#+caption: Gershorin Raddi for the coupled plant -#+RESULTS: -[[file:figs/gershorin_raddii_coupled_analytical.png]] - -#+begin_src matlab - figure; - title('Decoupled plant (10 Hz)') - loglog(wf,g_r2(:,1),wf,g_r2(:,2),wf,g_r2(:,3),wf,g_r2(:,4),wf,g_r2(:,5),wf,g_r2(:,6),wf,g_lim,'--'); - legend('$S_1$','$S_2$','$S_3$','$S_4$','$S_5$','$S_6$','Limit'); - xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii') -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/gershorin_raddii_decoupled_analytical.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:gershorin_raddii_decoupled_analytical -#+caption: Gershorin Raddi for the decoupled plant -#+RESULTS: -[[file:figs/gershorin_raddii_decoupled_analytical.png]] - -** Decoupled Plant -#+begin_src matlab - figure; - bodemag(U'*sys1*V,opts) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/stewart_platform_analytical_decoupled_plant.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:stewart_platform_analytical_decoupled_plant -#+caption: Decoupled Plant -#+RESULTS: -[[file:figs/stewart_platform_analytical_decoupled_plant.png]] - -** Controller -#+begin_src matlab - fc = 2*pi*0.1; % Crossover Frequency [rad/s] - c_gain = 50; % - - cont = eye(6)*c_gain/(s+fc); -#+end_src - -** Closed Loop System -#+begin_src matlab - FEEDIN = [7:12]; % Input of controller - FEEDOUT = [1:6]; % Output of controller -#+end_src - -Centralized Control -#+begin_src matlab - STcen = feedback(ST, inv(Bj)*cont, FEEDIN, FEEDOUT); - TRcen = STcen*[eye(6); zeros(6)]; -#+end_src - -SVD Control -#+begin_src matlab - STsvd = feedback(ST, pinv(V')*cont*pinv(U), FEEDIN, FEEDOUT); - TRsvd = STsvd*[eye(6); zeros(6)]; -#+end_src - -** Results -#+begin_src matlab - figure - subplot(231) - bodemag(TR(1,1),TRcen(1,1),TRsvd(1,1),opts) - legend('OL','Centralized','SVD') - subplot(232) - bodemag(TR(2,2),TRcen(2,2),TRsvd(2,2),opts) - legend('OL','Centralized','SVD') - subplot(233) - bodemag(TR(3,3),TRcen(3,3),TRsvd(3,3),opts) - legend('OL','Centralized','SVD') - subplot(234) - bodemag(TR(4,4),TRcen(4,4),TRsvd(4,4),opts) - legend('OL','Centralized','SVD') - subplot(235) - bodemag(TR(5,5),TRcen(5,5),TRsvd(5,5),opts) - legend('OL','Centralized','SVD') - subplot(236) - bodemag(TR(6,6),TRcen(6,6),TRsvd(6,6),opts) - legend('OL','Centralized','SVD') -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace - exportFig('figs/stewart_platform_analytical_svd_cen_comp.pdf', 'width', 'full', 'height', 'full'); -#+end_src - -#+name: fig:stewart_platform_analytical_svd_cen_comp -#+caption: Comparison of the obtained transmissibility for the centralized control and the SVD control -#+RESULTS: -[[file:figs/stewart_platform_analytical_svd_cen_comp.png]]