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

Huge Update

Browse Source 
- Modify the joints for Ty, Ry, Rz to have only one bushing joint.
- Compensation of gravity
This commit is contained in:
Thomas Dehaeze
2020-02-25 17:49:08 +01:00
parent dfdfcff4db
commit 12c2e4a508
36 changed files with 798 additions and 545 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/computeReferencePose.m
View File

@@ -70,7 +70,7 @@ function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
0 0 1 Dn(3) ;
0 0 0 1 ];
Rn(1:3, 1:3) = Rnx*Rny*Rnz;
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
%% Total Homogeneous transformation
WTr = Rty*Rry*Rrz*Rh*Rn;

27
src/initializeGranite.m
View File

@@ -1,7 +1,8 @@
function [granite] = initializeGranite(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis'})} = 'flexible'
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
@@ -19,23 +20,23 @@ switch args.type
granite.type = 2;
case 'modal-analysis'
granite.type = 3;
case 'init'
granite.type = 4;
end
granite.density = args.density; % [kg/m3]
granite.STEP = './STEPS/granite/granite.STEP';
granite.k.x = 4e9; % [N/m]
granite.k.y = 3e8; % [N/m]
granite.k.z = 8e8; % [N/m]
granite.c.x = 4.0e5; % [N/(m/s)]
granite.c.y = 1.1e5; % [N/(m/s)]
granite.c.z = 9.0e5; % [N/(m/s)]
granite.x0 = args.x0;
granite.y0 = args.y0;
granite.z0 = args.z0;
granite.sample_pos = 0.8; % [m]
granite.K = [4e9; 3e8; 8e8]; % [N/m]
granite.C = [4.0e5; 1.1e5; 9.0e5]; % [N/(m/s)]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fgm');
granite.Deq = -Fgm'./granite.K;
else
granite.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'granite', '-append');

4
src/initializeLoggingConfiguration.m
View File

@@ -1,7 +1,7 @@
function [] = initializeLoggingConfiguration(args)
arguments
args.log char {mustBeMember(args.log,{'none', 'all'})} = 'none'
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
end
@@ -12,6 +12,8 @@ switch args.log
conf_log.type = 0;
case 'all'
conf_log.type = 1;
case 'forces'
conf_log.type = 2;
end
conf_log.Ts = args.Ts;

15
src/initializeMicroHexapod.m
View File

@@ -1,7 +1,7 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
@@ -32,8 +32,8 @@ arguments
% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
% Equilibrium position of each leg
args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
% Force that stiffness of each joint should apply at t=0
args.Foffset logical {mustBeNumericOrLogical} = true
end
micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
@@ -47,7 +47,12 @@ micro_hexapod = computeJacobian(micro_hexapod);
micro_hexapod.Li = Li;
micro_hexapod.dLi = dLi;
micro_hexapod.dLeq = args.dLeq;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fhm');
micro_hexapod.dLeq = -Fhm'./args.Ki;
else
micro_hexapod.dLeq = zeros(6,1);
end
switch args.type
case 'none'
@@ -58,6 +63,8 @@ switch args.type
micro_hexapod.type = 2;
case 'modal-analysis'
micro_hexapod.type = 3;
case 'init'
micro_hexapod.type = 4;
end
save('./mat/stages.mat', 'micro_hexapod', '-append');

16
src/initializeMirror.m
View File

@@ -15,23 +15,23 @@ switch args.type
mirror.type = 1;
end
mirror.h = 50; % Height of the mirror [mm]
mirror.h = 0.05; % Height of the mirror [m]
mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
mirror.thickness = 0.025; % Thickness of the plate supporting the sample [m]
mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
mirror.hole_rad = 0.120; % radius of the hole in the mirror [m]
mirror.support_rad = 100; % radius of the support plate [mm]
mirror.support_rad = 0.1; % radius of the support plate [m]
% point of interest offset in z (above the top surfave) [mm]
% point of interest offset in z (above the top surfave) [m]
switch args.type
case 'none'
mirror.jacobian = 200;
mirror.jacobian = 0.20;
case 'rigid'
mirror.jacobian = 200 - mirror.h;
mirror.jacobian = 0.20 - mirror.h;
end
mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m]
mirror.density = 2400; % Density of the material [kg/m3]

28
src/initializePosError.m Normal file
View File

@@ -0,0 +1,28 @@
function [] = initializePosError(args)
% initializePosError - Initialize the position errors
%
% Syntax: [] = initializePosError(args)
%
% Inputs:
% - args -
arguments
args.error logical {mustBeNumericOrLogical} = false
args.Dy (1,1) double {mustBeNumeric} = 0 % [m]
args.Ry (1,1) double {mustBeNumeric} = 0 % [m]
args.Rz (1,1) double {mustBeNumeric} = 0 % [m]
end
pos_error = struct();
if args.error
pos_error.type = 1;
else
pos_error.type = 0;
end
pos_error.Dy = args.Dy;
pos_error.Ry = args.Ry;
pos_error.Rz = args.Rz;
save('mat/pos_error.mat', 'pos_error');

24
src/initializeReferences.m
View File

@@ -186,12 +186,34 @@ Dn = zeros(length(t), 6);
switch args.Dn_type
case 'constant'
Dn = [args.Dn_pos, args.Dn_pos];
load('mat/stages.mat', 'nano_hexapod');
AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];
tx = args.Dn_pos(4);
ty = args.Dn_pos(5);
tz = args.Dn_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)];
[~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
Dnl = [Dnl, Dnl];
otherwise
warning('Dn_type is not set correctly');
end
Dn = struct('time', t, 'signals', struct('values', Dn));
Dnl = struct('time', t, 'signals', struct('values', Dnl));
%% Save
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'Ts');
end

53
src/initializeRy.m
View File

@@ -1,19 +1,9 @@
function [ry] = initializeRy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
args.Ry_init (1,1) double {mustBeNumeric} = 0
end
ry = struct();
@@ -27,6 +17,8 @@ switch args.type
ry.type = 2;
case 'modal-analysis'
ry.type = 3;
case 'init'
ry.type = 4;
end
% Ry - Guide for the tilt stage
@@ -45,29 +37,18 @@ ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
ry.stage.density = 7800; % [kg/m3]
ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
ry.k.h = 1e8; % Stiffness in the direction of the guidance [N/m]
ry.k.rad = 1e8; % Stiffness in the top direction [N/m]
ry.k.rrad = 1e8; % Stiffness in the side direction [N/m]
ry.c.tilt = 2.8e2;
ry.c.h = 2.8e4;
ry.c.rad = 2.8e4;
ry.c.rrad = 2.8e4;
ry.x0_11 = args.x11;
ry.y0_11 = args.y11;
ry.z0_11 = args.z11;
ry.x0_12 = args.x12;
ry.y0_12 = args.y12;
ry.z0_12 = args.z12;
ry.x0_21 = args.x21;
ry.y0_21 = args.y21;
ry.z0_21 = args.z21;
ry.x0_22 = args.x22;
ry.y0_22 = args.y22;
ry.z0_22 = args.z22;
ry.z_offset = 0.58178; % [m]
ry.Ry_init = args.Ry_init; % [rad]
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fym');
ry.Deq = -Fym'./ry.K;
else
ry.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'ry', '-append');

32
src/initializeRz.m
View File

@@ -1,12 +1,8 @@
function [rz] = initializeRz(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [m]
args.rx0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.ry0 (1,1) double {mustBeNumeric} = 0 % Equilibrium position of the Joint [rad]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
end
rz = struct();
@@ -20,6 +16,8 @@ switch args.type
rz.type = 2;
case 'modal-analysis'
rz.type = 3;
case 'init'
rz.type = 4;
end
% Spindle - Slip Ring
@@ -34,20 +32,14 @@ rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
rz.stator.density = 7800; % [kg/m3]
rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
rz.k.rot = 1e6; % TODO - Rotational Stiffness (Rz) [N*m/deg]
rz.k.tilt = 1e6; % Rotational Stiffness (Rx, Ry) [N*m/deg]
rz.k.ax = 2e9; % Axial Stiffness (Z) [N/m]
rz.k.rad = 7e8; % Radial Stiffness (X, Y) [N/m]
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
rz.c.rot = 1.6e3;
rz.c.tilt = 1.6e3;
rz.c.ax = 7.1e4;
rz.c.rad = 4.2e4;
rz.x0 = args.x0;
rz.y0 = args.y0;
rz.z0 = args.z0;
rz.rx0 = args.rx0;
rz.ry0 = args.ry0;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fzm');
rz.Deq = -Fzm'./rz.K;
else
rz.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'rz', '-append');

38
src/initializeSample.m
View File

@@ -1,15 +1,13 @@
function [sample] = initializeSample(args)
arguments
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m]
args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m]
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible'
args.radius (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % [m]
args.height (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % [m]
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg]
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
args.Foffset logical {mustBeNumericOrLogical} = true
end
sample = struct();
@@ -21,23 +19,23 @@ switch args.type
sample.type = 1;
case 'flexible'
sample.type = 2;
case 'init'
sample.type = 3;
end
sample.radius = args.radius; % [m]
sample.height = args.height; % [m]
sample.mass = args.mass; % [kg]
sample.mass = args.mass; % [kg]
sample.offset = args.offset; % [m]
sample.k.x = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.k.y = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.k.z = sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.K = ones(3,1) * sample.mass * (2*pi * args.freq)^2; % [N/m]
sample.C = 0.1 * sqrt(sample.K*sample.mass); % [N/(m/s)]
sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); % [N/(m/s)]
sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); % [N/(m/s)]
sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); % [N/(m/s)]
sample.x0 = args.x0; % [m]
sample.y0 = args.y0; % [m]
sample.z0 = args.z0; % [m]
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Fsm');
sample.Deq = -Fsm'./sample.K;
else
sample.Deq = zeros(3,1);
end
save('./mat/stages.mat', 'sample', '-append');

34
src/initializeTy.m
View File

@@ -1,15 +1,8 @@
function [ty] = initializeTy(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis'})} = 'flexible'
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
args.Foffset logical {mustBeNumericOrLogical} = true
end
ty = struct();
@@ -23,6 +16,8 @@ switch args.type
ty.type = 2;
case 'modal-analysis'
ty.type = 3;
case 'init'
ty.type = 4;
end
% Ty Granite frame
@@ -61,19 +56,14 @@ ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
ty.rotor.density = 5400; % [kg/m3]
ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
ty.k.ax = 5e8; % Axial Stiffness for each of the 4 guidance (y) [N/m]
ty.k.rad = 5e7; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]
ty.c.ax = 70710; % [N/(m/s)]
ty.c.rad = 22360; % [N/(m/s)]
ty.x0_11 = args.x11;
ty.z0_11 = args.z11;
ty.x0_12 = args.x12;
ty.z0_12 = args.z12;
ty.x0_21 = args.x21;
ty.z0_21 = args.z21;
ty.x0_22 = args.x22;
ty.z0_22 = args.z22;
if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
load('mat/Foffset.mat', 'Ftym');
ty.Deq = -Ftym'./ty.K;
else
ty.Deq = zeros(6,1);
end
save('./mat/stages.mat', 'ty', '-append');

20
src/inverseKinematicsHexapod.m
View File

@@ -1,20 +0,0 @@
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: inverseKinematicsHexapod(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-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