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

Configurable Model: stages (solid/flexible)

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-02-17 18:21:20 +01:00
parent 9a5841f3c0
commit aa2f3254c2
50 changed files with 779 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/initializeAxisc.m
View File

@@ -1,7 +1,20 @@
function [axisc] = initializeAxisc()
function [axisc] = initializeAxisc(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
end
axisc = struct();
switch args.type
case 'none'
axisc.type = 0;
case 'rigid'
axisc.type = 1;
case 'flexible'
axisc.type = 2;
end
% Structure
axisc.structure.density = 3400; % [kg/m3]
axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP';

21
src/initializeController.m Normal file
View File

@@ -0,0 +1,21 @@
function [] = initializeController(args)
arguments
args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf'})} = 'open-loop'
args.K (6,6) = ss(zeros(6, 6))
end
controller = struct();
switch args.type
case 'open-loop'
controller.type = 1;
case 'dvf'
controller.type = 2;
case 'iff'
controller.type = 3;
end
controller.K = args.K;
save('./mat/controller.mat', 'controller');

18
src/initializeGranite.m
View File

@@ -1,14 +1,24 @@
function [granite] = initializeGranite(args)
arguments
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]
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
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]
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
end
granite = struct();
switch args.type
case 'none'
granite.type = 0;
case 'rigid'
granite.type = 1;
case 'flexible'
granite.type = 2;
end
granite.density = args.density; % [kg/m3]
granite.STEP = './STEPS/granite/granite.STEP';

17
src/initializeGround.m
View File

@@ -1,8 +1,19 @@
function [ground] = initializeGround()
function [ground] = initializeGround(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'solid'})} = 'solid'
end
ground = struct();
ground.shape = [2, 2, 0.5]; % [m]
ground.density = 2800; % [kg/m3]
switch args.type
case 'none'
ground.type = 0;
case 'solid'
ground.type = 1;
end
ground.shape = [2, 2, 0.5]; % [m]
ground.density = 2800; % [kg/m3]
save('./mat/stages.mat', 'ground', '-append');

2
src/initializeMicroHexapod.m
View File

@@ -1,4 +1,4 @@
function [micro_hexapod] = initializeMicroHexapodNew(args)
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
% initializeFramesPositions

196
src/initializeMicroHexapodOld.m Normal file
View File

@@ -0,0 +1,196 @@
function [micro_hexapod] = initializeMicroHexapod(args)
arguments
args.rigid logical {mustBeNumericOrLogical} = false
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
%% Stewart Object
micro_hexapod = struct();
micro_hexapod.h = 350; % Total height of the platform [mm]
micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
%% Bottom Plate - Mechanical Design
BP = struct();
BP.rad.int = 110; % Internal Radius [mm]
BP.rad.ext = 207.5; % External Radius [mm]
BP.thickness = 26; % Thickness [mm]
BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm]
BP.leg.ang = 9.5; % Angle Offset [deg]
BP.density = 8000; % Density of the material [kg/m^3]
BP.color = [0.6 0.6 0.6]; % Color [rgb]
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
%% Top Plate - Mechanical Design
TP = struct();
TP.rad.int = 82; % Internal Radius [mm]
TP.rad.ext = 150; % Internal Radius [mm]
TP.thickness = 26; % Thickness [mm]
TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm]
TP.leg.ang = 12.1; % Angle Offset [deg]
TP.density = 8000; % Density of the material [kg/m^3]
TP.color = [0.6 0.6 0.6]; % Color [rgb]
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
%% Struts
Leg = struct();
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
if args.rigid
Leg.k.ax = 1e12; % Stiffness of each leg [N/m]
else
Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
end
Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) []
Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
Leg.rad.top = 17; % Radius of the cylinder of the top part [mm]
Leg.density = 8000; % Density of the material [kg/m^3]
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
Leg = updateDamping(Leg);
%% Sphere
SP = struct();
SP.height.bottom = 27; % [mm]
SP.height.top = 27; % [mm]
SP.density.bottom = 8000; % [kg/m^3]
SP.density.top = 8000; % [kg/m^3]
SP.color.bottom = [0.6 0.6 0.6]; % [rgb]
SP.color.top = [0.6 0.6 0.6]; % [rgb]
SP.k.ax = 0; % [N*m/deg]
SP.ksi.ax = 10;
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
SP.rad.bottom = Leg.sphere.bottom; % [mm]
SP.rad.top = Leg.sphere.top; % [mm]
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
SP = updateDamping(SP);
%%
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
%%
micro_hexapod.BP = BP;
micro_hexapod.TP = TP;
micro_hexapod.Leg = Leg;
micro_hexapod.SP = SP;
%%
micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
%% Save
save('./mat/stages.mat', 'micro_hexapod', '-append');
%%
function [element] = updateDamping(element)
field = fieldnames(element.k);
for i = 1:length(field)
element.c.(field{i}) = 2*element.ksi.(field{i})*sqrt(element.k.(field{i})*element.m);
end
end
%%
function [stewart] = initializeParameters(stewart)
%% Connection points on base and top plate w.r.t. World frame at the center of the base plate
stewart.pos_base = zeros(6, 3);
stewart.pos_top = zeros(6, 3);
alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
for i = 1:3
% base points
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
% top points
% Top points are 60 degrees offset
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
end
% permute pos_top points so that legs are end points of base and top points
stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
%% leg vectors
legs = stewart.pos_top - stewart.pos_base;
leg_length = zeros(6, 1);
leg_vectors = zeros(6, 3);
for i = 1:6
leg_length(i) = norm(legs(i,:));
leg_vectors(i,:) = legs(i,:) / leg_length(i);
end
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
stewart.Leg.shape.bot = [0 0; ...
stewart.Leg.rad.bottom 0; ...
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
stewart.Leg.rad.top stewart.Leg.lenght; ...
stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
0 0.2*stewart.Leg.lenght];
%% Calculate revolute and cylindrical axes
rev1 = zeros(6, 3);
rev2 = zeros(6, 3);
cyl1 = zeros(6, 3);
for i = 1:6
rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
cyl1(i,:) = leg_vectors(i,:);
end
%% Coordinate systems
stewart.lower_leg = struct('rotation', eye(3));
stewart.upper_leg = struct('rotation', eye(3));
for i = 1:6
stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
end
%% Position Matrix
stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
%% Compute Jacobian Matrix
aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
stewart.J = getJacobianMatrix(leg_vectors', aa');
end
%%
function J = getJacobianMatrix(RM, M_pos_base)
% RM: [3x6] unit vector of each leg in the fixed frame
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
J = zeros(6);
J(:, 1:3) = RM';
J(:, 4:6) = cross(M_pos_base, RM)';
end
end

25
src/initializeMirror.m
View File

@@ -1,17 +1,36 @@
function [] = initializeMirror(args)
arguments
args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
end
mirror = struct();
switch args.type
case 'none'
mirror.type = 0;
case 'rigid'
mirror.type = 1;
end
mirror.h = 50; % Height of the mirror [mm]
mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
mirror.support_rad = 100; % radius of the support plate [mm]
mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
% point of interest offset in z (above the top surfave) [mm]
switch args.type
case 'none'
mirror.jacobian = 200;
case 'rigid'
mirror.jacobian = 200 - mirror.h;
end
mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
mirror.density = 2400; % Density of the material [kg/m3]

6
src/initializeReferences.m
View File

@@ -54,9 +54,9 @@ Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1);
switch args.Dy_type
case 'constant'
Dy(:) = args.Dy_amplitude;
Dyd(:) = 0;
Dydd(:) = 0;
Dy(:) = args.Dy_amplitude;
Dyd(:) = 0;
Dydd(:) = 0;
case 'triangular'
% This is done to unsure that we start with no displacement
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);

34
src/initializeRy.m
View File

@@ -1,22 +1,32 @@
function [ry] = initializeRy(args)
arguments
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'})} = '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]
end
ry = struct();
switch args.type
case 'none'
ry.type = 0;
case 'rigid'
ry.type = 1;
case 'flexible'
ry.type = 2;
end
% Ry - Guide for the tilt stage
ry.guide.density = 7800; % [kg/m3]
ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP';

20
src/initializeRz.m
View File

@@ -1,15 +1,25 @@
function [rz] = initializeRz(args)
arguments
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'})} = '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]
end
rz = struct();
switch args.type
case 'none'
rz.type = 0;
case 'rigid'
rz.type = 1;
case 'flexible'
rz.type = 2;
end
% Spindle - Slip Ring
rz.slipring.density = 7800; % [kg/m3]
rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP';

26
src/initializeSample.m
View File

@@ -1,18 +1,28 @@
function [sample] = initializeSample(args)
arguments
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'})} = '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]
end
sample = struct();
switch args.type
case 'none'
sample.type = 0;
case 'rigid'
sample.type = 1;
case 'flexible'
sample.type = 2;
end
sample.radius = args.radius; % [m]
sample.height = args.height; % [m]
sample.mass = args.mass; % [kg]

15
src/initializeSimscapeConfiguration.m Normal file
View File

@@ -0,0 +1,15 @@
function [] = initializeSimscapeConfiguration(args)
arguments
args.gravity logical {mustBeNumericOrLogical} = true
end
conf_simscape = struct();
if args.gravity
conf_simscape.type = 1;
else
conf_simscape.type = 2;
end
save('./mat/conf_simscape.mat', 'conf_simscape');

26
src/initializeTy.m
View File

@@ -1,18 +1,28 @@
function [ty] = initializeTy(args)
arguments
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'})} = '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]
end
ty = struct();
switch args.type
case 'none'
ty.type = 0;
case 'rigid'
ty.type = 1;
case 'flexible'
ty.type = 2;
end
% Ty Granite frame
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP';