Matlab Functions used for the NASS Project
Table of Contents
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
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
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
4 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) = Rnz*Rny*Rnx; %% Total Homogeneous transformation WTr = Rty*Rry*Rrz*Rh*Rn; end
5 Compute the Sample Position Error w.r.t. the NASS
This Matlab function is accessible here.
function [MTr] = computeSampleError(WTm, WTr) % computeSampleError - % % Syntax: [MTr] = computeSampleError(WTm, WTr) % % Inputs: % - WTm - Homoegeneous transformation that represent the % wanted pose of the sample with respect to the granite % - WTr - Homoegeneous transformation that represent the % measured pose of the sample with respect to the granite % % Outputs: % - MTr - Homoegeneous transformation that represent the % wanted pose of the sample expressed in a frame % attached to the top platform of the nano-hexapod MTr = zeros(4,4); MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; end