tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Remove badly tangled files

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-02-25 18:31:33 +01:00
parent e3e6043810
commit f1dd11c3a4
9 changed files with 2 additions and 1313 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/computePsdDispl.m
View File

@@ -1,23 +0,0 @@
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

59
src/computeSetpoint.m
View File

@@ -1,59 +0,0 @@
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

125
src/converErrorBasis.m
View File

@@ -1,125 +0,0 @@
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