Update the way the micro-hexapod is initialize (pitch-roll-yaw)
Next verified that the function to compute the wanted sample position is working => yes
This commit is contained in:
@@ -9,13 +9,17 @@
|
||||
% This Matlab function is accessible [[file:src/computeReferencePose.m][here]].
|
||||
|
||||
|
||||
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
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)
|
||||
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
||||
%
|
||||
% Inputs:
|
||||
% - Dy, Ry, Rz, Dh -
|
||||
% - 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 -
|
||||
@@ -57,8 +61,28 @@ function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
|
||||
0 0 1 Dh(3) ;
|
||||
0 0 0 1 ];
|
||||
|
||||
Rh(1:3, 1:3) = Rhx*Rhy*Rhz;
|
||||
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;
|
||||
WTr = Rty*Rry*Rrz*Rh*Rn;
|
||||
end
|
||||
|
||||
@@ -109,7 +109,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
micro_hexapod = initializeParameters(micro_hexapod);
|
||||
|
||||
%% Setup equilibrium position of each leg
|
||||
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
|
||||
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB);
|
||||
|
||||
%% Save
|
||||
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
||||
@@ -212,26 +212,4 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
||||
J(:, 1:3) = RM';
|
||||
J(:, 4:6) = cross(M_pos_base, RM)';
|
||||
end
|
||||
|
||||
%%
|
||||
function [L] = initializeHexapodPosition(hexapod, AP, ARB)
|
||||
% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
|
||||
%
|
||||
% Syntax: initializeHexapodPosition(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-micro_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
|
||||
end
|
||||
|
||||
@@ -17,13 +17,13 @@ function [ref] = initializeReferences(opts_param)
|
||||
'Dy_amplitude', 0, ... % Amplitude of the displacement [m]
|
||||
'Dy_period', 1, ... % Period of the displacement [s]
|
||||
'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal"
|
||||
'Ry_amplitude', 0, ... % Amplitude [deg]
|
||||
'Ry_amplitude', 0, ... % Amplitude [rad]
|
||||
'Ry_period', 10, ... % Period of the displacement [s]
|
||||
'Rz_type', 'constant', ... % Either "constant" / "rotating"
|
||||
'Rz_amplitude', 0, ... % Initial angle [deg]
|
||||
'Rz_amplitude', 0, ... % Initial angle [rad]
|
||||
'Rz_period', 1, ... % Period of the rotating [s]
|
||||
'Dh_type', 'constant', ... % For now, only constant is implemented
|
||||
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform
|
||||
'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
||||
'Rm_type', 'constant', ... % For now, only constant is implemented
|
||||
'Rm_pos', [0; pi], ... % Initial position of the two masses
|
||||
'Dn_type', 'constant', ... % For now, only constant is implemented
|
||||
@@ -64,13 +64,13 @@ function [ref] = initializeReferences(opts_param)
|
||||
|
||||
switch opts.Ry_type
|
||||
case 'constant'
|
||||
Ry(:) = pi/180*opts.Ry_amplitude;
|
||||
Ry(:) = opts.Ry_amplitude;
|
||||
case 'triangular'
|
||||
Ry(:) = -4*(pi/180*opts.Ry_amplitude) + 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t;
|
||||
Ry(t<0.75*opts.Ry_period) = 2*(pi/180*opts.Ry_amplitude) - 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.75*opts.Ry_period);
|
||||
Ry(t<0.25*opts.Ry_period) = 4*(pi/180*opts.Ry_amplitude)/opts.Ry_period*t(t<0.25*opts.Ry_period);
|
||||
Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
|
||||
Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
|
||||
Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
|
||||
case 'sinusoidal'
|
||||
Ry(:) = opts.Ry_amplitude*sin(2*pi/opts.Ry_period*t);
|
||||
|
||||
otherwise
|
||||
warning('Ry_type is not set correctly');
|
||||
end
|
||||
@@ -82,9 +82,9 @@ function [ref] = initializeReferences(opts_param)
|
||||
|
||||
switch opts.Rz_type
|
||||
case 'constant'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude;
|
||||
Rz(:) = opts.Rz_amplitude;
|
||||
case 'rotating'
|
||||
Rz(:) = pi/180*opts.Rz_amplitude+2*pi/opts.Rz_period*t;
|
||||
Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
|
||||
otherwise
|
||||
warning('Rz_type is not set correctly');
|
||||
end
|
||||
@@ -93,16 +93,37 @@ function [ref] = initializeReferences(opts_param)
|
||||
%% Micro-Hexapod
|
||||
t = [0, Ts];
|
||||
Dh = zeros(length(t), 6);
|
||||
|
||||
opts.Dh_pos(4:6) = pi/180*opts.Dh_pos(4:6); % convert from [deg] to [rad]
|
||||
Dhl = zeros(length(t), 6);
|
||||
|
||||
switch opts.Dh_type
|
||||
case 'constant'
|
||||
Dh = [opts.Dh_pos, opts.Dh_pos];
|
||||
|
||||
load('./mat/stages.mat', 'micro_hexapod');
|
||||
|
||||
AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];
|
||||
|
||||
tx = opts.Dh_pos(4);
|
||||
ty = opts.Dh_pos(5);
|
||||
tz = opts.Dh_pos(6);
|
||||
|
||||
ARB = [cos(tz) -sin(tz) 0;
|
||||
sin(tz) cos(tz) 0;
|
||||
0 0 1]*...
|
||||
[ cos(ty) 0 sin(ty);
|
||||
0 1 0;
|
||||
-sin(ty) 0 cos(ty)]*...
|
||||
[1 0 0;
|
||||
0 cos(tx) -sin(tx);
|
||||
0 sin(tx) cos(tx)];
|
||||
|
||||
[Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
|
||||
Dhl = [Dhl, Dhl];
|
||||
otherwise
|
||||
warning('Dh_type is not set correctly');
|
||||
end
|
||||
Dh = struct('time', t, 'signals', struct('values', Dh));
|
||||
Dhl = struct('time', t, 'signals', struct('values', Dhl));
|
||||
|
||||
%% Axis Compensation - Rm
|
||||
t = [0, Ts];
|
||||
@@ -113,8 +134,6 @@ function [ref] = initializeReferences(opts_param)
|
||||
t = [0, Ts];
|
||||
Dn = zeros(length(t), 6);
|
||||
|
||||
opts.Dn_pos(4:6) = pi/180*opts.Dn_pos(4:6); % convert from [deg] to [rad]
|
||||
|
||||
switch opts.Dn_type
|
||||
case 'constant'
|
||||
Dn = [opts.Dn_pos, opts.Dn_pos];
|
||||
@@ -124,5 +143,5 @@ function [ref] = initializeReferences(opts_param)
|
||||
Dn = struct('time', t, 'signals', struct('values', Dn));
|
||||
|
||||
%% Save
|
||||
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
|
||||
save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
|
||||
end
|
||||
|
||||
Reference in New Issue
Block a user