Matlab Functions used for the NASS Project

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

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

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

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

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

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

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