From 85c0b6e031b5b25224631cca49c33934f7aecd10 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 11 Dec 2019 17:09:32 +0100 Subject: [PATCH] Slip the simscape folder into 3 folders --- functions/index.org | 508 +++++++ simscape/index.html | 2553 +------------------------------- simscape/index.org | 1970 +----------------------- simscape_subsystems/index.html | 1476 ++++++++++++++++++ simscape_subsystems/index.org | 1122 ++++++++++++++ simulink_project/index.html | 325 ++++ simulink_project/index.org | 82 + 7 files changed, 3542 insertions(+), 4494 deletions(-) create mode 100644 functions/index.org create mode 100644 simscape_subsystems/index.html create mode 100644 simscape_subsystems/index.org create mode 100644 simulink_project/index.html create mode 100644 simulink_project/index.org diff --git a/functions/index.org b/functions/index.org new file mode 100644 index 0000000..f5d0173 --- /dev/null +++ b/functions/index.org @@ -0,0 +1,508 @@ +#+TITLE: Matlab Functions used for the NASS Project +: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: + +* Functions +<> +** computePsdDispl +:PROPERTIES: +:header-args:matlab+: :tangle ../src/computePsdDispl.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/computePsdDispl.m][here]]. + +#+begin_src matlab + 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 +#+end_src + +** computeSetpoint +:PROPERTIES: +:header-args:matlab+: :tangle ../src/computeSetpoint.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/computeSetpoint.m][here]]. + +#+begin_src matlab + 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 +#+end_src + +** converErrorBasis +:PROPERTIES: +:header-args:matlab+: :tangle ../src/converErrorBasis.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/converErrorBasis.m][here]]. + +#+begin_src matlab + 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 +#+end_src + +** generateDiagPidControl +:PROPERTIES: +:header-args:matlab+: :tangle ../src/generateDiagPidControl.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/generateDiagPidControl.m][here]]. + +#+begin_src matlab + function [K] = generateDiagPidControl(G, fs) + %% + pid_opts = pidtuneOptions(... + 'PhaseMargin', 50, ... + 'DesignFocus', 'disturbance-rejection'); + + %% + K = tf(zeros(6)); + + for i = 1:6 + input_name = G.InputName(i); + output_name = G.OutputName(i); + K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts)); + end + + K.InputName = G.OutputName; + K.OutputName = G.InputName; + end +#+end_src + +** identifyPlant +:PROPERTIES: +:header-args:matlab+: :tangle ../src/identifyPlant.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/identifyPlant.m][here]]. + +#+begin_src matlab + function [sys] = identifyPlant(opts_param) + %% Default values for opts + opts = struct(); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% Options for Linearized + options = linearizeOptions; + options.SampleTime = 0; + + %% Name of the Simulink File + mdl = 'sim_nano_station_id'; + + %% Input/Output definition + io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS + io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion + io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample + io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs + io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces + io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample + io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs + io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs + io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base + io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs + + %% Run the linearization + G = linearize(mdl, io, options); + G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ... + 'Dgx', 'Dgy', 'Dgz', ... + 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ... + 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ... + 'Frzz', 'Ftyx', 'Ftyz'}; + G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ... + 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ... + 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ... + 'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ... + 'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}; + + %% Create the sub transfer functions + minreal_tol = sqrt(eps); + % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame + sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false); + % From ground motion to Sample displacement + sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false); + % From direct forces applied on the sample to displacement of the sample + sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false); + % From forces applied on NASS's legs to force sensor in each leg + sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); + % From forces applied on NASS's legs to displacement of each leg + sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); + % From forces/torques applied by the NASS to position error + sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false); + % From forces/torques applied by the NASS to velocity of the actuator + sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); + % From various disturbance forces to position error + sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false); + + %% We remove low frequency and high frequency dynamics that are usually unstable + % using =freqsep= is risky as it may change the shape of the transfer functions + % f_min = 0.1; % [Hz] + % f_max = 1e4; % [Hz] + + % [~, sys.G_cart] = freqsep(freqsep(sys.G_cart, 2*pi*f_max), 2*pi*f_min); + % [~, sys.G_gm] = freqsep(freqsep(sys.G_gm, 2*pi*f_max), 2*pi*f_min); + % [~, sys.G_fs] = freqsep(freqsep(sys.G_fs, 2*pi*f_max), 2*pi*f_min); + % [~, sys.G_iff] = freqsep(freqsep(sys.G_iff, 2*pi*f_max), 2*pi*f_min); + % [~, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, 2*pi*f_max), 2*pi*f_min); + % [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min); + + %% We finally verify that the system is stable + if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant) + warning('One of the identified system is unstable'); + end + end +#+end_src + +** runSimulation +:PROPERTIES: +:header-args:matlab+: :tangle ../src/runSimulation.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/runSimulation.m][here]]. + +#+begin_src matlab + function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp) + %% Load the controller and save it for the simulation + if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none') + K_obj = load('./mat/K_fb.mat'); + K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok + save('./mat/controllers.mat', 'K'); + elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff') + K_obj = load('./mat/K_fb_iff.mat'); + K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok + save('./mat/controllers.mat', 'K'); + elseif strcmp(ctrl_type, 'ol') + K = tf(zeros(6)); %#ok + save('./mat/controllers.mat', 'K'); + else + error('ctrl_type should be cl or ol'); + end + + %% Active Damping + if strcmp(act_damp, 'iff') + K_iff_crit = load('./mat/K_iff_crit.mat'); + K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok + save('./mat/controllers.mat', 'K_iff', '-append'); + elseif strcmp(act_damp, 'none') + K_iff = tf(zeros(6)); %#ok + save('./mat/controllers.mat', 'K_iff', '-append'); + end + + %% + if strcmp(sys_name, 'pz') + initializeNanoHexapod(struct('actuator', 'piezo')); + elseif strcmp(sys_name, 'vc') + initializeNanoHexapod(struct('actuator', 'lorentz')); + else + error('sys_name should be pz or vc'); + end + + if strcmp(sys_mass, 'light') + initializeSample(struct('mass', 1)); + elseif strcmp(sys_mass, 'heavy') + initializeSample(struct('mass', 50)); + else + error('sys_mass should be light or heavy'); + end + + %% Run the simulation + sim('sim_nano_station_ctrl.slx'); + + %% Split the Dsample matrix into vectors + [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok + time = Dsample.Time; %#ok + + %% Save the result + filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp); + save(sprintf('./mat/%s.mat', filename), ... + 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K'); + end +#+end_src + +** Inverse Kinematics of the Hexapod +:PROPERTIES: +:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]]. + +#+begin_src matlab + 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 +#+end_src diff --git a/simscape/index.html b/simscape/index.html index 295be48..45ff355 100644 --- a/simscape/index.html +++ b/simscape/index.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Simscape Model @@ -258,156 +258,15 @@ for the JavaScript code in this tag.

Table of Contents

-

-This file is used to explain how this Simscape Model works. -

-
    -
  • In section 1, the simulink project with the associated scripts are presented
    • -
  • In section 2, an introduction to Simscape Multibody is done
    • -
  • In section 3, each simscape files are presented with the associated signal names and joint architectures
    • -
  • In section 4, the list of the Simulink library elements are described
    • -
  • In section 5, a list of Matlab function that will be used are defined here
    • -
  • In section 6, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.
    • -
- -
-

1 Simulink Project - Startup and Shutdown scripts

-
-

- -

- -

-From the Simulink project mathworks page: -

-
-

-Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place. -

- -

-With Simulink Projects, you can: -

-
    -
  • Collaborate: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.
    • -
  • Automate: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.
    • -
  • Integrate with source control: Enable easy integration with source control and configuration management tools.
    • -
-
- -

-The project can be opened using the simulinkproject function: -

- -
-
simulinkproject('./');
-
-
- -

-When the project opens, a startup script is ran. -The startup script is defined below and is exported to the project_startup.m script. -

-
-
project = simulinkproject;
-projectRoot = project.RootFolder;
-
-myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
-myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
-
-Simulink.fileGenControl('set',...
-    'CacheFolder', myCacheFolder,...
-    'CodeGenFolder', myCodeFolder,...
-    'createDir', true);
-
-
- -

-When the project closes, it runs the project_shutdown.m script defined below. -

-
-
Simulink.fileGenControl('reset');
-
-
- -

-The project also permits to automatically add defined folder to the path when the project is opened. -

-
-
- -
-

2 Simscape Multibody - Presentation

-
-

- -

-

A simscape model permits to model multi-physics systems.

@@ -415,11 +274,10 @@ A simscape model per

Simscape Multibody permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors.

-
-
-

2.1 Solid bodies

-
+
+

1 Solid bodies

+

Each solid body is represented by a solid block. The geometry of the solid body can be imported using a step file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the step file. They also can be set by hand. @@ -427,9 +285,9 @@ The geometry of the solid body can be imported using a step file. T

-
-

2.2 Frames

-
+
+

2 Frames

+

Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.

@@ -440,9 +298,9 @@ They can be defined from the solid body geometry, or using the -

2.3 Joints

-
+
+

3 Joints

+

Solid Bodies are connected with joints (between frames of the two solid bodies).

@@ -451,7 +309,7 @@ Solid Bodies are connected with joints (between frames of the two solid bodies). There are various types of joints that are all described here.

- +
@@ -584,7 +442,7 @@ Joint blocks are assortments of joint primitives:
  • Constant Velocity: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: CV
    • -
    Table 1: Degrees of freedom associated with each joint
    +
    @@ -876,9 +734,9 @@ Composite Force/Torque sensing: -
    -

    2.4 Measurements

    -
    +
    +

    4 Measurements

    +

    A transform sensor block measures the spatial relationship between two frames: the base B and the follower F.

    @@ -901,9 +759,9 @@ If we want to simulate an inertial sensor, we just have to choose B
    -
    -

    2.5 Excitation

    -
    +
    +

    5 Excitation

    +

    We can apply external forces to the model by using an external force and torque block.

    @@ -914,2372 +772,9 @@ Internal force, acting reciprocally between base and following origins is implem
    - -
    -

    3 Simulink files and signal names

    -
    -

    - -

    - -

    -In order to "normalize" things, the names of all the signal are listed here. -

    -
    - -
    -

    3.1 List of Simscape files

    -
    -

    -Few different Simulink files are used: -

    -
      -
    • kinematics
      • -
    • identification - micro station
      • -
    • identification - nano station
      • -
    • control
      • -
    - -
    Table 2: Joint primitives for each joint type
    - - --- -- -- -- -- -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Table 3: List of simscape files
    Simscape NameTyRyRzHexaNASS
    id micro stationFFFF 
    id nano station stagesFFFFF
    id nano station configDDDDF
    control nano stationDDDDF
    -
    -
    - -
    -

    3.2 List of Inputs

    -
    -
    -
    -

    3.2.1 Perturbations

    -
    - - - --- -- -- -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Table 4: List of Disturbances
    VariableMeaningSizeUnit
    DwGround motion3[m]
    FgExternal force applied on granite3[N]
    FsExternal force applied on the Sample3[N]
    -
    -
    - -
    -

    3.2.2 Measurement Noise

    -
    - - - --- -- -- -- - - - - - - - - - - - - - - - - -
    Table 5: List of Measurement Noise
    VariableMeaningSizeUnit
        
    -
    -
    - -
    -

    3.2.3 Control Inputs

    -
    - - - --- -- -- -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Table 6: List of Control Inputs
    VariableMeaningSizeUnit
    FyActuation force for Ty1[N]
    DyImposed displacement for Ty1[m]
    MyActuation torque for Ry1[N.m]
    RyImposed rotation for Ry1[rad]
    MzActuation torque for Rz1[N.m]
    RzImposed rotation for Rz1[rad]
    FhActuation force/torque for hexapod (cart)6[N, N.m]
    FhlActuation force/torque for hexapod (legs)6[N]
    DhImposed position for hexapod (cart)6[m, rad]
    RmPosition of the two masses2[rad]
    FnActuation force for the NASS (cart)6[N, N.m]
    FnlActuation force for the NASS's legs6[N]
    DnImposed position for the NASS (cart)6[m, rad]
    -
    -
    -
    - -
    -

    3.3 List of Outputs

    -
    - - - --- -- -- -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
    Table 7: List of Outputs
    VariableMeaningSizeUnit
    DgmAbsolute displacement of the granite3[m]
    VgmAbsolute Velocity of the granite3[m/s]
    DymMeasured displacement of Ty1[m]
    RymMeasured rotation of Ry1[rad]
    RzmMeasured rotation of Rz1[rad]
    DhmMeasured position of hexapod (cart)6[m, rad]
    FnlmMeasured force of NASS's legs6[N]
    DnlmMeasured elongation of NASS's legs6[m]
    DnmMeasured position of NASS w.r.t NASS's base6[m, rad]
    VnmMeasured absolute velocity of NASS platform6[m/s, rad/s]
    VnlmMeasured absolute velocity of NASS's legs6[m/s]
    DsmPosition of Sample w.r.t. granite frame6[m, rad]
    -
    -
    -
    - -
    -

    4 Simulink Library

    -
    -

    - -

    - -

    -A simulink library is developed in order to share elements between the different simulink files. -

    -
    - -
    -

    4.1 inputs

    -
    -

    -inputs.slx -

    -
    -
    - -
    -

    4.2 nass_library

    -
    -

    -nasslibrary.slx -

    -
    -
    - -
    -

    4.3 pos_error_wrt_nass_base

    -
    -

    -poserrorwrtnassbase.slx -

    -
    -
    - -
    -

    4.4 QuaternionToAngles

    -
    -

    -QuaternionToAngles.slx -

    -
    -
    - -
    -

    4.5 RotationMatrixToAngle

    -
    -

    -RotationMatrixToAngle.slx -

    -
    -
    -
    - -
    -

    5 Functions

    -
    -

    - -

    -
    -
    -

    5.1 computePsdDispl

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    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
-
    -
    -
    -
    - -
    -

    5.2 computeSetpoint

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    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
-
    -
    -
    -
    - -
    -

    5.3 converErrorBasis

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    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
-
    -
    -
    -
    - -
    -

    5.4 generateDiagPidControl

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [K] = generateDiagPidControl(G, fs)
-    %%
-    pid_opts = pidtuneOptions(...
-        'PhaseMargin', 50, ...
-        'DesignFocus', 'disturbance-rejection');
-
-    %%
-    K = tf(zeros(6));
-
-    for i = 1:6
-        input_name = G.InputName(i);
-        output_name = G.OutputName(i);
-        K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
-    end
-
-    K.InputName = G.OutputName;
-    K.OutputName = G.InputName;
-end
-
    -
    -
    -
    -
    -

    5.5 identifyPlant

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [sys] = identifyPlant(opts_param)
-    %% Default values for opts
-    opts = struct();
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Options for Linearized
-    options = linearizeOptions;
-    options.SampleTime = 0;
-
-    %% Name of the Simulink File
-    mdl = 'sim_nano_station_id';
-
-    %% Input/Output definition
-    io(1)  = linio([mdl, '/Fn'],   1, 'input');  % Cartesian forces applied by NASS
-    io(2)  = linio([mdl, '/Dw'],   1, 'input');  % Ground Motion
-    io(3)  = linio([mdl, '/Fs'],   1, 'input');  % External forces on the sample
-    io(4)  = linio([mdl, '/Fnl'],  1, 'input');  % Forces applied on the NASS's legs
-    io(5)  = linio([mdl, '/Fd'],   1, 'input');  % Disturbance Forces
-    io(6)  = linio([mdl, '/Dsm'],  1, 'output'); % Displacement of the sample
-    io(7)  = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
-    io(8)  = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
-    io(9)  = linio([mdl, '/Es'],   1, 'output'); % Position Error w.r.t. NASS base
-    io(10) = linio([mdl, '/Vlm'],  1, 'output'); % Measured absolute velocity of the legs
-
-    %% Run the linearization
-    G = linearize(mdl, io, options);
-    G.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
-                    'Dgx', 'Dgy', 'Dgz', ...
-                    'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
-                    'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
-                    'Frzz', 'Ftyx', 'Ftyz'};
-    G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
-                    'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
-                    'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
-                    'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
-                    'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
-
-    %% Create the sub transfer functions
-    minreal_tol = sqrt(eps);
-    % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
-    sys.G_cart  = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
-    % From ground motion to Sample displacement
-    sys.G_gm    = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
-    % From direct forces applied on the sample to displacement of the sample
-    sys.G_fs    = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
-    % From forces applied on NASS's legs to force sensor in each leg
-    sys.G_iff   = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
-    % From forces applied on NASS's legs to displacement of each leg
-    sys.G_dleg  = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
-    % From forces/torques applied by the NASS to position error
-    sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
-    % From forces/torques applied by the NASS to velocity of the actuator
-    sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
-    % From various disturbance forces to position error
-    sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
-
-    %% We remove low frequency and high frequency dynamics that are usually unstable
-    % using =freqsep= is risky as it may change the shape of the transfer functions
-    % f_min = 0.1; % [Hz]
-    % f_max = 1e4; % [Hz]
-
-    % [~, sys.G_cart]  = freqsep(freqsep(sys.G_cart,  2*pi*f_max), 2*pi*f_min);
-    % [~, sys.G_gm]    = freqsep(freqsep(sys.G_gm,    2*pi*f_max), 2*pi*f_min);
-    % [~, sys.G_fs]    = freqsep(freqsep(sys.G_fs,    2*pi*f_max), 2*pi*f_min);
-    % [~, sys.G_iff]   = freqsep(freqsep(sys.G_iff,   2*pi*f_max), 2*pi*f_min);
-    % [~, sys.G_dleg]  = freqsep(freqsep(sys.G_dleg,  2*pi*f_max), 2*pi*f_min);
-    % [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min);
-
-    %% We finally verify that the system is stable
-    if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant)
-      warning('One of the identified system is unstable');
-    end
-end
-
    -
    -
    -
    - -
    -

    5.6 runSimulation

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
-    %% Load the controller and save it for the simulation
-    if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
-      K_obj = load('./mat/K_fb.mat');
-      K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
-      save('./mat/controllers.mat', 'K');
-    elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
-      K_obj = load('./mat/K_fb_iff.mat');
-      K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
-      save('./mat/controllers.mat', 'K');
-    elseif strcmp(ctrl_type, 'ol')
-      K = tf(zeros(6)); %#ok
-      save('./mat/controllers.mat', 'K');
-    else
-      error('ctrl_type should be cl or ol');
-    end
-
-    %% Active Damping
-    if strcmp(act_damp, 'iff')
-      K_iff_crit = load('./mat/K_iff_crit.mat');
-      K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
-      save('./mat/controllers.mat', 'K_iff', '-append');
-    elseif strcmp(act_damp, 'none')
-      K_iff = tf(zeros(6)); %#ok
-      save('./mat/controllers.mat', 'K_iff', '-append');
-    end
-
-    %%
-    if strcmp(sys_name, 'pz')
-      initializeNanoHexapod(struct('actuator', 'piezo'));
-    elseif strcmp(sys_name, 'vc')
-      initializeNanoHexapod(struct('actuator', 'lorentz'));
-    else
-      error('sys_name should be pz or vc');
-    end
-
-    if strcmp(sys_mass, 'light')
-      initializeSample(struct('mass', 1));
-    elseif strcmp(sys_mass, 'heavy')
-      initializeSample(struct('mass', 50));
-    else
-      error('sys_mass should be light or heavy');
-    end
-
-    %% Run the simulation
-    sim('sim_nano_station_ctrl.slx');
-
-    %% Split the Dsample matrix into vectors
-    [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok
-    time = Dsample.Time; %#ok
-
-    %% Save the result
-    filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
-    save(sprintf('./mat/%s.mat', filename), ...
-        'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
-end
-
    -
    -
    -
    -
    -
    -

    6 Initialize Elements

    -
    -

    - -

    -
    -
    -

    6.1 Experiment

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [] = initializeExperiment(exp_name, sys_mass)
-    if strcmp(exp_name, 'tomography')
-        if strcmp(sys_mass, 'light')
-            opts_inputs = struct(...
-                'Dw', true,  ...
-                'Rz', 60     ... % rpm
-            );
-        elseif strcpm(sys_mass, 'heavy')
-            opts_inputs = struct(...
-                'Dw', true,  ...
-                'Rz', 1     ... % rpm
-            );
-        else
-            error('sys_mass should be light or heavy');
-        end
-
-        initializeInputs(opts_inputs);
-    else
-        error('exp_name is only configured for tomography');
-    end
-end
-
    -
    -
    -
    - -
    -

    6.2 Generate Reference Signals

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [ref] = initializeReferences(opts_param)
-    %% Default values for opts
-    opts = struct(   ...
-        'Ts',           1e-3, ...       % Sampling Frequency [s]
-        'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
-        'Dy_amplitude', 0, ...       % Amplitude of the displacement [m]
-        'Dy_period',    1, ...          % Period of the displacement [s]
-        'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
-        'Ry_amplitude', 0, ...          % Amplitude [rad]
-        'Ry_period',    10, ...         % Period of the displacement [s]
-        'Rz_type',      'constant', ... % Either "constant" / "rotating"
-        'Rz_amplitude', 0, ...          % Initial angle [rad]
-        'Rz_period',    1, ...          % Period of the rotating [s]
-        'Dh_type',      'constant', ... % For now, only constant is implemented
-        'Dh_pos',       [0; 0; 0; 0; 0; 0], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
-        'Rm_type',      'constant', ... % For now, only constant is implemented
-        'Rm_pos',       [0; pi],  ...   % Initial position of the two masses
-        'Dn_type',      'constant', ... % For now, only constant is implemented
-        'Dn_pos',       [0; 0; 0; 0; 0; 0]  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Set Sampling Time
-    Ts = opts.Ts;
-
-    %% Translation stage - Dy
-    t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
-    Dy = zeros(length(t), 1);
-    switch opts.Dy_type
-      case 'constant'
-        Dy(:) = opts.Dy_amplitude;
-      case 'triangular'
-        Dy(:)                     = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
-        Dy(t<0.75*opts.Dy_period) =  2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
-        Dy(t<0.25*opts.Dy_period) =  4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
-      case 'sinusoidal'
-        Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
-      otherwise
-        warning('Dy_type is not set correctly');
-    end
-    Dy = struct('time', t, 'signals', struct('values', Dy));
-
-
-    %% Tilt Stage - Ry
-    t = 0:Ts:opts.Ry_period-Ts;
-    Ry = zeros(length(t), 1);
-
-    switch opts.Ry_type
-      case 'constant'
-        Ry(:) = opts.Ry_amplitude;
-      case 'triangular'
-        Ry(:)                     = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
-        Ry(t<0.75*opts.Ry_period) =  2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
-        Ry(t<0.25*opts.Ry_period) =  4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
-      case 'sinusoidal'
-
-      otherwise
-        warning('Ry_type is not set correctly');
-    end
-    Ry = struct('time', t, 'signals', struct('values', Ry));
-
-    %% Spindle - Rz
-    t = 0:Ts:opts.Rz_period-Ts;
-    Rz = zeros(length(t), 1);
-
-    switch opts.Rz_type
-      case 'constant'
-        Rz(:) = opts.Rz_amplitude;
-      case 'rotating'
-        Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
-      otherwise
-        warning('Rz_type is not set correctly');
-    end
-    Rz = struct('time', t, 'signals', struct('values', Rz));
-
-    %% Micro-Hexapod
-    t = [0, Ts];
-    Dh = zeros(length(t), 6);
-
-    switch opts.Dh_type
-      case 'constant'
-        Dh = [opts.Dh_pos, opts.Dh_pos];
-      otherwise
-        warning('Dh_type is not set correctly');
-    end
-    Dh = struct('time', t, 'signals', struct('values', Dh));
-
-    %% Axis Compensation - Rm
-    t = [0, Ts];
-    Rm = [opts.Rm_pos, opts.Rm_pos];
-    Rm = struct('time', t, 'signals', struct('values', Rm));
-
-    %% Nano-Hexapod
-    t = [0, Ts];
-    Dn = zeros(length(t), 6);
-
-    switch opts.Dn_type
-      case 'constant'
-        Dn = [opts.Dn_pos, opts.Dn_pos];
-      otherwise
-        warning('Dn_type is not set correctly');
-    end
-    Dn = struct('time', t, 'signals', struct('values', Dn));
-
-    %% Save
-    save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
-end
-
    -
    -
    -
    - -
    -

    6.3 TODO Inputs

    -
    -

    - -

    - -
      -
    • [ ] This function should not be used anymore. Now there are two functions to initialize disturbances and references.
      • -
    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [inputs] = initializeInputs(opts_param)
-    %% Default values for opts
-    opts = struct(   ...
-        'Dw', false, ...
-        'Dy', false, ...
-        'Ry', false, ...
-        'Rz', false, ...
-        'Dh', false, ...
-        'Rm', false, ...
-        'Dn', false  ...
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %% Load Sampling Time and Simulation Time
-    load('./mat/sim_conf.mat', 'sim_conf');
-
-    %% Define the time vector
-    t = 0:sim_conf.Ts:sim_conf.Tsim;
-
-    %% Ground motion - Dw
-    if islogical(opts.Dw) && opts.Dw == true
-        load('./mat/perturbations.mat', 'Wxg');
-        Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3);
-        Dw(:, 1) = lsim(Wxg, Dw(:, 1), t);
-        Dw(:, 2) = lsim(Wxg, Dw(:, 2), t);
-        Dw(:, 3) = lsim(Wxg, Dw(:, 3), t);
-    elseif islogical(opts.Dw) && opts.Dw == false
-        Dw = zeros(length(t), 3);
-    else
-        Dw = opts.Dw;
-    end
-
-    %% Translation stage - Dy
-    if islogical(opts.Dy) && opts.Dy == true
-        Dy = zeros(length(t), 1);
-    elseif islogical(opts.Dy) && opts.Dy == false
-        Dy = zeros(length(t), 1);
-    else
-        Dy = opts.Dy;
-    end
-
-    %% Tilt Stage - Ry
-    if islogical(opts.Ry) && opts.Ry == true
-        Ry = 3*(2*pi/360)*sin(2*pi*0.2*t);
-    elseif islogical(opts.Ry) && opts.Ry == false
-        Ry = zeros(length(t), 1);
-    elseif isnumeric(opts.Ry) && length(opts.Ry) == 1
-        Ry = opts.Ry*(2*pi/360)*ones(length(t), 1);
-    else
-        Ry = opts.Ry;
-    end
-
-    %% Spindle - Rz
-    if islogical(opts.Rz) && opts.Rz == true
-        Rz = 2*pi*0.5*t;
-    elseif islogical(opts.Rz) && opts.Rz == false
-        Rz = zeros(length(t), 1);
-    elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
-        Rz = opts.Rz*(2*pi/60)*t;
-    else
-        Rz = opts.Rz;
-    end
-
-    %% Micro Hexapod - Dh
-    if islogical(opts.Dh) && opts.Dh == true
-        Dh = zeros(length(t), 6);
-    elseif islogical(opts.Dh) && opts.Dh == false
-        Dh = zeros(length(t), 6);
-    else
-        Dh = opts.Dh;
-    end
-
-    %% Axis Compensation - Rm
-    if islogical(opts.Rm)
-        Rm = zeros(length(t), 2);
-        Rm(:, 2) = pi*ones(length(t), 1);
-    else
-        Rm = opts.Rm;
-    end
-
-    %% Nano Hexapod - Dn
-    if islogical(opts.Dn) && opts.Dn == true
-        Dn = zeros(length(t), 6);
-    elseif islogical(opts.Dn) && opts.Dn == false
-        Dn = zeros(length(t), 6);
-    else
-        Dn = opts.Dn;
-    end
-
-    %% Setpoint - Ds
-    Ds = zeros(length(t), 6);
-    for i = 1:length(t)
-        Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i));
-    end
-
-    %% External Forces applied on the Granite
-    Fg = zeros(length(t), 3);
-
-    %% External Forces applied on the Sample
-    Fs = zeros(length(t), 6);
-
-    %% Create the input Structure that will contain all the inputs
-    inputs = struct( ...
-        'Ts', sim_conf.Ts,       ...
-        'Dw', timeseries(Dw, t), ...
-        'Dy', timeseries(Dy, t), ...
-        'Ry', timeseries(Ry, t), ...
-        'Rz', timeseries(Rz, t), ...
-        'Dh', timeseries(Dh, t), ...
-        'Rm', timeseries(Rm, t), ...
-        'Dn', timeseries(Dn, t), ...
-        'Ds', timeseries(Ds, t), ...
-        'Fg', timeseries(Fg, t), ...
-        'Fs', timeseries(Fs, t)  ...
-    );
-
-    %% Save
-    save('./mat/inputs.mat', 'inputs');
-
-    %% Custom Functions
-    function setpoint = computeSetpoint(ty, ry, rz)
-    %%
-    setpoint = zeros(6, 1);
-
-    %% Ty
-    TMTy = [1 0 0 0  ;
-          0 1 0 ty ;
-          0 0 1 0  ;
-          0 0 0 1 ];
-
-    %% Ry
-    TMRy = [ cos(ry) 0 sin(ry) 0 ;
-          0       1 0       0 ;
-          -sin(ry) 0 cos(ry) 0 ;
-          0        0 0       1 ];
-
-    %% Rz
-    TMRz = [cos(rz) -sin(rz) 0 0 ;
-          sin(rz)  cos(rz) 0 0 ;
-          0        0       1 0 ;
-          0        0       0 1 ];
-
-    %% All stages
-    TM = TMTy*TMRy*TMRz;
-
-    [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
-end
-
    -
    -
    -
    - -
    -

    6.4 Ground

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    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
-
    -
    -
    -
    - -
    -

    6.5 Granite

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [granite] = initializeGranite(opts_param)
-    %% Default values for opts
-    opts = struct('rigid', false);
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-      for opt = fieldnames(opts_param)'
-        opts.(opt{1}) = opts_param.(opt{1});
-      end
-    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 opts.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
-
    -
    -
    -
    - -
    -

    6.6 Translation Stage

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [ty] = initializeTy(opts_param)
-    %% Default values for opts
-    opts = struct('rigid', false);
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-      for opt = fieldnames(opts_param)'
-        opts.(opt{1}) = opts_param.(opt{1});
-      end
-    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 opts.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
-
    -
    -
    -
    - -
    -

    6.7 Tilt Stage

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [ry] = initializeRy(opts_param)
-    %% Default values for opts
-    opts = struct('rigid', false);
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-      for opt = fieldnames(opts_param)'
-        opts.(opt{1}) = opts_param.(opt{1});
-      end
-    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 opts.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
-
    -
    -
    -
    - -
    -

    6.8 Spindle

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [rz] = initializeRz(opts_param)
-    %% Default values for opts
-    opts = struct('rigid', false);
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-      for opt = fieldnames(opts_param)'
-        opts.(opt{1}) = opts_param.(opt{1});
-      end
-    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 opts.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
-
    -
    -
    -
    - -
    -

    6.9 Initialize Hexapod legs' length

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB)
-% initializeHexapodPosition -
-%
-% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
-%
-% Inputs:
-%    - hexapod - Hexapod object containing the geometry of the hexapod
-%    - AP - Position vector of point OB expressed in frame {A}
-%    - ARB - Rotation Matrix expressed in frame {A}
-
-  L0 = zeros(6, 1);
-
-  for i = 1:length(L)
-    Bbi = hexapod.pos_top_tranform(i, :)';
-    Aai = hexapod.pos_base(i, :)';
-
-    L0(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
-  end
-
-  hexapod.L0 = L0;
-end
-
    -
    -
    -
    - -
    -

    6.10 Micro Hexapod

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [micro_hexapod] = initializeMicroHexapod(opts_param)
-    %% Default values for opts
-    opts = struct(...
-      'rigid', false, ...
-      'AP',    zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
-      'ARB',   eye(3) ...       % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-      for opt = fieldnames(opts_param)'
-        opts.(opt{1}) = opts_param.(opt{1});
-      end
-    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 opts.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 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.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
-
-    %%
-    function [L] = initializeHexapodPosition(hexapod, AP, ARB)
-    % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
-    %
-    % Syntax: initializeHexapodPosition(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-micro_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
-end
-
    -
    -
    -
    - -
    -

    6.11 Center of gravity compensation

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    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
-
    -
    -
    -
    - -
    -

    6.12 Mirror

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [] = initializeMirror(opts_param)
-    %% Default values for opts
-    opts = struct(...
-        'shape', 'spherical', ... % spherical or conical
-        'angle', 45 ...
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    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(opts.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(opts.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(opts.shape, 'conical')
-        mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.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
-
    -
    -
    -
    - -
    -

    6.13 Nano Hexapod

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [nano_hexapod] = initializeNanoHexapod(opts_param)
-    %% Default values for opts
-    opts = struct('actuator', 'piezo');
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            opts.(opt{1}) = opts_param.(opt{1});
-        end
-    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]
-%     nano_hexapod.jacobian = 174.26; % 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(opts.actuator, 'piezo')
-        Leg.k.ax   = 1e7;   % Stiffness of each leg [N/m]
-    elseif strcmp(opts.actuator, 'lorentz')
-        Leg.k.ax   = 1e4;   % Stiffness of each leg [N/m]
-    else
-        error('opts.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);
-
-    %% 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
-
    -
    -
    -
    - -
    -

    6.14 Cedrat Actuator

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [cedrat] = initializeCedratPiezo(opts_param)
-  %% Default values for opts
-  opts = struct();
-
-  %% Populate opts with input parameters
-  if exist('opts_param','var')
-      for opt = fieldnames(opts_param)'
-          opts.(opt{1}) = opts_param.(opt{1});
-      end
-  end
-
-  %% 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
-
    -
    -
    -
    - -
    -

    6.15 Sample

    -
    -

    - -

    - -

    -This Matlab function is accessible here. -

    - -
    -
    function [sample] = initializeSample(opts_param)
-    %% Default values for opts
-    sample = struct('radius', 100, ...
-                    'height', 300, ...
-                    'mass',   50,  ...
-                    'offset', 0,   ...
-                    'color',  [0.45, 0.45, 0.45] ...
-    );
-
-    %% Populate opts with input parameters
-    if exist('opts_param','var')
-        for opt = fieldnames(opts_param)'
-            sample.(opt{1}) = opts_param.(opt{1});
-        end
-    end
-
-    %%
-    sample.k.x = 1e8;
-    sample.c.x = 0.1*sqrt(sample.k.x*sample.mass);
-
-    sample.k.y = 1e8;
-    sample.c.y = 0.1*sqrt(sample.k.y*sample.mass);
-
-    sample.k.z = 1e8;
-    sample.c.z = 0.1*sqrt(sample.k.z*sample.mass);
-
-    %% Save
-    save('./mat/stages.mat', 'sample', '-append');
-end
-
    -
    -
    -
    -
    -

    Author: Dehaeze Thomas

    -

    Created: 2019-12-10 mar. 18:05

    +

    Created: 2019-12-11 mer. 17:06

    Validate

    diff --git a/simscape/index.org b/simscape/index.org index 5225cef..521a433 100644 --- a/simscape/index.org +++ b/simscape/index.org @@ -42,85 +42,20 @@ :END: * Introduction :ignore: -This file is used to explain how this Simscape Model works. -- In section [[sec:simulink_project]], the simulink project with the associated scripts are presented -- In section [[sec:simscape_model]], an introduction to Simscape Multibody is done -- In section [[sec:simulink_files_signal_names]], each simscape files are presented with the associated signal names and joint architectures -- In section [[sec:simulink_library]], the list of the Simulink library elements are described -- In section [[sec:functions]], a list of Matlab function that will be used are defined here -- In section [[sec:initialize_elements]], all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance. - -* Matlab Init :noexport:ignore: -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) - <> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes - <> -#+end_src - -#+begin_src matlab - cd('../'); -#+end_src - -* Simulink Project - Startup and Shutdown scripts -<> - -From the [[https://mathworks.com/products/simulink/projects.html][Simulink project]] mathworks page: -#+begin_quote - Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place. - - With Simulink Projects, you can: - - *Collaborate*: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives. - - *Automate*: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models. - - *Integrate with source control*: Enable easy integration with source control and configuration management tools. -#+end_quote - -The project can be opened using the =simulinkproject= function: - -#+begin_src matlab - simulinkproject('./'); -#+end_src - -When the project opens, a startup script is ran. -The startup script is defined below and is exported to the =project_startup.m= script. -#+begin_src matlab :eval no :tangle ../src/project_startup.m - project = simulinkproject; - projectRoot = project.RootFolder; - - myCacheFolder = fullfile(projectRoot, '.SimulinkCache'); - myCodeFolder = fullfile(projectRoot, '.SimulinkCode'); - - Simulink.fileGenControl('set',... - 'CacheFolder', myCacheFolder,... - 'CodeGenFolder', myCodeFolder,... - 'createDir', true); -#+end_src - -When the project closes, it runs the =project_shutdown.m= script defined below. -#+begin_src matlab :eval no :tangle ../src/project_shutdown.m - Simulink.fileGenControl('reset'); -#+end_src - -The project also permits to automatically add defined folder to the path when the project is opened. - -* Simscape Multibody - Presentation -<> - A [[https://.mathworks.com/products/simscape.html][simscape]] model permits to model multi-physics systems. [[https://mathworks.com/products/simmechanics.html][Simscape Multibody]] permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors. -** Solid bodies +* Solid bodies Each solid body is represented by a [[https://mathworks.com/help/physmod/sm/ref/solid.html][solid block]]. The geometry of the solid body can be imported using a =step= file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the =step= file. They also can be set by hand. -** Frames +* Frames Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made. They can be defined from the solid body geometry, or using the [[https://mathworks.com/help/physmod/sm/ref/rigidtransform.html][rigid transform block]]. -** Joints +* Joints Solid Bodies are connected with joints (between frames of the two solid bodies). There are various types of joints that are all described [[https://mathworks.com/help/physmod/sm/ug/joints.html][here]]. @@ -190,7 +125,7 @@ Composite Force/Torque sensing: - Constraint force - Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers. -** Measurements +* Measurements A transform sensor block measures the spatial relationship between two frames: the base =B= and the follower =F=. It can give the rotational and translational position, velocity and acceleration. @@ -201,1902 +136,7 @@ If we want to simulate an *inertial sensor*, we just have to choose =B= to be th *Force sensors* are included in the joints blocks. -** Excitation +* Excitation We can apply *external forces* to the model by using an [[https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html][external force and torque block]]. Internal force, acting reciprocally between base and following origins is implemented using the [[https://mathworks.com/help/physmod/sm/ref/internalforce.html][internal force block]] even though it is usually included in one joint block. - -* Simulink files and signal names -<> - -In order to "normalize" things, the names of all the signal are listed here. - -** List of Simscape files - -Few different Simulink files are used: -- kinematics -- identification - micro station -- identification - nano station -- control - -#+name: tab:simscape_files -#+caption: List of simscape files -| Simscape Name | Ty | Ry | Rz | Hexa | NASS | -|------------------------+----+----+----+------+------| -| id micro station | F | F | F | F | | -| id nano station stages | F | F | F | F | F | -| id nano station config | D | D | D | D | F | -| control nano station | D | D | D | D | F | - -** List of Inputs -*** Perturbations - -#+name: tab:perturbations_names -#+caption: List of Disturbances -| Variable | Meaning | Size | Unit | -|----------+--------------------------------------+------+------| -| ~Dw~ | Ground motion | 3 | [m] | -| ~Fg~ | External force applied on granite | 3 | [N] | -| ~Fs~ | External force applied on the Sample | 3 | [N] | - -*** Measurement Noise - -#+name: tab:noise_names -#+caption: List of Measurement Noise -| Variable | Meaning | Size | Unit | -|----------+---------+------+------| -| | | | | - -*** Control Inputs - -#+name: tab:control_inputs_names -#+caption: List of Control Inputs -| Variable | Meaning | Size | Unit | -|----------+-------------------------------------------+------+----------| -| ~Fy~ | Actuation force for Ty | 1 | [N] | -| ~Dy~ | Imposed displacement for Ty | 1 | [m] | -|----------+-------------------------------------------+------+----------| -| ~My~ | Actuation torque for Ry | 1 | [N.m] | -| ~Ry~ | Imposed rotation for Ry | 1 | [rad] | -|----------+-------------------------------------------+------+----------| -| ~Mz~ | Actuation torque for Rz | 1 | [N.m] | -| ~Rz~ | Imposed rotation for Rz | 1 | [rad] | -|----------+-------------------------------------------+------+----------| -| ~Fh~ | Actuation force/torque for hexapod (cart) | 6 | [N, N.m] | -| ~Fhl~ | Actuation force/torque for hexapod (legs) | 6 | [N] | -| ~Dh~ | Imposed position for hexapod (cart) | 6 | [m, rad] | -|----------+-------------------------------------------+------+----------| -| ~Rm~ | Position of the two masses | 2 | [rad] | -|----------+-------------------------------------------+------+----------| -| ~Fn~ | Actuation force for the NASS (cart) | 6 | [N, N.m] | -| ~Fnl~ | Actuation force for the NASS's legs | 6 | [N] | -| ~Dn~ | Imposed position for the NASS (cart) | 6 | [m, rad] | - -** List of Outputs - -#+name: tab:outputs_names -#+caption: List of Outputs -| Variable | Meaning | Size | Unit | -|----------+---------------------------------------------+------+--------------| -| ~Dgm~ | Absolute displacement of the granite | 3 | [m] | -| ~Vgm~ | Absolute Velocity of the granite | 3 | [m/s] | -|----------+---------------------------------------------+------+--------------| -| ~Dym~ | Measured displacement of Ty | 1 | [m] | -|----------+---------------------------------------------+------+--------------| -| ~Rym~ | Measured rotation of Ry | 1 | [rad] | -|----------+---------------------------------------------+------+--------------| -| ~Rzm~ | Measured rotation of Rz | 1 | [rad] | -|----------+---------------------------------------------+------+--------------| -| ~Dhm~ | Measured position of hexapod (cart) | 6 | [m, rad] | -|----------+---------------------------------------------+------+--------------| -| ~Fnlm~ | Measured force of NASS's legs | 6 | [N] | -| ~Dnlm~ | Measured elongation of NASS's legs | 6 | [m] | -| ~Dnm~ | Measured position of NASS w.r.t NASS's base | 6 | [m, rad] | -| ~Vnm~ | Measured absolute velocity of NASS platform | 6 | [m/s, rad/s] | -| ~Vnlm~ | Measured absolute velocity of NASS's legs | 6 | [m/s] | -|----------+---------------------------------------------+------+--------------| -| ~Dsm~ | Position of Sample w.r.t. granite frame | 6 | [m, rad] | - -* Simulink Library -<> - -A simulink library is developed in order to share elements between the different simulink files. - -** =inputs= -[[../nass_library/inputs.slx][inputs.slx]] - -** =nass_library= -[[../nass_library/nass_library.slx][nass_library.slx]] - -** =pos_error_wrt_nass_base= -[[../nass_library/pos_error_wrt_nass_base.slx][pos_error_wrt_nass_base.slx]] - -** =QuaternionToAngles= -[[../nass_library/QuaternionToAngles.slx][QuaternionToAngles.slx]] - -** =RotationMatrixToAngle= -[[../nass_library/RotationMatrixToAngle.slx][RotationMatrixToAngle.slx]] - -* Functions -<> -** computePsdDispl -:PROPERTIES: -:header-args:matlab+: :tangle ../src/computePsdDispl.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/computePsdDispl.m][here]]. - -#+begin_src matlab - 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 -#+end_src - -** computeSetpoint -:PROPERTIES: -:header-args:matlab+: :tangle ../src/computeSetpoint.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/computeSetpoint.m][here]]. - -#+begin_src matlab - 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 -#+end_src - -** converErrorBasis -:PROPERTIES: -:header-args:matlab+: :tangle ../src/converErrorBasis.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/converErrorBasis.m][here]]. - -#+begin_src matlab - 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 -#+end_src - -** generateDiagPidControl -:PROPERTIES: -:header-args:matlab+: :tangle ../src/generateDiagPidControl.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/generateDiagPidControl.m][here]]. - -#+begin_src matlab - function [K] = generateDiagPidControl(G, fs) - %% - pid_opts = pidtuneOptions(... - 'PhaseMargin', 50, ... - 'DesignFocus', 'disturbance-rejection'); - - %% - K = tf(zeros(6)); - - for i = 1:6 - input_name = G.InputName(i); - output_name = G.OutputName(i); - K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts)); - end - - K.InputName = G.OutputName; - K.OutputName = G.InputName; - end -#+end_src -** identifyPlant -:PROPERTIES: -:header-args:matlab+: :tangle ../src/identifyPlant.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/identifyPlant.m][here]]. - -#+begin_src matlab - function [sys] = identifyPlant(opts_param) - %% Default values for opts - opts = struct(); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end - - %% Options for Linearized - options = linearizeOptions; - options.SampleTime = 0; - - %% Name of the Simulink File - mdl = 'sim_nano_station_id'; - - %% Input/Output definition - io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS - io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion - io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample - io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs - io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces - io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample - io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs - io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs - io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base - io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs - - %% Run the linearization - G = linearize(mdl, io, options); - G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ... - 'Dgx', 'Dgy', 'Dgz', ... - 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ... - 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ... - 'Frzz', 'Ftyx', 'Ftyz'}; - G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ... - 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ... - 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ... - 'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ... - 'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}; - - %% Create the sub transfer functions - minreal_tol = sqrt(eps); - % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame - sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false); - % From ground motion to Sample displacement - sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false); - % From direct forces applied on the sample to displacement of the sample - sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false); - % From forces applied on NASS's legs to force sensor in each leg - sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); - % From forces applied on NASS's legs to displacement of each leg - sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); - % From forces/torques applied by the NASS to position error - sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false); - % From forces/torques applied by the NASS to velocity of the actuator - sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false); - % From various disturbance forces to position error - sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false); - - %% We remove low frequency and high frequency dynamics that are usually unstable - % using =freqsep= is risky as it may change the shape of the transfer functions - % f_min = 0.1; % [Hz] - % f_max = 1e4; % [Hz] - - % [~, sys.G_cart] = freqsep(freqsep(sys.G_cart, 2*pi*f_max), 2*pi*f_min); - % [~, sys.G_gm] = freqsep(freqsep(sys.G_gm, 2*pi*f_max), 2*pi*f_min); - % [~, sys.G_fs] = freqsep(freqsep(sys.G_fs, 2*pi*f_max), 2*pi*f_min); - % [~, sys.G_iff] = freqsep(freqsep(sys.G_iff, 2*pi*f_max), 2*pi*f_min); - % [~, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, 2*pi*f_max), 2*pi*f_min); - % [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min); - - %% We finally verify that the system is stable - if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant) - warning('One of the identified system is unstable'); - end - end -#+end_src - -** runSimulation -:PROPERTIES: -:header-args:matlab+: :tangle ../src/runSimulation.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/runSimulation.m][here]]. - -#+begin_src matlab - function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp) - %% Load the controller and save it for the simulation - if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none') - K_obj = load('./mat/K_fb.mat'); - K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok - save('./mat/controllers.mat', 'K'); - elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff') - K_obj = load('./mat/K_fb_iff.mat'); - K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok - save('./mat/controllers.mat', 'K'); - elseif strcmp(ctrl_type, 'ol') - K = tf(zeros(6)); %#ok - save('./mat/controllers.mat', 'K'); - else - error('ctrl_type should be cl or ol'); - end - - %% Active Damping - if strcmp(act_damp, 'iff') - K_iff_crit = load('./mat/K_iff_crit.mat'); - K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok - save('./mat/controllers.mat', 'K_iff', '-append'); - elseif strcmp(act_damp, 'none') - K_iff = tf(zeros(6)); %#ok - save('./mat/controllers.mat', 'K_iff', '-append'); - end - - %% - if strcmp(sys_name, 'pz') - initializeNanoHexapod(struct('actuator', 'piezo')); - elseif strcmp(sys_name, 'vc') - initializeNanoHexapod(struct('actuator', 'lorentz')); - else - error('sys_name should be pz or vc'); - end - - if strcmp(sys_mass, 'light') - initializeSample(struct('mass', 1)); - elseif strcmp(sys_mass, 'heavy') - initializeSample(struct('mass', 50)); - else - error('sys_mass should be light or heavy'); - end - - %% Run the simulation - sim('sim_nano_station_ctrl.slx'); - - %% Split the Dsample matrix into vectors - [Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok - time = Dsample.Time; %#ok - - %% Save the result - filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp); - save(sprintf('./mat/%s.mat', filename), ... - 'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K'); - end -#+end_src -* Helping Functions -** Experiment -:PROPERTIES: -:header-args:matlab+: :tangle ../src/initializeExperiment.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeExperiment.m][here]]. - -#+begin_src matlab - function [] = initializeExperiment(exp_name, sys_mass) - if strcmp(exp_name, 'tomography') - if strcmp(sys_mass, 'light') - opts_inputs = struct(... - 'Dw', true, ... - 'Rz', 60 ... % rpm - ); - elseif strcpm(sys_mass, 'heavy') - opts_inputs = struct(... - 'Dw', true, ... - 'Rz', 1 ... % rpm - ); - else - error('sys_mass should be light or heavy'); - end - - initializeInputs(opts_inputs); - else - error('exp_name is only configured for tomography'); - end - end -#+end_src - -** Generate Reference Signals -:PROPERTIES: -:header-args:matlab+: :tangle ../src/initializeReferences.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no -:END: -<> - -This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. - -#+begin_src matlab - function [ref] = initializeReferences(opts_param) - %% Default values for opts - opts = struct( ... - 'Ts', 1e-3, ... % Sampling Frequency [s] - 'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" - 'Dy_amplitude', 0, ... % Amplitude of the displacement [m] - 'Dy_period', 1, ... % Period of the displacement [s] - 'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" - 'Ry_amplitude', 0, ... % Amplitude [rad] - 'Ry_period', 10, ... % Period of the displacement [s] - 'Rz_type', 'constant', ... % Either "constant" / "rotating" - 'Rz_amplitude', 0, ... % Initial angle [rad] - 'Rz_period', 1, ... % Period of the rotating [s] - 'Dh_type', 'constant', ... % For now, only constant is implemented - 'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles) - 'Rm_type', 'constant', ... % For now, only constant is implemented - 'Rm_pos', [0; pi], ... % Initial position of the two masses - 'Dn_type', 'constant', ... % For now, only constant is implemented - 'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform - ); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end - - %% Set Sampling Time - Ts = opts.Ts; - - %% Translation stage - Dy - t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s] - Dy = zeros(length(t), 1); - switch opts.Dy_type - case 'constant' - Dy(:) = opts.Dy_amplitude; - case 'triangular' - Dy(:) = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t; - Dy(t<0.75*opts.Dy_period) = 2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period); - Dy(t<0.25*opts.Dy_period) = 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period); - case 'sinusoidal' - Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t); - otherwise - warning('Dy_type is not set correctly'); - end - Dy = struct('time', t, 'signals', struct('values', Dy)); - - - %% Tilt Stage - Ry - t = 0:Ts:opts.Ry_period-Ts; - Ry = zeros(length(t), 1); - - switch opts.Ry_type - case 'constant' - Ry(:) = opts.Ry_amplitude; - case 'triangular' - Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t; - Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period); - Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period); - case 'sinusoidal' - - otherwise - warning('Ry_type is not set correctly'); - end - Ry = struct('time', t, 'signals', struct('values', Ry)); - - %% Spindle - Rz - t = 0:Ts:opts.Rz_period-Ts; - Rz = zeros(length(t), 1); - - switch opts.Rz_type - case 'constant' - Rz(:) = opts.Rz_amplitude; - case 'rotating' - Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t; - otherwise - warning('Rz_type is not set correctly'); - end - Rz = struct('time', t, 'signals', struct('values', Rz)); - - %% Micro-Hexapod - t = [0, Ts]; - Dh = zeros(length(t), 6); - Dhl = zeros(length(t), 6); - - switch opts.Dh_type - case 'constant' - Dh = [opts.Dh_pos, opts.Dh_pos]; - - load('./mat/stages.mat', 'micro_hexapod'); - - AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)]; - - tx = opts.Dh_pos(4); - ty = opts.Dh_pos(5); - tz = opts.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)); - - %% Axis Compensation - Rm - t = [0, Ts]; - Rm = [opts.Rm_pos, opts.Rm_pos]; - Rm = struct('time', t, 'signals', struct('values', Rm)); - - %% Nano-Hexapod - t = [0, Ts]; - Dn = zeros(length(t), 6); - - switch opts.Dn_type - case 'constant' - Dn = [opts.Dn_pos, opts.Dn_pos]; - otherwise - warning('Dn_type is not set correctly'); - end - Dn = struct('time', t, 'signals', struct('values', Dn)); - - %% Save - save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts'); - end -#+end_src - -** TODO Inputs -:PROPERTIES: -:header-args:matlab+: :tangle ../src/initializeInputs.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -:END: -<> - -- [ ] *This function should not be used anymore*. Now there are two functions to initialize disturbances and references. - -This Matlab function is accessible [[file:../src/initializeInputs.m][here]]. - -#+begin_src matlab - function [inputs] = initializeInputs(opts_param) - %% Default values for opts - opts = struct( ... - 'Dw', false, ... - 'Dy', false, ... - 'Ry', false, ... - 'Rz', false, ... - 'Dh', false, ... - 'Rm', false, ... - 'Dn', false ... - ); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end - - %% Load Sampling Time and Simulation Time - load('./mat/sim_conf.mat', 'sim_conf'); - - %% Define the time vector - t = 0:sim_conf.Ts:sim_conf.Tsim; - - %% Ground motion - Dw - if islogical(opts.Dw) && opts.Dw == true - load('./mat/perturbations.mat', 'Wxg'); - Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3); - Dw(:, 1) = lsim(Wxg, Dw(:, 1), t); - Dw(:, 2) = lsim(Wxg, Dw(:, 2), t); - Dw(:, 3) = lsim(Wxg, Dw(:, 3), t); - elseif islogical(opts.Dw) && opts.Dw == false - Dw = zeros(length(t), 3); - else - Dw = opts.Dw; - end - - %% Translation stage - Dy - if islogical(opts.Dy) && opts.Dy == true - Dy = zeros(length(t), 1); - elseif islogical(opts.Dy) && opts.Dy == false - Dy = zeros(length(t), 1); - else - Dy = opts.Dy; - end - - %% Tilt Stage - Ry - if islogical(opts.Ry) && opts.Ry == true - Ry = 3*(2*pi/360)*sin(2*pi*0.2*t); - elseif islogical(opts.Ry) && opts.Ry == false - Ry = zeros(length(t), 1); - elseif isnumeric(opts.Ry) && length(opts.Ry) == 1 - Ry = opts.Ry*(2*pi/360)*ones(length(t), 1); - else - Ry = opts.Ry; - end - - %% Spindle - Rz - if islogical(opts.Rz) && opts.Rz == true - Rz = 2*pi*0.5*t; - elseif islogical(opts.Rz) && opts.Rz == false - Rz = zeros(length(t), 1); - elseif isnumeric(opts.Rz) && length(opts.Rz) == 1 - Rz = opts.Rz*(2*pi/60)*t; - else - Rz = opts.Rz; - end - - %% Micro Hexapod - Dh - if islogical(opts.Dh) && opts.Dh == true - Dh = zeros(length(t), 6); - elseif islogical(opts.Dh) && opts.Dh == false - Dh = zeros(length(t), 6); - else - Dh = opts.Dh; - end - - %% Axis Compensation - Rm - if islogical(opts.Rm) - Rm = zeros(length(t), 2); - Rm(:, 2) = pi*ones(length(t), 1); - else - Rm = opts.Rm; - end - - %% Nano Hexapod - Dn - if islogical(opts.Dn) && opts.Dn == true - Dn = zeros(length(t), 6); - elseif islogical(opts.Dn) && opts.Dn == false - Dn = zeros(length(t), 6); - else - Dn = opts.Dn; - end - - %% Setpoint - Ds - Ds = zeros(length(t), 6); - for i = 1:length(t) - Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i)); - end - - %% External Forces applied on the Granite - Fg = zeros(length(t), 3); - - %% External Forces applied on the Sample - Fs = zeros(length(t), 6); - - %% Create the input Structure that will contain all the inputs - inputs = struct( ... - 'Ts', sim_conf.Ts, ... - 'Dw', timeseries(Dw, t), ... - 'Dy', timeseries(Dy, t), ... - 'Ry', timeseries(Ry, t), ... - 'Rz', timeseries(Rz, t), ... - 'Dh', timeseries(Dh, t), ... - 'Rm', timeseries(Rm, t), ... - 'Dn', timeseries(Dn, t), ... - 'Ds', timeseries(Ds, t), ... - 'Fg', timeseries(Fg, t), ... - 'Fs', timeseries(Fs, t) ... - ); - - %% Save - save('./mat/inputs.mat', 'inputs'); - - %% Custom Functions - function setpoint = computeSetpoint(ty, ry, rz) - %% - setpoint = zeros(6, 1); - - %% Ty - TMTy = [1 0 0 0 ; - 0 1 0 ty ; - 0 0 1 0 ; - 0 0 0 1 ]; - - %% Ry - TMRy = [ cos(ry) 0 sin(ry) 0 ; - 0 1 0 0 ; - -sin(ry) 0 cos(ry) 0 ; - 0 0 0 1 ]; - - %% Rz - TMRz = [cos(rz) -sin(rz) 0 0 ; - sin(rz) cos(rz) 0 0 ; - 0 0 1 0 ; - 0 0 0 1 ]; - - %% All stages - TM = TMTy*TMRy*TMRz; - - [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 - end -#+end_src - -** Inverse Kinematics of the Hexapod -:PROPERTIES: -:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no -:END: -<> - -This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]]. - -#+begin_src matlab - 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 -#+end_src - - -* Initialize Elements -<> -** Ground -:PROPERTIES: -:header-args:matlab+: :tangle ../src/initializeGround.m -:header-args:matlab+: :comments none :mkdirp yes -:header-args:matlab+: :eval no :results none -: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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeGranite.m][here]]. - -#+begin_src matlab - function [granite] = initializeGranite(opts_param) - %% Default values for opts - opts = struct('rigid', false); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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 opts.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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeTy.m][here]]. - -#+begin_src matlab - function [ty] = initializeTy(opts_param) - %% Default values for opts - opts = struct('rigid', false); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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 opts.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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeRy.m][here]]. - -#+begin_src matlab - function [ry] = initializeRy(opts_param) - %% Default values for opts - opts = struct('rigid', false); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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 opts.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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeRz.m][here]]. - -#+begin_src matlab - function [rz] = initializeRz(opts_param) - %% Default values for opts - opts = struct('rigid', false); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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 opts.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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]]. - -#+begin_src matlab - function [micro_hexapod] = initializeMicroHexapod(opts_param) - %% Default values for opts - opts = struct(... - 'rigid', false, ... - 'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} - 'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A} - ); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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 opts.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, opts.AP, opts.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 -:header-args:matlab+: :eval no :results none -: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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeMirror.m][here]]. - -#+begin_src matlab - function [] = initializeMirror(opts_param) - %% Default values for opts - opts = struct(... - 'shape', 'spherical', ... % spherical or conical - 'angle', 45 ... - ); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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(opts.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(opts.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(opts.shape, 'conical') - mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]]. - -#+begin_src matlab - function [nano_hexapod] = initializeNanoHexapod(opts_param) - %% Default values for opts - opts = struct('actuator', 'piezo'); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - 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] - % nano_hexapod.jacobian = 174.26; % 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(opts.actuator, 'piezo') - Leg.k.ax = 1e7; % Stiffness of each leg [N/m] - elseif strcmp(opts.actuator, 'lorentz') - Leg.k.ax = 1e4; % Stiffness of each leg [N/m] - else - error('opts.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); - - %% 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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]]. - -#+begin_src matlab - function [cedrat] = initializeCedratPiezo(opts_param) - %% Default values for opts - opts = struct(); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - opts.(opt{1}) = opts_param.(opt{1}); - end - end - - %% 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 -:header-args:matlab+: :eval no :results none -:END: -<> - -This Matlab function is accessible [[file:../src/initializeSample.m][here]]. - -#+begin_src matlab - function [sample] = initializeSample(opts_param) - %% Default values for opts - sample = struct('radius', 100, ... - 'height', 300, ... - 'mass', 50, ... - 'offset', 0, ... - 'color', [0.45, 0.45, 0.45] ... - ); - - %% Populate opts with input parameters - if exist('opts_param','var') - for opt = fieldnames(opts_param)' - sample.(opt{1}) = opts_param.(opt{1}); - end - end - - %% - sample.k.x = 1e8; - sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); - - sample.k.y = 1e8; - sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); - - sample.k.z = 1e8; - sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); - - %% Save - save('./mat/stages.mat', 'sample', '-append'); - end -#+end_src diff --git a/simscape_subsystems/index.html b/simscape_subsystems/index.html new file mode 100644 index 0000000..6d75863 --- /dev/null +++ b/simscape_subsystems/index.html @@ -0,0 +1,1476 @@ + + + + + + + +Subsystems used for the Simscape Models + + + + + + + + + + + + + +
    + UP + | + HOME +
    +

    Subsystems used for the Simscape Models

    + + +
    +

    1 Helping Functions

    +
    +
    +
    +

    1.1 Generate Reference Signals

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [ref] = initializeReferences(opts_param)
+    %% Default values for opts
+    opts = struct(   ...
+        'Ts',           1e-3, ...       % Sampling Frequency [s]
+        'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
+        'Dy_amplitude', 0, ...       % Amplitude of the displacement [m]
+        'Dy_period',    1, ...          % Period of the displacement [s]
+        'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
+        'Ry_amplitude', 0, ...          % Amplitude [rad]
+        'Ry_period',    10, ...         % Period of the displacement [s]
+        'Rz_type',      'constant', ... % Either "constant" / "rotating"
+        'Rz_amplitude', 0, ...          % Initial angle [rad]
+        'Rz_period',    1, ...          % Period of the rotating [s]
+        'Dh_type',      'constant', ... % For now, only constant is implemented
+        'Dh_pos',       [0; 0; 0; 0; 0; 0], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
+        'Rm_type',      'constant', ... % For now, only constant is implemented
+        'Rm_pos',       [0; pi],  ...   % Initial position of the two masses
+        'Dn_type',      'constant', ... % For now, only constant is implemented
+        'Dn_pos',       [0; 0; 0; 0; 0; 0]  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
+    );
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    end
+
+    %% Set Sampling Time
+    Ts = opts.Ts;
+
+    %% Translation stage - Dy
+    t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
+    Dy = zeros(length(t), 1);
+    switch opts.Dy_type
+      case 'constant'
+        Dy(:) = opts.Dy_amplitude;
+      case 'triangular'
+        Dy(:)                     = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
+        Dy(t<0.75*opts.Dy_period) =  2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
+        Dy(t<0.25*opts.Dy_period) =  4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
+      case 'sinusoidal'
+        Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
+      otherwise
+        warning('Dy_type is not set correctly');
+    end
+    Dy = struct('time', t, 'signals', struct('values', Dy));
+
+
+    %% Tilt Stage - Ry
+    t = 0:Ts:opts.Ry_period-Ts;
+    Ry = zeros(length(t), 1);
+
+    switch opts.Ry_type
+      case 'constant'
+        Ry(:) = opts.Ry_amplitude;
+      case 'triangular'
+        Ry(:)                     = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
+        Ry(t<0.75*opts.Ry_period) =  2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
+        Ry(t<0.25*opts.Ry_period) =  4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
+      case 'sinusoidal'
+
+      otherwise
+        warning('Ry_type is not set correctly');
+    end
+    Ry = struct('time', t, 'signals', struct('values', Ry));
+
+    %% Spindle - Rz
+    t = 0:Ts:opts.Rz_period-Ts;
+    Rz = zeros(length(t), 1);
+
+    switch opts.Rz_type
+      case 'constant'
+        Rz(:) = opts.Rz_amplitude;
+      case 'rotating'
+        Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
+      otherwise
+        warning('Rz_type is not set correctly');
+    end
+    Rz = struct('time', t, 'signals', struct('values', Rz));
+
+    %% Micro-Hexapod
+    t = [0, Ts];
+    Dh = zeros(length(t), 6);
+    Dhl = zeros(length(t), 6);
+
+    switch opts.Dh_type
+      case 'constant'
+        Dh = [opts.Dh_pos, opts.Dh_pos];
+
+        load('./mat/stages.mat', 'micro_hexapod');
+
+        AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];
+
+        tx = opts.Dh_pos(4);
+        ty = opts.Dh_pos(5);
+        tz = opts.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));
+
+    %% Axis Compensation - Rm
+    t = [0, Ts];
+    Rm = [opts.Rm_pos, opts.Rm_pos];
+    Rm = struct('time', t, 'signals', struct('values', Rm));
+
+    %% Nano-Hexapod
+    t = [0, Ts];
+    Dn = zeros(length(t), 6);
+
+    switch opts.Dn_type
+      case 'constant'
+        Dn = [opts.Dn_pos, opts.Dn_pos];
+      otherwise
+        warning('Dn_type is not set correctly');
+    end
+    Dn = struct('time', t, 'signals', struct('values', Dn));
+
+    %% Save
+    save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
+end
+
    +
    +
    +
    + +
    +

    1.2 Inverse Kinematics of the Hexapod

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    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
+
    +
    +
    +
    +
    + + +
    +

    2 Initialize Elements

    +
    +

    + +

    +
    +
    +

    2.1 Ground

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    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
+
    +
    +
    +
    + +
    +

    2.2 Granite

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [granite] = initializeGranite(opts_param)
+    %% Default values for opts
+    opts = struct('rigid', false);
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+        opts.(opt{1}) = opts_param.(opt{1});
+      end
+    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 opts.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
+
    +
    +
    +
    + +
    +

    2.3 Translation Stage

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [ty] = initializeTy(opts_param)
+    %% Default values for opts
+    opts = struct('rigid', false);
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+        opts.(opt{1}) = opts_param.(opt{1});
+      end
+    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 opts.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
+
    +
    +
    +
    + +
    +

    2.4 Tilt Stage

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [ry] = initializeRy(opts_param)
+    %% Default values for opts
+    opts = struct('rigid', false);
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+        opts.(opt{1}) = opts_param.(opt{1});
+      end
+    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 opts.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
+
    +
    +
    +
    + +
    +

    2.5 Spindle

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [rz] = initializeRz(opts_param)
+    %% Default values for opts
+    opts = struct('rigid', false);
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+        opts.(opt{1}) = opts_param.(opt{1});
+      end
+    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 opts.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
+
    +
    +
    +
    + +
    +

    2.6 Micro Hexapod

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [micro_hexapod] = initializeMicroHexapod(opts_param)
+    %% Default values for opts
+    opts = struct(...
+      'rigid', false, ...
+      'AP',    zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
+      'ARB',   eye(3) ...       % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
+    );
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+        opts.(opt{1}) = opts_param.(opt{1});
+      end
+    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 opts.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, opts.AP, opts.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
+
    +
    +
    +
    + +
    +

    2.7 Center of gravity compensation

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    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
+
    +
    +
    +
    + +
    +

    2.8 Mirror

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [] = initializeMirror(opts_param)
+    %% Default values for opts
+    opts = struct(...
+        'shape', 'spherical', ... % spherical or conical
+        'angle', 45 ...
+    );
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    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(opts.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(opts.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(opts.shape, 'conical')
+        mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.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
+
    +
    +
    +
    + +
    +

    2.9 Nano Hexapod

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [nano_hexapod] = initializeNanoHexapod(opts_param)
+    %% Default values for opts
+    opts = struct('actuator', 'piezo');
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            opts.(opt{1}) = opts_param.(opt{1});
+        end
+    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]
+%     nano_hexapod.jacobian = 174.26; % 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(opts.actuator, 'piezo')
+        Leg.k.ax   = 1e7;   % Stiffness of each leg [N/m]
+    elseif strcmp(opts.actuator, 'lorentz')
+        Leg.k.ax   = 1e4;   % Stiffness of each leg [N/m]
+    else
+        error('opts.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);
+
+    %% 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
+
    +
    +
    +
    + +
    +

    2.10 Cedrat Actuator

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [cedrat] = initializeCedratPiezo(opts_param)
+  %% Default values for opts
+  opts = struct();
+
+  %% Populate opts with input parameters
+  if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+          opts.(opt{1}) = opts_param.(opt{1});
+      end
+  end
+
+  %% 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
+
    +
    +
    +
    + +
    +

    2.11 Sample

    +
    +

    + +

    + +

    +This Matlab function is accessible here. +

    + +
    +
    function [sample] = initializeSample(opts_param)
+    %% Default values for opts
+    sample = struct('radius', 100, ...
+                    'height', 300, ...
+                    'mass',   50,  ...
+                    'offset', 0,   ...
+                    'color',  [0.45, 0.45, 0.45] ...
+    );
+
+    %% Populate opts with input parameters
+    if exist('opts_param','var')
+        for opt = fieldnames(opts_param)'
+            sample.(opt{1}) = opts_param.(opt{1});
+        end
+    end
+
+    %%
+    sample.k.x = 1e8;
+    sample.c.x = 0.1*sqrt(sample.k.x*sample.mass);
+
+    sample.k.y = 1e8;
+    sample.c.y = 0.1*sqrt(sample.k.y*sample.mass);
+
+    sample.k.z = 1e8;
+    sample.c.z = 0.1*sqrt(sample.k.z*sample.mass);
+
+    %% Save
+    save('./mat/stages.mat', 'sample', '-append');
+end
+
    +
    +
    +
    +
    +
    +
    +

    Author: Dehaeze Thomas

    +

    Created: 2019-12-11 mer. 16:56

    +

    Validate

    +
    + + diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org new file mode 100644 index 0000000..02069a7 --- /dev/null +++ b/simscape_subsystems/index.org @@ -0,0 +1,1122 @@ +#+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: + +* 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]]. + +#+begin_src matlab + function [ref] = initializeReferences(opts_param) + %% Default values for opts + opts = struct( ... + 'Ts', 1e-3, ... % Sampling Frequency [s] + 'Dy_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" + 'Dy_amplitude', 0, ... % Amplitude of the displacement [m] + 'Dy_period', 1, ... % Period of the displacement [s] + 'Ry_type', 'constant', ... % Either "constant" / "triangular" / "sinusoidal" + 'Ry_amplitude', 0, ... % Amplitude [rad] + 'Ry_period', 10, ... % Period of the displacement [s] + 'Rz_type', 'constant', ... % Either "constant" / "rotating" + 'Rz_amplitude', 0, ... % Initial angle [rad] + 'Rz_period', 1, ... % Period of the rotating [s] + 'Dh_type', 'constant', ... % For now, only constant is implemented + 'Dh_pos', [0; 0; 0; 0; 0; 0], ... % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles) + 'Rm_type', 'constant', ... % For now, only constant is implemented + 'Rm_pos', [0; pi], ... % Initial position of the two masses + 'Dn_type', 'constant', ... % For now, only constant is implemented + 'Dn_pos', [0; 0; 0; 0; 0; 0] ... % Initial position [m,m,m,rad,rad,rad] of the top platform + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% Set Sampling Time + Ts = opts.Ts; + + %% Translation stage - Dy + t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s] + Dy = zeros(length(t), 1); + switch opts.Dy_type + case 'constant' + Dy(:) = opts.Dy_amplitude; + case 'triangular' + Dy(:) = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t; + Dy(t<0.75*opts.Dy_period) = 2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period); + Dy(t<0.25*opts.Dy_period) = 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period); + case 'sinusoidal' + Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t); + otherwise + warning('Dy_type is not set correctly'); + end + Dy = struct('time', t, 'signals', struct('values', Dy)); + + + %% Tilt Stage - Ry + t = 0:Ts:opts.Ry_period-Ts; + Ry = zeros(length(t), 1); + + switch opts.Ry_type + case 'constant' + Ry(:) = opts.Ry_amplitude; + case 'triangular' + Ry(:) = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t; + Ry(t<0.75*opts.Ry_period) = 2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period); + Ry(t<0.25*opts.Ry_period) = 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period); + case 'sinusoidal' + + otherwise + warning('Ry_type is not set correctly'); + end + Ry = struct('time', t, 'signals', struct('values', Ry)); + + %% Spindle - Rz + t = 0:Ts:opts.Rz_period-Ts; + Rz = zeros(length(t), 1); + + switch opts.Rz_type + case 'constant' + Rz(:) = opts.Rz_amplitude; + case 'rotating' + Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t; + otherwise + warning('Rz_type is not set correctly'); + end + Rz = struct('time', t, 'signals', struct('values', Rz)); + + %% Micro-Hexapod + t = [0, Ts]; + Dh = zeros(length(t), 6); + Dhl = zeros(length(t), 6); + + switch opts.Dh_type + case 'constant' + Dh = [opts.Dh_pos, opts.Dh_pos]; + + load('./mat/stages.mat', 'micro_hexapod'); + + AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)]; + + tx = opts.Dh_pos(4); + ty = opts.Dh_pos(5); + tz = opts.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)); + + %% Axis Compensation - Rm + t = [0, Ts]; + Rm = [opts.Rm_pos, opts.Rm_pos]; + Rm = struct('time', t, 'signals', struct('values', Rm)); + + %% Nano-Hexapod + t = [0, Ts]; + Dn = zeros(length(t), 6); + + switch opts.Dn_type + case 'constant' + Dn = [opts.Dn_pos, opts.Dn_pos]; + otherwise + warning('Dn_type is not set correctly'); + end + Dn = struct('time', t, 'signals', struct('values', Dn)); + + %% Save + save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts'); + 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(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 opts.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(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 opts.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(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 opts.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(opts_param) + %% Default values for opts + opts = struct('rigid', false); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 opts.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(opts_param) + %% Default values for opts + opts = struct(... + 'rigid', false, ... + 'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A} + 'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A} + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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 opts.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, opts.AP, opts.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(opts_param) + %% Default values for opts + opts = struct(... + 'shape', 'spherical', ... % spherical or conical + 'angle', 45 ... + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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(opts.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(opts.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(opts.shape, 'conical') + mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.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(opts_param) + %% Default values for opts + opts = struct('actuator', 'piezo'); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + 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] + % nano_hexapod.jacobian = 174.26; % 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(opts.actuator, 'piezo') + Leg.k.ax = 1e7; % Stiffness of each leg [N/m] + elseif strcmp(opts.actuator, 'lorentz') + Leg.k.ax = 1e4; % Stiffness of each leg [N/m] + else + error('opts.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); + + %% 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(opts_param) + %% Default values for opts + opts = struct(); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + opts.(opt{1}) = opts_param.(opt{1}); + end + end + + %% 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(opts_param) + %% Default values for opts + sample = struct('radius', 100, ... + 'height', 300, ... + 'mass', 50, ... + 'offset', 0, ... + 'color', [0.45, 0.45, 0.45] ... + ); + + %% Populate opts with input parameters + if exist('opts_param','var') + for opt = fieldnames(opts_param)' + sample.(opt{1}) = opts_param.(opt{1}); + end + end + + %% + sample.k.x = 1e8; + sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); + + sample.k.y = 1e8; + sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); + + sample.k.z = 1e8; + sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); + + %% Save + save('./mat/stages.mat', 'sample', '-append'); + end +#+end_src diff --git a/simulink_project/index.html b/simulink_project/index.html new file mode 100644 index 0000000..290e3c4 --- /dev/null +++ b/simulink_project/index.html @@ -0,0 +1,325 @@ + + + + + + + +Simulink Project for the NASS + + + + + + + + + + + + + +
    + UP + | + HOME +
    +

    Simulink Project for the NASS

    + +

    +Simulink Project is used for the study of the NASS using Simscape. +

    + +

    +From the Simulink project mathworks page: +

    +
    +

    +Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place. +

    + +

    +With Simulink Projects, you can: +

    +
      +
    • Collaborate: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.
      • +
    • Automate: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.
      • +
    • Integrate with source control: Enable easy integration with source control and configuration management tools.
      • +
    +
    + +

    +The project can be opened using the simulinkproject function: +

    + +
    +
    simulinkproject('./');
+
    +
    + +

    +When the project opens, a startup script is ran. +The startup script is defined below and is exported to the project_startup.m script. +

    +
    +
    project = simulinkproject;
+projectRoot = project.RootFolder;
+
+myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
+myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
+
+Simulink.fileGenControl('set',...
+    'CacheFolder', myCacheFolder,...
+    'CodeGenFolder', myCodeFolder,...
+    'createDir', true);
+
    +
    + +

    +When the project closes, it runs the project_shutdown.m script defined below. +

    +
    +
    Simulink.fileGenControl('reset');
+
    +
    + +

    +The project also permits to automatically add defined folder to the path when the project is opened. +

    +
    +
    +

    Author: Dehaeze Thomas

    +

    Created: 2019-12-11 mer. 17:06

    +

    Validate

    +
    + + diff --git a/simulink_project/index.org b/simulink_project/index.org new file mode 100644 index 0000000..44a310f --- /dev/null +++ b/simulink_project/index.org @@ -0,0 +1,82 @@ +#+TITLE: Simulink Project for the NASS +: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: + +Simulink Project is used for the study of the NASS using Simscape. + +From the [[https://mathworks.com/products/simulink/projects.html][Simulink project]] mathworks page: +#+begin_quote + Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place. + + With Simulink Projects, you can: + - *Collaborate*: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives. + - *Automate*: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models. + - *Integrate with source control*: Enable easy integration with source control and configuration management tools. +#+end_quote + +The project can be opened using the =simulinkproject= function: + +#+begin_src matlab + simulinkproject('./'); +#+end_src + +When the project opens, a startup script is ran. +The startup script is defined below and is exported to the =project_startup.m= script. +#+begin_src matlab :eval no :tangle ../src/project_startup.m + project = simulinkproject; + projectRoot = project.RootFolder; + + myCacheFolder = fullfile(projectRoot, '.SimulinkCache'); + myCodeFolder = fullfile(projectRoot, '.SimulinkCode'); + + Simulink.fileGenControl('set',... + 'CacheFolder', myCacheFolder,... + 'CodeGenFolder', myCodeFolder,... + 'createDir', true); +#+end_src + +When the project closes, it runs the =project_shutdown.m= script defined below. +#+begin_src matlab :eval no :tangle ../src/project_shutdown.m + Simulink.fileGenControl('reset'); +#+end_src + +The project also permits to automatically add defined folder to the path when the project is opened.