65 lines
1.5 KiB
Mathematica
65 lines
1.5 KiB
Mathematica
|
% computeReferencePose
|
||
|
|
% :PROPERTIES:
|
||
|
|
% :header-args:matlab+: :tangle ../src/computeReferencePose.m
|
||
|
|
% :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)
|
||
|
|
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
||
|
|
%
|
||
|
|
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - Dy, Ry, Rz, Dh -
|
||
|
|
%
|
||
|
|
% 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) = Rhx*Rhy*Rhz;
|
||
|
|
|
||
|
|
%% Total Homogeneous transformation
|
||
|
|
WTr = Rty*Rry*Rrz*Rh;
|
||
|
|
end
|