nass-simscape/simscape/index.org

Simscape Model

• Introduction
• Simulink Project - Startup and Shutdown scripts
• Simscape Multibody - Presentation
  • Solid bodies
  • Frames
  • Joints
  • Measurements
  • Excitation
• Simulink files and signal names
  • List of Simscape files
  • List of Inputs
    • Perturbations
    • Measurement Noise
    • Control Inputs
  • List of Outputs
• Simulink Library
  • inputs
  • nass_library
  • pos_error_wrt_nass_base
  • QuaternionToAngles
  • RotationMatrixToAngle
• Functions
  • computePsdDispl
  • computeSetpoint
  • converErrorBasis
  • generateDiagPidControl
  • identifyPlant
  • runSimulation
• Helping Functions
  • Experiment
  • Generate Reference Signals
  • Inputs
  • Inverse Kinematics of the Hexapod
• Initialize Elements
  • Ground
  • Granite
  • Translation Stage
  • Tilt Stage
  • Spindle
  • Micro Hexapod
  • Center of gravity compensation
  • Mirror
  • Nano Hexapod
  • Cedrat Actuator
  • Sample

Introduction   ignore

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

• In section sec:simulink_project, the simulink project with the associated scripts are presented
• In section sec:simscape_model, an introduction to Simscape Multibody is done
• In section sec:simulink_files_signal_names, each simscape files are presented with the associated signal names and joint architectures
• In section sec:simulink_library, the list of the Simulink library elements are described
• In section sec:functions, a list of Matlab function that will be used are defined here
• In section sec:initialize_elements, all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.

Simulink Project - Startup and Shutdown scripts

<<sec: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.

  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.

Simscape Multibody - Presentation

<<sec: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.

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.

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.

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.

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

Degrees of freedom associated with each joint

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

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

Joint primitives for each joint type

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.

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.

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.

Simulink files and signal names

<<sec:simulink_files_signal_names>>

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

List of Simscape files

Few different Simulink files are used:

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

Simscape Name	Ty	Ry	Rz	Hexa	NASS
id micro station	F	F	F	F
id nano station stages	F	F	F	F	F
id nano station config	D	D	D	D	F
control nano station	D	D	D	D	F

List of simscape files

List of Inputs

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]

List of Disturbances

Measurement Noise

Variable	Meaning	Size	Unit

List of Measurement Noise

Control Inputs

Variable	Meaning	Size	Unit
Fy	Actuation force for Ty	1	[N]
Dy	Imposed displacement for Ty	1	[m]
My	Actuation torque for Ry	1	[N.m]
Ry	Imposed rotation for Ry	1	[rad]
Mz	Actuation torque for Rz	1	[N.m]
Rz	Imposed rotation for Rz	1	[rad]
Fh	Actuation force/torque for hexapod (cart)	6	[N, N.m]
Fhl	Actuation force/torque for hexapod (legs)	6	[N]
Dh	Imposed position for hexapod (cart)	6	[m, rad]
Rm	Position of the two masses	2	[rad]
Fn	Actuation force for the NASS (cart)	6	[N, N.m]
Fnl	Actuation force for the NASS's legs	6	[N]
Dn	Imposed position for the NASS (cart)	6	[m, rad]

List of Control Inputs

List of Outputs

Variable	Meaning	Size	Unit
Dgm	Absolute displacement of the granite	3	[m]
Vgm	Absolute Velocity of the granite	3	[m/s]
Dym	Measured displacement of Ty	1	[m]
Rym	Measured rotation of Ry	1	[rad]
Rzm	Measured rotation of Rz	1	[rad]
Dhm	Measured position of hexapod (cart)	6	[m, rad]
Fnlm	Measured force of NASS's legs	6	[N]
Dnlm	Measured elongation of NASS's legs	6	[m]
Dnm	Measured position of NASS w.r.t NASS's base	6	[m, rad]
Vnm	Measured absolute velocity of NASS platform	6	[m/s, rad/s]
Vnlm	Measured absolute velocity of NASS's legs	6	[m/s]
Dsm	Position of Sample w.r.t. granite frame	6	[m, rad]

List of Outputs

Simulink Library

<<sec:simulink_library>>

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

inputs

inputs.slx

nass_library

nass_library.slx

pos_error_wrt_nass_base

pos_error_wrt_nass_base.slx

QuaternionToAngles

QuaternionToAngles.slx

RotationMatrixToAngle

RotationMatrixToAngle.slx

Functions

<<sec:functions>>

computePsdDispl

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

computeSetpoint

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

converErrorBasis

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

generateDiagPidControl

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

identifyPlant

<<sec:identifyPlant>>

This Matlab function is accessible here.

  function [sys] = identifyPlant(opts_param)
      %% Default values for opts
      opts = struct();

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

      %% Options for Linearized
      options = linearizeOptions;
      options.SampleTime = 0;

      %% Name of the Simulink File
      mdl = 'sim_nano_station_id';

      %% Input/Output definition
      io(1)  = linio([mdl, '/Fn'],   1, 'input');  % Cartesian forces applied by NASS
      io(2)  = linio([mdl, '/Dw'],   1, 'input');  % Ground Motion
      io(3)  = linio([mdl, '/Fs'],   1, 'input');  % External forces on the sample
      io(4)  = linio([mdl, '/Fnl'],  1, 'input');  % Forces applied on the NASS's legs
      io(5)  = linio([mdl, '/Fd'],   1, 'input');  % Disturbance Forces
      io(6)  = linio([mdl, '/Dsm'],  1, 'output'); % Displacement of the sample
      io(7)  = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
      io(8)  = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
      io(9)  = linio([mdl, '/Es'],   1, 'output'); % Position Error w.r.t. NASS base
      io(10) = linio([mdl, '/Vlm'],  1, 'output'); % Measured absolute velocity of the legs

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

      %% Create the sub transfer functions
      minreal_tol = sqrt(eps);
      % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
      sys.G_cart  = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
      % From ground motion to Sample displacement
      sys.G_gm    = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
      % From direct forces applied on the sample to displacement of the sample
      sys.G_fs    = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
      % From forces applied on NASS's legs to force sensor in each leg
      sys.G_iff   = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
      % From forces applied on NASS's legs to displacement of each leg
      sys.G_dleg  = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
      % From forces/torques applied by the NASS to position error
      sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
      % From forces/torques applied by the NASS to velocity of the actuator
      sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
      % From various disturbance forces to position error
      sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);

      %% We remove low frequency and high frequency dynamics that are usually unstable
      % using =freqsep= is risky as it may change the shape of the transfer functions
      % f_min = 0.1; % [Hz]
      % f_max = 1e4; % [Hz]

      % [~, sys.G_cart]  = freqsep(freqsep(sys.G_cart,  2*pi*f_max), 2*pi*f_min);
      % [~, sys.G_gm]    = freqsep(freqsep(sys.G_gm,    2*pi*f_max), 2*pi*f_min);
      % [~, sys.G_fs]    = freqsep(freqsep(sys.G_fs,    2*pi*f_max), 2*pi*f_min);
      % [~, sys.G_iff]   = freqsep(freqsep(sys.G_iff,   2*pi*f_max), 2*pi*f_min);
      % [~, sys.G_dleg]  = freqsep(freqsep(sys.G_dleg,  2*pi*f_max), 2*pi*f_min);
      % [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min);

      %% We finally verify that the system is stable
      if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant)
        warning('One of the identified system is unstable');
      end
  end

runSimulation

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

Helping Functions

Experiment

<<sec:initializeExperiment>>

This Matlab function is accessible here.

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

          initializeInputs(opts_inputs);
      else
          error('exp_name is only configured for tomography');
      end
  end

Generate Reference Signals

<<sec:initializeReferences>>

This Matlab function is accessible here.

  function [ref] = initializeReferences(opts_param)
      %% Default values for opts
      opts = struct(   ...
          'Ts',           1e-3, ...       % Sampling Frequency [s]
          'Dy_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
          'Dy_amplitude', 0, ...       % Amplitude of the displacement [m]
          'Dy_period',    1, ...          % Period of the displacement [s]
          'Ry_type',      'constant', ... % Either "constant" / "triangular" / "sinusoidal"
          'Ry_amplitude', 0, ...          % Amplitude [rad]
          'Ry_period',    10, ...         % Period of the displacement [s]
          'Rz_type',      'constant', ... % Either "constant" / "rotating"
          'Rz_amplitude', 0, ...          % Initial angle [rad]
          'Rz_period',    1, ...          % Period of the rotating [s]
          'Dh_type',      'constant', ... % For now, only constant is implemented
          'Dh_pos',       [0; 0; 0; 0; 0; 0], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
          'Rm_type',      'constant', ... % For now, only constant is implemented
          'Rm_pos',       [0; pi],  ...   % Initial position of the two masses
          'Dn_type',      'constant', ... % For now, only constant is implemented
          'Dn_pos',       [0; 0; 0; 0; 0; 0]  ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
      );

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

      %% Set Sampling Time
      Ts = opts.Ts;

      %% Translation stage - Dy
      t = 0:Ts:opts.Dy_period-Ts; % Time Vector [s]
      Dy = zeros(length(t), 1);
      switch opts.Dy_type
        case 'constant'
          Dy(:) = opts.Dy_amplitude;
        case 'triangular'
          Dy(:)                     = -4*opts.Dy_amplitude + 4*opts.Dy_amplitude/opts.Dy_period*t;
          Dy(t<0.75*opts.Dy_period) =  2*opts.Dy_amplitude - 4*opts.Dy_amplitude/opts.Dy_period*t(t<0.75*opts.Dy_period);
          Dy(t<0.25*opts.Dy_period) =  4*opts.Dy_amplitude/opts.Dy_period*t(t<0.25*opts.Dy_period);
        case 'sinusoidal'
          Dy(:) = opts.Dy_amplitude*sin(2*pi/opts.Dy_period*t);
        otherwise
          warning('Dy_type is not set correctly');
      end
      Dy = struct('time', t, 'signals', struct('values', Dy));


      %% Tilt Stage - Ry
      t = 0:Ts:opts.Ry_period-Ts;
      Ry = zeros(length(t), 1);

      switch opts.Ry_type
        case 'constant'
          Ry(:) = opts.Ry_amplitude;
        case 'triangular'
          Ry(:)                     = -4*opts.Ry_amplitude + 4*opts.Ry_amplitude/opts.Ry_period*t;
          Ry(t<0.75*opts.Ry_period) =  2*opts.Ry_amplitude - 4*opts.Ry_amplitude/opts.Ry_period*t(t<0.75*opts.Ry_period);
          Ry(t<0.25*opts.Ry_period) =  4*opts.Ry_amplitude/opts.Ry_period*t(t<0.25*opts.Ry_period);
        case 'sinusoidal'

        otherwise
          warning('Ry_type is not set correctly');
      end
      Ry = struct('time', t, 'signals', struct('values', Ry));

      %% Spindle - Rz
      t = 0:Ts:opts.Rz_period-Ts;
      Rz = zeros(length(t), 1);

      switch opts.Rz_type
        case 'constant'
          Rz(:) = opts.Rz_amplitude;
        case 'rotating'
          Rz(:) = opts.Rz_amplitude+2*pi/opts.Rz_period*t;
        otherwise
          warning('Rz_type is not set correctly');
      end
      Rz = struct('time', t, 'signals', struct('values', Rz));

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

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

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

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

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

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

          [Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
          Dhl = [Dhl, Dhl];
        otherwise
          warning('Dh_type is not set correctly');
      end
      Dh = struct('time', t, 'signals', struct('values', Dh));
      Dhl = struct('time', t, 'signals', struct('values', Dhl));

      %% Axis Compensation - Rm
      t = [0, Ts];
      Rm = [opts.Rm_pos, opts.Rm_pos];
      Rm = struct('time', t, 'signals', struct('values', Rm));

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

      switch opts.Dn_type
        case 'constant'
          Dn = [opts.Dn_pos, opts.Dn_pos];
        otherwise
          warning('Dn_type is not set correctly');
      end
      Dn = struct('time', t, 'signals', struct('values', Dn));

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

TODO Inputs

<<sec:initializeInputs>>

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

This Matlab function is accessible here.

  function [inputs] = initializeInputs(opts_param)
      %% Default values for opts
      opts = struct(   ...
          'Dw', false, ...
          'Dy', false, ...
          'Ry', false, ...
          'Rz', false, ...
          'Dh', false, ...
          'Rm', false, ...
          'Dn', false  ...
      );

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

      %% Load Sampling Time and Simulation Time
      load('./mat/sim_conf.mat', 'sim_conf');

      %% Define the time vector
      t = 0:sim_conf.Ts:sim_conf.Tsim;

      %% Ground motion - Dw
      if islogical(opts.Dw) && opts.Dw == true
          load('./mat/perturbations.mat', 'Wxg');
          Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3);
          Dw(:, 1) = lsim(Wxg, Dw(:, 1), t);
          Dw(:, 2) = lsim(Wxg, Dw(:, 2), t);
          Dw(:, 3) = lsim(Wxg, Dw(:, 3), t);
      elseif islogical(opts.Dw) && opts.Dw == false
          Dw = zeros(length(t), 3);
      else
          Dw = opts.Dw;
      end

      %% Translation stage - Dy
      if islogical(opts.Dy) && opts.Dy == true
          Dy = zeros(length(t), 1);
      elseif islogical(opts.Dy) && opts.Dy == false
          Dy = zeros(length(t), 1);
      else
          Dy = opts.Dy;
      end

      %% Tilt Stage - Ry
      if islogical(opts.Ry) && opts.Ry == true
          Ry = 3*(2*pi/360)*sin(2*pi*0.2*t);
      elseif islogical(opts.Ry) && opts.Ry == false
          Ry = zeros(length(t), 1);
      elseif isnumeric(opts.Ry) && length(opts.Ry) == 1
          Ry = opts.Ry*(2*pi/360)*ones(length(t), 1);
      else
          Ry = opts.Ry;
      end

      %% Spindle - Rz
      if islogical(opts.Rz) && opts.Rz == true
          Rz = 2*pi*0.5*t;
      elseif islogical(opts.Rz) && opts.Rz == false
          Rz = zeros(length(t), 1);
      elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
          Rz = opts.Rz*(2*pi/60)*t;
      else
          Rz = opts.Rz;
      end

      %% Micro Hexapod - Dh
      if islogical(opts.Dh) && opts.Dh == true
          Dh = zeros(length(t), 6);
      elseif islogical(opts.Dh) && opts.Dh == false
          Dh = zeros(length(t), 6);
      else
          Dh = opts.Dh;
      end

      %% Axis Compensation - Rm
      if islogical(opts.Rm)
          Rm = zeros(length(t), 2);
          Rm(:, 2) = pi*ones(length(t), 1);
      else
          Rm = opts.Rm;
      end

      %% Nano Hexapod - Dn
      if islogical(opts.Dn) && opts.Dn == true
          Dn = zeros(length(t), 6);
      elseif islogical(opts.Dn) && opts.Dn == false
          Dn = zeros(length(t), 6);
      else
          Dn = opts.Dn;
      end

      %% Setpoint - Ds
      Ds = zeros(length(t), 6);
      for i = 1:length(t)
          Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i));
      end

      %% External Forces applied on the Granite
      Fg = zeros(length(t), 3);

      %% External Forces applied on the Sample
      Fs = zeros(length(t), 6);

      %% Create the input Structure that will contain all the inputs
      inputs = struct( ...
          'Ts', sim_conf.Ts,       ...
          'Dw', timeseries(Dw, t), ...
          'Dy', timeseries(Dy, t), ...
          'Ry', timeseries(Ry, t), ...
          'Rz', timeseries(Rz, t), ...
          'Dh', timeseries(Dh, t), ...
          'Rm', timeseries(Rm, t), ...
          'Dn', timeseries(Dn, t), ...
          'Ds', timeseries(Ds, t), ...
          'Fg', timeseries(Fg, t), ...
          'Fs', timeseries(Fs, t)  ...
      );

      %% Save
      save('./mat/inputs.mat', 'inputs');

      %% Custom Functions
      function setpoint = computeSetpoint(ty, ry, rz)
      %%
      setpoint = zeros(6, 1);

      %% Ty
      TMTy = [1 0 0 0  ;
            0 1 0 ty ;
            0 0 1 0  ;
            0 0 0 1 ];

      %% Ry
      TMRy = [ cos(ry) 0 sin(ry) 0 ;
            0       1 0       0 ;
            -sin(ry) 0 cos(ry) 0 ;
            0        0 0       1 ];

      %% Rz
      TMRz = [cos(rz) -sin(rz) 0 0 ;
            sin(rz)  cos(rz) 0 0 ;
            0        0       1 0 ;
            0        0       0 1 ];

      %% All stages
      TM = TMTy*TMRy*TMRz;

      [thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));

      setpoint(1:3) = TM(1:3, 4);
      setpoint(4:6) = [thetax, thetay, thetaz];

      %% Custom Functions
      function [thetax, thetay, thetaz] = RM2angle(R)
          if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
              thetay = -asin(R(3, 1));
              thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
              thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
          else
              thetaz = 0;
              if abs(R(3, 1)+1) < 1e-6 % R31 = -1
                  thetay = pi/2;
                  thetax = thetaz + atan2(R(1, 2), R(1, 3));
              else
                  thetay = -pi/2;
                  thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
              end
          end
      end
      end
  end

Inverse Kinematics of the Hexapod

<<sec:inverseKinematicsHexapod>>

This Matlab function is accessible here.

  function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
  % inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
  %
  % Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
  %
  % Inputs:
  %    - hexapod - Hexapod object containing the geometry of the hexapod
  %    - AP - Position vector of point OB expressed in frame {A} in [m]
  %    - ARB - Rotation Matrix expressed in frame {A}

    % Wanted Length of the hexapod's legs [m]
    L = zeros(6, 1);

    for i = 1:length(L)
      Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
      Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]

      L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
    end
  end

Initialize Elements

<<sec:initialize_elements>>

Ground

<<sec:initializeGround>>

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

Granite

<<sec:initializeGranite>>

This Matlab function is accessible here.

  function [granite] = initializeGranite(opts_param)
      %% Default values for opts
      opts = struct('rigid', false);

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

      %%
      granite = struct();

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

      granite.mass_top = 4000; % [kg] TODO

      %% Dynamical Properties
      if opts.rigid
        granite.k.x = 1e12; % [N/m]
        granite.k.y = 1e12; % [N/m]
        granite.k.z = 1e12; % [N/m]
        granite.k.rx = 1e10; % [N*m/deg]
        granite.k.ry = 1e10; % [N*m/deg]
        granite.k.rz = 1e10; % [N*m/deg]
      else
        granite.k.x = 4e9; % [N/m]
        granite.k.y = 3e8; % [N/m]
        granite.k.z = 8e8; % [N/m]
        granite.k.rx = 1e4; % [N*m/deg]
        granite.k.ry = 1e4; % [N*m/deg]
        granite.k.rz = 1e6; % [N*m/deg]
      end

      granite.c.x = 0.1*sqrt(granite.mass_top*granite.k.x); % [N/(m/s)]
      granite.c.y = 0.1*sqrt(granite.mass_top*granite.k.y); % [N/(m/s)]
      granite.c.z = 0.5*sqrt(granite.mass_top*granite.k.z); % [N/(m/s)]
      granite.c.rx = 0.1*sqrt(granite.mass_top*granite.k.rx); % [N*m/(deg/s)]
      granite.c.ry = 0.1*sqrt(granite.mass_top*granite.k.ry); % [N*m/(deg/s)]
      granite.c.rz = 0.1*sqrt(granite.mass_top*granite.k.rz); % [N*m/(deg/s)]

      %% Positioning parameters
      granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m]

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

Translation Stage

<<sec:initializeTy>>

This Matlab function is accessible here.

  function [ty] = initializeTy(opts_param)
      %% Default values for opts
      opts = struct('rigid', false);

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

      %%
      ty = struct();

      %% Y-Translation - Static Properties
      % Ty Granite frame
      ty.granite_frame.density = 7800; % [kg/m3]
      ty.granite_frame.color   = [0.753 1 0.753];
      ty.granite_frame.STEP    = './STEPS/Ty/Ty_Granite_Frame.STEP';
      % Guide Translation Ty
      ty.guide.density         = 7800; % [kg/m3]
      ty.guide.color           = [0.792 0.820 0.933];
      ty.guide.STEP            = './STEPS/ty/Ty_Guide.STEP';
      % Ty - Guide_Translation12
      ty.guide12.density       = 7800; % [kg/m3]
      ty.guide12.color         = [0.792 0.820 0.933];
      ty.guide12.STEP          = './STEPS/Ty/Ty_Guide_12.STEP';
      % Ty - Guide_Translation11
      ty.guide11.density       = 7800; % [kg/m3]
      ty.guide11.color         = [0.792 0.820 0.933];
      ty.guide11.STEP          = './STEPS/ty/Ty_Guide_11.STEP';
      % Ty - Guide_Translation22
      ty.guide22.density       = 7800; % [kg/m3]
      ty.guide22.color         = [0.792 0.820 0.933];
      ty.guide22.STEP          = './STEPS/ty/Ty_Guide_22.STEP';
      % Ty - Guide_Translation21
      ty.guide21.density       = 7800; % [kg/m3]
      ty.guide21.color         = [0.792 0.820 0.933];
      ty.guide21.STEP          = './STEPS/Ty/Ty_Guide_21.STEP';
      % Ty - Plateau translation
      ty.frame.density         = 7800; % [kg/m3]
      ty.frame.color           = [0.792 0.820 0.933];
      ty.frame.STEP            = './STEPS/ty/Ty_Stage.STEP';
      % Ty Stator Part
      ty.stator.density        = 5400; % [kg/m3]
      ty.stator.color          = [0.792 0.820 0.933];
      ty.stator.STEP           = './STEPS/ty/Ty_Motor_Stator.STEP';
      % Ty Rotor Part
      ty.rotor.density         = 5400; % [kg/m3]
      ty.rotor.color           = [0.792 0.820 0.933];
      ty.rotor.STEP            = './STEPS/ty/Ty_Motor_Rotor.STEP';

      ty.m = 1000; % TODO [kg]

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

      ty.c.ax  = 0.1*sqrt(ty.k.ax*ty.m);
      ty.c.rad = 0.1*sqrt(ty.k.rad*ty.m);

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

Tilt Stage

<<sec:initializeRy>>

This Matlab function is accessible here.

  function [ry] = initializeRy(opts_param)
      %% Default values for opts
      opts = struct('rigid', false);

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

      %%
      ry = struct();

      %% Tilt Stage - Static Properties
      % Ry - Guide for the tilt stage
      ry.guide.density = 7800; % [kg/m3]
      ry.guide.color   = [0.792 0.820 0.933];
      ry.guide.STEP    = './STEPS/ry/Tilt_Guide.STEP';
      % Ry - Rotor of the motor
      ry.rotor.density = 2400; % [kg/m3]
      ry.rotor.color   = [0.792 0.820 0.933];
      ry.rotor.STEP    = './STEPS/ry/Tilt_Motor_Axis.STEP';
      % Ry - Motor
      ry.motor.density = 3200; % [kg/m3]
      ry.motor.color   = [0.792 0.820 0.933];
      ry.motor.STEP    = './STEPS/ry/Tilt_Motor.STEP';
      % Ry - Plateau Tilt
      ry.stage.density = 7800; % [kg/m3]
      ry.stage.color   = [0.792 0.820 0.933];
      ry.stage.STEP    = './STEPS/ry/Tilt_Stage.STEP';

      ry.m = 800; % TODO [kg]

      %% Tilt Stage - Dynamical Properties
      if opts.rigid
        ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
        ry.k.h    = 1e12; % Stiffness in the direction of the guidance [N/m]
        ry.k.rad  = 1e12; % Stiffness in the top direction [N/m]
        ry.k.rrad = 1e12; % Stiffness in the side direction [N/m]
      else
        ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
        ry.k.h    = 1e8; % Stiffness in the direction of the guidance [N/m]
        ry.k.rad  = 1e8; % Stiffness in the top direction [N/m]
        ry.k.rrad = 1e8; % Stiffness in the side direction [N/m]
      end

      ry.c.h    = 0.1*sqrt(ry.k.h*ry.m);
      ry.c.rad  = 0.1*sqrt(ry.k.rad*ry.m);
      ry.c.rrad = 0.1*sqrt(ry.k.rrad*ry.m);
      ry.c.tilt = 0.1*sqrt(ry.k.tilt*ry.m);

      %% Positioning parameters
      ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]

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

Spindle

<<sec:initializeRz>>

This Matlab function is accessible here.

  function [rz] = initializeRz(opts_param)
      %% Default values for opts
      opts = struct('rigid', false);

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

      %%
      rz = struct();

      %% Spindle - Static Properties
      % Spindle - Slip Ring
      rz.slipring.density = 7800; % [kg/m3]
      rz.slipring.color   = [0.792 0.820 0.933];
      rz.slipring.STEP    = './STEPS/rz/Spindle_Slip_Ring.STEP';
      % Spindle - Rotor
      rz.rotor.density    = 7800; % [kg/m3]
      rz.rotor.color      = [0.792 0.820 0.933];
      rz.rotor.STEP       = './STEPS/rz/Spindle_Rotor.STEP';
      % Spindle - Stator
      rz.stator.density   = 7800; % [kg/m3]
      rz.stator.color     = [0.792 0.820 0.933];
      rz.stator.STEP      = './STEPS/rz/Spindle_Stator.STEP';

      % Estimated mass of the mooving part
      rz.m = 250; % [kg]

      %% Spindle - Dynamical Properties

      if opts.rigid
        rz.k.rot  = 1e10; % Rotational Stiffness (Rz) [N*m/deg]
        rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg]
        rz.k.ax   = 1e12; % Axial Stiffness (Z) [N/m]
        rz.k.rad  = 1e12; % Radial Stiffness (X, Y) [N/m]
      else
        rz.k.rot  = 1e6; % TODO - Rotational Stiffness (Rz) [N*m/deg]
        rz.k.tilt = 1e6; % Rotational Stiffness (Rx, Ry) [N*m/deg]
        rz.k.ax   = 2e9; % Axial Stiffness (Z) [N/m]
        rz.k.rad  = 7e8; % Radial Stiffness (X, Y) [N/m]
      end

      % Damping
      rz.c.ax   = 0.1*sqrt(rz.k.ax*rz.m);
      rz.c.rad  = 0.1*sqrt(rz.k.rad*rz.m);
      rz.c.tilt = 0.1*sqrt(rz.k.tilt*rz.m);
      rz.c.rot  = 0.1*sqrt(rz.k.rot*rz.m);

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

Micro Hexapod

<<sec:initializeMicroHexapod>>

This Matlab function is accessible here.

  function [micro_hexapod] = initializeMicroHexapod(opts_param)
      %% Default values for opts
      opts = struct(...
        'rigid', false, ...
        'AP',    zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
        'ARB',   eye(3) ...       % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
      );

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

      %% Stewart Object
      micro_hexapod = struct();
      micro_hexapod.h        = 350; % Total height of the platform [mm]
      micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]

      %% Bottom Plate - Mechanical Design
      BP = struct();

      BP.rad.int   = 110;   % Internal Radius [mm]
      BP.rad.ext   = 207.5; % External Radius [mm]
      BP.thickness = 26;    % Thickness [mm]
      BP.leg.rad   = 175.5; % Radius where the legs articulations are positionned [mm]
      BP.leg.ang   = 9.5;   % Angle Offset [deg]
      BP.density   = 8000;  % Density of the material [kg/m^3]
      BP.color     = [0.6 0.6 0.6]; % Color [rgb]
      BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];

      %% Top Plate - Mechanical Design
      TP = struct();

      TP.rad.int   = 82;   % Internal Radius [mm]
      TP.rad.ext   = 150;  % Internal Radius [mm]
      TP.thickness = 26;   % Thickness [mm]
      TP.leg.rad   = 118;  % Radius where the legs articulations are positionned [mm]
      TP.leg.ang   = 12.1; % Angle Offset [deg]
      TP.density   = 8000; % Density of the material [kg/m^3]
      TP.color     = [0.6 0.6 0.6]; % Color [rgb]
      TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];

      %% Struts
      Leg = struct();

      Leg.stroke     = 10e-3; % Maximum Stroke of each leg [m]
      if opts.rigid
        Leg.k.ax     = 1e12; % Stiffness of each leg [N/m]
      else
        Leg.k.ax     = 2e7; % Stiffness of each leg [N/m]
      end
      Leg.ksi.ax     = 0.1;   % Modal damping ksi = 1/2*c/sqrt(km) []
      Leg.rad.bottom = 25;    % Radius of the cylinder of the bottom part [mm]
      Leg.rad.top    = 17;    % Radius of the cylinder of the top part [mm]
      Leg.density    = 8000;  % Density of the material [kg/m^3]
      Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
      Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]

      Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
      Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
      Leg.m             = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
      Leg = updateDamping(Leg);


      %% Sphere
      SP = struct();

      SP.height.bottom  = 27; % [mm]
      SP.height.top     = 27; % [mm]
      SP.density.bottom = 8000; % [kg/m^3]
      SP.density.top    = 8000; % [kg/m^3]
      SP.color.bottom   = [0.6 0.6 0.6]; % [rgb]
      SP.color.top      = [0.6 0.6 0.6]; % [rgb]
      SP.k.ax           = 0; % [N*m/deg]
      SP.ksi.ax         = 10;

      SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
      SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
      SP.rad.bottom       = Leg.sphere.bottom; % [mm]
      SP.rad.top          = Leg.sphere.top; % [mm]
      SP.m                = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]

      SP = updateDamping(SP);

      %%
      Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
      Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];

      %%
      micro_hexapod.BP = BP;
      micro_hexapod.TP = TP;
      micro_hexapod.Leg = Leg;
      micro_hexapod.SP = SP;

      %%
      micro_hexapod = initializeParameters(micro_hexapod);

      %% Setup equilibrium position of each leg
      micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB);

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

      %%
      function [element] = updateDamping(element)
        field = fieldnames(element.k);
        for i = 1:length(field)
          element.c.(field{i}) = 2*element.ksi.(field{i})*sqrt(element.k.(field{i})*element.m);
        end
      end

      %%
      function [stewart] = initializeParameters(stewart)
          %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
          stewart.pos_base = zeros(6, 3);
          stewart.pos_top = zeros(6, 3);

          alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
          alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top

          height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO

          radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
          radius_t = stewart.TP.leg.rad*0.001; % top radius in meters

          for i = 1:3
            % base points
            angle_m_b = (2*pi/3)* (i-1) - alpha_b;
            angle_p_b = (2*pi/3)* (i-1) + alpha_b;
            stewart.pos_base(2*i-1,:) =  [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
            stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];

            % top points
            % Top points are 60 degrees offset
            angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
            angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
            stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
            stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
          end

          % permute pos_top points so that legs are end points of base and top points
          stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
          stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];

          %% leg vectors
          legs = stewart.pos_top - stewart.pos_base;
          leg_length = zeros(6, 1);
          leg_vectors = zeros(6, 3);
          for i = 1:6
            leg_length(i) = norm(legs(i,:));
            leg_vectors(i,:)  = legs(i,:) / leg_length(i);
          end

          stewart.Leg.lenght = 1000*leg_length(1)/1.5;
          stewart.Leg.shape.bot = [0 0; ...
                                  stewart.Leg.rad.bottom 0; ...
                                  stewart.Leg.rad.bottom stewart.Leg.lenght; ...
                                  stewart.Leg.rad.top stewart.Leg.lenght; ...
                                  stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
                                  0 0.2*stewart.Leg.lenght];

          %% Calculate revolute and cylindrical axes
          rev1 = zeros(6, 3);
          rev2 = zeros(6, 3);
          cyl1 = zeros(6, 3);
          for i = 1:6
            rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
            rev1(i,:) = rev1(i,:) / norm(rev1(i,:));

            rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
            rev2(i,:) = rev2(i,:) / norm(rev2(i,:));

            cyl1(i,:) = leg_vectors(i,:);
          end


          %% Coordinate systems
          stewart.lower_leg = struct('rotation', eye(3));
          stewart.upper_leg = struct('rotation', eye(3));

          for i = 1:6
            stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
            stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
          end

          %% Position Matrix
          stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];

          %% Compute Jacobian Matrix
          aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
          stewart.J  = getJacobianMatrix(leg_vectors', aa');
      end

      %%
      function J  = getJacobianMatrix(RM, M_pos_base)
          % RM: [3x6] unit vector of each leg in the fixed frame
          % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
          J = zeros(6);
          J(:, 1:3) = RM';
          J(:, 4:6) = cross(M_pos_base, RM)';
      end
  end

Center of gravity compensation

<<sec:initializeAxisc>>

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

Mirror

<<sec:initializeMirror>>

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

Nano Hexapod

<<sec:initializeNanoHexapod>>

This Matlab function is accessible here.

  function [nano_hexapod] = initializeNanoHexapod(opts_param)
      %% Default values for opts
      opts = struct('actuator', 'piezo');

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

      %% Stewart Object
      nano_hexapod = struct();
      nano_hexapod.h        = 90;  % Total height of the platform [mm]
      nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm]
  %     nano_hexapod.jacobian = 174.26; % Point where the Jacobian is computed => Center of rotation [mm]

      %% Bottom Plate
      BP = struct();

      BP.rad.int   = 0;   % Internal Radius [mm]
      BP.rad.ext   = 150; % External Radius [mm]
      BP.thickness = 10;  % Thickness [mm]
      BP.leg.rad   = 100; % Radius where the legs articulations are positionned [mm]
      BP.leg.ang   = 5;   % Angle Offset [deg]
      BP.density   = 8000;% Density of the material [kg/m^3]
      BP.color     = [0.7 0.7 0.7]; % Color [rgb]
      BP.shape     = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];

      %% Top Plate
      TP = struct();

      TP.rad.int   = 0;   % Internal Radius [mm]
      TP.rad.ext   = 100; % Internal Radius [mm]
      TP.thickness = 10;  % Thickness [mm]
      TP.leg.rad   = 90;  % Radius where the legs articulations are positionned [mm]
      TP.leg.ang   = 5;   % Angle Offset [deg]
      TP.density   = 8000;% Density of the material [kg/m^3]
      TP.color     = [0.7 0.7 0.7]; % Color [rgb]
      TP.shape     = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];

      %% Leg
      Leg = struct();

      Leg.stroke     = 80e-6; % Maximum Stroke of each leg [m]
      if strcmp(opts.actuator, 'piezo')
          Leg.k.ax   = 1e7;   % Stiffness of each leg [N/m]
      elseif strcmp(opts.actuator, 'lorentz')
          Leg.k.ax   = 1e4;   % Stiffness of each leg [N/m]
      else
          error('opts.actuator should be piezo or lorentz');
      end
      Leg.ksi.ax     = 10;    % Maximum amplification at resonance []
      Leg.rad.bottom = 12;    % Radius of the cylinder of the bottom part [mm]
      Leg.rad.top    = 10;    % Radius of the cylinder of the top part [mm]
      Leg.density    = 8000;  % Density of the material [kg/m^3]
      Leg.color.bottom  = [0.5 0.5 0.5]; % Color [rgb]
      Leg.color.top     = [0.5 0.5 0.5]; % Color [rgb]

      Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
      Leg.sphere.top    = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
      Leg.m             = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]

      Leg = updateDamping(Leg);


      %% Sphere
      SP = struct();

      SP.height.bottom  = 15; % [mm]
      SP.height.top     = 15; % [mm]
      SP.density.bottom = 8000; % [kg/m^3]
      SP.density.top    = 8000; % [kg/m^3]
      SP.color.bottom   = [0.7 0.7 0.7]; % [rgb]
      SP.color.top      = [0.7 0.7 0.7]; % [rgb]
      SP.k.ax           = 0; % [N*m/deg]
      SP.ksi.ax         = 0;

      SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
      SP.thickness.top    = SP.height.top-Leg.sphere.top; % [mm]
      SP.rad.bottom       = Leg.sphere.bottom; % [mm]
      SP.rad.top          = Leg.sphere.top; % [mm]
      SP.m                = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]

      SP = updateDamping(SP);

      %%
      Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
      Leg.support.top    = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];

      %%
      nano_hexapod.BP = BP;
      nano_hexapod.TP = TP;
      nano_hexapod.Leg = Leg;
      nano_hexapod.SP = SP;

      %%
      nano_hexapod = initializeParameters(nano_hexapod);

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

      %%
      function [element] = updateDamping(element)
          field = fieldnames(element.k);
          for i = 1:length(field)
              if element.ksi.(field{i}) > 0
                element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
              else
                element.c.(field{i}) = 0;
              end
          end
      end

      %%
      function [stewart] = initializeParameters(stewart)
          %% Connection points on base and top plate w.r.t. World frame at the center of the base plate
          stewart.pos_base = zeros(6, 3);
          stewart.pos_top = zeros(6, 3);

          alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
          alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top

          height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO

          radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
          radius_t = stewart.TP.leg.rad*0.001; % top radius in meters

          for i = 1:3
            % base points
            angle_m_b = (2*pi/3)* (i-1) - alpha_b;
            angle_p_b = (2*pi/3)* (i-1) + alpha_b;
            stewart.pos_base(2*i-1,:) =  [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
            stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];

            % top points
            % Top points are 60 degrees offset
            angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
            angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
            stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
            stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
          end

          % permute pos_top points so that legs are end points of base and top points
          stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
          stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];

          %% leg vectors
          legs = stewart.pos_top - stewart.pos_base;
          leg_length = zeros(6, 1);
          leg_vectors = zeros(6, 3);
          for i = 1:6
            leg_length(i) = norm(legs(i,:));
            leg_vectors(i,:)  = legs(i,:) / leg_length(i);
          end

          stewart.Leg.lenght = 1000*leg_length(1)/1.5;
          stewart.Leg.shape.bot = [0 0; ...
                                  stewart.Leg.rad.bottom 0; ...
                                  stewart.Leg.rad.bottom stewart.Leg.lenght; ...
                                  stewart.Leg.rad.top stewart.Leg.lenght; ...
                                  stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
                                  0 0.2*stewart.Leg.lenght];

          %% Calculate revolute and cylindrical axes
          rev1 = zeros(6, 3);
          rev2 = zeros(6, 3);
          cyl1 = zeros(6, 3);
          for i = 1:6
            rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
            rev1(i,:) = rev1(i,:) / norm(rev1(i,:));

            rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
            rev2(i,:) = rev2(i,:) / norm(rev2(i,:));

            cyl1(i,:) = leg_vectors(i,:);
          end


          %% Coordinate systems
          stewart.lower_leg = struct('rotation', eye(3));
          stewart.upper_leg = struct('rotation', eye(3));

          for i = 1:6
            stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
            stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
          end

          %% Position Matrix
          stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];

          %% Compute Jacobian Matrix
          aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
          stewart.J  = getJacobianMatrix(leg_vectors', aa');
      end

      function J  = getJacobianMatrix(RM,M_pos_base)
          % RM: [3x6] unit vector of each leg in the fixed frame
          % M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
          J = zeros(6);
          J(:, 1:3) = RM';
          J(:, 4:6) = cross(M_pos_base, RM)';
      end
  end

Cedrat Actuator

<<sec:initializeCedratPiezo>>

This Matlab function is accessible here.

  function [cedrat] = initializeCedratPiezo(opts_param)
    %% Default values for opts
    opts = struct();

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

    %% Stewart Object
    cedrat = struct();
    cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]
    cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m]

    cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)]
    cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)]

    cedrat.L = 80; % Total Width of the Actuator[mm]
    cedrat.H = 45; % Total Height of the Actuator [mm]
    cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm]
    cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg]

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

Sample

<<sec:initializeSample>>

This Matlab function is accessible here.

  function [sample] = initializeSample(opts_param)
      %% Default values for opts
      sample = struct('radius', 100, ...
                      'height', 300, ...
                      'mass',   50,  ...
                      'offset', 0,   ...
                      'color',  [0.45, 0.45, 0.45] ...
      );

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

      %%
      sample.k.x = 1e8;
      sample.c.x = 0.1*sqrt(sample.k.x*sample.mass);

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

      sample.k.z = 1e8;
      sample.c.z = 0.1*sqrt(sample.k.z*sample.mass);

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