UP | HOME

# Simscape Model of the Nano-Active-Stabilization-System

## Table of Contents

- 1. Simulink project
- 2. Simscape Model
  - 2.1. Solid bodies
  - 2.2. Frames
  - 2.3. Joints
  - 2.4. Measurements
  - 2.5. Excitation
- 3. Notes
  - 3.1. Simscape files for identification
  - 3.2. Inputs
    - 3.2.1. Perturbations
    - 3.2.2. Measurement Noise
    - 3.2.3. Control Inputs
  - 3.3. Outputs
- 4. Simulink files
- 5. Simulink Library
  - 5.1. inputs
  - 5.2. nasslibrary
  - 5.3. poserrorwrtnassbase
  - 5.4. QuaternionToAngles
  - 5.5. RotationMatrixToAngle
- 6. Scripts
  - 6.1. Simulation Initialization
- 7. Functions
  - 7.1. computePsdDispl
  - 7.2. computeSetpoint
  - 7.3. converErrorBasis
  - 7.4. generateDiagPidControl
  - 7.5. identifyPlant
  - 7.6. runSimulation
- 8. Initialize Elements
  - 8.1. Simulation Configuration
  - 8.2. Experiment
  - 8.3. Inputs
  - 8.4. Ground
  - 8.5. Granite
  - 8.6. Translation Stage
  - 8.7. Tilt Stage
  - 8.8. Spindle
  - 8.9. Micro Hexapod
  - 8.10. Center of gravity compensation
  - 8.11. Mirror
  - 8.12. Nano Hexapod
  - 8.13. Sample

- Identification of the Micro-Station
- Uniaxial Model
- Kinematics of the station
- Control
- Active Damping
- Plant Analysis
- HAC LAC
- Kinematics
- Modal Analysis
- Cedrat Piezo Actuator

## 1 Simulink project

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.

    freqs = logspace(-1, 3, 1000);
    save_fig = false;
    save('./mat/config.mat', 'freqs', 'save_fig');

    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 Model

A simscape model permits to model multi-physics systems.

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

### 2.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.

### 2.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.

They can be defined from the solid body geometry, or using the rigid transform block.

### 2.3 Joints

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

There are various types of joints that are all described here.

Table 1: Degrees of freedom associated with each joint
Joint Block | Translational DOFs | Rotational DOFs
--- | --- | ---
6-DOF | 3 | 3
Bearing | 1 | 3
Bushing | 3 | 3
Cartesian | 3 | 0
Constant Velocity | 0 | 2
Cylindrical | 1 | 1
Gimbal | 0 | 3
Leadscrew | 1 (coupled) | 1 (coupled)
Pin Slot | 1 | 1
Planar | 2 | 1
Prismatic | 1 | 0
Rectangular | 2 | 0
Revolute | 0 | 1
Spherical | 1 | 3
Telescoping | 1 | 3
Universal | 0 | 2
Weld | 0 | 0

Joint blocks are assortments of joint primitives:

- Prismatic: allows translation along a single standard axis: `Px`, `Py`, `Pz`
- Revolute: allows rotation about a single standard axis: `Rx`, `Ry`, `Rz`
- Spherical: allow rotation about any 3D axis: `S`
- Lead Screw: allows coupled rotation and translation on a standard axis: `LSz`
- Constant Velocity: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: `CV`

Table 2: Joint primitives for each joint type
Joint Block | Px | Py | Pz | Rx | Ry | Rz | S | CV | LSz
--- | --- | --- | --- | --- | --- | --- | --- | --- | ---
6-DOF | x | x | x |  |  |  | x |  |
Bearing |  |  | x | x | x | x |  |  |
Bushing | x | x | x | x | x | x |  |  |
Cartesian | x | x | x |  |  |  |  |  |
Constant Velocity |  |  |  |  |  |  |  | x |
Cylindrical |  |  | x |  |  | x |  |  |
Gimbal |  |  |  | x | x | x |  |  |
Leadscrew |  |  |  |  |  |  |  |  | x
Pin Slot | x |  |  |  |  | x |  |  |
Planar | x | x |  |  |  | x |  |  |
Prismatic |  |  | x |  |  |  |  |  |
Rectangular | x | x |  |  |  |  |  |  |
Revolute |  |  |  |  |  | x |  |  |
Spherical |  |  |  |  |  |  | x |  |
Telescoping |  |  | x |  |  |  | x |  |
Universal |  |  |  | x | x |  |  |  |
Weld |  |  |  |  |  |  |  |  |

For each of the joint primitive, we can specify the dynamical properties:

- The spring stiffness: either linear or rotational one
- The damping coefficient

For the actuation, we can either specify the motion or the force:

- the force applied in the corresponding DOF is provided by the input
- the motion is provided by the input

A sensor can be added to measure either the position, velocity or acceleration of the joint DOF.

Composite Force/Torque sensing:

- Constraint force
- Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers.

### 2.4 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.

The measurement frame should be specified: it corresponds to the frame in which to resolve the selected spatial measurements. The default is `world`.

If we want to simulate an inertial sensor, we just have to choose `B` to be the `world` frame.

Force sensors are included in the joints blocks.

### 2.5 Excitation

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

Internal force, acting reciprocally between base and following origins is implemented using the internal force block even though it is usually included in one joint block.

## 3 Notes

### 3.1 Simscape files for identification

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

### 3.2 Inputs

#### 3.2.1 Perturbations

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]

#### 3.2.2 Measurement Noise

Variable | Meaning | Size | Unit
--- | --- | --- | ---

#### 3.2.3 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]

### 3.3 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]

## 4 Simulink files

Few different Simulink files are used:

- kinematics
- identification - micro station
- identification - nano station
- control

## 5 Simulink Library

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

### 5.1 inputs

### 5.2 nasslibrary

### 5.3 poserrorwrtnassbase

### 5.4 QuaternionToAngles

### 5.5 RotationMatrixToAngle

## 6 Scripts

### 6.1 Simulation Initialization

This Matlab script is accessible here.

This script runs just before the simulation is started. It is used to load the simulation configuration and the controllers used for the simulation.

    %% Load all the data used for the simulation
    load('sim_conf.mat');

    %% Load Controller
    load('controllers.mat');

## 7 Functions

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

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

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

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

### 7.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, '/Dsm'],  1, 'output'); % Displacement of the sample
        io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
        io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
        io(8) = linio([mdl, '/Es'],   1, 'output'); % Position Error w.r.t. NASS base

        %% Run the linearization
        G = linearize(mdl, io, 0);
        G.InputName  = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
                        'Dgx', 'Dgy', 'Dgz', ...
                        'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
                        'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
        G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
                        'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
                        'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
                        'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'};

        %% Create the sub transfer functions
        % 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'}));
        % From ground motion to Sample displacement
        sys.G_gm   = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
        % 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'}));
        % 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'}));
        % 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'}));
        % From forces applied on NASS's legs to displacement of each leg
        sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
    end

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

## 8 Initialize Elements

### 8.1 Simulation Configuration

This Matlab function is accessible here.

    function [] = initializeSimConf(opts_param)
        %% Default values for opts
        opts = struct('Ts',      1e-4,  ... % Sampling time [s]
                      'Tsim',    10,    ... % Simulation time [s]
                      'cl_time', 0,     ... % Close Loop time [s]
                      'gravity', false  ... % Gravity along the z axis
        );

        %% Populate opts with input parameters
        if exist('opts_param','var')
            for opt = fieldnames(opts_param)'
                opts.(opt{1}) = opts_param.(opt{1});
            end
        end

        %%
        sim_conf = struct();

        %%
        sim_conf.Ts = opts.Ts;
        sim_conf.Tsim = opts.Tsim;
        sim_conf.cl_time = opts.cl_time;

        %% Gravity
        if opts.gravity
            sim_conf.g = -9.8; %#ok
        else
            sim_conf.g = 0; %#ok
        end

        %% Save
        save('./mat/sim_conf.mat', 'sim_conf');
    end

### 8.2 Experiment

This Matlab function is accessible here.

    function [] = initializeExperiment(exp_name, sys_mass)
        if strcmp(exp_name, 'tomography')
            opts_sim = struct(...
                'Tsim',    5, ...
                'cl_time', 5  ...
            );
            initializeSimConf(opts_sim);

            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

### 8.3 Inputs

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

### 8.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

### 8.5 Granite

This Matlab function is accessible here.

    function [granite] = initializeGranite()
        %%
        granite = struct();

        %% Static Properties
        granite.density = 2800; % [kg/m3]
        granite.volume  = 0.72; % [m3] TODO - should
        granite.mass    = granite.density*granite.volume; % [kg]
        granite.color   = [1 1 1];
        granite.STEP    = './STEPS/granite/granite.STEP';

        %% Dynamical Properties
        granite.k.x = 1e8; % [N/m]
        granite.c.x = 1e4; % [N/(m/s)]

        granite.k.y = 1e8; % [N/m]
        granite.c.y = 1e4; % [N/(m/s)]

        granite.k.z = 1e8; % [N/m]
        granite.c.z = 1e4; % [N/(m/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

### 8.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 = 250; % TODO [kg]

        %% Y-Translation - Dynamicals Properties
        if opts.rigid
            ty.k.ax  = 1e10; % Axial Stiffness for each of the 4 guidance (y) [N/m]
        else
            ty.k.ax  = 1e7/4; % Axial Stiffness for each of the 4 guidance (y) [N/m]
        end
        ty.k.rad = 9e9/4; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]

        ty.c.ax  = 100*(1/5)*sqrt(ty.k.ax/ty.m);
        ty.c.rad = 100*(1/5)*sqrt(ty.k.rad/ty.m);

        %% Save
        save('./mat/stages.mat', 'ty', '-append');
    end

### 8.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 = 200; % TODO [kg]

        %% Tilt Stage - Dynamical Properties
        if opts.rigid
            ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
        else
            ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
        end

        ry.k.h    = 357e6/4; % Stiffness in the direction of the guidance [N/m]
        ry.k.rad  = 555e6/4; % Stiffness in the top direction [N/m]
        ry.k.rrad = 238e6/4; % Stiffness in the side direction [N/m]

        ry.c.h    = 10*(1/5)*sqrt(ry.k.h/ry.m);
        ry.c.rad  = 10*(1/5)*sqrt(ry.k.rad/ry.m);
        ry.c.rrad = 10*(1/5)*sqrt(ry.k.rrad/ry.m);
        ry.c.tilt = 10*(1/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

### 8.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
        % Estimated stiffnesses
        rz.k.ax   = 2e9; % Axial Stiffness [N/m]
        rz.k.rad  = 7e8; % Radial Stiffness [N/m]
        rz.k.rot  = 100e6*2*pi/360; % Rotational Stiffness [N*m/deg]

        if opts.rigid
            rz.k.tilt = 1e10; % Vertical Rotational Stiffness [N*m/deg]
        else
            rz.k.tilt = 1e2; % TODO what value should I put? [N*m/deg]
        end

        % TODO
        rz.c.ax   = 2*sqrt(rz.k.ax/rz.m);
        rz.c.rad  = 2*sqrt(rz.k.rad/rz.m);
        rz.c.tilt = 100*sqrt(rz.k.tilt/rz.m);
        rz.c.rot  = 100*sqrt(rz.k.rot/rz.m);

        %% Save
        save('./mat/stages.mat', 'rz', '-append');
    end

### 8.9 Micro Hexapod

This Matlab function is accessible here.

    function [micro_hexapod] = initializeMicroHexapod(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
        micro_hexapod = struct();
        micro_hexapod.h        = 350; % Total height of the platform [mm]
    %     micro_hexapod.jacobian = 269.26; % Distance from the top platform to the Jacobian point [mm]
        micro_hexapod.jacobian = 270; % Distance from the top 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]
        Leg.k.ax       = 5e7;   % Stiffness of each leg [N/m]
        Leg.ksi.ax     = 3;     % Maximum amplification at resonance []
        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);

        %% 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}) = 1/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

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

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

### 8.12 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         = 3;

        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)
                element.c.(field{i}) = 1/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

### 8.13 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 = sqrt(sample.k.x*sample.mass)/10;

        sample.k.y = 1e8;
        sample.c.y = sqrt(sample.k.y*sample.mass)/10;

        sample.k.z = 1e8;
        sample.c.z = sqrt(sample.k.y*sample.mass)/10;

        %% Save
        save('./mat/stages.mat', 'sample', '-append');
    end

Author: Dehaeze Thomas

Created: 2019-10-29 mar. 10:52

Validate