406 lines
12 KiB
Org Mode
406 lines
12 KiB
Org Mode
#+TITLE: Matlab Functions used for the NASS Project
|
|
:DRAWER:
|
|
#+STARTUP: overview
|
|
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ../index.html
|
|
#+HTML_LINK_UP: ../index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
|
|
|
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
|
|
#+PROPERTY: header-args:shell :eval no-export
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports both
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
:END:
|
|
|
|
* TODO computePsdDispl
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/computePsdDispl.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:computePsdDispl>>
|
|
|
|
This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].
|
|
|
|
#+begin_src matlab
|
|
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
|
|
#+end_src
|
|
|
|
* TODO computeSetpoint
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/computeSetpoint.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:computeSetpoint>>
|
|
|
|
This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].
|
|
|
|
#+begin_src matlab
|
|
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
|
|
#+end_src
|
|
|
|
* TODO converErrorBasis
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/converErrorBasis.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:converErrorBasis>>
|
|
|
|
This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
|
|
|
|
#+begin_src matlab
|
|
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
|
|
#+end_src
|
|
|
|
* computeReferencePose
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/computeReferencePose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:computeReferencePose>>
|
|
|
|
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) = Rnz*Rny*Rnx;
|
|
|
|
%% Total Homogeneous transformation
|
|
WTr = Rty*Rry*Rrz*Rh*Rn;
|
|
end
|
|
#+end_src
|
|
* Compute the Sample Position Error w.r.t. the NASS
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/computeSampleError.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:computeSampleError>>
|
|
|
|
This Matlab function is accessible [[file:src/computeSampleError.m][here]].
|
|
|
|
#+begin_src matlab
|
|
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
|
|
#+end_src
|