nass-simscape/org/simscape_subsystems.org

Subsystems used for the Simscape Models

• Introduction
• Simscape Configuration
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Type
  • Save the Structure
• Logging Configuration
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Type
  • Sampling Time
  • Save the Structure
• Ground
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Type
  • Ground Solid properties
  • Rotation Point
  • Save the Structure
• Granite
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Granite Type
  • Material and Geometry
  • Stiffness and Damping properties
  • Equilibrium position of the each joint.
  • Save the Structure
• Translation Stage
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Translation Stage Type
  • Material and Geometry
  • Stiffness and Damping properties
  • Equilibrium position of the each joint.
  • Save the Structure
• Tilt Stage
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Tilt Type
  • Material and Geometry
  • Stiffness and Damping properties
  • Equilibrium position of the each joint.
  • Save the Structure
• Spindle
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Spindle Type
  • Material and Geometry
  • Stiffness and Damping properties
  • Equilibrium position of the each joint.
  • Save the Structure
• Micro Hexapod
  • Simscape Model
  • Function description
  • Optional Parameters
  • Function content
  • Add Type
  • Save the Structure
• Center of gravity compensation
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Type
  • Material and Geometry
  • Save the Structure
• Mirror
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Mirror Type
  • Mass and Inertia
  • Stiffness and Damping properties
  • Equilibrium position of the each joint.
  • Geometry
  • Save the Structure
• Nano Hexapod
  • Simscape Model
  • Function description
  • Optional Parameters
  • Function content
  • Add Type
  • Save the Structure
• Sample
  • Simscape Model
  • Function description
  • Optional Parameters
  • Structure initialization
  • Add Sample Type
  • Material and Geometry
  • Compute the Inertia
  • Stiffness and Damping properties
  • Equilibrium position of the each joint.
  • Save the Structure
• Initialize Controller
  • Function Declaration and Documentation
  • Optional Parameters
  • Structure initialization
  • Controller Type
  • Save the Structure
• Generate Reference Signals
  • Function Declaration and Documentation
  • Optional Parameters
  • Initialize Parameters
  • Translation Stage
  • Tilt Stage
  • Spindle
  • Micro Hexapod
  • Axis Compensation
  • Nano Hexapod
  • Save
• Initialize Disturbances
  • Function Declaration and Documentation
  • Optional Parameters
  • Load Data
  • Parameters
  • Ground Motion
  • Translation Stage - X direction
  • Translation Stage - Z direction
  • Spindle - Z direction
  • Direct Forces
  • Set initial value to zero
  • Save
• Initialize Position Errors
  • Function Declaration and Documentation
  • Optional Parameters
  • Structure initialization
  • Type
  • Position Errors
  • Save
• Z-Axis Geophone
• Z-Axis Accelerometer

Introduction   ignore

The full Simscape Model is represented in Figure fig:simscape_picture.

Screenshot of the Multi-Body Model representation

This model is divided into multiple subsystems that are independent. These subsystems are saved in separate files and imported in the main file using a block balled "subsystem reference".

Each stage is configured (geometry, mass properties, dynamic properties …) using one function.

These functions are defined below.

Simscape Configuration

<<sec:initializeSimscapeConfiguration>>

Function description

  function [] = initializeSimscapeConfiguration(args)

Optional Parameters

  arguments
    args.gravity logical {mustBeNumericOrLogical} = true
  end

Structure initialization

  conf_simscape = struct();

Add Type

  if args.gravity
    conf_simscape.type = 1;
  else
    conf_simscape.type = 2;
  end

Save the Structure

  save('./mat/conf_simscape.mat', 'conf_simscape');

Logging Configuration

<<sec:initializeLoggingConfiguration>>

Function description

  function [] = initializeLoggingConfiguration(args)

Optional Parameters

  arguments
    args.log      char   {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
    args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
  end

Structure initialization

  conf_log = struct();

Add Type

  switch args.log
    case 'none'
      conf_log.type = 0;
    case 'all'
      conf_log.type = 1;
    case 'forces'
      conf_log.type = 2;
  end

Sampling Time

  conf_log.Ts = args.Ts;

Save the Structure

  save('./mat/conf_log.mat', 'conf_log');

Ground

<<sec:initializeGround>>

Simscape Model

The model of the Ground is composed of:

• A Cartesian joint that is used to simulation the ground motion
• A solid that represents the ground on which the granite is located

Simscape model for the Ground

Simscape picture for the Ground

Function description

  function [ground] = initializeGround(args)

Optional Parameters

  arguments
    args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
    args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
  end

Structure initialization

First, we initialize the granite structure.

  ground = struct();

Add Type

  switch args.type
    case 'none'
      ground.type = 0;
    case 'rigid'
      ground.type = 1;
  end

Ground Solid properties

We set the shape and density of the ground solid element.

  ground.shape   = [2, 2, 0.5]; % [m]
  ground.density = 2800;        % [kg/m3]

Rotation Point

  ground.rot_point = args.rot_point;

Save the Structure

The ground structure is saved.

  save('./mat/stages.mat', 'ground', '-append');

Granite

<<sec:initializeGranite>>

Simscape Model

The Simscape model of the granite is composed of:

• A cartesian joint such that the granite can vibrations along x, y and z axis
• A solid

The output sample_pos corresponds to the impact point of the X-ray.

Simscape model for the Granite

Simscape picture for the Granite

Function description

  function [granite] = initializeGranite(args)

Optional Parameters

  arguments
    args.type          char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset       logical {mustBeNumericOrLogical} = false
    args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
    args.K  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4e9; 3e8; 8e8] % [N/m]
    args.C  (3,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5] % [N/(m/s)]
    args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
    args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
    args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
  end

Structure initialization

First, we initialize the granite structure.

  granite = struct();

Add Granite Type

  switch args.type
    case 'none'
      granite.type = 0;
    case 'rigid'
      granite.type = 1;
    case 'flexible'
      granite.type = 2;
    case 'modal-analysis'
      granite.type = 3;
    case 'init'
      granite.type = 4;
  end

Material and Geometry

Properties of the Material and link to the geometry of the granite.

  granite.density = args.density; % [kg/m3]
  granite.STEP    = './STEPS/granite/granite.STEP';

Z-offset for the initial position of the sample with respect to the granite top surface.

  granite.sample_pos = 0.8; % [m]

Stiffness and Damping properties

  granite.K = args.K; % [N/m]
  granite.C = args.C; % [N/(m/s)]

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fgm');
    granite.Deq = -Fgm'./granite.K;
  else
    granite.Deq = zeros(6,1);
  end

Save the Structure

The granite structure is saved.

  save('./mat/stages.mat', 'granite', '-append');

Translation Stage

<<sec:initializeTy>>

Simscape Model

The Simscape model of the Translation stage consist of:

• One rigid body for the fixed part of the translation stage
• One rigid body for the moving part of the translation stage
• Four 6-DOF Joints that only have some rigidity in the X and Z directions. The rigidity in rotation comes from the fact that we use multiple joints that are located at different points
• One 6-DOF joint that represent the Actuator. It is used to impose the motion in the Y direction
• One 6-DOF joint to inject force disturbance in the X and Z directions

Simscape model for the Translation Stage

Simscape picture for the Translation Stage

Function description

  function [ty] = initializeTy(args)

Optional Parameters

  arguments
    args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset logical {mustBeNumericOrLogical} = false
  end

Structure initialization

First, we initialize the ty structure.

  ty = struct();

Add Translation Stage Type

  switch args.type
    case 'none'
      ty.type = 0;
    case 'rigid'
      ty.type = 1;
    case 'flexible'
      ty.type = 2;
    case 'modal-analysis'
      ty.type = 3;
    case 'init'
      ty.type = 4;
  end

Material and Geometry

Define the density of the materials as well as the geometry (STEP files).

  % Ty Granite frame
  ty.granite_frame.density = 7800; % [kg/m3] => 43kg
  ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';

  % Guide Translation Ty
  ty.guide.density         = 7800; % [kg/m3] => 76kg
  ty.guide.STEP            = './STEPS/ty/Ty_Guide.STEP';

  % Ty - Guide_Translation12
  ty.guide12.density       = 7800; % [kg/m3]
  ty.guide12.STEP          = './STEPS/Ty/Ty_Guide_12.STEP';

  % Ty - Guide_Translation11
  ty.guide11.density       = 7800; % [kg/m3]
  ty.guide11.STEP          = './STEPS/ty/Ty_Guide_11.STEP';

  % Ty - Guide_Translation22
  ty.guide22.density       = 7800; % [kg/m3]
  ty.guide22.STEP          = './STEPS/ty/Ty_Guide_22.STEP';

  % Ty - Guide_Translation21
  ty.guide21.density       = 7800; % [kg/m3]
  ty.guide21.STEP          = './STEPS/Ty/Ty_Guide_21.STEP';

  % Ty - Plateau translation
  ty.frame.density         = 7800; % [kg/m3]
  ty.frame.STEP            = './STEPS/ty/Ty_Stage.STEP';

  % Ty Stator Part
  ty.stator.density        = 5400; % [kg/m3]
  ty.stator.STEP           = './STEPS/ty/Ty_Motor_Stator.STEP';

  % Ty Rotor Part
  ty.rotor.density         = 5400; % [kg/m3]
  ty.rotor.STEP            = './STEPS/ty/Ty_Motor_Rotor.STEP';

Stiffness and Damping properties

  ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
  ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 2e4]; % [N/(m/s), N*m/(rad/s)]

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Ftym');
    ty.Deq = -Ftym'./ty.K;
  else
    ty.Deq = zeros(6,1);
  end

Save the Structure

The ty structure is saved.

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

Tilt Stage

<<sec:initializeRy>>

Simscape Model

The Simscape model of the Tilt stage is composed of:

• Two solid bodies for the two part of the stage
• Four 6-DOF joints to model the flexibility of the stage. These joints are virtually located along the rotation axis and are connecting the two solid bodies. These joints have some translation stiffness in the u-v-w directions aligned with the joint. The stiffness in rotation between the two solids is due to the fact that the 4 joints are connecting the two solids are different locations
• A Bushing Joint used for the Actuator. The Ry motion is imposed by the input.

Simscape model for the Tilt Stage

Simscape picture for the Tilt Stage

Function description

  function [ry] = initializeRy(args)

Optional Parameters

  arguments
    args.type          char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset       logical {mustBeNumericOrLogical} = false
    args.Ry_init (1,1) double  {mustBeNumeric} = 0
  end

Structure initialization

First, we initialize the ry structure.

  ry = struct();

Add Tilt Type

  switch args.type
    case 'none'
      ry.type = 0;
    case 'rigid'
      ry.type = 1;
    case 'flexible'
      ry.type = 2;
    case 'modal-analysis'
      ry.type = 3;
    case 'init'
      ry.type = 4;
  end

Material and Geometry

Properties of the Material and link to the geometry of the Tilt stage.

  % Ry - Guide for the tilt stage
  ry.guide.density = 7800; % [kg/m3]
  ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';

  % Ry - Rotor of the motor
  ry.rotor.density = 2400; % [kg/m3]
  ry.rotor.STEP    = './STEPS/ry/Tilt_Motor_Axis.STEP';

  % Ry - Motor
  ry.motor.density = 3200; % [kg/m3]
  ry.motor.STEP    = './STEPS/ry/Tilt_Motor.STEP';

  % Ry - Plateau Tilt
  ry.stage.density = 7800; % [kg/m3]
  ry.stage.STEP    = './STEPS/ry/Tilt_Stage.STEP';

Z-Offset so that the center of rotation matches the sample center;

  ry.z_offset = 0.58178; % [m]

  ry.Ry_init = args.Ry_init; % [rad]

Stiffness and Damping properties

  ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
  ry.C = [1e5;   1e5; 1e5;   3e4;   1e3; 3e4];

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fym');
    ry.Deq = -Fym'./ry.K;
  else
    ry.Deq = zeros(6,1);
  end

Save the Structure

The ry structure is saved.

  save('./mat/stages.mat', 'ry', '-append');

Spindle

<<sec:initializeRz>>

Simscape Model

The Simscape model of the Spindle is composed of:

• Two rigid bodies: the stator and the rotor
• A Bushing Joint that is used both as the actuator (the Rz motion is imposed by the input) and as the force perturbation in the Z direction.
• The Bushing joint has some flexibility in the X-Y-Z directions as well as in Rx and Ry rotations

Simscape model for the Spindle

Simscape picture for the Spindle

Function description

  function [rz] = initializeRz(args)

Optional Parameters

  arguments
    args.type    char    {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init'})} = 'flexible'
    args.Foffset logical {mustBeNumericOrLogical} = false
  end

Structure initialization

First, we initialize the rz structure.

  rz = struct();

Add Spindle Type

  switch args.type
    case 'none'
      rz.type = 0;
    case 'rigid'
      rz.type = 1;
    case 'flexible'
      rz.type = 2;
    case 'modal-analysis'
      rz.type = 3;
    case 'init'
      rz.type = 4;
  end

Material and Geometry

Properties of the Material and link to the geometry of the spindle.

  % Spindle - Slip Ring
  rz.slipring.density = 7800; % [kg/m3]
  rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';

  % Spindle - Rotor
  rz.rotor.density    = 7800; % [kg/m3]
  rz.rotor.STEP       = './STEPS/rz/Spindle_Rotor.STEP';

  % Spindle - Stator
  rz.stator.density   = 7800; % [kg/m3]
  rz.stator.STEP      = './STEPS/rz/Spindle_Stator.STEP';

Stiffness and Damping properties

  rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
  rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fzm');
    rz.Deq = -Fzm'./rz.K;
  else
    rz.Deq = zeros(6,1);
  end

Save the Structure

The rz structure is saved.

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

Micro Hexapod

<<sec:initializeMicroHexapod>>

Simscape Model

Simscape model for the Micro-Hexapod

Simscape picture for the Micro-Hexapod

Function description

  function [micro_hexapod] = initializeMicroHexapod(args)

Optional Parameters

  arguments
      args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'modal-analysis', 'init', 'compliance'})} = 'flexible'
      % initializeFramesPositions
      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
      args.MO_B (1,1) double {mustBeNumeric} = 270e-3
      % generateGeneralConfiguration
      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
      args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
      args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
      % initializeStrutDynamics
      args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
      args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
      % initializeCylindricalPlatforms
      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
      % initializeCylindricalStruts
      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
      % inverseKinematics
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
      % Force that stiffness of each joint should apply at t=0
      args.Foffset      logical {mustBeNumericOrLogical} = false
  end

Function content

  micro_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
  micro_hexapod = generateGeneralConfiguration(micro_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
  micro_hexapod = computeJointsPose(micro_hexapod);
  micro_hexapod = initializeStrutDynamics(micro_hexapod, 'Ki', args.Ki, 'Ci', args.Ci);
  micro_hexapod = initializeCylindricalPlatforms(micro_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
  micro_hexapod = initializeCylindricalStruts(micro_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
  micro_hexapod = computeJacobian(micro_hexapod);
  [Li, dLi] = inverseKinematics(micro_hexapod, 'AP', args.AP, 'ARB', args.ARB);
  micro_hexapod.Li = Li;
  micro_hexapod.dLi = dLi;

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fhm');
    micro_hexapod.dLeq = -Fhm'./args.Ki;
  else
    micro_hexapod.dLeq = zeros(6,1);
  end

Add Type

  switch args.type
    case 'none'
      micro_hexapod.type = 0;
    case 'rigid'
      micro_hexapod.type = 1;
    case 'flexible'
      micro_hexapod.type = 2;
    case 'modal-analysis'
      micro_hexapod.type = 3;
    case 'init'
      micro_hexapod.type = 4;
    case 'compliance'
      micro_hexapod.type = 5;
  end

Save the Structure

The micro_hexapod structure is saved.

  save('./mat/stages.mat', 'micro_hexapod', '-append');

Center of gravity compensation

<<sec:initializeAxisc>>

Simscape Model

The Simscape model of the Center of gravity compensator is composed of:

• One main solid that is connected to two other solids (the masses to position of center of mass) through two revolute joints
• The angle of both revolute joints is set by the input

Simscape model for the Center of Mass compensation system

Simscape picture for the Center of Mass compensation system

Function description

  function [axisc] = initializeAxisc(args)

Optional Parameters

  arguments
    args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
  end

Structure initialization

First, we initialize the axisc structure.

  axisc = struct();

Add Type

  switch args.type
    case 'none'
      axisc.type = 0;
    case 'rigid'
      axisc.type = 1;
    case 'flexible'
      axisc.type = 2;
  end

Material and Geometry

Properties of the Material and link to the geometry files.

  % Structure
  axisc.structure.density = 3400; % [kg/m3]
  axisc.structure.STEP    = './STEPS/axisc/axisc_structure.STEP';

  % Wheel
  axisc.wheel.density     = 2700; % [kg/m3]
  axisc.wheel.STEP        = './STEPS/axisc/axisc_wheel.STEP';

  % Mass
  axisc.mass.density      = 7800; % [kg/m3]
  axisc.mass.STEP         = './STEPS/axisc/axisc_mass.STEP';

  % Gear
  axisc.gear.density      = 7800; % [kg/m3]
  axisc.gear.STEP         = './STEPS/axisc/axisc_gear.STEP';

Save the Structure

The axisc structure is saved.

  save('./mat/stages.mat', 'axisc', '-append');

Mirror

<<sec:initializeMirror>>

Simscape Model

The Simscape Model of the mirror is just a solid body. The output mirror_center corresponds to the center of the Sphere and is the point of measurement for the metrology

Simscape model for the Mirror

Simscape picture for the Mirror

Function description

  function [] = initializeMirror(args)

Optional Parameters

  arguments
      args.type        char   {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'rigid'
      args.shape       char   {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
      args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
      args.mass  (1,1) double {mustBeNumeric, mustBePositive} = 10 % [kg]
      args.freq  (6,1) double {mustBeNumeric, mustBeNonnegative} = 200*ones(6,1) % [Hz]
  end

Structure initialization

First, we initialize the mirror structure.

  mirror = struct();

Add Mirror Type

  switch args.type
    case 'none'
      mirror.type = 0;
    case 'rigid'
      mirror.type = 1;
    case 'flexible'
      mirror.type = 2;
  end

Mass and Inertia

  mirror.mass = args.mass;
  mirror.freq = args.freq;

Stiffness and Damping properties

  mirror.K = zeros(6,1);
  mirror.K(1:3) = mirror.mass * (2*pi*mirror.freq(1:3)).^2;

  mirror.C = zeros(6,1);
  mirror.C(1:3) = 0.2 * sqrt(mirror.K(1:3).*mirror.mass);

Equilibrium position of the each joint.

  mirror.Deq = zeros(6,1);

Geometry

We define the geometrical values.

  mirror.h = 0.05; % Height of the mirror [m]

  mirror.thickness = 0.025; % Thickness of the plate supporting the sample [m]

  mirror.hole_rad = 0.125; % radius of the hole in the mirror [m]

  mirror.support_rad = 0.1; % radius of the support plate [m]

  % point of interest offset in z (above the top surfave) [m]
  switch args.type
    case 'none'
      mirror.jacobian = 0.20;
    case 'rigid'
      mirror.jacobian = 0.20 - mirror.h;
    case 'flexible'
      mirror.jacobian = 0.20 - mirror.h;
  end

  mirror.rad = 0.180; % radius of the mirror (at the bottom surface) [m]

  mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point

Now we define the Shape of the mirror. We first start with the internal part.

  mirror.shape = [...
      mirror.support_rad+5e-3 mirror.h-mirror.thickness
      mirror.hole_rad mirror.h-mirror.thickness; ...
      mirror.hole_rad 0; ...
      mirror.rad 0 ...
  ];

Then, we define the reflective used part of the mirror.

  if strcmp(args.shape, 'spherical')
      mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]

      for z = linspace(0, mirror.h, 101)
          mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
      end
  elseif strcmp(args.shape, 'conical')
      mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(args.angle) mirror.h];
  else
      error('Shape should be either conical or spherical');
  end

Finally, we close the shape.

  mirror.shape = [mirror.shape; mirror.support_rad+5e-3 mirror.h];

Save the Structure

The mirror structure is saved.

  save('./mat/stages.mat', 'mirror', '-append');

Nano Hexapod

<<sec:initializeNanoHexapod>>

Simscape Model

Simscape model for the Nano Hexapod

Simscape picture for the Nano Hexapod

Function description

  function [nano_hexapod] = initializeNanoHexapod(args)

Optional Parameters

  arguments
      args.type      char   {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'init'})} = 'flexible'
      % initializeFramesPositions
      args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
      args.MO_B (1,1) double {mustBeNumeric} = 175e-3
      % generateGeneralConfiguration
      args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
      args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
      args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
      args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
      args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
      args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
      % initializeStrutDynamics
      args.actuator  char   {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
      args.k   (1,1) double {mustBeNumeric} = -1
      args.c   (1,1) double {mustBeNumeric} = -1
      % initializeCylindricalPlatforms
      args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
      args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
      args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
      args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
      args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 120e-3
      % initializeCylindricalStruts
      args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
      args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
      args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
      args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
      args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
      args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
      % inverseKinematics
      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
      args.ARB (3,3) double {mustBeNumeric} = eye(3)
      % Equilibrium position of each leg
      args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
      % Force that stiffness of each joint should apply at t=0
      args.Foffset      logical {mustBeNumericOrLogical} = false
  end

Function content

  nano_hexapod = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
  nano_hexapod = generateGeneralConfiguration(nano_hexapod, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
  nano_hexapod = computeJointsPose(nano_hexapod);

  if args.k > 0 && args.c > 0
      nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', args.k*ones(6,1), 'Ci', args.c*ones(6,1));
  elseif args.k > 0
      nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', args.k*ones(6,1), 'Ci', 1.5*sqrt(args.k)*ones(6,1));
  elseif strcmp(args.actuator, 'piezo')
      nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e7*ones(6,1), 'Ci', 1e2*ones(6,1));
  elseif strcmp(args.actuator, 'lorentz')
      nano_hexapod = initializeStrutDynamics(nano_hexapod, 'Ki', 1e4*ones(6,1), 'Ci', 1e2*ones(6,1));
  else
      error('args.actuator should be piezo or lorentz');
  end

  nano_hexapod = initializeCylindricalPlatforms(nano_hexapod, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
  nano_hexapod = initializeCylindricalStruts(nano_hexapod, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
  nano_hexapod = computeJacobian(nano_hexapod);
  [Li, dLi] = inverseKinematics(nano_hexapod, 'AP', args.AP, 'ARB', args.ARB);
  nano_hexapod.Li = Li;
  nano_hexapod.dLi = dLi;

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fnm');
    nano_hexapod.dLeq = -Fnm'./nano_hexapod.Ki;
  else
    nano_hexapod.dLeq = args.dLeq;
  end

Add Type

  switch args.type
    case 'none'
      nano_hexapod.type = 0;
    case 'rigid'
      nano_hexapod.type = 1;
    case 'flexible'
      nano_hexapod.type = 2;
    case 'init'
      nano_hexapod.type = 4;
  end

Save the Structure

  save('./mat/stages.mat', 'nano_hexapod', '-append');

Sample

<<sec:initializeSample>>

Simscape Model

The Simscape model of the sample environment is composed of:

• A rigid transform that can be used to translate the sample (position offset)
• A cartesian joint to add some flexibility to the sample environment mount
• A solid that represent the sample
• An input is added to apply some external forces and torques at the center of the sample environment. This could be the case for cable forces for instance.

Simscape model for the Sample

Simscape picture for the Sample

Function description

  function [sample] = initializeSample(args)

Optional Parameters

  arguments
    args.type         char    {mustBeMember(args.type,{'rigid', 'flexible', 'none', 'init'})} = 'flexible'
    args.radius (1,1) double  {mustBeNumeric, mustBePositive} = 0.1 % [m]
    args.height (1,1) double  {mustBeNumeric, mustBePositive} = 0.3 % [m]
    args.mass   (1,1) double  {mustBeNumeric, mustBePositive} = 50 % [kg]
    args.freq   (6,1) double  {mustBeNumeric, mustBePositive} = 100*ones(6,1) % [Hz]
    args.offset (1,1) double  {mustBeNumeric} = 0 % [m]
    args.Foffset      logical {mustBeNumericOrLogical} = false
  end

Structure initialization

First, we initialize the sample structure.

  sample = struct();

Add Sample Type

  switch args.type
    case 'none'
      sample.type = 0;
    case 'rigid'
      sample.type = 1;
    case 'flexible'
      sample.type = 2;
    case 'init'
      sample.type = 3;
  end

Material and Geometry

We define the geometrical parameters of the sample as well as its mass and position.

  sample.radius = args.radius; % [m]
  sample.height = args.height; % [m]
  sample.mass   = args.mass; % [kg]
  sample.offset = args.offset; % [m]

Compute the Inertia

  sample.inertia = [1/12 * sample.mass * (3*sample.radius^2 + sample.height^2); ...
                    1/12 * sample.mass * (3*sample.radius^2 + sample.height^2); ...
                    1/2  * sample.mass * sample.radius^2];

Stiffness and Damping properties

  sample.K = zeros(6, 1);
  sample.C = zeros(6, 1);

Translation Stiffness and Damping:

  sample.K(1:3) = sample.mass .* (2*pi * args.freq(1:3)).^2; % [N/m]
  sample.C(1:3) = 0.1 * sqrt(sample.K(1:3)*sample.mass); % [N/(m/s)]

Rotational Stiffness and Damping:

  sample.K(4:6) = sample.inertia .* (2*pi * args.freq(4:6)).^2; % [N/m]
  sample.C(4:6) = 0.1 * sqrt(sample.K(4:6).*sample.inertia); % [N/(m/s)]

Equilibrium position of the each joint.

  if args.Foffset && ~strcmp(args.type, 'none') && ~strcmp(args.type, 'rigid') && ~strcmp(args.type, 'init')
    load('mat/Foffset.mat', 'Fsm');
    sample.Deq = -Fsm'./sample.K;
  else
    sample.Deq = zeros(6,1);
  end

Save the Structure

The sample structure is saved.

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

Initialize Controller

<<sec:initializeController>>

Function Declaration and Documentation

  function [] = initializeController(args)

Optional Parameters

  arguments
    args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
  end

Structure initialization

First, we initialize the controller structure.

  controller = struct();

Controller Type

  switch args.type
    case 'open-loop'
      controller.type = 1;
      controller.name = 'Open-Loop';
    case 'dvf'
      controller.type = 2;
      controller.name = 'Decentralized Direct Velocity Feedback';
    case 'iff'
      controller.type = 3;
      controller.name = 'Decentralized Integral Force Feedback';
    case 'hac-dvf'
      controller.type = 4;
      controller.name = 'HAC-DVF';
    case 'ref-track-L'
      controller.type = 5;
      controller.name = 'Reference Tracking in the frame of the legs';
    case 'ref-track-iff-L'
      controller.type = 6;
      controller.name = 'Reference Tracking in the frame of the legs + IFF';
    case 'cascade-hac-lac'
      controller.type = 7;
      controller.name = 'Cascade Control + HAC-LAC';
    case 'hac-iff'
      controller.type = 8;
      controller.name = 'HAC-IFF';
    case 'stabilizing'
      controller.type = 9;
      controller.name = 'Stabilizing Controller';
  end

Save the Structure

The controller structure is saved.

  save('./mat/controller.mat', 'controller');

Generate Reference Signals

<<sec:initializeReferences>>

Function Declaration and Documentation

  function [ref] = initializeReferences(args)

Optional Parameters

  arguments
      % Sampling Frequency [s]
      args.Ts           (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
      % Maximum simulation time [s]
      args.Tmax         (1,1) double {mustBeNumeric, mustBePositive} = 100
      % Either "constant" / "triangular" / "sinusoidal"
      args.Dy_type      char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
      % Amplitude of the displacement [m]
      args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
      % Period of the displacement [s]
      args.Dy_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
      % Either "constant" / "triangular" / "sinusoidal"
      args.Ry_type      char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
      % Amplitude [rad]
      args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
      % Period of the displacement [s]
      args.Ry_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
      % Either "constant" / "rotating"
      args.Rz_type      char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
      % Initial angle [rad]
      args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
      % Period of the rotating [s]
      args.Rz_period    (1,1) double {mustBeNumeric, mustBePositive} = 1
      % For now, only constant is implemented
      args.Dh_type      char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
      % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
      args.Dh_pos       (6,1) double {mustBeNumeric} = zeros(6, 1), ...
      % For now, only constant is implemented
      args.Rm_type      char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
      % Initial position of the two masses
      args.Rm_pos       (2,1) double {mustBeNumeric} = [0; pi]
      % For now, only constant is implemented
      args.Dn_type      char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
      % Initial position [m,m,m,rad,rad,rad] of the top platform
      args.Dn_pos       (6,1) double {mustBeNumeric} = zeros(6,1)
  end

Initialize Parameters

  %% Set Sampling Time
  Ts = args.Ts;
  Tmax = args.Tmax;

  %% Low Pass Filter to filter out the references
  s = zpk('s');
  w0 = 2*pi*10;
  xi = 1;
  H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);

Translation Stage

  %% Translation stage - Dy
  t = 0:Ts:Tmax; % Time Vector [s]
  Dy   = zeros(length(t), 1);
  Dyd  = zeros(length(t), 1);
  Dydd = zeros(length(t), 1);
  switch args.Dy_type
    case 'constant'
      Dy(:)   = args.Dy_amplitude;
      Dyd(:)  = 0;
      Dydd(:) = 0;
    case 'triangular'
      % This is done to unsure that we start with no displacement
      Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
      i0 = find(t>=args.Dy_period/4,1);
      Dy(1:end-i0+1) = Dy_raw(i0:end);
      Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value

      % The signal is filtered out
      Dy   = lsim(H_lpf,     Dy, t);
      Dyd  = lsim(H_lpf*s,   Dy, t);
      Dydd = lsim(H_lpf*s^2, Dy, t);
    case 'sinusoidal'
      Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
      Dyd   = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
      Dydd  = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
    otherwise
      warning('Dy_type is not set correctly');
  end

  Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);

Tilt Stage

  %% Tilt Stage - Ry
  t = 0:Ts:Tmax; % Time Vector [s]
  Ry   = zeros(length(t), 1);
  Ryd  = zeros(length(t), 1);
  Rydd = zeros(length(t), 1);

  switch args.Ry_type
    case 'constant'
      Ry(:) = args.Ry_amplitude;
      Ryd(:)   = 0;
      Rydd(:)  = 0;
    case 'triangular'
      Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
      i0 = find(t>=args.Ry_period/4,1);
      Ry(1:end-i0+1) = Ry_raw(i0:end);
      Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value

      % The signal is filtered out
      Ry   = lsim(H_lpf,     Ry, t);
      Ryd  = lsim(H_lpf*s,   Ry, t);
      Rydd = lsim(H_lpf*s^2, Ry, t);
    case 'sinusoidal'
      Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);

      Ryd  = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
      Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
    otherwise
      warning('Ry_type is not set correctly');
  end

  Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);

Spindle

  %% Spindle - Rz
  t = 0:Ts:Tmax; % Time Vector [s]
  Rz   = zeros(length(t), 1);
  Rzd  = zeros(length(t), 1);
  Rzdd = zeros(length(t), 1);

  switch args.Rz_type
    case 'constant'
      Rz(:)   = args.Rz_amplitude;
      Rzd(:)  = 0;
      Rzdd(:) = 0;
    case 'rotating-not-filtered'
      Rz(:) = 2*pi/args.Rz_period*t;

      % The signal is filtered out
      Rz(:)   = 2*pi/args.Rz_period*t;
      Rzd(:)  = 2*pi/args.Rz_period;
      Rzdd(:) = 0;

      % We add the angle offset
      Rz = Rz + args.Rz_amplitude;

    case 'rotating'
      Rz(:) = 2*pi/args.Rz_period*t;

      % The signal is filtered out
      Rz   = lsim(H_lpf,     Rz, t);
      Rzd  = lsim(H_lpf*s,   Rz, t);
      Rzdd = lsim(H_lpf*s^2, Rz, t);

      % We add the angle offset
      Rz = Rz + args.Rz_amplitude;
    otherwise
      warning('Rz_type is not set correctly');
  end

  Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);

Micro Hexapod

  %% Micro-Hexapod
  t = [0, Ts];
  Dh = zeros(length(t), 6);
  Dhl = zeros(length(t), 6);

  switch args.Dh_type
    case 'constant'
      Dh = [args.Dh_pos, args.Dh_pos];

      load('mat/stages.mat', 'micro_hexapod');

      AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];

      tx = args.Dh_pos(4);
      ty = args.Dh_pos(5);
      tz = args.Dh_pos(6);

      ARB = [cos(tz) -sin(tz) 0;
             sin(tz)  cos(tz) 0;
             0        0       1]*...
            [ cos(ty) 0 sin(ty);
              0       1 0;
             -sin(ty) 0 cos(ty)]*...
            [1 0        0;
             0 cos(tx) -sin(tx);
             0 sin(tx)  cos(tx)];

      [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', 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

  %% Axis Compensation - Rm
  t = [0, Ts];

  Rm = [args.Rm_pos, args.Rm_pos];
  Rm = struct('time', t, 'signals', struct('values', Rm));

Nano Hexapod

  %% Nano-Hexapod
  t = [0, Ts];
  Dn = zeros(length(t), 6);

  switch args.Dn_type
    case 'constant'
      Dn = [args.Dn_pos, args.Dn_pos];

      load('mat/stages.mat', 'nano_hexapod');

      AP = [args.Dn_pos(1) ; args.Dn_pos(2) ; args.Dn_pos(3)];

      tx = args.Dn_pos(4);
      ty = args.Dn_pos(5);
      tz = args.Dn_pos(6);

      ARB = [cos(tz) -sin(tz) 0;
             sin(tz)  cos(tz) 0;
             0        0       1]*...
            [ cos(ty) 0 sin(ty);
              0       1 0;
             -sin(ty) 0 cos(ty)]*...
            [1 0        0;
             0 cos(tx) -sin(tx);
             0 sin(tx)  cos(tx)];

      [~, Dnl] = inverseKinematics(nano_hexapod, 'AP', AP, 'ARB', ARB);
      Dnl = [Dnl, Dnl];
    otherwise
      warning('Dn_type is not set correctly');
  end

  Dn = struct('time', t, 'signals', struct('values', Dn));
  Dnl = struct('time', t, 'signals', struct('values', Dnl));

Save

      %% Save
      save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Dnl', 'args', 'Ts');
  end

Initialize Disturbances

<<sec:initializeDisturbances>>

Function Declaration and Documentation

  function [] = initializeDisturbances(args)
  % initializeDisturbances - Initialize the disturbances
  %
  % Syntax: [] = initializeDisturbances(args)
  %
  % Inputs:
  %    - args -

Optional Parameters

  arguments
      % Global parameter to enable or disable the disturbances
      args.enable logical {mustBeNumericOrLogical} = true
      % Ground Motion - X direction
      args.Dwx logical {mustBeNumericOrLogical} = true
      % Ground Motion - Y direction
      args.Dwy logical {mustBeNumericOrLogical} = true
      % Ground Motion - Z direction
      args.Dwz logical {mustBeNumericOrLogical} = true
      % Translation Stage - X direction
      args.Fty_x logical {mustBeNumericOrLogical} = true
      % Translation Stage - Z direction
      args.Fty_z logical {mustBeNumericOrLogical} = true
      % Spindle - Z direction
      args.Frz_z logical {mustBeNumericOrLogical} = true
  end

Load Data

  load('./mat/dist_psd.mat', 'dist_f');

We remove the first frequency point that usually is very large.

Parameters

We define some parameters that will be used in the algorithm.

  Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
  N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
  T0 = N/Fs;                 % Signal Duration [s]
  df = 1/T0;                 % Frequency resolution of the DFT [Hz]
                             % Also equal to (dist_f.f(2)-dist_f.f(1))
  t = linspace(0, T0, N+1)'; % Time Vector [s]
  Ts = 1/Fs;                 % Sampling Time [s]

Ground Motion

  phi = dist_f.psd_gm;
  C = zeros(N/2,1);
  for i = 1:N/2
    C(i) = sqrt(phi(i)*df);
  end

  if args.Dwx && args.enable
    rng(111);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
  else
    Dwx = zeros(length(t), 1);
  end

  if args.Dwy && args.enable
    rng(112);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
  else
    Dwy = zeros(length(t), 1);
  end

  if args.Dwy && args.enable
    rng(113);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
  else
    Dwz = zeros(length(t), 1);
  end

Translation Stage - X direction

  if args.Fty_x && args.enable
    phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
    C = zeros(N/2,1);
    for i = 1:N/2
      C(i) = sqrt(phi(i)*df);
    end
    rng(121);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
    Fty_x = u;
  else
    Fty_x = zeros(length(t), 1);
  end

Translation Stage - Z direction

  if args.Fty_z && args.enable
    phi = dist_f.psd_ty;
    C = zeros(N/2,1);
    for i = 1:N/2
      C(i) = sqrt(phi(i)*df);
    end
    rng(122);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
    Fty_z = u;
  else
    Fty_z = zeros(length(t), 1);
  end

Spindle - Z direction

  if args.Frz_z && args.enable
    phi = dist_f.psd_rz;
    C = zeros(N/2,1);
    for i = 1:N/2
      C(i) = sqrt(phi(i)*df);
    end
    rng(131);
    theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
    Cx = [0 ; C.*complex(cos(theta),sin(theta))];
    Cx = [Cx; flipud(conj(Cx(2:end)))];;
    u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
    Frz_z = u;
  else
    Frz_z = zeros(length(t), 1);
  end

Direct Forces

  u = zeros(length(t), 6);
  Fd = u;

Set initial value to zero

  Dwx    = Dwx   - Dwx(1);
  Dwy    = Dwy   - Dwy(1);
  Dwz    = Dwz   - Dwz(1);
  Fty_x  = Fty_x - Fty_x(1);
  Fty_z  = Fty_z - Fty_z(1);
  Frz_z  = Frz_z - Frz_z(1);

Save

  save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't', 'args');

Initialize Position Errors

<<sec:initializePosError>>

Function Declaration and Documentation

  function [] = initializePosError(args)
  % initializePosError - Initialize the position errors
  %
  % Syntax: [] = initializePosError(args)
  %
  % Inputs:
  %    - args -

Optional Parameters

  arguments
      args.error    logical {mustBeNumericOrLogical} = false
      args.Dy (1,1) double  {mustBeNumeric} = 0 % [m]
      args.Ry (1,1) double  {mustBeNumeric} = 0 % [m]
      args.Rz (1,1) double  {mustBeNumeric} = 0 % [m]
  end

Structure initialization

First, we initialize the pos_error structure.

  pos_error = struct();

Type

  if args.error
    pos_error.type = 1;
  else
    pos_error.type = 0;
  end

Position Errors

  pos_error.Dy = args.Dy;
  pos_error.Ry = args.Ry;
  pos_error.Rz = args.Rz;

Save

  save('./mat/pos_error.mat', 'pos_error');

Z-Axis Geophone

<<sec:initializeZAxisGeophone>>

  function [geophone] = initializeZAxisGeophone(args)
      arguments
          args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
          args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1    % [Hz]
      end

      %%
      geophone.m = args.mass;

      %% The Stiffness is set to have the damping resonance frequency
      geophone.k = geophone.m * (2*pi*args.freq)^2;

      %% We set the damping value to have critical damping
      geophone.c = 2*sqrt(geophone.m * geophone.k);

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

Z-Axis Accelerometer

<<sec:initializeZAxisAccelerometer>>

  function [accelerometer] = initializeZAxisAccelerometer(args)
      arguments
          args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
          args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3  % [Hz]
      end

      %%
      accelerometer.m = args.mass;

      %% The Stiffness is set to have the damping resonance frequency
      accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;

      %% We set the damping value to have critical damping
      accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);

      %% Gain correction of the accelerometer to have a unity gain until the resonance
      accelerometer.gain = -accelerometer.k/accelerometer.m;

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