Analyze measurements with encoders fixed to plates

matlab/src/generateXYZTrajectory.m

@@ -0,0 +1,48 @@
+function [ref] = generateXYZTrajectory(args)
+% generateXYZTrajectory -
+%
+% Syntax: [ref] = generateXYZTrajectory(args)
+%
+% Inputs:
+%    - args
+%
+% Outputs:
+%    - ref - Reference Signal
+
+arguments
+    args.points double {mustBeNumeric} = zeros(2, 3) % [m]
+
+    args.ti    (1,1) double {mustBeNumeric, mustBePositive} = 1 % Time to go to first point and after last point [s]
+    args.tw    (1,1) double {mustBeNumeric, mustBePositive} = 0.5 % Time wait between each point [s]
+    args.tm    (1,1) double {mustBeNumeric, mustBePositive} = 1 % Motion time between points [s]
+
+    args.Ts    (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Sampling Time [s]
+end
+
+time_i = 0:args.Ts:args.ti;
+time_w = 0:args.Ts:args.tw;
+time_m = 0:args.Ts:args.tm;
+
+% Go to initial position
+xyz = (args.points(1,:))'*(time_i/args.ti);
+
+% Wait
+xyz = [xyz, xyz(:,end).*ones(size(time_w))];
+
+% Scans
+for i = 2:size(args.points, 1)
+    % Go to next point
+    xyz = [xyz, xyz(:,end) + (args.points(i,:)' - xyz(:,end))*(time_m/args.tm)];
+    % Wait a litle bit
+    xyz = [xyz, xyz(:,end).*ones(size(time_w))];
+end
+
+% End motion
+xyz = [xyz, xyz(:,end) - xyz(:,end)*(time_i/args.ti)];
+
+t = 0:args.Ts:args.Ts*(length(xyz) - 1);
+
+ref = zeros(length(xyz), 7);
+
+ref(:, 1) = t;
+ref(:, 2:4) = xyz';

matlab/src/generateYZScanTrajectory.m

@@ -0,0 +1,83 @@
+function [ref] = generateYZScanTrajectory(args)
+% generateYZScanTrajectory -
+%
+% Syntax: [ref] = generateYZScanTrajectory(args)
+%
+% Inputs:
+%    - args
+%
+% Outputs:
+%    - ref - Reference Signal
+
+arguments
+    args.y_tot (1,1) double {mustBeNumeric} = 10e-6 % [m]
+    args.z_tot (1,1) double {mustBeNumeric} = 10e-6 % [m]
+
+    args.n     (1,1) double {mustBeInteger, mustBePositive} = 10 % [-]
+
+    args.Ts    (1,1) double {mustBeNumeric, mustBePositive} = 1e-4 % [s]
+
+    args.ti    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+    args.tw    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+    args.ty    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+    args.tz    (1,1) double {mustBeNumeric, mustBePositive} = 1 % [s]
+end
+
+time_i = 0:args.Ts:args.ti;
+time_w = 0:args.Ts:args.tw;
+time_y = 0:args.Ts:args.ty;
+time_z = 0:args.Ts:args.tz;
+
+% Go to initial position
+y = (time_i/args.ti)*(args.y_tot/2);
+
+% Wait
+y = [y, y(end)*ones(size(time_w))];
+
+% Scans
+for i = 1:args.n
+    if mod(i,2) == 0
+        y = [y, -(args.y_tot/2) + (time_y/args.ty)*args.y_tot];
+    else
+        y = [y,  (args.y_tot/2) - (time_y/args.ty)*args.y_tot];
+    end
+
+    if i < args.n
+        y = [y, y(end)*ones(size(time_z))];
+    end
+end
+
+% Wait a litle bit
+y = [y, y(end)*ones(size(time_w))];
+
+% End motion
+y = [y, y(end) - y(end)*time_i/args.ti];
+
+% Go to initial position
+z = (time_i/args.ti)*(args.z_tot/2);
+
+% Wait
+z = [z, z(end)*ones(size(time_w))];
+
+% Scans
+for i = 1:args.n
+    z = [z, z(end)*ones(size(time_y))];
+
+    if i < args.n
+        z = [z, z(end) - (time_z/args.tz)*args.z_tot/(args.n-1)];
+    end
+end
+
+% Wait a litle bit
+z = [z, z(end)*ones(size(time_w))];
+
+% End motion
+z = [z, z(end) - z(end)*time_i/args.ti];
+
+t = 0:args.Ts:args.Ts*(length(y) - 1);
+
+ref = zeros(length(y), 7);
+
+ref(:, 1) = t;
+ref(:, 3) = y;
+ref(:, 4) = z;

matlab/src/getJacobianNanoHexapod.m

@@ -0,0 +1,34 @@
+function [J] = getJacobianNanoHexapod(Hbm)
+% getJacobianNanoHexapod -
+%
+% Syntax: [J] = getJacobianNanoHexapod(Hbm)
+%
+% Inputs:
+%    - Hbm - Height of {B} w.r.t. {M} [m]
+%
+% Outputs:
+%    - J - Jacobian Matrix
+
+Fa = [[-86.05,  -74.78, 22.49],
+      [ 86.05,  -74.78, 22.49],
+      [ 107.79, -37.13, 22.49],
+      [ 21.74,  111.91, 22.49],
+      [-21.74,  111.91, 22.49],
+      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
+
+Mb = [[-28.47, -106.25, -22.50],
+      [ 28.47, -106.25, -22.50],
+      [ 106.25, 28.47,  -22.50],
+      [ 77.78,  77.78,  -22.50],
+      [-77.78,  77.78,  -22.50],
+      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
+
+H = 95e-3; % Stewart platform height [m]
+Fb = Mb + [0; 0; H]; % Bi w.r.t. {F} [m]
+
+si = Fb - Fa;
+si = si./vecnorm(si); % Normalize
+
+Bb = Mb - [0; 0; Hbm];
+
+J = [si', cross(Bb, si)'];

matlab/src/getTransformationMatrixAcc.m

@@ -0,0 +1,23 @@
+function [M] = getTransformationMatrixAcc(Opm, Osm)
+% getTransformationMatrixAcc -
+%
+% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm)
+%
+% Inputs:
+%    - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers
+%    - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation
+%
+% Outputs:
+%    - M - Transformation Matrix
+
+M = zeros(length(Opm), 6);
+
+for i = 1:length(Opm)
+    Ri = [0,         Opm(3,i), -Opm(2,i);
+         -Opm(3,i),  0,         Opm(1,i);
+          Opm(2,i), -Opm(1,i),  0];
+    M(i, 1:3) = Osm(:,i)';
+    M(i, 4:6) = Osm(:,i)'*Ri;
+end
+
+end