nass-simscape/metrology/matlab/modal_frf_coh.m

89 lines
2.2 KiB
Matlab

% computeReferencePose
% :PROPERTIES:
% :header-args:matlab+: :comments none :mkdirp yes
% :header-args:matlab+: :comments org :mkdirp yes
% :header-args:matlab+: :eval no :results none
% :END:
% <<sec:computeReferencePose>>
% This Matlab function is accessible [[file:src/computeReferencePose.m][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