#+TITLE: Subsystems used for the Simscape Models :DRAWER: #+STARTUP: overview #+LANGUAGE: en #+EMAIL: dehaeze.thomas@gmail.com #+AUTHOR: Dehaeze Thomas #+HTML_LINK_HOME: ../index.html #+HTML_LINK_UP: ../index.html #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_HEAD: #+HTML_MATHJAX: align: center tagside: right font: TeX #+PROPERTY: header-args:matlab :session *MATLAB* #+PROPERTY: header-args:matlab+ :comments org #+PROPERTY: header-args:matlab+ :results none #+PROPERTY: header-args:matlab+ :exports both #+PROPERTY: header-args:matlab+ :eval no-export #+PROPERTY: header-args:matlab+ :output-dir figs #+PROPERTY: header-args:matlab+ :tangle no #+PROPERTY: header-args:matlab+ :mkdirp yes #+PROPERTY: header-args:shell :eval no-export #+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}") #+PROPERTY: header-args:latex+ :imagemagick t :fit yes #+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 #+PROPERTY: header-args:latex+ :imoutoptions -quality 100 #+PROPERTY: header-args:latex+ :results raw replace :buffer no #+PROPERTY: header-args:latex+ :eval no-export #+PROPERTY: header-args:latex+ :exports both #+PROPERTY: header-args:latex+ :mkdirp yes #+PROPERTY: header-args:latex+ :output-dir figs :END: * Introduction :ignore: * General Subsystems <> ** Generate Reference Signals :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeReferences.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. *** Function Declaration and Documentation #+begin_src matlab function [ref] = initializeReferences(args) #+end_src *** Optional Parameters #+begin_src matlab arguments % Sampling Frequency [s] args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Maximum simulation time [s] args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100 % Either "constant" / "triangular" / "sinusoidal" args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant' % Amplitude of the displacement [m] args.Dy_amplitude (1,1) double {mustBeNumeric} = 0 % Period of the displacement [s] args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % Either "constant" / "triangular" / "sinusoidal" args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant' % Amplitude [rad] args.Ry_amplitude (1,1) double {mustBeNumeric} = 0 % Period of the displacement [s] args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % Either "constant" / "rotating" args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant' % Initial angle [rad] args.Rz_amplitude (1,1) double {mustBeNumeric} = 0 % Period of the rotating [s] args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % For now, only constant is implemented args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant' % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles) args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ... % For now, only constant is implemented args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant' % Initial position of the two masses args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi] % For now, only constant is implemented args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant' % Initial position [m,m,m,rad,rad,rad] of the top platform args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1) end #+end_src *** Initialize Parameters #+begin_src matlab %% Set Sampling Time Ts = args.Ts; Tmax = args.Tmax; %% Low Pass Filter to filter out the references s = zpk('s'); w0 = 2*pi*10; xi = 1; H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2); #+end_src *** Translation Stage #+begin_src matlab %% Translation stage - Dy t = 0:Ts:Tmax; % Time Vector [s] Dy = zeros(length(t), 1); Dyd = zeros(length(t), 1); Dydd = zeros(length(t), 1); switch args.Dy_type case 'constant' 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); i0 = find(t>=args.Dy_period/4,1); Dy(1:end-i0+1) = Dy_raw(i0:end); Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value % The signal is filtered out Dy = lsim(H_lpf, Dy, t); Dyd = lsim(H_lpf*s, Dy, t); Dydd = lsim(H_lpf*s^2, Dy, t); case 'sinusoidal' Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t); Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t); Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t); otherwise warning('Dy_type is not set correctly'); end Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd); #+end_src *** Tilt Stage #+begin_src matlab %% Tilt Stage - Ry t = 0:Ts:Tmax; % Time Vector [s] Ry = zeros(length(t), 1); Ryd = zeros(length(t), 1); Rydd = zeros(length(t), 1); switch args.Ry_type case 'constant' Ry(:) = args.Ry_amplitude; Ryd(:) = 0; Rydd(:) = 0; case 'triangular' Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2); i0 = find(t>=args.Ry_period/4,1); Ry(1:end-i0+1) = Ry_raw(i0:end); Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value % The signal is filtered out Ry = lsim(H_lpf, Ry, t); Ryd = lsim(H_lpf*s, Ry, t); Rydd = lsim(H_lpf*s^2, Ry, t); case 'sinusoidal' Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t); Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t); Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t); otherwise warning('Ry_type is not set correctly'); end Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd); #+end_src *** Spindle #+begin_src matlab %% Spindle - Rz t = 0:Ts:Tmax; % Time Vector [s] Rz = zeros(length(t), 1); Rzd = zeros(length(t), 1); Rzdd = zeros(length(t), 1); switch args.Rz_type case 'constant' Rz(:) = args.Rz_amplitude; Rzd(:) = 0; Rzdd(:) = 0; case 'rotating' Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t; % The signal is filtered out Rz = lsim(H_lpf, Rz, t); Rzd = lsim(H_lpf*s, Rz, t); Rzdd = lsim(H_lpf*s^2, Rz, t); otherwise warning('Rz_type is not set correctly'); end Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd); #+end_src *** Micro Hexapod #+begin_src matlab %% Micro-Hexapod t = [0, Ts]; Dh = zeros(length(t), 6); Dhl = zeros(length(t), 6); switch args.Dh_type case 'constant' Dh = [args.Dh_pos, args.Dh_pos]; load('mat/stages.mat', 'micro_hexapod'); AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)]; tx = args.Dh_pos(4); ty = args.Dh_pos(5); tz = args.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)); #+end_src *** Axis Compensation #+begin_src matlab %% Axis Compensation - Rm t = [0, Ts]; Rm = [args.Rm_pos, args.Rm_pos]; Rm = struct('time', t, 'signals', struct('values', Rm)); #+end_src *** Nano Hexapod #+begin_src matlab %% Nano-Hexapod t = [0, Ts]; Dn = zeros(length(t), 6); switch args.Dn_type case 'constant' Dn = [args.Dn_pos, args.Dn_pos]; otherwise warning('Dn_type is not set correctly'); end Dn = struct('time', t, 'signals', struct('values', Dn)); #+end_src *** Save #+begin_src matlab %% Save save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts'); end #+end_src ** Initialize Disturbances :PROPERTIES: :header-args:matlab+: :tangle ../src/initDisturbances.m :header-args:matlab+: :comments none :mkdirp yes :header-args:matlab+: :eval no :results none :END: <> This Matlab function is accessible [[file:src/initDisturbances.m][here]]. *** Function Declaration and Documentation #+begin_src matlab function [] = initDisturbances(args) % initDisturbances - Initialize the disturbances % % Syntax: [] = initDisturbances(args) % % Inputs: % - args - #+end_src *** Optional Parameters #+begin_src matlab arguments % Ground Motion - X direction args.Dwx logical {mustBeNumericOrLogical} = true % Ground Motion - Y direction args.Dwy logical {mustBeNumericOrLogical} = true % Ground Motion - Z direction args.Dwz logical {mustBeNumericOrLogical} = true % Translation Stage - X direction args.Fty_x logical {mustBeNumericOrLogical} = true % Translation Stage - Z direction args.Fty_z logical {mustBeNumericOrLogical} = true % Spindle - Z direction args.Frz_z logical {mustBeNumericOrLogical} = true end #+end_src *** Load Data #+begin_src matlab load('./disturbances/mat/dist_psd.mat', 'dist_f'); #+end_src We remove the first frequency point that usually is very large. #+begin_src matlab :exports none dist_f.f = dist_f.f(2:end); dist_f.psd_gm = dist_f.psd_gm(2:end); dist_f.psd_ty = dist_f.psd_ty(2:end); dist_f.psd_rz = dist_f.psd_rz(2:end); #+end_src *** Parameters We define some parameters that will be used in the algorithm. #+begin_src matlab Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD T0 = N/Fs; % Signal Duration [s] df = 1/T0; % Frequency resolution of the DFT [Hz] % Also equal to (dist_f.f(2)-dist_f.f(1)) t = linspace(0, T0, N+1)'; % Time Vector [s] Ts = 1/Fs; % Sampling Time [s] #+end_src *** Ground Motion #+begin_src matlab phi = dist_f.psd_gm; C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(phi(i)*df); end #+end_src #+begin_src matlab if args.Dwx theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] else Dwx = zeros(length(t), 1); end #+end_src #+begin_src matlab if args.Dwy theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] else Dwy = zeros(length(t), 1); end #+end_src #+begin_src matlab if args.Dwy theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] else Dwz = zeros(length(t), 1); end #+end_src *** Translation Stage - X direction #+begin_src matlab if args.Fty_x phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(phi(i)*df); end theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N] Fty_x = u; else Fty_x = zeros(length(t), 1); end #+end_src *** Translation Stage - Z direction #+begin_src matlab if args.Fty_z phi = dist_f.psd_ty; C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(phi(i)*df); end theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N] Fty_z = u; else Fty_z = zeros(length(t), 1); end #+end_src *** Spindle - Z direction #+begin_src matlab if args.Frz_z phi = dist_f.psd_rz; C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(phi(i)*df); end theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N] Frz_z = u; else Frz_z = zeros(length(t), 1); end #+end_src *** Direct Forces #+begin_src matlab u = zeros(length(t), 6); Fd = u; #+end_src *** Set initial value to zero #+begin_src matlab Dwx = Dwx - Dwx(1); Dwy = Dwy - Dwy(1); Dwz = Dwz - Dwz(1); Fty_x = Fty_x - Fty_x(1); Fty_z = Fty_z - Fty_z(1); Frz_z = Frz_z - Frz_z(1); #+end_src *** Save #+begin_src matlab save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't'); #+end_src ** Z-Axis Geophone :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeZAxisGeophone.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeZAxisGeophone.m][here]]. #+begin_src matlab function [geophone] = initializeZAxisGeophone(args) arguments args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz] end %% geophone.m = args.mass; %% The Stiffness is set to have the damping resonance frequency geophone.k = geophone.m * (2*pi*args.freq)^2; %% We set the damping value to have critical damping geophone.c = 2*sqrt(geophone.m * geophone.k); %% Save save('./mat/geophone_z_axis.mat', 'geophone'); end #+end_src ** Z-Axis Accelerometer :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeZAxisAccelerometer.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeZAxisAccelerometer.m][here]]. #+begin_src matlab function [accelerometer] = initializeZAxisAccelerometer(args) arguments args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg] args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e2 % [Hz] end %% accelerometer.m = args.mass; %% The Stiffness is set to have the damping resonance frequency accelerometer.k = accelerometer.m * (2*pi*args.freq)^2; %% We set the damping value to have critical damping accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k); %% Save save('./mat/accelerometer_z_axis.mat', 'accelerometer'); end #+end_src * Initialize Elements :PROPERTIES: :ID: a0819dea-8d7a-4d55-b961-2b2ca2312344 :END: <> ** Ground :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeGround.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeGround.m][here]]. #+begin_src matlab function [ground] = initializeGround() %% ground = struct(); ground.shape = [2, 2, 0.5]; % [m] ground.density = 2800; % [kg/m3] ground.color = [0.5, 0.5, 0.5]; %% Save save('./mat/stages.mat', 'ground', '-append'); end #+end_src ** Granite :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeGranite.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeGranite.m][here]]. #+begin_src matlab function [granite] = initializeGranite(args) arguments args.rigid logical {mustBeNumericOrLogical} = false end %% granite = struct(); %% Static Properties granite.density = 2800; % [kg/m3] granite.volume = 0.749; % [m3] TODO - should granite.mass = granite.density*granite.volume; % [kg] granite.color = [1 1 1]; granite.STEP = './STEPS/granite/granite.STEP'; granite.mass_top = 4000; % [kg] TODO %% Dynamical Properties if args.rigid granite.k.x = 1e12; % [N/m] granite.k.y = 1e12; % [N/m] granite.k.z = 1e12; % [N/m] granite.k.rx = 1e10; % [N*m/deg] granite.k.ry = 1e10; % [N*m/deg] granite.k.rz = 1e10; % [N*m/deg] else granite.k.x = 4e9; % [N/m] granite.k.y = 3e8; % [N/m] granite.k.z = 8e8; % [N/m] granite.k.rx = 1e4; % [N*m/deg] granite.k.ry = 1e4; % [N*m/deg] granite.k.rz = 1e6; % [N*m/deg] end granite.c.x = 0.1*sqrt(granite.mass_top*granite.k.x); % [N/(m/s)] granite.c.y = 0.1*sqrt(granite.mass_top*granite.k.y); % [N/(m/s)] granite.c.z = 0.5*sqrt(granite.mass_top*granite.k.z); % [N/(m/s)] granite.c.rx = 0.1*sqrt(granite.mass_top*granite.k.rx); % [N*m/(deg/s)] granite.c.ry = 0.1*sqrt(granite.mass_top*granite.k.ry); % [N*m/(deg/s)] granite.c.rz = 0.1*sqrt(granite.mass_top*granite.k.rz); % [N*m/(deg/s)] %% Positioning parameters granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m] %% Save save('./mat/stages.mat', 'granite', '-append'); end #+end_src ** Translation Stage :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeTy.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeTy.m][here]]. #+begin_src matlab function [ty] = initializeTy(args) arguments args.rigid logical {mustBeNumericOrLogical} = false end %% ty = struct(); %% Y-Translation - Static Properties % Ty Granite frame ty.granite_frame.density = 7800; % [kg/m3] ty.granite_frame.color = [0.753 1 0.753]; ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP'; % Guide Translation Ty ty.guide.density = 7800; % [kg/m3] ty.guide.color = [0.792 0.820 0.933]; ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP'; % Ty - Guide_Translation12 ty.guide12.density = 7800; % [kg/m3] ty.guide12.color = [0.792 0.820 0.933]; ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP'; % Ty - Guide_Translation11 ty.guide11.density = 7800; % [kg/m3] ty.guide11.color = [0.792 0.820 0.933]; ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP'; % Ty - Guide_Translation22 ty.guide22.density = 7800; % [kg/m3] ty.guide22.color = [0.792 0.820 0.933]; ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP'; % Ty - Guide_Translation21 ty.guide21.density = 7800; % [kg/m3] ty.guide21.color = [0.792 0.820 0.933]; ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP'; % Ty - Plateau translation ty.frame.density = 7800; % [kg/m3] ty.frame.color = [0.792 0.820 0.933]; ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP'; % Ty Stator Part ty.stator.density = 5400; % [kg/m3] ty.stator.color = [0.792 0.820 0.933]; ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP'; % Ty Rotor Part ty.rotor.density = 5400; % [kg/m3] ty.rotor.color = [0.792 0.820 0.933]; ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP'; ty.m = 1000; % TODO [kg] %% Y-Translation - Dynamicals Properties if args.rigid ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m] ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m] else 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] end ty.c.ax = 0.1*sqrt(ty.k.ax*ty.m); ty.c.rad = 0.1*sqrt(ty.k.rad*ty.m); %% Save save('./mat/stages.mat', 'ty', '-append'); end #+end_src ** Tilt Stage :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeRy.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeRy.m][here]]. #+begin_src matlab function [ry] = initializeRy(args) arguments args.rigid logical {mustBeNumericOrLogical} = false end %% ry = struct(); %% Tilt Stage - Static Properties % Ry - Guide for the tilt stage ry.guide.density = 7800; % [kg/m3] ry.guide.color = [0.792 0.820 0.933]; ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP'; % Ry - Rotor of the motor ry.rotor.density = 2400; % [kg/m3] ry.rotor.color = [0.792 0.820 0.933]; ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP'; % Ry - Motor ry.motor.density = 3200; % [kg/m3] ry.motor.color = [0.792 0.820 0.933]; ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP'; % Ry - Plateau Tilt ry.stage.density = 7800; % [kg/m3] ry.stage.color = [0.792 0.820 0.933]; ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP'; ry.m = 800; % TODO [kg] %% Tilt Stage - Dynamical Properties if args.rigid ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg] ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m] ry.k.rad = 1e12; % Stiffness in the top direction [N/m] ry.k.rrad = 1e12; % Stiffness in the side direction [N/m] else 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] end ry.c.h = 0.1*sqrt(ry.k.h*ry.m); ry.c.rad = 0.1*sqrt(ry.k.rad*ry.m); ry.c.rrad = 0.1*sqrt(ry.k.rrad*ry.m); ry.c.tilt = 0.1*sqrt(ry.k.tilt*ry.m); %% Positioning parameters ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m] %% Save save('./mat/stages.mat', 'ry', '-append'); end #+end_src ** Spindle :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeRz.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeRz.m][here]]. #+begin_src matlab function [rz] = initializeRz(args) arguments args.rigid logical {mustBeNumericOrLogical} = false end %% rz = struct(); %% Spindle - Static Properties % Spindle - Slip Ring rz.slipring.density = 7800; % [kg/m3] rz.slipring.color = [0.792 0.820 0.933]; rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP'; % Spindle - Rotor rz.rotor.density = 7800; % [kg/m3] rz.rotor.color = [0.792 0.820 0.933]; rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP'; % Spindle - Stator rz.stator.density = 7800; % [kg/m3] rz.stator.color = [0.792 0.820 0.933]; rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP'; % Estimated mass of the mooving part rz.m = 250; % [kg] %% Spindle - Dynamical Properties if args.rigid rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg] rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg] rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m] rz.k.rad = 1e12; % Radial Stiffness (X, Y) [N/m] else 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] end % Damping rz.c.ax = 0.1*sqrt(rz.k.ax*rz.m); rz.c.rad = 0.1*sqrt(rz.k.rad*rz.m); rz.c.tilt = 0.1*sqrt(rz.k.tilt*rz.m); rz.c.rot = 0.1*sqrt(rz.k.rot*rz.m); %% Save save('./mat/stages.mat', 'rz', '-append'); end #+end_src ** Micro Hexapod :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeMicroHexapod.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]]. #+begin_src matlab 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 #+end_src ** Center of gravity compensation :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeAxisc.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeAxisc.m][here]]. #+begin_src matlab function [axisc] = initializeAxisc() %% axisc = struct(); %% Axis Compensator - Static Properties % Structure axisc.structure.density = 3400; % [kg/m3] axisc.structure.color = [0.792 0.820 0.933]; axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP'; % Wheel axisc.wheel.density = 2700; % [kg/m3] axisc.wheel.color = [0.753 0.753 0.753]; axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP'; % Mass axisc.mass.density = 7800; % [kg/m3] axisc.mass.color = [0.792 0.820 0.933]; axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP'; % Gear axisc.gear.density = 7800; % [kg/m3] axisc.gear.color = [0.792 0.820 0.933]; axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP'; axisc.m = 40; % TODO [kg] %% Axis Compensator - Dynamical Properties % axisc.k.ax = 1; % TODO [N*m/deg)] % axisc.c.ax = (1/1)*sqrt(axisc.k.ax/axisc.m); %% Save save('./mat/stages.mat', 'axisc', '-append'); end #+end_src ** Mirror :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeMirror.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeMirror.m][here]]. #+begin_src matlab function [] = initializeMirror(args) arguments args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical' args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 end %% mirror = struct(); 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] mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm] mirror.density = 2400; % Density of the mirror [kg/m3] mirror.color = [0.4 1.0 1.0]; % Color of the mirror mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point %% Shape mirror.shape = [... 0 mirror.h-mirror.thickness mirror.hole_rad mirror.h-mirror.thickness; ... mirror.hole_rad 0; ... mirror.rad 0 ... ]; if strcmp(args.shape, 'spherical') mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm] for z = linspace(0, mirror.h, 101) mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z]; end elseif strcmp(args.shape, 'conical') mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h]; else error('Shape should be either conical or spherical'); end mirror.shape = [mirror.shape; 0 mirror.h]; %% Save save('./mat/stages.mat', 'mirror', '-append'); end #+end_src ** Nano Hexapod :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeNanoHexapod.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]. #+begin_src matlab function [nano_hexapod] = initializeNanoHexapod(args) arguments args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo' args.AP (3,1) double {mustBeNumeric} = zeros(3,1) args.ARB (3,3) double {mustBeNumeric} = eye(3) end %% Stewart Object nano_hexapod = struct(); nano_hexapod.h = 90; % Total height of the platform [mm] nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm] %% Bottom Plate BP = struct(); BP.rad.int = 0; % Internal Radius [mm] BP.rad.ext = 150; % External Radius [mm] BP.thickness = 10; % Thickness [mm] BP.leg.rad = 100; % Radius where the legs articulations are positionned [mm] BP.leg.ang = 5; % Angle Offset [deg] BP.density = 8000;% Density of the material [kg/m^3] BP.color = [0.7 0.7 0.7]; % Color [rgb] BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness]; %% Top Plate TP = struct(); TP.rad.int = 0; % Internal Radius [mm] TP.rad.ext = 100; % Internal Radius [mm] TP.thickness = 10; % Thickness [mm] TP.leg.rad = 90; % Radius where the legs articulations are positionned [mm] TP.leg.ang = 5; % Angle Offset [deg] TP.density = 8000;% Density of the material [kg/m^3] TP.color = [0.7 0.7 0.7]; % Color [rgb] TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness]; %% Leg Leg = struct(); Leg.stroke = 80e-6; % Maximum Stroke of each leg [m] if strcmp(args.actuator, 'piezo') Leg.k.ax = 1e7; % Stiffness of each leg [N/m] elseif strcmp(args.actuator, 'lorentz') Leg.k.ax = 1e4; % Stiffness of each leg [N/m] else error('args.actuator should be piezo or lorentz'); end Leg.ksi.ax = 10; % Maximum amplification at resonance [] Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm] Leg.rad.top = 10; % 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 = 15; % [mm] SP.height.top = 15; % [mm] SP.density.bottom = 8000; % [kg/m^3] SP.density.top = 8000; % [kg/m^3] SP.color.bottom = [0.7 0.7 0.7]; % [rgb] SP.color.top = [0.7 0.7 0.7]; % [rgb] SP.k.ax = 0; % [N*m/deg] SP.ksi.ax = 0; 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]; %% nano_hexapod.BP = BP; nano_hexapod.TP = TP; nano_hexapod.Leg = Leg; nano_hexapod.SP = SP; %% nano_hexapod = initializeParameters(nano_hexapod); %% Setup equilibrium position of each leg nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, args.AP, args.ARB); %% Save save('./mat/stages.mat', 'nano_hexapod', '-append'); %% function [element] = updateDamping(element) field = fieldnames(element.k); for i = 1:length(field) if element.ksi.(field{i}) > 0 element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m); else element.c.(field{i}) = 0; end 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 #+end_src ** Cedrat Actuator :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeCedratPiezo.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]]. #+begin_src matlab function [cedrat] = initializeCedratPiezo() %% Stewart Object cedrat = struct(); cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m] cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m] cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)] cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)] cedrat.L = 80; % Total Width of the Actuator[mm] cedrat.H = 45; % Total Height of the Actuator [mm] cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm] cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg] %% Save save('./mat/stages.mat', 'cedrat', '-append'); end #+end_src ** Sample :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeSample.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/initializeSample.m][here]]. #+begin_src matlab function [sample] = initializeSample(sample) arguments sample.radius (1,1) double {mustBeNumeric, mustBePositive} = 100 % [mm] sample.height (1,1) double {mustBeNumeric, mustBePositive} = 300 % [mm] sample.mass (1,1) double {mustBeNumeric, mustBePositive} = 50 % [kg] sample.freq (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz] sample.offset (1,1) double {mustBeNumeric} = 0 % [mm] sample.color (1,3) double {mustBeNumeric} = [0.45, 0.45, 0.45] end %% sample.k.x = sample.mass * (2*pi * sample.freq)^2; sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); sample.k.y = sample.mass * (2*pi * sample.freq)^2; sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); sample.k.z = sample.mass * (2*pi * sample.freq)^2; sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); %% Save save('./mat/stages.mat', 'sample', '-append'); end #+end_src