1991 lines
72 KiB
Org Mode
1991 lines
72 KiB
Org Mode
#+TITLE: Simscape Model
|
|
: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 matlab/modal_frf_coh.m
|
|
#+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:
|
|
This file is used to explain how this Simscape Model works.
|
|
- In section [[sec:simulink_project]], the simulink project with the associated scripts are presented
|
|
- In section [[sec:simscape_model]], an introduction to Simscape Multibody is done
|
|
- In section [[sec:simulink_files_signal_names]], each simscape files are presented with the associated signal names and joint architectures
|
|
- In section [[sec:simulink_library]], the list of the Simulink library elements are described
|
|
- In section [[sec:scripts]], the scripts used for the simulations (initialization for instance) are described
|
|
- In section [[sec:functions]], a list of Matlab function that will be used are defined here
|
|
- In section [[sec:initialize_elements]], all the functions that are used to initialize the Simscape Multibody elements are defined here. This includes the mass of all solids for instance.
|
|
|
|
* Matlab Init :noexport:ignore:
|
|
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
cd('../');
|
|
#+end_src
|
|
|
|
* Simulink Project - Startup and Shutdown scripts
|
|
<<sec:simulink_project>>
|
|
|
|
From the [[https://mathworks.com/products/simulink/projects.html][Simulink project]] mathworks page:
|
|
#+begin_quote
|
|
Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place.
|
|
|
|
With Simulink Projects, you can:
|
|
- *Collaborate*: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.
|
|
- *Automate*: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.
|
|
- *Integrate with source control*: Enable easy integration with source control and configuration management tools.
|
|
#+end_quote
|
|
|
|
The project can be opened using the =simulinkproject= function:
|
|
|
|
#+begin_src matlab
|
|
simulinkproject('./');
|
|
#+end_src
|
|
|
|
When the project opens, a startup script is ran.
|
|
The startup script is defined below and is exported to the =project_startup.m= script.
|
|
#+begin_src matlab :eval no :tangle ../src/project_startup.m
|
|
freqs = logspace(-1, 3, 1000);
|
|
save_fig = false;
|
|
save('./mat/config.mat', 'freqs', 'save_fig');
|
|
|
|
project = simulinkproject;
|
|
projectRoot = project.RootFolder;
|
|
|
|
myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
|
|
myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
|
|
|
|
Simulink.fileGenControl('set',...
|
|
'CacheFolder', myCacheFolder,...
|
|
'CodeGenFolder', myCodeFolder,...
|
|
'createDir', true);
|
|
#+end_src
|
|
|
|
When the project closes, it runs the =project_shutdown.m= script defined below.
|
|
#+begin_src matlab :eval no :tangle ../src/project_shutdown.m
|
|
Simulink.fileGenControl('reset');
|
|
#+end_src
|
|
|
|
The project also permits to automatically add defined folder to the path when the project is opened.
|
|
|
|
* Simscape Multibody - Presentation
|
|
<<sec:simscape_model>>
|
|
|
|
A [[https://.mathworks.com/products/simscape.html][simscape]] model permits to model multi-physics systems.
|
|
|
|
[[https://mathworks.com/products/simmechanics.html][Simscape Multibody]] permits to model multibody systems using blocks representing bodies, joints, constraints, force elements, and sensors.
|
|
|
|
** Solid bodies
|
|
Each solid body is represented by a [[https://mathworks.com/help/physmod/sm/ref/solid.html][solid block]].
|
|
The geometry of the solid body can be imported using a =step= file. The properties of the solid body such as its mass, center of mass and moment of inertia can be derived from its density and its geometry as defined by the =step= file. They also can be set by hand.
|
|
|
|
** Frames
|
|
Frames are very important in simscape multibody, they defined where the forces are applied, where the joints are located and where the measurements are made.
|
|
|
|
They can be defined from the solid body geometry, or using the [[https://mathworks.com/help/physmod/sm/ref/rigidtransform.html][rigid transform block]].
|
|
|
|
** Joints
|
|
Solid Bodies are connected with joints (between frames of the two solid bodies).
|
|
|
|
There are various types of joints that are all described [[https://mathworks.com/help/physmod/sm/ug/joints.html][here]].
|
|
|
|
#+name: tab:joint_dof
|
|
#+caption: Degrees of freedom associated with each joint
|
|
| Joint Block | Translational DOFs | Rotational DOFs |
|
|
|-------------------+--------------------+-----------------|
|
|
| 6-DOF | 3 | 3 |
|
|
| Bearing | 1 | 3 |
|
|
| Bushing | 3 | 3 |
|
|
| Cartesian | 3 | 0 |
|
|
| Constant Velocity | 0 | 2 |
|
|
| Cylindrical | 1 | 1 |
|
|
| Gimbal | 0 | 3 |
|
|
| Leadscrew | 1 (coupled) | 1 (coupled) |
|
|
| Pin Slot | 1 | 1 |
|
|
| Planar | 2 | 1 |
|
|
| Prismatic | 1 | 0 |
|
|
| Rectangular | 2 | 0 |
|
|
| Revolute | 0 | 1 |
|
|
| Spherical | 1 | 3 |
|
|
| Telescoping | 1 | 3 |
|
|
| Universal | 0 | 2 |
|
|
| Weld | 0 | 0 |
|
|
|
|
Joint blocks are assortments of joint primitives:
|
|
- *Prismatic*: allows translation along a single standard axis: =Px=, =Py=, =Pz=
|
|
- *Revolute*: allows rotation about a single standard axis: =Rx=, =Ry=, =Rz=
|
|
- *Spherical*: allow rotation about any 3D axis: =S=
|
|
- *Lead Screw*: allows coupled rotation and translation on a standard axis: =LSz=
|
|
- *Constant Velocity*: Allows rotation at constant velocity between intersection through arbitrarily aligned shafts: =CV=
|
|
|
|
#+name: tab:joint_primitive
|
|
#+caption: Joint primitives for each joint type
|
|
| Joint Block | Px | Py | Pz | Rx | Ry | Rz | S | CV | LSz |
|
|
|-------------------+----+----+----+----+----+----+---+----+-----|
|
|
| 6-DOF | x | x | x | | | | x | | |
|
|
| Bearing | | | x | x | x | x | | | |
|
|
| Bushing | x | x | x | x | x | x | | | |
|
|
| Cartesian | x | x | x | | | | | | |
|
|
| Constant Velocity | | | | | | | | x | |
|
|
| Cylindrical | | | x | | | x | | | |
|
|
| Gimbal | | | | x | x | x | | | |
|
|
| Leadscrew | | | | | | | | | x |
|
|
| Pin Slot | x | | | | | x | | | |
|
|
| Planar | x | x | | | | x | | | |
|
|
| Prismatic | | | x | | | | | | |
|
|
| Rectangular | x | x | | | | | | | |
|
|
| Revolute | | | | | | x | | | |
|
|
| Spherical | | | | | | | x | | |
|
|
| Telescoping | | | x | | | | x | | |
|
|
| Universal | | | | x | x | | | | |
|
|
| Weld | | | | | | | | | |
|
|
|
|
For each of the joint primitive, we can specify the dynamical properties:
|
|
- The *spring stiffness*: either linear or rotational one
|
|
- The *damping coefficient*
|
|
|
|
For the actuation, we can either specify the motion or the force:
|
|
- the force applied in the corresponding DOF is provided by the input
|
|
- the motion is provided by the input
|
|
|
|
A sensor can be added to measure either the position, velocity or acceleration of the joint DOF.
|
|
|
|
Composite Force/Torque sensing:
|
|
- Constraint force
|
|
- Total force: gives the sum across all joint primitives over all sources: actuation inputs, internal springs and dampers.
|
|
|
|
** Measurements
|
|
A transform sensor block measures the spatial relationship between two frames: the base =B= and the follower =F=.
|
|
|
|
It can give the rotational and translational position, velocity and acceleration.
|
|
|
|
The measurement frame should be specified: it corresponds to the frame in which to resolve the selected spatial measurements. The default is =world=.
|
|
|
|
If we want to simulate an *inertial sensor*, we just have to choose =B= to be the =world= frame.
|
|
|
|
*Force sensors* are included in the joints blocks.
|
|
|
|
** Excitation
|
|
We can apply *external forces* to the model by using an [[https://mathworks.com/help/physmod/sm/ref/externalforceandtorque.html][external force and torque block]].
|
|
|
|
Internal force, acting reciprocally between base and following origins is implemented using the [[https://mathworks.com/help/physmod/sm/ref/internalforce.html][internal force block]] even though it is usually included in one joint block.
|
|
|
|
* Simulink files and signal names
|
|
<<sec:simulink_files_signal_names>>
|
|
|
|
In order to "normalize" things, the names of all the signal are listed here.
|
|
|
|
** List of Simscape files
|
|
|
|
Few different Simulink files are used:
|
|
- kinematics
|
|
- identification - micro station
|
|
- identification - nano station
|
|
- control
|
|
|
|
#+name: tab:simscape_files
|
|
#+caption: List of simscape files
|
|
| Simscape Name | Ty | Ry | Rz | Hexa | NASS |
|
|
|------------------------+----+----+----+------+------|
|
|
| id micro station | F | F | F | F | |
|
|
| id nano station stages | F | F | F | F | F |
|
|
| id nano station config | D | D | D | D | F |
|
|
| control nano station | D | D | D | D | F |
|
|
|
|
** List of Inputs
|
|
*** Perturbations
|
|
|
|
#+name: tab:perturbations_names
|
|
#+caption: List of Disturbances
|
|
| Variable | Meaning | Size | Unit |
|
|
|----------+--------------------------------------+------+------|
|
|
| ~Dw~ | Ground motion | 3 | [m] |
|
|
| ~Fg~ | External force applied on granite | 3 | [N] |
|
|
| ~Fs~ | External force applied on the Sample | 3 | [N] |
|
|
|
|
*** Measurement Noise
|
|
|
|
#+name: tab:noise_names
|
|
#+caption: List of Measurement Noise
|
|
| Variable | Meaning | Size | Unit |
|
|
|----------+---------+------+------|
|
|
| | | | |
|
|
|
|
*** Control Inputs
|
|
|
|
#+name: tab:control_inputs_names
|
|
#+caption: List of Control Inputs
|
|
| Variable | Meaning | Size | Unit |
|
|
|----------+-------------------------------------------+------+----------|
|
|
| ~Fy~ | Actuation force for Ty | 1 | [N] |
|
|
| ~Dy~ | Imposed displacement for Ty | 1 | [m] |
|
|
|----------+-------------------------------------------+------+----------|
|
|
| ~My~ | Actuation torque for Ry | 1 | [N.m] |
|
|
| ~Ry~ | Imposed rotation for Ry | 1 | [rad] |
|
|
|----------+-------------------------------------------+------+----------|
|
|
| ~Mz~ | Actuation torque for Rz | 1 | [N.m] |
|
|
| ~Rz~ | Imposed rotation for Rz | 1 | [rad] |
|
|
|----------+-------------------------------------------+------+----------|
|
|
| ~Fh~ | Actuation force/torque for hexapod (cart) | 6 | [N, N.m] |
|
|
| ~Fhl~ | Actuation force/torque for hexapod (legs) | 6 | [N] |
|
|
| ~Dh~ | Imposed position for hexapod (cart) | 6 | [m, rad] |
|
|
|----------+-------------------------------------------+------+----------|
|
|
| ~Rm~ | Position of the two masses | 2 | [rad] |
|
|
|----------+-------------------------------------------+------+----------|
|
|
| ~Fn~ | Actuation force for the NASS (cart) | 6 | [N, N.m] |
|
|
| ~Fnl~ | Actuation force for the NASS's legs | 6 | [N] |
|
|
| ~Dn~ | Imposed position for the NASS (cart) | 6 | [m, rad] |
|
|
|
|
** List of Outputs
|
|
|
|
#+name: tab:outputs_names
|
|
#+caption: List of Outputs
|
|
| Variable | Meaning | Size | Unit |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Dgm~ | Absolute displacement of the granite | 3 | [m] |
|
|
| ~Vgm~ | Absolute Velocity of the granite | 3 | [m/s] |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Dym~ | Measured displacement of Ty | 1 | [m] |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Rym~ | Measured rotation of Ry | 1 | [rad] |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Rzm~ | Measured rotation of Rz | 1 | [rad] |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Dhm~ | Measured position of hexapod (cart) | 6 | [m, rad] |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Fnlm~ | Measured force of NASS's legs | 6 | [N] |
|
|
| ~Dnlm~ | Measured elongation of NASS's legs | 6 | [m] |
|
|
| ~Dnm~ | Measured position of NASS w.r.t NASS's base | 6 | [m, rad] |
|
|
| ~Vnm~ | Measured absolute velocity of NASS platform | 6 | [m/s, rad/s] |
|
|
| ~Vnlm~ | Measured absolute velocity of NASS's legs | 6 | [m/s] |
|
|
|----------+---------------------------------------------+------+--------------|
|
|
| ~Dsm~ | Position of Sample w.r.t. granite frame | 6 | [m, rad] |
|
|
|
|
* Simulink Library
|
|
<<sec:simulink_library>>
|
|
|
|
A simulink library is developed in order to share elements between the different simulink files.
|
|
|
|
** =inputs=
|
|
[[../nass_library/inputs.slx][inputs.slx]]
|
|
|
|
** =nass_library=
|
|
[[../nass_library/nass_library.slx][nass_library.slx]]
|
|
|
|
** =pos_error_wrt_nass_base=
|
|
[[../nass_library/pos_error_wrt_nass_base.slx][pos_error_wrt_nass_base.slx]]
|
|
|
|
** =QuaternionToAngles=
|
|
[[../nass_library/QuaternionToAngles.slx][QuaternionToAngles.slx]]
|
|
|
|
** =RotationMatrixToAngle=
|
|
[[../nass_library/RotationMatrixToAngle.slx][RotationMatrixToAngle.slx]]
|
|
|
|
* Scripts
|
|
<<sec:scripts>>
|
|
|
|
** Simulation Initialization
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/init_simulation.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:init_simulation>>
|
|
|
|
This Matlab script is accessible [[file:../src/init_simulation.m][here]].
|
|
|
|
This script runs just before the simulation is started.
|
|
It is used to load the simulation configuration and the controllers used for the simulation.
|
|
|
|
#+begin_src matlab
|
|
%% Load all the data used for the simulation
|
|
load('sim_conf.mat');
|
|
|
|
%% Load Controller
|
|
load('controllers.mat');
|
|
#+end_src
|
|
|
|
* Functions
|
|
<<sec:functions>>
|
|
|
|
** computePsdDispl
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/computePsdDispl.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:computePsdDispl>>
|
|
|
|
This Matlab function is accessible [[file:../src/computePsdDispl.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [psd_object] = computePsdDispl(sys_data, t_init, n_av)
|
|
i_init = find(sys_data.time > t_init, 1);
|
|
|
|
han_win = hanning(ceil(length(sys_data.Dx(i_init:end, :))/n_av));
|
|
Fs = 1/sys_data.time(2);
|
|
|
|
[pdx, f] = pwelch(sys_data.Dx(i_init:end, :), han_win, [], [], Fs);
|
|
[pdy, ~] = pwelch(sys_data.Dy(i_init:end, :), han_win, [], [], Fs);
|
|
[pdz, ~] = pwelch(sys_data.Dz(i_init:end, :), han_win, [], [], Fs);
|
|
|
|
[prx, ~] = pwelch(sys_data.Rx(i_init:end, :), han_win, [], [], Fs);
|
|
[pry, ~] = pwelch(sys_data.Ry(i_init:end, :), han_win, [], [], Fs);
|
|
[prz, ~] = pwelch(sys_data.Rz(i_init:end, :), han_win, [], [], Fs);
|
|
|
|
psd_object = struct(...
|
|
'f', f, ...
|
|
'dx', pdx, ...
|
|
'dy', pdy, ...
|
|
'dz', pdz, ...
|
|
'rx', prx, ...
|
|
'ry', pry, ...
|
|
'rz', prz);
|
|
end
|
|
#+end_src
|
|
|
|
** computeSetpoint
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/computeSetpoint.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:computeSetpoint>>
|
|
|
|
This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function setpoint = computeSetpoint(ty, ry, rz)
|
|
%%
|
|
setpoint = zeros(6, 1);
|
|
|
|
%% Ty
|
|
Ty = [1 0 0 0 ;
|
|
0 1 0 ty ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
% Tyinv = [1 0 0 0 ;
|
|
% 0 1 0 -ty ;
|
|
% 0 0 1 0 ;
|
|
% 0 0 0 1 ];
|
|
|
|
%% Ry
|
|
Ry = [ cos(ry) 0 sin(ry) 0 ;
|
|
0 1 0 0 ;
|
|
-sin(ry) 0 cos(ry) 0 ;
|
|
0 0 0 1 ];
|
|
|
|
% TMry = Ty*Ry*Tyinv;
|
|
|
|
%% Rz
|
|
Rz = [cos(rz) -sin(rz) 0 0 ;
|
|
sin(rz) cos(rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
% TMrz = Ty*TMry*Rz*TMry'*Tyinv;
|
|
|
|
%% All stages
|
|
% TM = TMrz*TMry*Ty;
|
|
|
|
TM = Ty*Ry*Rz;
|
|
|
|
[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
|
|
|
|
setpoint(1:3) = TM(1:3, 4);
|
|
setpoint(4:6) = [thetax, thetay, thetaz];
|
|
|
|
%% Custom Functions
|
|
function [thetax, thetay, thetaz] = RM2angle(R)
|
|
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
|
|
thetay = -asin(R(3, 1));
|
|
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
|
|
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
|
|
else
|
|
thetaz = 0;
|
|
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
|
|
thetay = pi/2;
|
|
thetax = thetaz + atan2(R(1, 2), R(1, 3));
|
|
else
|
|
thetay = -pi/2;
|
|
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
|
|
end
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** converErrorBasis
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/converErrorBasis.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:converErrorBasis>>
|
|
|
|
This Matlab function is accessible [[file:../src/converErrorBasis.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
|
|
% convertErrorBasis -
|
|
%
|
|
% Syntax: convertErrorBasis(p_error, ty, ry, rz)
|
|
%
|
|
% Inputs:
|
|
% - p_error - Position error of the sample w.r.t. the granite [m, rad]
|
|
% - ty - Measured translation of the Ty stage [m]
|
|
% - ry - Measured rotation of the Ry stage [rad]
|
|
% - rz - Measured rotation of the Rz stage [rad]
|
|
%
|
|
% Outputs:
|
|
% - P_nass - Position error of the sample w.r.t. the NASS base [m]
|
|
% - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
|
|
%
|
|
% Example:
|
|
%
|
|
|
|
%% If line vector => column vector
|
|
if size(pos, 2) == 6
|
|
pos = pos';
|
|
end
|
|
|
|
if size(setpoint, 2) == 6
|
|
setpoint = setpoint';
|
|
end
|
|
|
|
%% Position of the sample in the frame fixed to the Granite
|
|
P_granite = [pos(1:3); 1]; % Position [m]
|
|
R_granite = [setpoint(1:3); 1]; % Reference [m]
|
|
|
|
%% Transformation matrices of the stages
|
|
% T-y
|
|
TMty = [1 0 0 0 ;
|
|
0 1 0 ty ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
% R-y
|
|
TMry = [ cos(ry) 0 sin(ry) 0 ;
|
|
0 1 0 0 ;
|
|
-sin(ry) 0 cos(ry) 0 ;
|
|
0 0 0 1 ];
|
|
|
|
% R-z
|
|
TMrz = [cos(rz) -sin(rz) 0 0 ;
|
|
sin(rz) cos(rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
%% Compute Point coordinates in the new reference fixed to the NASS base
|
|
% P_nass = TMrz*TMry*TMty*P_granite;
|
|
P_nass = TMrz\TMry\TMty\P_granite;
|
|
R_nass = TMrz\TMry\TMty\R_granite;
|
|
|
|
dx = R_nass(1)-P_nass(1);
|
|
dy = R_nass(2)-P_nass(2);
|
|
dz = R_nass(3)-P_nass(3);
|
|
|
|
%% Compute new basis vectors linked to the NASS base
|
|
% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
|
|
% ux_nass = ux_nass(1:3);
|
|
% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
|
|
% uy_nass = uy_nass(1:3);
|
|
% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
|
|
% uz_nass = uz_nass(1:3);
|
|
|
|
ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
|
|
ux_nass = ux_nass(1:3);
|
|
uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
|
|
uy_nass = uy_nass(1:3);
|
|
uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
|
|
uz_nass = uz_nass(1:3);
|
|
|
|
%% Rotations error w.r.t. granite Frame
|
|
% Rotations error w.r.t. granite Frame
|
|
rx_nass = pos(4);
|
|
ry_nass = pos(5);
|
|
rz_nass = pos(6);
|
|
|
|
% Rotation matrices of the Sample w.r.t. the Granite
|
|
Mrx_error = [1 0 0 ;
|
|
0 cos(-rx_nass) -sin(-rx_nass) ;
|
|
0 sin(-rx_nass) cos(-rx_nass)];
|
|
|
|
Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
|
|
0 1 0 ;
|
|
-sin(-ry_nass) 0 cos(-ry_nass)];
|
|
|
|
Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
|
|
sin(-rz_nass) cos(-rz_nass) 0 ;
|
|
0 0 1];
|
|
|
|
% Rotation matrix of the Sample w.r.t. the Granite
|
|
Mr_error = Mrz_error*Mry_error*Mrx_error;
|
|
|
|
%% Use matrix to solve
|
|
R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample
|
|
|
|
[thetax, thetay, thetaz] = RM2angle(R);
|
|
|
|
error_nass = [dx; dy; dz; thetax; thetay; thetaz];
|
|
|
|
%% Custom Functions
|
|
function [thetax, thetay, thetaz] = RM2angle(R)
|
|
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
|
|
thetay = -asin(R(3, 1));
|
|
% thetaybis = pi-thetay;
|
|
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
|
|
% thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
|
|
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
|
|
% thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
|
|
else
|
|
thetaz = 0;
|
|
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
|
|
thetay = pi/2;
|
|
thetax = thetaz + atan2(R(1, 2), R(1, 3));
|
|
else
|
|
thetay = -pi/2;
|
|
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
|
|
end
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
** generateDiagPidControl
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/generateDiagPidControl.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:generateDiagPidControl>>
|
|
|
|
This Matlab function is accessible [[file:../src/generateDiagPidControl.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [K] = generateDiagPidControl(G, fs)
|
|
%%
|
|
pid_opts = pidtuneOptions(...
|
|
'PhaseMargin', 50, ...
|
|
'DesignFocus', 'disturbance-rejection');
|
|
|
|
%%
|
|
K = tf(zeros(6));
|
|
|
|
for i = 1:6
|
|
input_name = G.InputName(i);
|
|
output_name = G.OutputName(i);
|
|
K(i, i) = tf(pidtune(minreal(G(output_name, input_name)), 'PIDF', 2*pi*fs, pid_opts));
|
|
end
|
|
|
|
K.InputName = G.OutputName;
|
|
K.OutputName = G.InputName;
|
|
end
|
|
#+end_src
|
|
** identifyPlant
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/identifyPlant.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:identifyPlant>>
|
|
|
|
This Matlab function is accessible [[file:../src/identifyPlant.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [sys] = identifyPlant(opts_param)
|
|
%% Default values for opts
|
|
opts = struct();
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'sim_nano_station_id';
|
|
|
|
%% Input/Output definition
|
|
io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
|
|
io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
|
|
io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
|
|
io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
|
|
io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces
|
|
io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
|
|
io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
|
|
io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
|
|
io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
|
|
io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, options);
|
|
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
|
|
'Dgx', 'Dgy', 'Dgz', ...
|
|
'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
|
|
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
|
|
'Frzz', 'Ftyx', 'Ftyz'};
|
|
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
|
|
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
|
|
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
|
|
'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
|
|
'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
|
|
|
%% Create the sub transfer functions
|
|
minreal_tol = sqrt(eps);
|
|
% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
|
|
sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
|
% From ground motion to Sample displacement
|
|
sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
|
|
% From direct forces applied on the sample to displacement of the sample
|
|
sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
|
|
% From forces applied on NASS's legs to force sensor in each leg
|
|
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
% From forces applied on NASS's legs to displacement of each leg
|
|
sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
% From forces/torques applied by the NASS to position error
|
|
sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
|
% From forces/torques applied by the NASS to velocity of the actuator
|
|
sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
% From various disturbance forces to position error
|
|
sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
|
|
|
|
%% We remove low frequency and high frequency dynamics that are usually unstable
|
|
% using =freqsep= is risky as it may change the shape of the transfer functions
|
|
% f_min = 0.1; % [Hz]
|
|
% f_max = 1e4; % [Hz]
|
|
|
|
% [~, sys.G_cart] = freqsep(freqsep(sys.G_cart, 2*pi*f_max), 2*pi*f_min);
|
|
% [~, sys.G_gm] = freqsep(freqsep(sys.G_gm, 2*pi*f_max), 2*pi*f_min);
|
|
% [~, sys.G_fs] = freqsep(freqsep(sys.G_fs, 2*pi*f_max), 2*pi*f_min);
|
|
% [~, sys.G_iff] = freqsep(freqsep(sys.G_iff, 2*pi*f_max), 2*pi*f_min);
|
|
% [~, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, 2*pi*f_max), 2*pi*f_min);
|
|
% [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min);
|
|
|
|
%% We finally verify that the system is stable
|
|
if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant)
|
|
warning('One of the identified system is unstable');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** runSimulation
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/runSimulation.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:runSimulation>>
|
|
|
|
This Matlab function is accessible [[file:../src/runSimulation.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [] = runSimulation(sys_name, sys_mass, ctrl_type, act_damp)
|
|
%% Load the controller and save it for the simulation
|
|
if strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'none')
|
|
K_obj = load('./mat/K_fb.mat');
|
|
K = K_obj.(sprintf('K_%s_%s', sys_mass, sys_name)); %#ok
|
|
save('./mat/controllers.mat', 'K');
|
|
elseif strcmp(ctrl_type, 'cl') && strcmp(act_damp, 'iff')
|
|
K_obj = load('./mat/K_fb_iff.mat');
|
|
K = K_obj.(sprintf('K_%s_%s_iff', sys_mass, sys_name)); %#ok
|
|
save('./mat/controllers.mat', 'K');
|
|
elseif strcmp(ctrl_type, 'ol')
|
|
K = tf(zeros(6)); %#ok
|
|
save('./mat/controllers.mat', 'K');
|
|
else
|
|
error('ctrl_type should be cl or ol');
|
|
end
|
|
|
|
%% Active Damping
|
|
if strcmp(act_damp, 'iff')
|
|
K_iff_crit = load('./mat/K_iff_crit.mat');
|
|
K_iff = K_iff_crit.(sprintf('K_iff_%s_%s', sys_mass, sys_name)); %#ok
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
elseif strcmp(act_damp, 'none')
|
|
K_iff = tf(zeros(6)); %#ok
|
|
save('./mat/controllers.mat', 'K_iff', '-append');
|
|
end
|
|
|
|
%%
|
|
if strcmp(sys_name, 'pz')
|
|
initializeNanoHexapod(struct('actuator', 'piezo'));
|
|
elseif strcmp(sys_name, 'vc')
|
|
initializeNanoHexapod(struct('actuator', 'lorentz'));
|
|
else
|
|
error('sys_name should be pz or vc');
|
|
end
|
|
|
|
if strcmp(sys_mass, 'light')
|
|
initializeSample(struct('mass', 1));
|
|
elseif strcmp(sys_mass, 'heavy')
|
|
initializeSample(struct('mass', 50));
|
|
else
|
|
error('sys_mass should be light or heavy');
|
|
end
|
|
|
|
%% Run the simulation
|
|
sim('sim_nano_station_ctrl.slx');
|
|
|
|
%% Split the Dsample matrix into vectors
|
|
[Dx, Dy, Dz, Rx, Ry, Rz] = matSplit(Es.Data, 1); %#ok
|
|
time = Dsample.Time; %#ok
|
|
|
|
%% Save the result
|
|
filename = sprintf('sim_%s_%s_%s_%s', sys_mass, sys_name, ctrl_type, act_damp);
|
|
save(sprintf('./mat/%s.mat', filename), ...
|
|
'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
|
|
end
|
|
#+end_src
|
|
* Initialize Elements
|
|
<<sec:initialize_elements>>
|
|
** Simulation Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeSimConf.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeSimConf>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeSimConf.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [] = initializeSimConf(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('Ts', 1e-4, ... % Sampling time [s]
|
|
'Tsim', 10, ... % Simulation time [s]
|
|
'cl_time', 0, ... % Close Loop time [s]
|
|
'gravity', false ... % Gravity along the z axis
|
|
);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
sim_conf = struct();
|
|
|
|
%%
|
|
sim_conf.Ts = opts.Ts;
|
|
sim_conf.Tsim = opts.Tsim;
|
|
sim_conf.cl_time = opts.cl_time;
|
|
|
|
%% Gravity
|
|
if opts.gravity
|
|
sim_conf.g = -9.8; %#ok
|
|
else
|
|
sim_conf.g = 0; %#ok
|
|
end
|
|
|
|
%% Save
|
|
save('./mat/sim_conf.mat', 'sim_conf');
|
|
end
|
|
#+end_src
|
|
|
|
** Experiment
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeExperiment.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeExperiment>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeExperiment.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [] = initializeExperiment(exp_name, sys_mass)
|
|
if strcmp(exp_name, 'tomography')
|
|
opts_sim = struct(...
|
|
'Tsim', 5, ...
|
|
'cl_time', 5 ...
|
|
);
|
|
initializeSimConf(opts_sim);
|
|
|
|
if strcmp(sys_mass, 'light')
|
|
opts_inputs = struct(...
|
|
'Dw', true, ...
|
|
'Rz', 60 ... % rpm
|
|
);
|
|
elseif strcpm(sys_mass, 'heavy')
|
|
opts_inputs = struct(...
|
|
'Dw', true, ...
|
|
'Rz', 1 ... % rpm
|
|
);
|
|
else
|
|
error('sys_mass should be light or heavy');
|
|
end
|
|
|
|
initializeInputs(opts_inputs);
|
|
else
|
|
error('exp_name is only configured for tomography');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Inputs
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeInputs.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeInputs>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [inputs] = initializeInputs(opts_param)
|
|
%% Default values for opts
|
|
opts = struct( ...
|
|
'Dw', false, ...
|
|
'Dy', false, ...
|
|
'Ry', false, ...
|
|
'Rz', false, ...
|
|
'Dh', false, ...
|
|
'Rm', false, ...
|
|
'Dn', false ...
|
|
);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%% Load Sampling Time and Simulation Time
|
|
load('./mat/sim_conf.mat', 'sim_conf');
|
|
|
|
%% Define the time vector
|
|
t = 0:sim_conf.Ts:sim_conf.Tsim;
|
|
|
|
%% Ground motion - Dw
|
|
if islogical(opts.Dw) && opts.Dw == true
|
|
load('./mat/perturbations.mat', 'Wxg');
|
|
Dw = 1/sqrt(2)*100*random('norm', 0, 1, length(t), 3);
|
|
Dw(:, 1) = lsim(Wxg, Dw(:, 1), t);
|
|
Dw(:, 2) = lsim(Wxg, Dw(:, 2), t);
|
|
Dw(:, 3) = lsim(Wxg, Dw(:, 3), t);
|
|
elseif islogical(opts.Dw) && opts.Dw == false
|
|
Dw = zeros(length(t), 3);
|
|
else
|
|
Dw = opts.Dw;
|
|
end
|
|
|
|
%% Translation stage - Dy
|
|
if islogical(opts.Dy) && opts.Dy == true
|
|
Dy = zeros(length(t), 1);
|
|
elseif islogical(opts.Dy) && opts.Dy == false
|
|
Dy = zeros(length(t), 1);
|
|
else
|
|
Dy = opts.Dy;
|
|
end
|
|
|
|
%% Tilt Stage - Ry
|
|
if islogical(opts.Ry) && opts.Ry == true
|
|
Ry = 3*(2*pi/360)*sin(2*pi*0.2*t);
|
|
elseif islogical(opts.Ry) && opts.Ry == false
|
|
Ry = zeros(length(t), 1);
|
|
elseif isnumeric(opts.Ry) && length(opts.Ry) == 1
|
|
Ry = opts.Ry*(2*pi/360)*ones(length(t), 1);
|
|
else
|
|
Ry = opts.Ry;
|
|
end
|
|
|
|
%% Spindle - Rz
|
|
if islogical(opts.Rz) && opts.Rz == true
|
|
Rz = 2*pi*0.5*t;
|
|
elseif islogical(opts.Rz) && opts.Rz == false
|
|
Rz = zeros(length(t), 1);
|
|
elseif isnumeric(opts.Rz) && length(opts.Rz) == 1
|
|
Rz = opts.Rz*(2*pi/60)*t;
|
|
else
|
|
Rz = opts.Rz;
|
|
end
|
|
|
|
%% Micro Hexapod - Dh
|
|
if islogical(opts.Dh) && opts.Dh == true
|
|
Dh = zeros(length(t), 6);
|
|
elseif islogical(opts.Dh) && opts.Dh == false
|
|
Dh = zeros(length(t), 6);
|
|
else
|
|
Dh = opts.Dh;
|
|
end
|
|
|
|
%% Axis Compensation - Rm
|
|
if islogical(opts.Rm)
|
|
Rm = zeros(length(t), 2);
|
|
Rm(:, 2) = pi*ones(length(t), 1);
|
|
else
|
|
Rm = opts.Rm;
|
|
end
|
|
|
|
%% Nano Hexapod - Dn
|
|
if islogical(opts.Dn) && opts.Dn == true
|
|
Dn = zeros(length(t), 6);
|
|
elseif islogical(opts.Dn) && opts.Dn == false
|
|
Dn = zeros(length(t), 6);
|
|
else
|
|
Dn = opts.Dn;
|
|
end
|
|
|
|
%% Setpoint - Ds
|
|
Ds = zeros(length(t), 6);
|
|
for i = 1:length(t)
|
|
Ds(i, :) = computeSetpoint(Dy(i), Ry(i), Rz(i));
|
|
end
|
|
|
|
%% External Forces applied on the Granite
|
|
Fg = zeros(length(t), 3);
|
|
|
|
%% External Forces applied on the Sample
|
|
Fs = zeros(length(t), 6);
|
|
|
|
%% Create the input Structure that will contain all the inputs
|
|
inputs = struct( ...
|
|
'Ts', sim_conf.Ts, ...
|
|
'Dw', timeseries(Dw, t), ...
|
|
'Dy', timeseries(Dy, t), ...
|
|
'Ry', timeseries(Ry, t), ...
|
|
'Rz', timeseries(Rz, t), ...
|
|
'Dh', timeseries(Dh, t), ...
|
|
'Rm', timeseries(Rm, t), ...
|
|
'Dn', timeseries(Dn, t), ...
|
|
'Ds', timeseries(Ds, t), ...
|
|
'Fg', timeseries(Fg, t), ...
|
|
'Fs', timeseries(Fs, t) ...
|
|
);
|
|
|
|
%% Save
|
|
save('./mat/inputs.mat', 'inputs');
|
|
|
|
%% Custom Functions
|
|
function setpoint = computeSetpoint(ty, ry, rz)
|
|
%%
|
|
setpoint = zeros(6, 1);
|
|
|
|
%% Ty
|
|
TMTy = [1 0 0 0 ;
|
|
0 1 0 ty ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
%% Ry
|
|
TMRy = [ cos(ry) 0 sin(ry) 0 ;
|
|
0 1 0 0 ;
|
|
-sin(ry) 0 cos(ry) 0 ;
|
|
0 0 0 1 ];
|
|
|
|
%% Rz
|
|
TMRz = [cos(rz) -sin(rz) 0 0 ;
|
|
sin(rz) cos(rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
%% All stages
|
|
TM = TMTy*TMRy*TMRz;
|
|
|
|
[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));
|
|
|
|
setpoint(1:3) = TM(1:3, 4);
|
|
setpoint(4:6) = [thetax, thetay, thetaz];
|
|
|
|
%% Custom Functions
|
|
function [thetax, thetay, thetaz] = RM2angle(R)
|
|
if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
|
|
thetay = -asin(R(3, 1));
|
|
thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
|
|
thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
|
|
else
|
|
thetaz = 0;
|
|
if abs(R(3, 1)+1) < 1e-6 % R31 = -1
|
|
thetay = pi/2;
|
|
thetax = thetaz + atan2(R(1, 2), R(1, 3));
|
|
else
|
|
thetay = -pi/2;
|
|
thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Ground
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeGround.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeGround>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [ground] = initializeGround()
|
|
%%
|
|
ground = struct();
|
|
|
|
ground.shape = [2, 2, 0.5]; % [m]
|
|
ground.density = 2800; % [kg/m3]
|
|
ground.color = [0.5, 0.5, 0.5];
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'ground', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Granite
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeGranite.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeGranite>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeGranite.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [granite] = initializeGranite(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('rigid', false);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
granite = struct();
|
|
|
|
%% Static Properties
|
|
granite.density = 2800; % [kg/m3]
|
|
granite.volume = 0.749; % [m3] TODO - should
|
|
granite.mass = granite.density*granite.volume; % [kg]
|
|
granite.color = [1 1 1];
|
|
granite.STEP = './STEPS/granite/granite.STEP';
|
|
|
|
granite.mass_top = 4000; % [kg] TODO
|
|
|
|
%% Dynamical Properties
|
|
if opts.rigid
|
|
granite.k.x = 1e12; % [N/m]
|
|
granite.k.y = 1e12; % [N/m]
|
|
granite.k.z = 1e12; % [N/m]
|
|
granite.k.rx = 1e10; % [N*m/deg]
|
|
granite.k.ry = 1e10; % [N*m/deg]
|
|
granite.k.rz = 1e10; % [N*m/deg]
|
|
else
|
|
granite.k.x = 4e9; % [N/m]
|
|
granite.k.y = 3e8; % [N/m]
|
|
granite.k.z = 8e8; % [N/m]
|
|
granite.k.rx = 1e4; % [N*m/deg]
|
|
granite.k.ry = 1e4; % [N*m/deg]
|
|
granite.k.rz = 1e6; % [N*m/deg]
|
|
end
|
|
|
|
granite.c.x = 0.1*sqrt(granite.mass_top*granite.k.x); % [N/(m/s)]
|
|
granite.c.y = 0.1*sqrt(granite.mass_top*granite.k.y); % [N/(m/s)]
|
|
granite.c.z = 0.5*sqrt(granite.mass_top*granite.k.z); % [N/(m/s)]
|
|
granite.c.rx = 0.1*sqrt(granite.mass_top*granite.k.rx); % [N*m/(deg/s)]
|
|
granite.c.ry = 0.1*sqrt(granite.mass_top*granite.k.ry); % [N*m/(deg/s)]
|
|
granite.c.rz = 0.1*sqrt(granite.mass_top*granite.k.rz); % [N*m/(deg/s)]
|
|
|
|
%% Positioning parameters
|
|
granite.sample_pos = 0.8; % Z-offset for the initial position of the sample [m]
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'granite', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Translation Stage
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeTy.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeTy>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeTy.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [ty] = initializeTy(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('rigid', false);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
ty = struct();
|
|
|
|
%% Y-Translation - Static Properties
|
|
% Ty Granite frame
|
|
ty.granite_frame.density = 7800; % [kg/m3]
|
|
ty.granite_frame.color = [0.753 1 0.753];
|
|
ty.granite_frame.STEP = './STEPS/Ty/Ty_Granite_Frame.STEP';
|
|
% Guide Translation Ty
|
|
ty.guide.density = 7800; % [kg/m3]
|
|
ty.guide.color = [0.792 0.820 0.933];
|
|
ty.guide.STEP = './STEPS/ty/Ty_Guide.STEP';
|
|
% Ty - Guide_Translation12
|
|
ty.guide12.density = 7800; % [kg/m3]
|
|
ty.guide12.color = [0.792 0.820 0.933];
|
|
ty.guide12.STEP = './STEPS/Ty/Ty_Guide_12.STEP';
|
|
% Ty - Guide_Translation11
|
|
ty.guide11.density = 7800; % [kg/m3]
|
|
ty.guide11.color = [0.792 0.820 0.933];
|
|
ty.guide11.STEP = './STEPS/ty/Ty_Guide_11.STEP';
|
|
% Ty - Guide_Translation22
|
|
ty.guide22.density = 7800; % [kg/m3]
|
|
ty.guide22.color = [0.792 0.820 0.933];
|
|
ty.guide22.STEP = './STEPS/ty/Ty_Guide_22.STEP';
|
|
% Ty - Guide_Translation21
|
|
ty.guide21.density = 7800; % [kg/m3]
|
|
ty.guide21.color = [0.792 0.820 0.933];
|
|
ty.guide21.STEP = './STEPS/Ty/Ty_Guide_21.STEP';
|
|
% Ty - Plateau translation
|
|
ty.frame.density = 7800; % [kg/m3]
|
|
ty.frame.color = [0.792 0.820 0.933];
|
|
ty.frame.STEP = './STEPS/ty/Ty_Stage.STEP';
|
|
% Ty Stator Part
|
|
ty.stator.density = 5400; % [kg/m3]
|
|
ty.stator.color = [0.792 0.820 0.933];
|
|
ty.stator.STEP = './STEPS/ty/Ty_Motor_Stator.STEP';
|
|
% Ty Rotor Part
|
|
ty.rotor.density = 5400; % [kg/m3]
|
|
ty.rotor.color = [0.792 0.820 0.933];
|
|
ty.rotor.STEP = './STEPS/ty/Ty_Motor_Rotor.STEP';
|
|
|
|
ty.m = 1000; % TODO [kg]
|
|
|
|
%% Y-Translation - Dynamicals Properties
|
|
if opts.rigid
|
|
ty.k.ax = 1e12; % Axial Stiffness for each of the 4 guidance (y) [N/m]
|
|
ty.k.rad = 1e12; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
|
|
else
|
|
ty.k.ax = 5e8; % Axial Stiffness for each of the 4 guidance (y) [N/m]
|
|
ty.k.rad = 5e7; % Radial Stiffness for each of the 4 guidance (x-z) [N/m]
|
|
end
|
|
|
|
ty.c.ax = 0.1*sqrt(ty.k.ax*ty.m);
|
|
ty.c.rad = 0.1*sqrt(ty.k.rad*ty.m);
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'ty', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Tilt Stage
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeRy.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeRy>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeRy.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [ry] = initializeRy(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('rigid', false);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
ry = struct();
|
|
|
|
%% Tilt Stage - Static Properties
|
|
% Ry - Guide for the tilt stage
|
|
ry.guide.density = 7800; % [kg/m3]
|
|
ry.guide.color = [0.792 0.820 0.933];
|
|
ry.guide.STEP = './STEPS/ry/Tilt_Guide.STEP';
|
|
% Ry - Rotor of the motor
|
|
ry.rotor.density = 2400; % [kg/m3]
|
|
ry.rotor.color = [0.792 0.820 0.933];
|
|
ry.rotor.STEP = './STEPS/ry/Tilt_Motor_Axis.STEP';
|
|
% Ry - Motor
|
|
ry.motor.density = 3200; % [kg/m3]
|
|
ry.motor.color = [0.792 0.820 0.933];
|
|
ry.motor.STEP = './STEPS/ry/Tilt_Motor.STEP';
|
|
% Ry - Plateau Tilt
|
|
ry.stage.density = 7800; % [kg/m3]
|
|
ry.stage.color = [0.792 0.820 0.933];
|
|
ry.stage.STEP = './STEPS/ry/Tilt_Stage.STEP';
|
|
|
|
ry.m = 800; % TODO [kg]
|
|
|
|
%% Tilt Stage - Dynamical Properties
|
|
if opts.rigid
|
|
ry.k.tilt = 1e10; % Rotation stiffness around y [N*m/deg]
|
|
ry.k.h = 1e12; % Stiffness in the direction of the guidance [N/m]
|
|
ry.k.rad = 1e12; % Stiffness in the top direction [N/m]
|
|
ry.k.rrad = 1e12; % Stiffness in the side direction [N/m]
|
|
else
|
|
ry.k.tilt = 1e4; % Rotation stiffness around y [N*m/deg]
|
|
ry.k.h = 1e8; % Stiffness in the direction of the guidance [N/m]
|
|
ry.k.rad = 1e8; % Stiffness in the top direction [N/m]
|
|
ry.k.rrad = 1e8; % Stiffness in the side direction [N/m]
|
|
end
|
|
|
|
ry.c.h = 0.1*sqrt(ry.k.h*ry.m);
|
|
ry.c.rad = 0.1*sqrt(ry.k.rad*ry.m);
|
|
ry.c.rrad = 0.1*sqrt(ry.k.rrad*ry.m);
|
|
ry.c.tilt = 0.1*sqrt(ry.k.tilt*ry.m);
|
|
|
|
%% Positioning parameters
|
|
ry.z_offset = 0.58178; % Z-Offset so that the center of rotation matches the sample center [m]
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'ry', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Spindle
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeRz.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeRz>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeRz.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [rz] = initializeRz(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('rigid', false);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
rz = struct();
|
|
|
|
%% Spindle - Static Properties
|
|
% Spindle - Slip Ring
|
|
rz.slipring.density = 7800; % [kg/m3]
|
|
rz.slipring.color = [0.792 0.820 0.933];
|
|
rz.slipring.STEP = './STEPS/rz/Spindle_Slip_Ring.STEP';
|
|
% Spindle - Rotor
|
|
rz.rotor.density = 7800; % [kg/m3]
|
|
rz.rotor.color = [0.792 0.820 0.933];
|
|
rz.rotor.STEP = './STEPS/rz/Spindle_Rotor.STEP';
|
|
% Spindle - Stator
|
|
rz.stator.density = 7800; % [kg/m3]
|
|
rz.stator.color = [0.792 0.820 0.933];
|
|
rz.stator.STEP = './STEPS/rz/Spindle_Stator.STEP';
|
|
|
|
% Estimated mass of the mooving part
|
|
rz.m = 250; % [kg]
|
|
|
|
%% Spindle - Dynamical Properties
|
|
|
|
if opts.rigid
|
|
rz.k.rot = 1e10; % Rotational Stiffness (Rz) [N*m/deg]
|
|
rz.k.tilt = 1e10; % Rotational Stiffness (Rx, Ry) [N*m/deg]
|
|
rz.k.ax = 1e12; % Axial Stiffness (Z) [N/m]
|
|
rz.k.rad = 1e12; % Radial Stiffness (X, Y) [N/m]
|
|
else
|
|
rz.k.rot = 1e6; % TODO - Rotational Stiffness (Rz) [N*m/deg]
|
|
rz.k.tilt = 1e6; % Rotational Stiffness (Rx, Ry) [N*m/deg]
|
|
rz.k.ax = 2e9; % Axial Stiffness (Z) [N/m]
|
|
rz.k.rad = 7e8; % Radial Stiffness (X, Y) [N/m]
|
|
end
|
|
|
|
% Damping
|
|
rz.c.ax = 0.1*sqrt(rz.k.ax*rz.m);
|
|
rz.c.rad = 0.1*sqrt(rz.k.rad*rz.m);
|
|
rz.c.tilt = 0.1*sqrt(rz.k.tilt*rz.m);
|
|
rz.c.rot = 0.1*sqrt(rz.k.rot*rz.m);
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'rz', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Micro Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeMicroHexapod.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeMicroHexapod>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [micro_hexapod] = initializeMicroHexapod(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('rigid', false);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%% Stewart Object
|
|
micro_hexapod = struct();
|
|
micro_hexapod.h = 350; % Total height of the platform [mm]
|
|
micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm]
|
|
|
|
%% Bottom Plate - Mechanical Design
|
|
BP = struct();
|
|
|
|
BP.rad.int = 110; % Internal Radius [mm]
|
|
BP.rad.ext = 207.5; % External Radius [mm]
|
|
BP.thickness = 26; % Thickness [mm]
|
|
BP.leg.rad = 175.5; % Radius where the legs articulations are positionned [mm]
|
|
BP.leg.ang = 9.5; % Angle Offset [deg]
|
|
BP.density = 8000; % Density of the material [kg/m^3]
|
|
BP.color = [0.6 0.6 0.6]; % Color [rgb]
|
|
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
|
|
|
|
%% Top Plate - Mechanical Design
|
|
TP = struct();
|
|
|
|
TP.rad.int = 82; % Internal Radius [mm]
|
|
TP.rad.ext = 150; % Internal Radius [mm]
|
|
TP.thickness = 26; % Thickness [mm]
|
|
TP.leg.rad = 118; % Radius where the legs articulations are positionned [mm]
|
|
TP.leg.ang = 12.1; % Angle Offset [deg]
|
|
TP.density = 8000; % Density of the material [kg/m^3]
|
|
TP.color = [0.6 0.6 0.6]; % Color [rgb]
|
|
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
|
|
|
|
%% Struts
|
|
Leg = struct();
|
|
|
|
Leg.stroke = 10e-3; % Maximum Stroke of each leg [m]
|
|
if opts.rigid
|
|
Leg.k.ax = 1e12; % Stiffness of each leg [N/m]
|
|
else
|
|
Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
|
|
end
|
|
Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) []
|
|
Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
|
|
Leg.rad.top = 17; % Radius of the cylinder of the top part [mm]
|
|
Leg.density = 8000; % Density of the material [kg/m^3]
|
|
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
|
|
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
|
|
|
|
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
|
|
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
|
|
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
|
|
Leg = updateDamping(Leg);
|
|
|
|
|
|
%% Sphere
|
|
SP = struct();
|
|
|
|
SP.height.bottom = 27; % [mm]
|
|
SP.height.top = 27; % [mm]
|
|
SP.density.bottom = 8000; % [kg/m^3]
|
|
SP.density.top = 8000; % [kg/m^3]
|
|
SP.color.bottom = [0.6 0.6 0.6]; % [rgb]
|
|
SP.color.top = [0.6 0.6 0.6]; % [rgb]
|
|
SP.k.ax = 0; % [N*m/deg]
|
|
SP.ksi.ax = 10;
|
|
|
|
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
|
|
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
|
|
SP.rad.bottom = Leg.sphere.bottom; % [mm]
|
|
SP.rad.top = Leg.sphere.top; % [mm]
|
|
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
|
|
|
|
SP = updateDamping(SP);
|
|
|
|
%%
|
|
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
|
|
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
|
|
|
|
%%
|
|
micro_hexapod.BP = BP;
|
|
micro_hexapod.TP = TP;
|
|
micro_hexapod.Leg = Leg;
|
|
micro_hexapod.SP = SP;
|
|
|
|
%%
|
|
micro_hexapod = initializeParameters(micro_hexapod);
|
|
|
|
%% 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
|
|
|
|
** Center of gravity compensation
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeAxisc.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeAxisc>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeAxisc.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [axisc] = initializeAxisc()
|
|
%%
|
|
axisc = struct();
|
|
|
|
%% Axis Compensator - Static Properties
|
|
% Structure
|
|
axisc.structure.density = 3400; % [kg/m3]
|
|
axisc.structure.color = [0.792 0.820 0.933];
|
|
axisc.structure.STEP = './STEPS/axisc/axisc_structure.STEP';
|
|
% Wheel
|
|
axisc.wheel.density = 2700; % [kg/m3]
|
|
axisc.wheel.color = [0.753 0.753 0.753];
|
|
axisc.wheel.STEP = './STEPS/axisc/axisc_wheel.STEP';
|
|
% Mass
|
|
axisc.mass.density = 7800; % [kg/m3]
|
|
axisc.mass.color = [0.792 0.820 0.933];
|
|
axisc.mass.STEP = './STEPS/axisc/axisc_mass.STEP';
|
|
% Gear
|
|
axisc.gear.density = 7800; % [kg/m3]
|
|
axisc.gear.color = [0.792 0.820 0.933];
|
|
axisc.gear.STEP = './STEPS/axisc/axisc_gear.STEP';
|
|
|
|
axisc.m = 40; % TODO [kg]
|
|
|
|
%% Axis Compensator - Dynamical Properties
|
|
% axisc.k.ax = 1; % TODO [N*m/deg)]
|
|
% axisc.c.ax = (1/1)*sqrt(axisc.k.ax/axisc.m);
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'axisc', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Mirror
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeMirror.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeMirror>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [] = initializeMirror(opts_param)
|
|
%% Default values for opts
|
|
opts = struct(...
|
|
'shape', 'spherical', ... % spherical or conical
|
|
'angle', 45 ...
|
|
);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
mirror = struct();
|
|
mirror.h = 50; % height of the mirror [mm]
|
|
mirror.thickness = 25; % Thickness of the plate supporting the sample [mm]
|
|
mirror.hole_rad = 120; % radius of the hole in the mirror [mm]
|
|
mirror.support_rad = 100; % radius of the support plate [mm]
|
|
mirror.jacobian = 150; % point of interest offset in z (above the top surfave) [mm]
|
|
mirror.rad = 180; % radius of the mirror (at the bottom surface) [mm]
|
|
|
|
mirror.density = 2400; % Density of the mirror [kg/m3]
|
|
mirror.color = [0.4 1.0 1.0]; % Color of the mirror
|
|
|
|
mirror.cone_length = mirror.rad*tand(opts.angle)+mirror.h+mirror.jacobian; % Distance from Apex point of the cone to jacobian point
|
|
|
|
%% Shape
|
|
mirror.shape = [...
|
|
0 mirror.h-mirror.thickness
|
|
mirror.hole_rad mirror.h-mirror.thickness; ...
|
|
mirror.hole_rad 0; ...
|
|
mirror.rad 0 ...
|
|
];
|
|
|
|
if strcmp(opts.shape, 'spherical')
|
|
mirror.sphere_radius = sqrt((mirror.jacobian+mirror.h)^2+mirror.rad^2); % Radius of the sphere [mm]
|
|
|
|
for z = linspace(0, mirror.h, 101)
|
|
mirror.shape = [mirror.shape; sqrt(mirror.sphere_radius^2-(z-mirror.jacobian-mirror.h)^2) z];
|
|
end
|
|
elseif strcmp(opts.shape, 'conical')
|
|
mirror.shape = [mirror.shape; mirror.rad+mirror.h/tand(opts.angle) mirror.h];
|
|
else
|
|
error('Shape should be either conical or spherical');
|
|
end
|
|
|
|
mirror.shape = [mirror.shape; 0 mirror.h];
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'mirror', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Nano Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeNanoHexapod.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeNanoHexapod>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [nano_hexapod] = initializeNanoHexapod(opts_param)
|
|
%% Default values for opts
|
|
opts = struct('actuator', 'piezo');
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%% Stewart Object
|
|
nano_hexapod = struct();
|
|
nano_hexapod.h = 90; % Total height of the platform [mm]
|
|
nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm]
|
|
% nano_hexapod.jacobian = 174.26; % Point where the Jacobian is computed => Center of rotation [mm]
|
|
|
|
%% Bottom Plate
|
|
BP = struct();
|
|
|
|
BP.rad.int = 0; % Internal Radius [mm]
|
|
BP.rad.ext = 150; % External Radius [mm]
|
|
BP.thickness = 10; % Thickness [mm]
|
|
BP.leg.rad = 100; % Radius where the legs articulations are positionned [mm]
|
|
BP.leg.ang = 5; % Angle Offset [deg]
|
|
BP.density = 8000;% Density of the material [kg/m^3]
|
|
BP.color = [0.7 0.7 0.7]; % Color [rgb]
|
|
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
|
|
|
|
%% Top Plate
|
|
TP = struct();
|
|
|
|
TP.rad.int = 0; % Internal Radius [mm]
|
|
TP.rad.ext = 100; % Internal Radius [mm]
|
|
TP.thickness = 10; % Thickness [mm]
|
|
TP.leg.rad = 90; % Radius where the legs articulations are positionned [mm]
|
|
TP.leg.ang = 5; % Angle Offset [deg]
|
|
TP.density = 8000;% Density of the material [kg/m^3]
|
|
TP.color = [0.7 0.7 0.7]; % Color [rgb]
|
|
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
|
|
|
|
%% Leg
|
|
Leg = struct();
|
|
|
|
Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
|
|
if strcmp(opts.actuator, 'piezo')
|
|
Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
|
|
elseif strcmp(opts.actuator, 'lorentz')
|
|
Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
|
|
else
|
|
error('opts.actuator should be piezo or lorentz');
|
|
end
|
|
Leg.ksi.ax = 10; % Maximum amplification at resonance []
|
|
Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
|
|
Leg.rad.top = 10; % Radius of the cylinder of the top part [mm]
|
|
Leg.density = 8000; % Density of the material [kg/m^3]
|
|
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
|
|
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
|
|
|
|
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
|
|
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
|
|
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
|
|
|
|
Leg = updateDamping(Leg);
|
|
|
|
|
|
%% Sphere
|
|
SP = struct();
|
|
|
|
SP.height.bottom = 15; % [mm]
|
|
SP.height.top = 15; % [mm]
|
|
SP.density.bottom = 8000; % [kg/m^3]
|
|
SP.density.top = 8000; % [kg/m^3]
|
|
SP.color.bottom = [0.7 0.7 0.7]; % [rgb]
|
|
SP.color.top = [0.7 0.7 0.7]; % [rgb]
|
|
SP.k.ax = 0; % [N*m/deg]
|
|
SP.ksi.ax = 0;
|
|
|
|
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
|
|
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
|
|
SP.rad.bottom = Leg.sphere.bottom; % [mm]
|
|
SP.rad.top = Leg.sphere.top; % [mm]
|
|
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
|
|
|
|
SP = updateDamping(SP);
|
|
|
|
%%
|
|
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
|
|
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
|
|
|
|
%%
|
|
nano_hexapod.BP = BP;
|
|
nano_hexapod.TP = TP;
|
|
nano_hexapod.Leg = Leg;
|
|
nano_hexapod.SP = SP;
|
|
|
|
%%
|
|
nano_hexapod = initializeParameters(nano_hexapod);
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
|
|
|
%%
|
|
function [element] = updateDamping(element)
|
|
field = fieldnames(element.k);
|
|
for i = 1:length(field)
|
|
if element.ksi.(field{i}) > 0
|
|
element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
|
|
else
|
|
element.c.(field{i}) = 0;
|
|
end
|
|
end
|
|
end
|
|
|
|
%%
|
|
function [stewart] = initializeParameters(stewart)
|
|
%% Connection points on base and top plate w.r.t. World frame at the center of the base plate
|
|
stewart.pos_base = zeros(6, 3);
|
|
stewart.pos_top = zeros(6, 3);
|
|
|
|
alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
|
|
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
|
|
|
|
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
|
|
|
|
radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
|
|
radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
|
|
|
|
for i = 1:3
|
|
% base points
|
|
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
|
|
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
|
|
stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
|
|
stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
|
|
|
|
% top points
|
|
% Top points are 60 degrees offset
|
|
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
|
|
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
|
|
stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
|
|
stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
|
|
end
|
|
|
|
% permute pos_top points so that legs are end points of base and top points
|
|
stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
|
|
stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
|
|
|
|
%% leg vectors
|
|
legs = stewart.pos_top - stewart.pos_base;
|
|
leg_length = zeros(6, 1);
|
|
leg_vectors = zeros(6, 3);
|
|
for i = 1:6
|
|
leg_length(i) = norm(legs(i,:));
|
|
leg_vectors(i,:) = legs(i,:) / leg_length(i);
|
|
end
|
|
|
|
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
|
|
stewart.Leg.shape.bot = [0 0; ...
|
|
stewart.Leg.rad.bottom 0; ...
|
|
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
|
|
stewart.Leg.rad.top stewart.Leg.lenght; ...
|
|
stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
|
|
0 0.2*stewart.Leg.lenght];
|
|
|
|
%% Calculate revolute and cylindrical axes
|
|
rev1 = zeros(6, 3);
|
|
rev2 = zeros(6, 3);
|
|
cyl1 = zeros(6, 3);
|
|
for i = 1:6
|
|
rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
|
|
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
|
|
|
|
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
|
|
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
|
|
|
|
cyl1(i,:) = leg_vectors(i,:);
|
|
end
|
|
|
|
|
|
%% Coordinate systems
|
|
stewart.lower_leg = struct('rotation', eye(3));
|
|
stewart.upper_leg = struct('rotation', eye(3));
|
|
|
|
for i = 1:6
|
|
stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
|
stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
|
end
|
|
|
|
%% Position Matrix
|
|
stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
|
|
|
%% Compute Jacobian Matrix
|
|
aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
|
|
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
|
end
|
|
|
|
function J = getJacobianMatrix(RM,M_pos_base)
|
|
% RM: [3x6] unit vector of each leg in the fixed frame
|
|
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
|
|
J = zeros(6);
|
|
J(:, 1:3) = RM';
|
|
J(:, 4:6) = cross(M_pos_base, RM)';
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Cedrat Actuator
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeCedratPiezo.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeCedratPiezo>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeCedratPiezo.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [cedrat] = initializeCedratPiezo(opts_param)
|
|
%% Default values for opts
|
|
opts = struct();
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%% Stewart Object
|
|
cedrat = struct();
|
|
cedrat.k = 10e7; % Linear Stiffness of each "blade" [N/m]
|
|
cedrat.ka = 10e7; % Linear Stiffness of the stack [N/m]
|
|
|
|
cedrat.c = 0.1*sqrt(1*cedrat.k); % [N/(m/s)]
|
|
cedrat.ca = 0.1*sqrt(1*cedrat.ka); % [N/(m/s)]
|
|
|
|
cedrat.L = 80; % Total Width of the Actuator[mm]
|
|
cedrat.H = 45; % Total Height of the Actuator [mm]
|
|
cedrat.L2 = sqrt((cedrat.L/2)^2 + (cedrat.H/2)^2); % Length of the elipsoidal sections [mm]
|
|
cedrat.alpha = 180/pi*atan2(cedrat.L/2, cedrat.H/2); % [deg]
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'cedrat', '-append');
|
|
end
|
|
#+end_src
|
|
|
|
** Sample
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeSample.m
|
|
:header-args:matlab+: :comments org :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
<<sec:initializeSample>>
|
|
|
|
This Matlab function is accessible [[file:../src/initializeSample.m][here]].
|
|
|
|
#+begin_src matlab
|
|
function [sample] = initializeSample(opts_param)
|
|
%% Default values for opts
|
|
sample = struct('radius', 100, ...
|
|
'height', 300, ...
|
|
'mass', 50, ...
|
|
'offset', 0, ...
|
|
'color', [0.45, 0.45, 0.45] ...
|
|
);
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
sample.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
|
|
%%
|
|
sample.k.x = 1e8;
|
|
sample.c.x = 0.1*sqrt(sample.k.x*sample.mass);
|
|
|
|
sample.k.y = 1e8;
|
|
sample.c.y = 0.1*sqrt(sample.k.y*sample.mass);
|
|
|
|
sample.k.z = 1e8;
|
|
sample.c.z = 0.1*sqrt(sample.k.z*sample.mass);
|
|
|
|
%% Save
|
|
save('./mat/stages.mat', 'sample', '-append');
|
|
end
|
|
#+end_src
|