1816 lines
56 KiB
Org Mode
1816 lines
56 KiB
Org Mode
#+TITLE: Subsystems used for the Simscape Models
|
|
:DRAWER:
|
|
#+STARTUP: overview
|
|
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ../index.html
|
|
#+HTML_LINK_UP: ../index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
|
|
|
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
|
|
#+PROPERTY: header-args:shell :eval no-export
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports both
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
:END:
|
|
|
|
* Introduction :ignore:
|
|
|
|
The full Simscape Model is represented in Figure [[fig:simscape_picture]].
|
|
|
|
#+name: fig:simscape_picture
|
|
#+caption: Screenshot of the Multi-Body Model representation
|
|
[[file:figs/images/simscape_picture.png]]
|
|
|
|
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.
|
|
|
|
* Ground
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeGround.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeGround>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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
|
|
|
|
#+name: fig:simscape_model_ground
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Ground
|
|
[[file:figs/images/simscape_model_ground.png]]
|
|
|
|
#+name: fig:simscape_picture_ground
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Ground
|
|
[[file:figs/images/simscape_picture_ground.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ground] = initializeGround()
|
|
#+end_src
|
|
|
|
** Function content
|
|
First, we initialize the =granite= structure.
|
|
#+begin_src matlab
|
|
ground = struct();
|
|
#+end_src
|
|
|
|
We set the shape and density of the ground solid element.
|
|
#+begin_src matlab
|
|
ground.shape = [2, 2, 0.5]; % [m]
|
|
ground.density = 2800; % [kg/m3]
|
|
#+end_src
|
|
|
|
The =ground= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'ground', '-append');
|
|
#+end_src
|
|
|
|
* Granite
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeGranite.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeGranite>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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.
|
|
|
|
#+name: fig:simscape_model_granite
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Granite
|
|
[[file:figs/images/simscape_model_granite.png]]
|
|
|
|
#+name: fig:simscape_picture_granite
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Granite
|
|
[[file:figs/images/simscape_picture_granite.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [granite] = initializeGranite(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
|
|
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
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =granite= structure.
|
|
#+begin_src matlab
|
|
granite = struct();
|
|
#+end_src
|
|
|
|
Properties of the Material and link to the geometry of the granite.
|
|
#+begin_src matlab
|
|
granite.density = args.density; % [kg/m3]
|
|
granite.STEP = './STEPS/granite/granite.STEP';
|
|
#+end_src
|
|
|
|
Stiffness of the connection with Ground.
|
|
#+begin_src matlab
|
|
granite.k.x = 4e9; % [N/m]
|
|
granite.k.y = 3e8; % [N/m]
|
|
granite.k.z = 8e8; % [N/m]
|
|
#+end_src
|
|
|
|
Damping of the connection with Ground.
|
|
#+begin_src matlab
|
|
granite.c.x = 4.0e5; % [N/(m/s)]
|
|
granite.c.y = 1.1e5; % [N/(m/s)]
|
|
granite.c.z = 9.0e5; % [N/(m/s)]
|
|
#+end_src
|
|
|
|
Equilibrium position of the Cartesian joint.
|
|
#+begin_src matlab
|
|
granite.x0 = args.x0;
|
|
granite.y0 = args.y0;
|
|
granite.z0 = args.z0;
|
|
#+end_src
|
|
|
|
Z-offset for the initial position of the sample with respect to the granite top surface.
|
|
#+begin_src matlab
|
|
granite.sample_pos = 0.8; % [m]
|
|
#+end_src
|
|
|
|
The =granite= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'granite', '-append');
|
|
#+end_src
|
|
|
|
* Translation Stage
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeTy.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeTy>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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
|
|
|
|
#+name: fig:simscape_model_ty
|
|
#+ATTR_ORG: :width 800
|
|
#+caption: Simscape model for the Translation Stage
|
|
[[file:figs/images/simscape_model_ty.png]]
|
|
|
|
#+name: fig:simscape_picture_ty
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Translation Stage
|
|
[[file:figs/images/simscape_picture_ty.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ty] = initializeTy(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =ty= structure.
|
|
#+begin_src matlab
|
|
ty = struct();
|
|
#+end_src
|
|
|
|
Define the density of the materials as well as the geometry (STEP files).
|
|
#+begin_src matlab
|
|
% 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';
|
|
#+end_src
|
|
|
|
Stiffness of the stage.
|
|
#+begin_src matlab
|
|
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_src
|
|
|
|
Damping of the stage.
|
|
#+begin_src matlab
|
|
ty.c.ax = 70710; % [N/(m/s)]
|
|
ty.c.rad = 22360; % [N/(m/s)]
|
|
#+end_src
|
|
|
|
Equilibrium position of the joints.
|
|
#+begin_src matlab
|
|
ty.x0_11 = args.x11;
|
|
ty.z0_11 = args.z11;
|
|
ty.x0_12 = args.x12;
|
|
ty.z0_12 = args.z12;
|
|
ty.x0_21 = args.x21;
|
|
ty.z0_21 = args.z21;
|
|
ty.x0_22 = args.x22;
|
|
ty.z0_22 = args.z22;
|
|
#+end_src
|
|
|
|
The =ty= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'ty', '-append');
|
|
#+end_src
|
|
|
|
* Tilt Stage
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeRy.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeRy>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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.
|
|
|
|
#+name: fig:simscape_model_ry
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Tilt Stage
|
|
[[file:figs/images/simscape_model_ry.png]]
|
|
|
|
#+name: fig:simscape_picture_ry
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Tilt Stage
|
|
[[file:figs/images/simscape_picture_ry.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ry] = initializeRy(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.x11 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.y11 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z11 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x12 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.y12 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z12 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x21 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.y21 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z21 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x22 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.y22 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z22 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =ry= structure.
|
|
#+begin_src matlab
|
|
ry = struct();
|
|
#+end_src
|
|
|
|
Properties of the Material and link to the geometry of the Tilt stage.
|
|
#+begin_src matlab
|
|
% 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';
|
|
#+end_src
|
|
|
|
Stiffness of the stage.
|
|
#+begin_src matlab
|
|
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_src
|
|
|
|
Damping of the stage.
|
|
#+begin_src matlab
|
|
ry.c.tilt = 2.8e2;
|
|
ry.c.h = 2.8e4;
|
|
ry.c.rad = 2.8e4;
|
|
ry.c.rrad = 2.8e4;
|
|
#+end_src
|
|
|
|
Equilibrium position of the joints.
|
|
#+begin_src matlab
|
|
ry.x0_11 = args.x11;
|
|
ry.y0_11 = args.y11;
|
|
ry.z0_11 = args.z11;
|
|
ry.x0_12 = args.x12;
|
|
ry.y0_12 = args.y12;
|
|
ry.z0_12 = args.z12;
|
|
ry.x0_21 = args.x21;
|
|
ry.y0_21 = args.y21;
|
|
ry.z0_21 = args.z21;
|
|
ry.x0_22 = args.x22;
|
|
ry.y0_22 = args.y22;
|
|
ry.z0_22 = args.z22;
|
|
#+end_src
|
|
|
|
Z-Offset so that the center of rotation matches the sample center;
|
|
#+begin_src matlab
|
|
ry.z_offset = 0.58178; % [m]
|
|
#+end_src
|
|
|
|
The =ty= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'ry', '-append');
|
|
#+end_src
|
|
|
|
* Spindle
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeRz.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeRz>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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
|
|
|
|
#+name: fig:simscape_model_rz
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Spindle
|
|
[[file:figs/images/simscape_model_rz.png]]
|
|
|
|
#+name: fig:simscape_picture_rz
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Spindle
|
|
[[file:figs/images/simscape_picture_rz.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [rz] = initializeRz(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.rigid logical {mustBeNumericOrLogical} = false
|
|
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.rx0 (1,1) double {mustBeNumeric} = 0 % [rad]
|
|
args.ry0 (1,1) double {mustBeNumeric} = 0 % [rad]
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =rz= structure.
|
|
#+begin_src matlab
|
|
rz = struct();
|
|
#+end_src
|
|
|
|
Properties of the Material and link to the geometry of the spindle.
|
|
#+begin_src matlab
|
|
% 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';
|
|
#+end_src
|
|
|
|
Stiffness of the stage.
|
|
#+begin_src matlab
|
|
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_src
|
|
|
|
Damping of the stage.
|
|
#+begin_src matlab
|
|
rz.c.rot = 1.6e3;
|
|
rz.c.tilt = 1.6e3;
|
|
rz.c.ax = 7.1e4;
|
|
rz.c.rad = 4.2e4;
|
|
#+end_src
|
|
|
|
Equilibrium position of the joints.
|
|
#+begin_src matlab
|
|
rz.x0 = args.x0;
|
|
rz.y0 = args.y0;
|
|
rz.z0 = args.z0;
|
|
rz.rx0 = args.rx0;
|
|
rz.ry0 = args.ry0;
|
|
#+end_src
|
|
|
|
The =rz= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'rz', '-append');
|
|
#+end_src
|
|
|
|
* Micro Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeMicroHexapod>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+name: fig:simscape_model_micro_hexapod
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Micro-Hexapod
|
|
[[file:figs/images/simscape_model_micro_hexapod.png]]
|
|
|
|
#+name: fig:simscape_picture_micro_hexapod
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Micro-Hexapod
|
|
[[file:figs/images/simscape_picture_micro_hexapod.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [micro_hexapod] = initializeMicroHexapod(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
% 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)
|
|
% Equilibrium position of each leg
|
|
args.dLeq (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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;
|
|
#+end_src
|
|
|
|
Equilibrium position of the each joint.
|
|
#+begin_src matlab
|
|
micro_hexapod.dLeq = args.dLeq;
|
|
#+end_src
|
|
|
|
The =micro_hexapod= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
|
#+end_src
|
|
|
|
* Center of gravity compensation
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeAxisc.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeAxisc>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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
|
|
|
|
#+name: fig:simscape_model_axisc
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Center of Mass compensation system
|
|
[[file:figs/images/simscape_model_axisc.png]]
|
|
|
|
#+name: fig:simscape_picture_axisc
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Center of Mass compensation system
|
|
[[file:figs/images/simscape_picture_axisc.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [axisc] = initializeAxisc()
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =axisc= structure.
|
|
#+begin_src matlab
|
|
axisc = struct();
|
|
#+end_src
|
|
|
|
Properties of the Material and link to the geometry files.
|
|
#+begin_src matlab
|
|
% 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';
|
|
#+end_src
|
|
|
|
The =axisc= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'axisc', '-append');
|
|
#+end_src
|
|
|
|
* Mirror
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeMirror.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeMirror>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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
|
|
|
|
#+name: fig:simscape_model_mirror
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Mirror
|
|
[[file:figs/images/simscape_model_mirror.png]]
|
|
|
|
#+name: fig:simscape_picture_mirror
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Mirror
|
|
[[file:figs/images/simscape_picture_mirror.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = initializeMirror(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
args.shape char {mustBeMember(args.shape,{'spherical', 'conical'})} = 'spherical'
|
|
args.angle (1,1) double {mustBeNumeric, mustBePositive} = 45 % [deg]
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =mirror= structure.
|
|
#+begin_src matlab
|
|
mirror = struct();
|
|
#+end_src
|
|
|
|
We define the geometrical values.
|
|
#+begin_src matlab
|
|
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]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
mirror.density = 2400; % Density of the material [kg/m3]
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
mirror.cone_length = mirror.rad*tand(args.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
|
|
#+end_src
|
|
|
|
Now we define the Shape of the mirror.
|
|
We first start with the internal part.
|
|
#+begin_src matlab
|
|
mirror.shape = [...
|
|
0 mirror.h-mirror.thickness
|
|
mirror.hole_rad mirror.h-mirror.thickness; ...
|
|
mirror.hole_rad 0; ...
|
|
mirror.rad 0 ...
|
|
];
|
|
#+end_src
|
|
|
|
Then, we define the reflective used part of the mirror.
|
|
#+begin_src matlab
|
|
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
|
|
#+end_src
|
|
|
|
Finally, we close the shape.
|
|
#+begin_src matlab
|
|
mirror.shape = [mirror.shape; 0 mirror.h];
|
|
#+end_src
|
|
|
|
The =mirror= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'mirror', '-append');
|
|
#+end_src
|
|
|
|
* Nano Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeNanoHexapod.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeNanoHexapod>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+name: fig:simscape_model_nano_hexapod
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Nano Hexapod
|
|
[[file:figs/images/simscape_model_nano_hexapod.png]]
|
|
|
|
#+name: fig:simscape_picture_nano_hexapod
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Nano Hexapod
|
|
[[file:figs/images/simscape_picture_nano_hexapod.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [nano_hexapod] = initializeNanoHexapod(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
% 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'
|
|
% 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} = 100e-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)
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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 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;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.dLeq = args.dLeq;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
|
#+end_src
|
|
|
|
* Sample
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeSample.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeSample>>
|
|
|
|
** Simscape Model
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
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.
|
|
|
|
#+name: fig:simscape_model_sample
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape model for the Sample
|
|
[[file:figs/images/simscape_model_sample.png]]
|
|
|
|
#+name: fig:simscape_picture_sample
|
|
#+attr_org: :width 800
|
|
#+caption: Simscape picture for the Sample
|
|
[[file:figs/images/simscape_picture_sample.png]]
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [sample] = initializeSample(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
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 (1,1) double {mustBeNumeric, mustBePositive} = 100 % [Hz]
|
|
args.offset (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.x0 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.y0 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
args.z0 (1,1) double {mustBeNumeric} = 0 % [m]
|
|
end
|
|
#+end_src
|
|
|
|
** Function content
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
First, we initialize the =sample= structure.
|
|
#+begin_src matlab
|
|
sample = struct();
|
|
#+end_src
|
|
|
|
We define the geometrical parameters of the sample as well as its mass and position.
|
|
#+begin_src matlab
|
|
sample.radius = args.radius; % [m]
|
|
sample.height = args.height; % [m]
|
|
sample.mass = args.mass; % [kg]
|
|
sample.offset = args.offset; % [m]
|
|
#+end_src
|
|
|
|
Stiffness of the sample fixation.
|
|
#+begin_src matlab
|
|
sample.k.x = sample.mass * (2*pi * args.freq)^2; % [N/m]
|
|
sample.k.y = sample.mass * (2*pi * args.freq)^2; % [N/m]
|
|
sample.k.z = sample.mass * (2*pi * args.freq)^2; % [N/m]
|
|
#+end_src
|
|
|
|
Damping of the sample fixation.
|
|
#+begin_src matlab
|
|
sample.c.x = 0.1*sqrt(sample.k.x*sample.mass); % [N/(m/s)]
|
|
sample.c.y = 0.1*sqrt(sample.k.y*sample.mass); % [N/(m/s)]
|
|
sample.c.z = 0.1*sqrt(sample.k.z*sample.mass); % [N/(m/s)]
|
|
#+end_src
|
|
|
|
Equilibrium position of the Cartesian joint corresponding to the sample fixation.
|
|
#+begin_src matlab
|
|
sample.x0 = args.x0; % [m]
|
|
sample.y0 = args.y0; % [m]
|
|
sample.z0 = args.z0; % [m]
|
|
#+end_src
|
|
|
|
The =sample= structure is saved.
|
|
#+begin_src matlab
|
|
save('./mat/stages.mat', 'sample', '-append');
|
|
#+end_src
|
|
|
|
* Generate Reference Signals
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeReferences.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeReferences>>
|
|
|
|
** Function Declaration and Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [ref] = initializeReferences(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
% Sampling Frequency [s]
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
|
% Maximum simulation time [s]
|
|
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
|
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
|
% Amplitude of the displacement [m]
|
|
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the displacement [s]
|
|
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
|
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
|
% Amplitude [rad]
|
|
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the displacement [s]
|
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% Either "constant" / "rotating"
|
|
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating'})} = 'constant'
|
|
% Initial angle [rad]
|
|
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the rotating [s]
|
|
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% For now, only constant is implemented
|
|
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
|
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
|
% For now, only constant is implemented
|
|
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
|
|
% Initial position of the two masses
|
|
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
|
|
% For now, only constant is implemented
|
|
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform
|
|
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
end
|
|
#+end_src
|
|
|
|
|
|
** Initialize Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Set Sampling Time
|
|
Ts = args.Ts;
|
|
Tmax = args.Tmax;
|
|
|
|
%% Low Pass Filter to filter out the references
|
|
s = zpk('s');
|
|
w0 = 2*pi*10;
|
|
xi = 1;
|
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
|
#+end_src
|
|
|
|
** Translation Stage
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Translation stage - Dy
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Dy = zeros(length(t), 1);
|
|
Dyd = zeros(length(t), 1);
|
|
Dydd = zeros(length(t), 1);
|
|
switch args.Dy_type
|
|
case 'constant'
|
|
Dy(:) = args.Dy_amplitude;
|
|
Dyd(:) = 0;
|
|
Dydd(:) = 0;
|
|
case 'triangular'
|
|
% This is done to unsure that we start with no displacement
|
|
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
|
|
i0 = find(t>=args.Dy_period/4,1);
|
|
Dy(1:end-i0+1) = Dy_raw(i0:end);
|
|
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
|
|
|
|
% The signal is filtered out
|
|
Dy = lsim(H_lpf, Dy, t);
|
|
Dyd = lsim(H_lpf*s, Dy, t);
|
|
Dydd = lsim(H_lpf*s^2, Dy, t);
|
|
case 'sinusoidal'
|
|
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
|
|
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
|
|
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
|
|
otherwise
|
|
warning('Dy_type is not set correctly');
|
|
end
|
|
|
|
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
|
|
#+end_src
|
|
|
|
** Tilt Stage
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Tilt Stage - Ry
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Ry = zeros(length(t), 1);
|
|
Ryd = zeros(length(t), 1);
|
|
Rydd = zeros(length(t), 1);
|
|
|
|
switch args.Ry_type
|
|
case 'constant'
|
|
Ry(:) = args.Ry_amplitude;
|
|
Ryd(:) = 0;
|
|
Rydd(:) = 0;
|
|
case 'triangular'
|
|
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
|
|
i0 = find(t>=args.Ry_period/4,1);
|
|
Ry(1:end-i0+1) = Ry_raw(i0:end);
|
|
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
|
|
|
|
% The signal is filtered out
|
|
Ry = lsim(H_lpf, Ry, t);
|
|
Ryd = lsim(H_lpf*s, Ry, t);
|
|
Rydd = lsim(H_lpf*s^2, Ry, t);
|
|
case 'sinusoidal'
|
|
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
|
|
|
|
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
|
|
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
|
|
otherwise
|
|
warning('Ry_type is not set correctly');
|
|
end
|
|
|
|
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
|
|
#+end_src
|
|
|
|
** Spindle
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Spindle - Rz
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Rz = zeros(length(t), 1);
|
|
Rzd = zeros(length(t), 1);
|
|
Rzdd = zeros(length(t), 1);
|
|
|
|
switch args.Rz_type
|
|
case 'constant'
|
|
Rz(:) = args.Rz_amplitude;
|
|
Rzd(:) = 0;
|
|
Rzdd(:) = 0;
|
|
case 'rotating'
|
|
Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
|
|
|
|
% The signal is filtered out
|
|
Rz = lsim(H_lpf, Rz, t);
|
|
Rzd = lsim(H_lpf*s, Rz, t);
|
|
Rzdd = lsim(H_lpf*s^2, Rz, t);
|
|
otherwise
|
|
warning('Rz_type is not set correctly');
|
|
end
|
|
|
|
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
|
|
#+end_src
|
|
|
|
** Micro Hexapod
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Micro-Hexapod
|
|
t = [0, Ts];
|
|
Dh = zeros(length(t), 6);
|
|
Dhl = zeros(length(t), 6);
|
|
|
|
switch args.Dh_type
|
|
case 'constant'
|
|
Dh = [args.Dh_pos, args.Dh_pos];
|
|
|
|
load('mat/stages.mat', 'micro_hexapod');
|
|
|
|
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
|
|
|
|
tx = args.Dh_pos(4);
|
|
ty = args.Dh_pos(5);
|
|
tz = args.Dh_pos(6);
|
|
|
|
ARB = [cos(tz) -sin(tz) 0;
|
|
sin(tz) cos(tz) 0;
|
|
0 0 1]*...
|
|
[ cos(ty) 0 sin(ty);
|
|
0 1 0;
|
|
-sin(ty) 0 cos(ty)]*...
|
|
[1 0 0;
|
|
0 cos(tx) -sin(tx);
|
|
0 sin(tx) cos(tx)];
|
|
|
|
[~, Dhl] = 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));
|
|
#+end_src
|
|
|
|
** Axis Compensation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Axis Compensation - Rm
|
|
t = [0, Ts];
|
|
|
|
Rm = [args.Rm_pos, args.Rm_pos];
|
|
Rm = struct('time', t, 'signals', struct('values', Rm));
|
|
#+end_src
|
|
|
|
** Nano Hexapod
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Nano-Hexapod
|
|
t = [0, Ts];
|
|
Dn = zeros(length(t), 6);
|
|
|
|
switch args.Dn_type
|
|
case 'constant'
|
|
Dn = [args.Dn_pos, args.Dn_pos];
|
|
otherwise
|
|
warning('Dn_type is not set correctly');
|
|
end
|
|
|
|
Dn = struct('time', t, 'signals', struct('values', Dn));
|
|
#+end_src
|
|
|
|
** Save
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
%% Save
|
|
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
|
|
end
|
|
#+end_src
|
|
|
|
* Initialize Disturbances
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeDisturbances.m
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeDisturbances>>
|
|
|
|
** Function Declaration and Documentation
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = initializeDisturbances(args)
|
|
% initializeDisturbances - Initialize the disturbances
|
|
%
|
|
% Syntax: [] = initializeDisturbances(args)
|
|
%
|
|
% Inputs:
|
|
% - args -
|
|
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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
|
|
#+end_src
|
|
|
|
|
|
** Load Data
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
load('./disturbances/mat/dist_psd.mat', 'dist_f');
|
|
#+end_src
|
|
|
|
We remove the first frequency point that usually is very large.
|
|
#+begin_src matlab :exports none
|
|
dist_f.f = dist_f.f(2:end);
|
|
dist_f.psd_gm = dist_f.psd_gm(2:end);
|
|
dist_f.psd_ty = dist_f.psd_ty(2:end);
|
|
dist_f.psd_rz = dist_f.psd_rz(2:end);
|
|
#+end_src
|
|
|
|
** Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
We define some parameters that will be used in the algorithm.
|
|
#+begin_src matlab
|
|
Fs = 2*dist_f.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(dist_f.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
df = 1/T0; % Frequency resolution of the DFT [Hz]
|
|
% Also equal to (dist_f.f(2)-dist_f.f(1))
|
|
t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
Ts = 1/Fs; % Sampling Time [s]
|
|
#+end_src
|
|
|
|
** Ground Motion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
phi = dist_f.psd_gm;
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(phi(i)*df);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
if args.Dwx && args.enable
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dwx = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
|
else
|
|
Dwx = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
if args.Dwy && args.enable
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dwy = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
|
else
|
|
Dwy = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
if args.Dwy && args.enable
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dwz = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
|
else
|
|
Dwz = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
** Translation Stage - X direction
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
|
|
Fty_x = u;
|
|
else
|
|
Fty_x = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
** Translation Stage - Z direction
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
|
|
Fty_z = u;
|
|
else
|
|
Fty_z = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
** Spindle - Z direction
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
|
|
Frz_z = u;
|
|
else
|
|
Frz_z = zeros(length(t), 1);
|
|
end
|
|
#+end_src
|
|
|
|
** Direct Forces
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
u = zeros(length(t), 6);
|
|
Fd = u;
|
|
#+end_src
|
|
|
|
** Set initial value to zero
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
Dwx = Dwx - Dwx(1);
|
|
Dwy = Dwy - Dwy(1);
|
|
Dwz = Dwz - Dwz(1);
|
|
Fty_x = Fty_x - Fty_x(1);
|
|
Fty_z = Fty_z - Fty_z(1);
|
|
Frz_z = Frz_z - Frz_z(1);
|
|
#+end_src
|
|
|
|
** Save
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
|
|
#+end_src
|
|
|
|
* Z-Axis Geophone
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeZAxisGeophone.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeZAxisGeophone>>
|
|
|
|
#+begin_src matlab
|
|
function [geophone] = initializeZAxisGeophone(args)
|
|
arguments
|
|
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
|
|
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz]
|
|
end
|
|
|
|
%%
|
|
geophone.m = args.mass;
|
|
|
|
%% The Stiffness is set to have the damping resonance frequency
|
|
geophone.k = geophone.m * (2*pi*args.freq)^2;
|
|
|
|
%% We set the damping value to have critical damping
|
|
geophone.c = 2*sqrt(geophone.m * geophone.k);
|
|
|
|
%% Save
|
|
save('./mat/geophone_z_axis.mat', 'geophone');
|
|
end
|
|
#+end_src
|
|
|
|
* Z-Axis Accelerometer
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeZAxisAccelerometer.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeZAxisAccelerometer>>
|
|
|
|
#+begin_src matlab
|
|
function [accelerometer] = initializeZAxisAccelerometer(args)
|
|
arguments
|
|
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
|
|
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 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
|
|
#+end_src
|
|
* Old functions :noexport:
|
|
** Micro Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeMicroHexapodOld.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeMicroHexapod>>
|
|
|
|
#+begin_src matlab
|
|
function [micro_hexapod] = initializeMicroHexapod(args)
|
|
arguments
|
|
args.rigid logical {mustBeNumericOrLogical} = false
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
%% Stewart Object
|
|
micro_hexapod = struct();
|
|
micro_hexapod.h = 350; % Total height of the platform [mm]
|
|
micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
|
|
|
|
%% Bottom Plate - Mechanical Design
|
|
BP = struct();
|
|
|
|
BP.rad.int = 110; % Internal Radius [mm]
|
|
BP.rad.ext = 207.5; % External Radius [mm]
|
|
BP.thickness = 26; % Thickness [mm]
|
|
BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm]
|
|
BP.leg.ang = 9.5; % Angle Offset [deg]
|
|
BP.density = 8000; % Density of the material [kg/m^3]
|
|
BP.color = [0.6 0.6 0.6]; % Color [rgb]
|
|
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
|
|
|
|
%% Top Plate - Mechanical Design
|
|
TP = struct();
|
|
|
|
TP.rad.int = 82; % Internal Radius [mm]
|
|
TP.rad.ext = 150; % Internal Radius [mm]
|
|
TP.thickness = 26; % Thickness [mm]
|
|
TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm]
|
|
TP.leg.ang = 12.1; % Angle Offset [deg]
|
|
TP.density = 8000; % Density of the material [kg/m^3]
|
|
TP.color = [0.6 0.6 0.6]; % Color [rgb]
|
|
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
|
|
|
|
%% Struts
|
|
Leg = struct();
|
|
|
|
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
|
|
if args.rigid
|
|
Leg.k.ax = 1e12; % Stiffness of each leg [N/m]
|
|
else
|
|
Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
|
|
end
|
|
Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) []
|
|
Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
|
|
Leg.rad.top = 17; % Radius of the cylinder of the top part [mm]
|
|
Leg.density = 8000; % Density of the material [kg/m^3]
|
|
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
|
|
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
|
|
|
|
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
|
|
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
|
|
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
|
|
Leg = updateDamping(Leg);
|
|
|
|
|
|
%% Sphere
|
|
SP = struct();
|
|
|
|
SP.height.bottom = 27; % [mm]
|
|
SP.height.top = 27; % [mm]
|
|
SP.density.bottom = 8000; % [kg/m^3]
|
|
SP.density.top = 8000; % [kg/m^3]
|
|
SP.color.bottom = [0.6 0.6 0.6]; % [rgb]
|
|
SP.color.top = [0.6 0.6 0.6]; % [rgb]
|
|
SP.k.ax = 0; % [N*m/deg]
|
|
SP.ksi.ax = 10;
|
|
|
|
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
|
|
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
|
|
SP.rad.bottom = Leg.sphere.bottom; % [mm]
|
|
SP.rad.top = Leg.sphere.top; % [mm]
|
|
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
|
|
|
|
SP = updateDamping(SP);
|
|
|
|
%%
|
|
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
|
|
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
|
|
|
|
%%
|
|
micro_hexapod.BP = BP;
|
|
micro_hexapod.TP = TP;
|
|
micro_hexapod.Leg = Leg;
|
|
micro_hexapod.SP = SP;
|
|
|
|
%%
|
|
micro_hexapod = initializeParameters(micro_hexapod);
|
|
|
|
%% Setup equilibrium position of each leg
|
|
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'micro_hexapod', '-append');
|
|
|
|
%%
|
|
function [element] = updateDamping(element)
|
|
field = fieldnames(element.k);
|
|
for i = 1:length(field)
|
|
element.c.(field{i}) = 2*element.ksi.(field{i})*sqrt(element.k.(field{i})*element.m);
|
|
end
|
|
end
|
|
|
|
%%
|
|
function [stewart] = initializeParameters(stewart)
|
|
%% Connection points on base and top plate w.r.t. World frame at the center of the base plate
|
|
stewart.pos_base = zeros(6, 3);
|
|
stewart.pos_top = zeros(6, 3);
|
|
|
|
alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
|
|
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
|
|
|
|
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
|
|
|
|
radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
|
|
radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
|
|
|
|
for i = 1:3
|
|
% base points
|
|
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
|
|
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
|
|
stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
|
|
stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
|
|
|
|
% top points
|
|
% Top points are 60 degrees offset
|
|
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
|
|
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
|
|
stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
|
|
stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
|
|
end
|
|
|
|
% permute pos_top points so that legs are end points of base and top points
|
|
stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
|
|
stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
|
|
|
|
%% leg vectors
|
|
legs = stewart.pos_top - stewart.pos_base;
|
|
leg_length = zeros(6, 1);
|
|
leg_vectors = zeros(6, 3);
|
|
for i = 1:6
|
|
leg_length(i) = norm(legs(i,:));
|
|
leg_vectors(i,:) = legs(i,:) / leg_length(i);
|
|
end
|
|
|
|
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
|
|
stewart.Leg.shape.bot = [0 0; ...
|
|
stewart.Leg.rad.bottom 0; ...
|
|
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
|
|
stewart.Leg.rad.top stewart.Leg.lenght; ...
|
|
stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
|
|
0 0.2*stewart.Leg.lenght];
|
|
|
|
%% Calculate revolute and cylindrical axes
|
|
rev1 = zeros(6, 3);
|
|
rev2 = zeros(6, 3);
|
|
cyl1 = zeros(6, 3);
|
|
for i = 1:6
|
|
rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
|
|
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
|
|
|
|
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
|
|
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
|
|
|
|
cyl1(i,:) = leg_vectors(i,:);
|
|
end
|
|
|
|
|
|
%% Coordinate systems
|
|
stewart.lower_leg = struct('rotation', eye(3));
|
|
stewart.upper_leg = struct('rotation', eye(3));
|
|
|
|
for i = 1:6
|
|
stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
|
stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
|
end
|
|
|
|
%% Position Matrix
|
|
stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
|
|
|
%% Compute Jacobian Matrix
|
|
aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
|
|
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
|
end
|
|
|
|
%%
|
|
function J = getJacobianMatrix(RM, M_pos_base)
|
|
% RM: [3x6] unit vector of each leg in the fixed frame
|
|
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
|
|
J = zeros(6);
|
|
J(:, 1:3) = RM';
|
|
J(:, 4:6) = cross(M_pos_base, RM)';
|
|
end
|
|
end
|
|
#+end_src
|
|
** Cedrat Actuator
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeCedratPiezo.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeCedratPiezo>>
|
|
|
|
#+begin_src matlab
|
|
function [cedrat] = initializeCedratPiezo()
|
|
%% Stewart Object
|
|
cedrat = struct();
|
|
cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]
|
|
cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m]
|
|
|
|
cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)]
|
|
cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)]
|
|
|
|
cedrat.L = 80; % Total Width of the Actuator[mm]
|
|
cedrat.H = 45; % Total Height of the Actuator [mm]
|
|
cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm]
|
|
cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg]
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'cedrat', '-append');
|
|
end
|
|
#+end_src
|