tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
958d07376d
nass-simscape/simscape/index.org

1920 lines
69 KiB
Org Mode
Raw Normal View History

Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
#+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, '/Dsm'], 1, 'output'); % Displacement of the sample
io(6) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
io(7) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
io(8) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
'Dgx', 'Dgy', 'Dgz', ...
'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'};
%% Create the sub transfer functions
% 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'}));
% From ground motion to Sample displacement
sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}));
% 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'}));
% 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'}));
% 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'}));
% From forces applied on NASS's legs to displacement of each leg
sys.G_plant = minreal(G({'Edx', 'Rdy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}));
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
The system is now quite well tuned
2019-10-16 14:57:03 +02:00
function [granite] = initializeGranite(opts_param)
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
%% 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
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
%%
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
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
granite.c.x = 0.1*sqrt(granite.mass_top*granite.k.x); % [N/(m/s)]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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')
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
else
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
ty.k.ax = 5e7; % 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]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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')
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
else
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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')
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
else
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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]
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
opts = struct('rigid', false);
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
%% Populate opts with input parameters
if exist('opts_param','var')
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
for opt = fieldnames(opts_param)'
opts.(opt{1}) = opts_param.(opt{1});
end
Add tilt flexibility to granite
2019-10-16 11:18:06 +02: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 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]
The system is now quite well tuned
2019-10-16 14:57:03 +02:00
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
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) []
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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)
Axis Compensator has been added
2019-10-16 14:25:25 +02:00
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
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
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
The system is now quite well tuned
2019-10-16 14:57:03 +02:00
% axisc.k.ax = 1; % TODO [N*m/deg)]
% axisc.c.ax = (1/1)*sqrt(axisc.k.ax/axisc.m);
Add tilt flexibility to granite
2019-10-16 11:18:06 +02:00
%% 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 = 3;
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)
element.c.(field{i}) = 1/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
** 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 = sqrt(sample.k.x*sample.mass)/10;
sample.k.y = 1e8;
sample.c.y = sqrt(sample.k.y*sample.mass)/10;
sample.k.z = 1e8;
sample.c.z = sqrt(sample.k.y*sample.mass)/10;
%% Save
save('./mat/stages.mat', 'sample', '-append');
end
#+end_src
Reference in New Issue Copy Permalink