From 9d3e5849307690a9c0f52f8ce82021480470b256 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 11 Dec 2019 17:33:45 +0100 Subject: [PATCH] Move the computeReferencePose function --- disturbances/index.html | 141 ++++--- disturbances/index.org | 147 ------- functions/index.html | 883 ++++++++++++++++++++++++++++++++++++++++ functions/index.org | 88 ++++ 4 files changed, 1061 insertions(+), 198 deletions(-) create mode 100644 functions/index.html diff --git a/disturbances/index.html b/disturbances/index.html index ffca036..c46be66 100644 --- a/disturbances/index.html +++ b/disturbances/index.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Identification of the disturbances @@ -262,7 +262,10 @@ for the JavaScript code in this tag. TeX: { equationNumbers: {autoNumber: "AMS"}, MultLineWidth: "85%", TagSide: "right", - TagIndent: ".8em" + TagIndent: ".8em", + Macros: { + bm: ["{\\boldsymbol #1}",1], + } } }); @@ -280,13 +283,14 @@ for the JavaScript code in this tag.

Table of Contents

@@ -296,7 +300,7 @@ The goal here is to extract the Power Spectral Density of the sources of perturb

-The sources of perturbations are (schematically shown in figure 1): +The sources of perturbations are (schematically shown in figure 1):

-Because we cannot measure directly the perturbation forces, we have the measure the effect of those perturbations on the system (in terms of velocity for instance using geophones, \(D\) on figure 1) and then, using a model, compute the forces that induced such velocity. +Because we cannot measure directly the perturbation forces, we have the measure the effect of those perturbations on the system (in terms of velocity for instance using geophones, \(D\) on figure 1) and then, using a model, compute the forces that induced such velocity.

-
+

uniaxial-model-micro-station.png

Figure 1: Schematic of the Micro Station and the sources of disturbance

@@ -321,18 +325,18 @@ Because we cannot measure directly the perturbation forces, we have the measure This file is divided in the following sections:

    -
  • Section 1: transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup
    • -
  • Section 2: the bode plot of those transfer functions are shown
    • -
  • Section 3: the measured PSD of the effect of the disturbances are shown
    • -
  • Section 4: from the model and the measured PSD, the PSD of the disturbance forces are computed
    • -
  • Section 5: with the computed PSD, the noise budget of the system is done
    • +
  • Section 1: transfer functions from the disturbance forces to the relative velocity of the hexapod with respect to the granite are computed using the Simscape Model representing the experimental setup
    • +
  • Section 2: the bode plot of those transfer functions are shown
    • +
  • Section 3: the measured PSD of the effect of the disturbances are shown
    • +
  • Section 4: from the model and the measured PSD, the PSD of the disturbance forces are computed
    • +
  • Section 5: with the computed PSD, the noise budget of the system is done
-
-

1 Identification

+
+

1 Identification

- +

@@ -373,15 +377,15 @@ G.OutputName = {

-
-

2 Sensitivity to Disturbances

+
+

2 Sensitivity to Disturbances

- +

-
+

sensitivity_dist_gm.png

Figure 2: Sensitivity to Ground Motion (png, pdf)

@@ -389,7 +393,7 @@ G.OutputName = { +

sensitivity_dist_fty.png

Figure 3: Sensitivity to vertical forces applied by the Ty stage (png, pdf)

@@ -397,7 +401,7 @@ G.OutputName = { +

sensitivity_dist_frz.png

Figure 4: Sensitivity to vertical forces applied by the Rz stage (png, pdf)

@@ -405,11 +409,11 @@ G.OutputName = {
-
-

3 Power Spectral Density of the effect of the disturbances

+
+

3 Power Spectral Density of the effect of the disturbances

- + The PSD of the relative velocity between the hexapod and the marble in \([(m/s)^2/Hz]\) are loaded for the following sources of disturbance:

    @@ -438,15 +442,15 @@ We now compute the relative velocity between the hexapod and the granite due to

-The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures 5 and 6. +The Power Spectral Density of the relative motion/velocity of the hexapod with respect to the granite are shown in figures 5 and 6.

-The Cumulative Amplitude Spectrum of the relative motion is shown in figure 7. +The Cumulative Amplitude Spectrum of the relative motion is shown in figure 7.

-
+

dist_effect_relative_velocity.png

Figure 5: Amplitude Spectral Density of the relative velocity of the hexapod with respect to the granite due to different sources of perturbation (png, pdf)

@@ -454,14 +458,14 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure + -
+

dist_effect_relative_motion_cas.png

Figure 7: Cumulative Amplitude Spectrum of the relative motion due to different sources of perturbation (png, pdf)

@@ -469,15 +473,15 @@ The Cumulative Amplitude Spectrum of the relative motion is shown in figure
-
-

4 Compute the Power Spectral Density of the disturbance force

+
+

4 Compute the Power Spectral Density of the disturbance force

- +

-Now, from the extracted transfer functions from the disturbance force to the relative motion of the hexapod with respect to the granite (section 2) and from the measured PSD of the relative motion (section 3), we can compute the PSD of the disturbance force. +Now, from the extracted transfer functions from the disturbance force to the relative motion of the hexapod with respect to the granite (section 2) and from the measured PSD of the relative motion (section 3), we can compute the PSD of the disturbance force.

@@ -487,7 +491,7 @@ tyz.psd_f = tyz.pxz_ty_r./abs +

dist_force_psd.png

Figure 8: Amplitude Spectral Density of the disturbance force (png, pdf)

@@ -495,11 +499,11 @@ tyz.psd_f = tyz.pxz_ty_r./abs -

5 Noise Budget

+
+

5 Noise Budget

- +

@@ -508,7 +512,7 @@ We should verify that this is coherent with the measurements.

-
+

psd_effect_dist_verif.png

Figure 9: Computed Effect of the disturbances on the relative displacement hexapod/granite (png, pdf)

@@ -516,7 +520,7 @@ We should verify that this is coherent with the measurements. -
+

cas_computed_relative_displacement.png

Figure 10: CAS of the total Relative Displacement due to all considered sources of perturbation (png, pdf)

@@ -524,8 +528,8 @@ We should verify that this is coherent with the measurements.
-
-

6 Approximation

+
+

6 Approximation

We approximate the PSD of the disturbance with the following transfer functions. @@ -541,14 +545,14 @@ G_gm = 0. +

estimate_spectral_density_disturbances.png

Figure 11: Estimated spectral density of the disturbances (png, pdf)

-
+

comp_estimation_cas_disturbances.png

Figure 12: Comparison of the CAS of the disturbances with the approximate ones (png, pdf)

@@ -556,8 +560,8 @@ We compute the effect of these approximate disturbances on \(D\).
-
-

7 Save

+
+

7 Save

The PSD of the disturbance force are now saved for further noise budgeting when control is applied (the mat file is accessible here). @@ -581,10 +585,45 @@ save( +

8 Display Obtained Disturbances

+
+
+
initDisturbances();
+load('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
+
+
+ +
+
figure;
+hold on;
+plot(t, Dwx, 'DisplayName', 'Dw x');
+plot(t, Dwy, 'DisplayName', 'Dw y');
+plot(t, Dwz, 'DisplayName', 'Dw z');
+hold off;
+xlabel('Time [s]'); ylabel('Amplitude [m]');
+legend('location', 'north east');
+
+
+ +
+
figure;
+hold on;
+plot(t, Fty_x, 'DisplayName', 'Ty x');
+plot(t, Fty_z, 'DisplayName', 'Ty z');
+plot(t, Frz_z,  'DisplayName', 'Rz z');
+hold off;
+xlabel('Time [s]'); ylabel('Force [N]');
+legend('location', 'north east');
+
+
+
+

Author: Dehaeze Thomas

-

Created: 2019-11-22 ven. 15:42

+

Created: 2019-12-11 mer. 17:19

Validate

diff --git a/disturbances/index.org b/disturbances/index.org index 6e56835..afaee94 100644 --- a/disturbances/index.org +++ b/disturbances/index.org @@ -587,153 +587,6 @@ The PSD of the disturbance force are now saved for further noise budgeting when save('./disturbances/mat/dist_psd.mat', 'dist_f'); #+end_src -* Function that initialize the disturbances -:PROPERTIES: -:header-args:matlab+: :tangle ../src/initDisturbances.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:src/initDisturbances.m][here]]. - -#+begin_src matlab - function [] = initDisturbances(opts_param) - % initDisturbances - Initialize the disturbances - % - % Syntax: [] = initDisturbances(opts_param) - % - % Inputs: - % - opts_param - - -#+end_src - -** Default values for the Options -#+begin_src matlab - %% Default values for opts - opts = struct(); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end -#+end_src - -** Load Data -#+begin_src matlab - load('./disturbances/mat/dist_psd.mat', 'dist_f'); -#+end_src - -We remove the first frequency point that usually is very large. -#+begin_src matlab :exports none - dist_f.f = dist_f.f(2:end); - dist_f.psd_gm = dist_f.psd_gm(2:end); - dist_f.psd_ty = dist_f.psd_ty(2:end); - dist_f.psd_rz = dist_f.psd_rz(2:end); -#+end_src - -** Parameters -We define some parameters that will be used in the algorithm. -#+begin_src matlab - Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] - N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD - T0 = N/Fs; % Signal Duration [s] - df = 1/T0; % Frequency resolution of the DFT [Hz] - % Also equal to (dist_f.f(2)-dist_f.f(1)) - t = linspace(0, T0, N+1)'; % Time Vector [s] - Ts = 1/Fs; % Sampling Time [s] -#+end_src - -** Ground Motion -#+begin_src matlab - phi = dist_f.psd_gm; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - theta = 2*pi*rand(N/2,1); % Generate random phase [rad] - Cx = [0 ; C.*complex(cos(theta),sin(theta))]; - Cx = [Cx; flipud(conj(Cx(2:end)))];; - u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] - % Dwx = struct('time', t, 'signals', struct('values', u)); - Dwx = u; - theta = 2*pi*rand(N/2,1); % Generate random phase [rad] - Cx = [0 ; C.*complex(cos(theta),sin(theta))]; - Cx = [Cx; flipud(conj(Cx(2:end)))];; - u = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] - Dwy = u; - theta = 2*pi*rand(N/2,1); % Generate random phase [rad] - Cx = [0 ; C.*complex(cos(theta),sin(theta))]; - Cx = [Cx; flipud(conj(Cx(2:end)))];; - u = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] - Dwz = u; -#+end_src - -** Translation Stage - X direction -#+begin_src matlab - phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - theta = 2*pi*rand(N/2,1); % Generate random phase [rad] - Cx = [0 ; C.*complex(cos(theta),sin(theta))]; - Cx = [Cx; flipud(conj(Cx(2:end)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N] - Fty_x = u; -#+end_src - -** Translation Stage - Z direction -#+begin_src matlab - phi = dist_f.psd_ty; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - theta = 2*pi*rand(N/2,1); % Generate random phase [rad] - Cx = [0 ; C.*complex(cos(theta),sin(theta))]; - Cx = [Cx; flipud(conj(Cx(2:end)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N] - Fty_z = u; -#+end_src - -** Spindle - Z direction -#+begin_src matlab - phi = dist_f.psd_rz; - C = zeros(N/2,1); - for i = 1:N/2 - C(i) = sqrt(phi(i)*df); - end - theta = 2*pi*rand(N/2,1); % Generate random phase [rad] - Cx = [0 ; C.*complex(cos(theta),sin(theta))]; - Cx = [Cx; flipud(conj(Cx(2:end)))];; - u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N] - Frz_z = u; -#+end_src - -** Direct Forces -#+begin_src matlab - u = zeros(length(t), 6); - Fd = u; -#+end_src - -** Set initial value to zero -#+begin_src matlab - Dwx = Dwx - Dwx(1); - Dwy = Dwy - Dwy(1); - Dwz = Dwz - Dwz(1); - Fty_x = Fty_x - Fty_x(1); - Fty_z = Fty_z - Fty_z(1); - Frz_z = Frz_z - Frz_z(1); -#+end_src - -** Save -#+begin_src matlab - save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't'); -#+end_src - * Display Obtained Disturbances #+begin_src matlab initDisturbances(); diff --git a/functions/index.html b/functions/index.html new file mode 100644 index 0000000..75fb670 --- /dev/null +++ b/functions/index.html @@ -0,0 +1,883 @@ + + + + + + + +Matlab Functions used for the NASS Project + + + + + + + + + + + + + +
+ UP + | + HOME +
+

Matlab Functions used for the NASS Project

+ + +
+

1 Functions

+
+

+ +

+
+
+

1.1 computePsdDispl

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
+    i_init = find(sys_data.time > t_init, 1);
+
+    han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
+    Fs = 1/sys_data.time(2);
+
+    [pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
+    [pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
+    [pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
+
+    [prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
+    [pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
+    [prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
+
+    psd_object = struct(...
+        'f',  f,   ...
+        'dx', pdx, ...
+        'dy', pdy, ...
+        'dz', pdz, ...
+        'rx', prx, ...
+        'ry', pry, ...
+        'rz', prz);
+end
+
+
+
+
+ +
+

1.2 computeSetpoint

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function setpoint = computeSetpoint(ty, ry, rz)
+%%
+setpoint = zeros(6, 1);
+
+%% Ty
+Ty = [1 0 0 0  ;
+      0 1 0 ty ;
+      0 0 1 0  ;
+      0 0 0 1 ];
+
+% Tyinv = [1 0 0  0  ;
+%          0 1 0 -ty ;
+%          0 0 1  0  ;
+%          0 0 0  1 ];
+
+%% Ry
+Ry = [ cos(ry) 0 sin(ry) 0 ;
+      0       1 0       0 ;
+      -sin(ry) 0 cos(ry) 0 ;
+      0        0 0       1 ];
+
+% TMry = Ty*Ry*Tyinv;
+
+%% Rz
+Rz = [cos(rz) -sin(rz) 0 0 ;
+      sin(rz)  cos(rz) 0 0 ;
+      0        0       1 0 ;
+      0        0       0 1 ];
+
+% TMrz = Ty*TMry*Rz*TMry'*Tyinv;
+
+%% All stages
+% TM = TMrz*TMry*Ty;
+
+TM = Ty*Ry*Rz;
+
+[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
+
+setpoint(1:3) = TM(1:3, 4);
+setpoint(4:6) = [thetax, thetay, thetaz];
+
+%% Custom Functions
+function [thetax, thetay, thetaz] = RM2angle(R)
+    if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+        thetay = -asin(R(3, 1));
+        thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+        thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+    else
+        thetaz = 0;
+        if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+            thetay = pi/2;
+            thetax = thetaz + atan2(R(1, 2), R(1, 3));
+        else
+            thetay = -pi/2;
+            thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+        end
+    end
+end
+end
+
+
+
+
+ +
+

1.3 converErrorBasis

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
+% convertErrorBasis -
+%
+% Syntax: convertErrorBasis(p_error, ty, ry, rz)
+%
+% Inputs:
+%    - p_error - Position error of the sample w.r.t. the granite [m, rad]
+%    - ty - Measured translation of the Ty stage [m]
+%    - ry - Measured rotation of the Ry stage [rad]
+%    - rz - Measured rotation of the Rz stage [rad]
+%
+% Outputs:
+%    - P_nass - Position error of the sample w.r.t. the NASS base [m]
+%    - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
+%
+% Example:
+%
+
+%% If line vector => column vector
+if size(pos, 2) == 6
+    pos = pos';
+end
+
+if size(setpoint, 2) == 6
+    setpoint = setpoint';
+end
+
+%% Position of the sample in the frame fixed to the Granite
+P_granite = [pos(1:3); 1]; % Position [m]
+R_granite = [setpoint(1:3); 1]; % Reference [m]
+
+%% Transformation matrices of the stages
+% T-y
+TMty = [1 0 0 0  ;
+        0 1 0 ty ;
+        0 0 1 0  ;
+        0 0 0 1 ];
+
+% R-y
+TMry = [ cos(ry) 0 sin(ry) 0 ;
+        0       1 0       0 ;
+        -sin(ry) 0 cos(ry) 0 ;
+        0        0 0       1 ];
+
+% R-z
+TMrz = [cos(rz) -sin(rz) 0 0 ;
+        sin(rz)  cos(rz) 0 0 ;
+        0        0       1 0 ;
+        0        0       0 1 ];
+
+%% Compute Point coordinates in the new reference fixed to the NASS base
+% P_nass = TMrz*TMry*TMty*P_granite;
+P_nass = TMrz\TMry\TMty\P_granite;
+R_nass = TMrz\TMry\TMty\R_granite;
+
+dx = R_nass(1)-P_nass(1);
+dy = R_nass(2)-P_nass(2);
+dz = R_nass(3)-P_nass(3);
+
+%% Compute new basis vectors linked to the NASS base
+% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
+% ux_nass = ux_nass(1:3);
+% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
+% uy_nass = uy_nass(1:3);
+% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
+% uz_nass = uz_nass(1:3);
+
+ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
+ux_nass = ux_nass(1:3);
+uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
+uy_nass = uy_nass(1:3);
+uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
+uz_nass = uz_nass(1:3);
+
+%% Rotations error w.r.t. granite Frame
+% Rotations error w.r.t. granite Frame
+rx_nass = pos(4);
+ry_nass = pos(5);
+rz_nass = pos(6);
+
+% Rotation matrices of the Sample w.r.t. the Granite
+Mrx_error = [1 0              0 ;
+            0 cos(-rx_nass) -sin(-rx_nass) ;
+            0 sin(-rx_nass)  cos(-rx_nass)];
+
+Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
+              0             1 0 ;
+            -sin(-ry_nass) 0 cos(-ry_nass)];
+
+Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
+            sin(-rz_nass)  cos(-rz_nass) 0 ;
+            0              0             1];
+
+% Rotation matrix of the Sample w.r.t. the Granite
+Mr_error = Mrz_error*Mry_error*Mrx_error;
+
+%% Use matrix to solve
+R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
+
+[thetax, thetay, thetaz] = RM2angle(R);
+
+error_nass = [dx; dy; dz; thetax; thetay; thetaz];
+
+%% Custom Functions
+function [thetax, thetay, thetaz] = RM2angle(R)
+    if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
+        thetay = -asin(R(3, 1));
+        % thetaybis = pi-thetay;
+        thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
+        % thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
+        thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
+        % thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
+    else
+        thetaz = 0;
+        if abs(R(3, 1)+1) < 1e-6 % R31 = -1
+            thetay = pi/2;
+            thetax = thetaz + atan2(R(1, 2), R(1, 3));
+        else
+            thetay = -pi/2;
+            thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
+        end
+    end
+end
+
+end
+
+
+
+
+ +
+

1.4 generateDiagPidControl

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [K] = generateDiagPidControl(G, fs)
+    %%
+    pid_opts = pidtuneOptions(...
+        'PhaseMargin', 50, ...
+        'DesignFocus', 'disturbance-rejection');
+
+    %%
+    K = tf(zeros(6));
+
+    for i = 1:6
+        input_name = G.InputName(i);
+        output_name = G.OutputName(i);
+        K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
+    end
+
+    K.InputName = G.OutputName;
+    K.OutputName = G.InputName;
+end
+
+
+
+
+ +
+

1.5 identifyPlant

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [sys] = identifyPlant(opts_param)
+    %% Default values for opts
+    opts = struct();
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    end
+
+    %% Options for Linearized
+    options = linearizeOptions;
+    options.SampleTime = 0;
+
+    %% Name of the Simulink File
+    mdl = 'sim_nano_station_id';
+
+    %% Input/Output definition
+    io(1)  = linio([mdl, '/Fn'],   1, 'input');  % Cartesian forces applied by NASS
+    io(2)  = linio([mdl, '/Dw'],   1, 'input');  % Ground Motion
+    io(3)  = linio([mdl, '/Fs'],   1, 'input');  % External forces on the sample
+    io(4)  = linio([mdl, '/Fnl'],  1, 'input');  % Forces applied on the NASS's legs
+    io(5)  = linio([mdl, '/Fd'],   1, 'input');  % Disturbance Forces
+    io(6)  = linio([mdl, '/Dsm'],  1, 'output'); % Displacement of the sample
+    io(7)  = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
+    io(8)  = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
+    io(9)  = linio([mdl, '/Es'],   1, 'output'); % Position Error w.r.t. NASS base
+    io(10) = linio([mdl, '/Vlm'],  1, 'output'); % Measured absolute velocity of the legs
+
+    %% Run the linearization
+    G = linearize(mdl, io, options);
+    G.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
+                    'Dgx', 'Dgy', 'Dgz', ...
+                    'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
+                    'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
+                    'Frzz', 'Ftyx', 'Ftyz'};
+    G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
+                    'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
+                    'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
+                    'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
+                    'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
+
+    %% Create the sub transfer functions
+    minreal_tol = sqrt(eps);
+    % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
+    sys.G_cart  = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
+    % From ground motion to Sample displacement
+    sys.G_gm    = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
+    % From direct forces applied on the sample to displacement of the sample
+    sys.G_fs    = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
+    % From forces applied on NASS's legs to force sensor in each leg
+    sys.G_iff   = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
+    % From forces applied on NASS's legs to displacement of each leg
+    sys.G_dleg  = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
+    % From forces/torques applied by the NASS to position error
+    sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
+    % From forces/torques applied by the NASS to velocity of the actuator
+    sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
+    % From various disturbance forces to position error
+    sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
+
+    %% We remove low frequency and high frequency dynamics that are usually unstable
+    % using =freqsep= is risky as it may change the shape of the transfer functions
+    % f_min = 0.1; % [Hz]
+    % f_max = 1e4; % [Hz]
+
+    % [~, sys.G_cart]  = freqsep(freqsep(sys.G_cart,  2*pi*f_max), 2*pi*f_min);
+    % [~, sys.G_gm]    = freqsep(freqsep(sys.G_gm,    2*pi*f_max), 2*pi*f_min);
+    % [~, sys.G_fs]    = freqsep(freqsep(sys.G_fs,    2*pi*f_max), 2*pi*f_min);
+    % [~, sys.G_iff]   = freqsep(freqsep(sys.G_iff,   2*pi*f_max), 2*pi*f_min);
+    % [~, sys.G_dleg]  = freqsep(freqsep(sys.G_dleg,  2*pi*f_max), 2*pi*f_min);
+    % [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min);
+
+    %% We finally verify that the system is stable
+    if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant)
+      warning('One of the identified system is unstable');
+    end
+end
+
+
+
+
+ +
+

1.6 runSimulation

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
+    %% Load the controller and save it for the simulation
+    if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
+      K_obj = load('./mat/K_fb.mat');
+      K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
+      save('./mat/controllers.mat', 'K');
+    elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
+      K_obj = load('./mat/K_fb_iff.mat');
+      K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
+      save('./mat/controllers.mat', 'K');
+    elseif strcmp(ctrl_type, 'ol')
+      K = tf(zeros(6)); %#ok
+      save('./mat/controllers.mat', 'K');
+    else
+      error('ctrl_type should be cl or ol');
+    end
+
+    %% Active Damping
+    if strcmp(act_damp, 'iff')
+      K_iff_crit = load('./mat/K_iff_crit.mat');
+      K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
+      save('./mat/controllers.mat', 'K_iff', '-append');
+    elseif strcmp(act_damp, 'none')
+      K_iff = tf(zeros(6)); %#ok
+      save('./mat/controllers.mat', 'K_iff', '-append');
+    end
+
+    %%
+    if strcmp(sys_name, 'pz')
+      initializeNanoHexapod(struct('actuator', 'piezo'));
+    elseif strcmp(sys_name, 'vc')
+      initializeNanoHexapod(struct('actuator', 'lorentz'));
+    else
+      error('sys_name should be pz or vc');
+    end
+
+    if strcmp(sys_mass, 'light')
+      initializeSample(struct('mass', 1));
+    elseif strcmp(sys_mass, 'heavy')
+      initializeSample(struct('mass', 50));
+    else
+      error('sys_mass should be light or heavy');
+    end
+
+    %% Run the simulation
+    sim('sim_nano_station_ctrl.slx');
+
+    %% Split the Dsample matrix into vectors
+    [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok
+    time = Dsample.Time; %#ok
+
+    %% Save the result
+    filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
+    save(sprintf('./mat/%s.mat', filename), ...
+        'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
+end
+
+
+
+
+ +
+

1.7 Inverse Kinematics of the Hexapod

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
+% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
+%
+% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
+%
+% Inputs:
+%    - hexapod - Hexapod object containing the geometry of the hexapod
+%    - AP - Position vector of point OB expressed in frame {A} in [m]
+%    - ARB - Rotation Matrix expressed in frame {A}
+
+  % Wanted Length of the hexapod's legs [m]
+  L = zeros(6, 1);
+
+  for i = 1:length(L)
+    Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
+    Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
+
+    L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
+  end
+end
+
+
+
+
+
+

1.8 computeReferencePose

+
+

+ +

+ +

+This Matlab function is accessible here. +

+ +
+
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
+% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
+%
+% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
+%
+% Inputs:
+%    - Dy - Reference of the Translation Stage [m]
+%    - Ry - Reference of the Tilt Stage [rad]
+%    - Rz - Reference of the Spindle [rad]
+%    - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
+%    - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
+%
+% Outputs:
+%    - WTr -
+
+  %% Translation Stage
+  Rty = [1 0 0 0;
+         0 1 0 Dy;
+         0 0 1 0;
+         0 0 0 1];
+
+  %% Tilt Stage - Pure rotating aligned with Ob
+  Rry = [ cos(Ry) 0 sin(Ry) 0;
+          0       1 0       0;
+         -sin(Ry) 0 cos(Ry) 0;
+          0       0 0       1];
+
+  %% Spindle - Rotation along the Z axis
+  Rrz = [cos(Rz) -sin(Rz) 0 0 ;
+         sin(Rz)  cos(Rz) 0 0 ;
+         0        0       1 0 ;
+         0        0       0 1 ];
+
+
+  %% Micro-Hexapod
+  Rhx = [1 0           0;
+         0 cos(Dh(4)) -sin(Dh(4));
+         0 sin(Dh(4))  cos(Dh(4))];
+
+  Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
+         0           1 0;
+         -sin(Dh(5)) 0 cos(Dh(5))];
+
+  Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
+         sin(Dh(6))  cos(Dh(6)) 0;
+         0           0          1];
+
+  Rh = [1 0 0 Dh(1) ;
+        0 1 0 Dh(2) ;
+        0 0 1 Dh(3) ;
+        0 0 0 1 ];
+
+  Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
+
+  %% Nano-Hexapod
+  Rnx = [1 0           0;
+         0 cos(Dn(4)) -sin(Dn(4));
+         0 sin(Dn(4))  cos(Dn(4))];
+
+  Rny = [ cos(Dn(5)) 0 sin(Dn(5));
+         0           1 0;
+         -sin(Dn(5)) 0 cos(Dn(5))];
+
+  Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
+         sin(Dn(6))  cos(Dn(6)) 0;
+         0           0          1];
+
+  Rn = [1 0 0 Dn(1) ;
+        0 1 0 Dn(2) ;
+        0 0 1 Dn(3) ;
+        0 0 0 1 ];
+
+  Rn(1:3, 1:3) = Rnx*Rny*Rnz;
+
+  %% Total Homogeneous transformation
+  WTr = Rty*Rry*Rrz*Rh*Rn;
+end
+
+
+
+
+
+
+
+

Author: Dehaeze Thomas

+

Created: 2019-12-11 mer. 17:33

+

Validate

+
+ + diff --git a/functions/index.org b/functions/index.org index f5d0173..93f7dc7 100644 --- a/functions/index.org +++ b/functions/index.org @@ -506,3 +506,91 @@ This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]] end end #+end_src +** computeReferencePose +:PROPERTIES: +:header-args:matlab+: :tangle ../src/computeReferencePose.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/computeReferencePose.m][here]]. + +#+begin_src matlab + function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn) + % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample + % + % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn) + % + % Inputs: + % - Dy - Reference of the Translation Stage [m] + % - Ry - Reference of the Tilt Stage [rad] + % - Rz - Reference of the Spindle [rad] + % - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad] + % - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad] + % + % Outputs: + % - WTr - + + %% Translation Stage + Rty = [1 0 0 0; + 0 1 0 Dy; + 0 0 1 0; + 0 0 0 1]; + + %% Tilt Stage - Pure rotating aligned with Ob + Rry = [ cos(Ry) 0 sin(Ry) 0; + 0 1 0 0; + -sin(Ry) 0 cos(Ry) 0; + 0 0 0 1]; + + %% Spindle - Rotation along the Z axis + Rrz = [cos(Rz) -sin(Rz) 0 0 ; + sin(Rz) cos(Rz) 0 0 ; + 0 0 1 0 ; + 0 0 0 1 ]; + + + %% Micro-Hexapod + Rhx = [1 0 0; + 0 cos(Dh(4)) -sin(Dh(4)); + 0 sin(Dh(4)) cos(Dh(4))]; + + Rhy = [ cos(Dh(5)) 0 sin(Dh(5)); + 0 1 0; + -sin(Dh(5)) 0 cos(Dh(5))]; + + Rhz = [cos(Dh(6)) -sin(Dh(6)) 0; + sin(Dh(6)) cos(Dh(6)) 0; + 0 0 1]; + + Rh = [1 0 0 Dh(1) ; + 0 1 0 Dh(2) ; + 0 0 1 Dh(3) ; + 0 0 0 1 ]; + + Rh(1:3, 1:3) = Rhz*Rhy*Rhx; + + %% Nano-Hexapod + Rnx = [1 0 0; + 0 cos(Dn(4)) -sin(Dn(4)); + 0 sin(Dn(4)) cos(Dn(4))]; + + Rny = [ cos(Dn(5)) 0 sin(Dn(5)); + 0 1 0; + -sin(Dn(5)) 0 cos(Dn(5))]; + + Rnz = [cos(Dn(6)) -sin(Dn(6)) 0; + sin(Dn(6)) cos(Dn(6)) 0; + 0 0 1]; + + Rn = [1 0 0 Dn(1) ; + 0 1 0 Dn(2) ; + 0 0 1 Dn(3) ; + 0 0 0 1 ]; + + Rn(1:3, 1:3) = Rnx*Rny*Rnz; + + %% Total Homogeneous transformation + WTr = Rty*Rry*Rrz*Rh*Rn; + end +#+end_src