509 lines
18 KiB
Org Mode
509 lines
18 KiB
Org Mode
|
#+TITLE: Matlab Functions used for the NASS Project
|
||
|
|
:DRAWER:
|
||
|
|
#+STARTUP: overview
|
||
|
|
|
||
|
|
#+LANGUAGE: en
|
||
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
||
|
|
#+AUTHOR: Dehaeze Thomas
|
||
|
|
|
||
|
|
#+HTML_LINK_HOME: ../index.html
|
||
|
|
#+HTML_LINK_UP: ../index.html
|
||
|
|
|
||
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/htmlize.css"/>
|
||
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/readtheorg.css"/>
|
||
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="../css/zenburn.css"/>
|
||
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.min.js"></script>
|
||
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/bootstrap.min.js"></script>
|
||
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/jquery.stickytableheaders.min.js"></script>
|
||
|
|
#+HTML_HEAD: <script type="text/javascript" src="../js/readtheorg.js"></script>
|
||
|
|
|
||
|
|
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||
|
|
|
||
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
||
|
|
#+PROPERTY: header-args:matlab+ :results none
|
||
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
||
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
||
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
||
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||
|
|
|
||
|
|
#+PROPERTY: header-args:shell :eval no-export
|
||
|
|
|
||
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||
|
|
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
||
|
|
#+PROPERTY: header-args:latex+ :exports both
|
||
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
||
|
|
:END:
|
||
|
|
|
||
|
|
* Functions
|
||
|
|
<<sec:functions>>
|
||
|
|
** computePsdDispl
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ../src/computePsdDispl.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
: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 none :mkdirp yes :eval no
|
||
|
|
: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 none :mkdirp yes :eval no
|
||
|
|
: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 none :mkdirp yes :eval no
|
||
|
|
: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 none :mkdirp yes :eval no
|
||
|
|
: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 none :mkdirp yes :eval no
|
||
|
|
: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
|
||
|
|
|
||
|
|
** Inverse Kinematics of the Hexapod
|
||
|
|
:PROPERTIES:
|
||
|
|
:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
|
||
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||
|
|
:END:
|
||
|
|
<<sec:inverseKinematicsHexapod>>
|
||
|
|
|
||
|
|
This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].
|
||
|
|
|
||
|
|
#+begin_src matlab
|
||
|
|
function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
|
||
|
|
% inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
|
||
|
|
%
|
||
|
|
% Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
|
||
|
|
%
|
||
|
|
% Inputs:
|
||
|
|
% - hexapod - Hexapod object containing the geometry of the hexapod
|
||
|
|
% - AP - Position vector of point OB expressed in frame {A} in [m]
|
||
|
|
% - ARB - Rotation Matrix expressed in frame {A}
|
||
|
|
|
||
|
|
% Wanted Length of the hexapod's legs [m]
|
||
|
|
L = zeros(6, 1);
|
||
|
|
|
||
|
|
for i = 1:length(L)
|
||
|
|
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
|
||
|
|
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
|
||
|
|
|
||
|
|
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
#+end_src
|