tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
2fb08445ff
nass-simscape/simscape_subsystems/index.org

1816 lines
56 KiB
Org Mode
Raw Normal View History

Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
#+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:
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
* 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
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeReferences.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeReferences>>
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
#+begin_src matlab
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
function [ref] = initializeReferences(args)
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+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
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Initialize Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Add Z-axis accelerometer and geophone subsystems
2020-01-16 11:49:13 +01:00
%% 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);
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Translation Stage
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% Translation stage - Dy
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
t = 0:Ts:Tmax; % Time Vector [s]
Dy = zeros(length(t), 1);
Dyd = zeros(length(t), 1);
Dydd = zeros(length(t), 1);
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
switch args.Dy_type
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'constant'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Dy(:) = args.Dy_amplitude;
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Dyd(:) = 0;
Dydd(:) = 0;
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'triangular'
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
% This is done to unsure that we start with no displacement
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
i0 = find(t>=args.Dy_period/4,1);
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
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);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'sinusoidal'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
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);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
otherwise
warning('Dy_type is not set correctly');
end
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Tilt Stage
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% Tilt Stage - Ry
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
t = 0:Ts:Tmax; % Time Vector [s]
Ry = zeros(length(t), 1);
Ryd = zeros(length(t), 1);
Rydd = zeros(length(t), 1);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
switch args.Ry_type
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'constant'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Ry(:) = args.Ry_amplitude;
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Ryd(:) = 0;
Rydd(:) = 0;
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'triangular'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
i0 = find(t>=args.Ry_period/4,1);
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
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);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'sinusoidal'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
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);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
otherwise
warning('Ry_type is not set correctly');
end
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Spindle
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% Spindle - Rz
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
t = 0:Ts:Tmax; % Time Vector [s]
Rz = zeros(length(t), 1);
Rzd = zeros(length(t), 1);
Rzdd = zeros(length(t), 1);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
switch args.Rz_type
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'constant'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Rz(:) = args.Rz_amplitude;
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Rzd(:) = 0;
Rzdd(:) = 0;
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'rotating'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Rz(:) = args.Rz_amplitude+2*pi/args.Rz_period*t;
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
% 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);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
otherwise
warning('Rz_type is not set correctly');
end
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Micro Hexapod
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
%% 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
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
Dh = struct('time', t, 'signals', struct('values', Dh));
Dhl = struct('time', t, 'signals', struct('values', Dhl));
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Axis Compensation
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% Axis Compensation - Rm
t = [0, Ts];
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Rm = [args.Rm_pos, args.Rm_pos];
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Rm = struct('time', t, 'signals', struct('values', Rm));
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Nano Hexapod
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% Nano-Hexapod
t = [0, Ts];
Dn = zeros(length(t), 6);
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
switch args.Dn_type
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
case 'constant'
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Dn = [args.Dn_pos, args.Dn_pos];
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
otherwise
warning('Dn_type is not set correctly');
end
Important change for the implementation of motion input Now we provide the first and second derivatives. This permits to not have any motion error. Also, many experiments (tomography, ty-scans) are simulated
2019-12-17 18:03:21 +01:00
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Dn = struct('time', t, 'signals', struct('values', Dn));
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+end_src
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Save
:PROPERTIES:
:UNNUMBERED: t
:END:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
#+begin_src matlab
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% Save
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
save('mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
end
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
* Initialize Disturbances
Move one function
2019-12-11 17:34:42 +01:00
:PROPERTIES:
Rename initDisturbances to initializeDisturbances
2020-01-21 15:49:02 +01:00
:header-args:matlab+: :tangle ../src/initializeDisturbances.m
Move one function
2019-12-11 17:34:42 +01:00
:header-args:matlab+: :comments none :mkdirp yes
:header-args:matlab+: :eval no :results none
:END:
Rename initDisturbances to initializeDisturbances
2020-01-21 15:49:02 +01:00
<<sec:initializeDisturbances>>
Move one function
2019-12-11 17:34:42 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Function Declaration and Documentation
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
Rename initDisturbances to initializeDisturbances
2020-01-21 15:49:02 +01:00
function [] = initializeDisturbances(args)
% initializeDisturbances - Initialize the disturbances
Move one function
2019-12-11 17:34:42 +01:00
%
Rename initDisturbances to initializeDisturbances
2020-01-21 15:49:02 +01:00
% Syntax: [] = initializeDisturbances(args)
Move one function
2019-12-11 17:34:42 +01:00
%
% Inputs:
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
% - args -
Move one function
2019-12-11 17:34:42 +01:00
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
arguments
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
% Global parameter to enable or disable the disturbances
args.enable logical {mustBeNumericOrLogical} = true
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
% 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
Move one function
2019-12-11 17:34:42 +01:00
end
#+end_src
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Load Data
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+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
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
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
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Ground Motion
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+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
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
#+end_src
#+begin_src matlab
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
if args.Dwx && args.enable
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
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
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
if args.Dwy && args.enable
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
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
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
if args.Dwy && args.enable
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
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
Move one function
2019-12-11 17:34:42 +01:00
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Translation Stage - X direction
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
if args.Fty_x && args.enable
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
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);
Move one function
2019-12-11 17:34:42 +01:00
end
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Translation Stage - Z direction
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
if args.Fty_z && args.enable
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
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);
Move one function
2019-12-11 17:34:42 +01:00
end
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Spindle - Z direction
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
Add parameter to enable/disable all disturbances
2020-01-21 15:46:34 +01:00
if args.Frz_z && args.enable
Tomography Experiment with and without disturbances
2019-12-13 19:07:54 +01:00
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);
Move one function
2019-12-11 17:34:42 +01:00
end
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Direct Forces
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
u = zeros(length(t), 6);
Fd = u;
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Set initial value to zero
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+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
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
** Save
:PROPERTIES:
:UNNUMBERED: t
:END:
Move one function
2019-12-11 17:34:42 +01:00
#+begin_src matlab
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
save('mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
Move one function
2019-12-11 17:34:42 +01:00
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
* Z-Axis Geophone
Add Z-axis accelerometer and geophone subsystems
2020-01-16 11:49:13 +01:00
: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
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
* Z-Axis Accelerometer
Add Z-axis accelerometer and geophone subsystems
2020-01-16 11:49:13 +01:00
: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]
Add accelerometer to the stewart platform
2020-01-20 17:21:11 +01:00
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz]
Add Z-axis accelerometer and geophone subsystems
2020-01-16 11:49:13 +01:00
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);
Add accelerometer to the stewart platform
2020-01-20 17:21:11 +01:00
%% Gain correction of the accelerometer to have a unity gain until the resonance
accelerometer.gain = -accelerometer.k/accelerometer.m;
Add Z-axis accelerometer and geophone subsystems
2020-01-16 11:49:13 +01:00
%% Save
save('./mat/accelerometer_z_axis.mat', 'accelerometer');
end
#+end_src
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
* Old functions :noexport:
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
** Micro Hexapod
:PROPERTIES:
Add documentation about the stage initialization
2020-02-03 17:50:32 +01:00
:header-args:matlab+: :tangle ../src/initializeMicroHexapodOld.m
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeMicroHexapod>>
#+begin_src matlab
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
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)
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
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]
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
if args.rigid
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
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
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, args.AP, args.ARB);
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% 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
Use new argument function validation technique
2020-01-13 11:42:31 +01:00
function [cedrat] = initializeCedratPiezo()
Slip the simscape folder into 3 folders
2019-12-11 17:09:32 +01:00
%% 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
Reference in New Issue Copy Permalink