Add few study
This commit is contained in:
parent
be78e8b801
commit
ffe6b8ed4b
244
control-study.org
Normal file
244
control-study.org
Normal file
@ -0,0 +1,244 @@
|
|||||||
|
#+TITLE: Stewart Platform - Control Study
|
||||||
|
:DRAWER:
|
||||||
|
#+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: <script src="./js/jquery.min.js"></script>
|
||||||
|
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||||
|
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||||
|
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||||
|
|
||||||
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||||
|
#+PROPERTY: header-args:matlab+ :comments org
|
||||||
|
#+PROPERTY: header-args:matlab+ :exports both
|
||||||
|
#+PROPERTY: header-args:matlab+ :results none
|
||||||
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||||
|
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||||
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||||
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||||
|
|
||||||
|
#+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 file raw replace
|
||||||
|
#+PROPERTY: header-args:latex+ :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
|
||||||
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
||||||
|
:END:
|
||||||
|
|
||||||
|
* First Control Architecture
|
||||||
|
** Matlab Init :noexport:
|
||||||
|
#+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
|
||||||
|
addpath('./src/')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Control Schematic
|
||||||
|
#+begin_src latex :file control_measure_rotating_2dof.pdf
|
||||||
|
\begin{tikzpicture}
|
||||||
|
% Blocs
|
||||||
|
\node[block] (J) at (0, 0) {$J$};
|
||||||
|
\node[addb={+}{}{}{}{-}, right=1 of J] (subr) {};
|
||||||
|
\node[block, right=0.8 of subr] (K) {$K_{L}$};
|
||||||
|
\node[block, right=1 of K] (G) {$G_{L}$};
|
||||||
|
|
||||||
|
% Connections and labels
|
||||||
|
\draw[<-] (J.west)node[above left]{$\bm{r}_{n}$} -- ++(-1, 0);
|
||||||
|
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{L}$};
|
||||||
|
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{L}$};
|
||||||
|
\draw[->] (K.east) -- (G.west) node[above left]{$\bm{\tau}$};
|
||||||
|
\draw[->] (G.east) node[above right]{$\bm{L}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
|
||||||
|
\end{tikzpicture}
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/control_measure_rotating_2dof.png]]
|
||||||
|
|
||||||
|
** Initialize the Stewart platform
|
||||||
|
#+begin_src matlab
|
||||||
|
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
|
||||||
|
stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
|
||||||
|
stewart = computeJointsPose(stewart);
|
||||||
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||||
|
stewart = computeJacobian(stewart);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Identification of the plant
|
||||||
|
Let's identify the transfer function from $\bm{\tau}}$ to $\bm{L}$.
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_control';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||||
|
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Plant Analysis
|
||||||
|
Diagonal terms
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(1, 4, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Compare to off-diagonal terms
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(1, 4, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:5
|
||||||
|
for j = i+1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:5
|
||||||
|
for j = i+1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Controller Design
|
||||||
|
One integrator should be present in the controller.
|
||||||
|
|
||||||
|
A lead is added around the crossover frequency which is set to be around 500Hz.
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
% wint = 2*pi*100; % Integrate until [rad]
|
||||||
|
% wlead = 2*pi*500; % Location of the lead [rad]
|
||||||
|
% hlead = 2; % Lead strengh
|
||||||
|
|
||||||
|
% Kl = 1e6 * ... % Gain
|
||||||
|
% (s + wint)/(s) * ... % Integrator until 100Hz
|
||||||
|
% (1 + s/(wlead/hlead)/(1 + s/(wlead*hlead))); % Lead
|
||||||
|
|
||||||
|
wc = 2*pi*100;
|
||||||
|
Kl = 1/abs(freqresp(G(1,1), wc)) * wc/s * 1/(1 + s/(3*wc));
|
||||||
|
Kl = Kl * eye(6);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
freqs = logspace(1, 4, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
ax1 = subplot(2, 1, 1);
|
||||||
|
hold on;
|
||||||
|
for i = 1:5
|
||||||
|
for j = i+1:6
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Kl(i,i)*G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||||
|
|
||||||
|
ax2 = subplot(2, 1, 2);
|
||||||
|
hold on;
|
||||||
|
for i = 1:5
|
||||||
|
for j = i+1:6
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(i, i)*G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
#+end_src
|
||||||
285
dynamics-study.org
Normal file
285
dynamics-study.org
Normal file
@ -0,0 +1,285 @@
|
|||||||
|
#+TITLE: Stewart Platform - Dynamics Study
|
||||||
|
:DRAWER:
|
||||||
|
#+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: <script src="./js/jquery.min.js"></script>
|
||||||
|
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||||
|
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||||
|
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||||
|
|
||||||
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||||
|
#+PROPERTY: header-args:matlab+ :comments org
|
||||||
|
#+PROPERTY: header-args:matlab+ :exports both
|
||||||
|
#+PROPERTY: header-args:matlab+ :results none
|
||||||
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||||
|
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||||
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||||
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||||
|
:END:
|
||||||
|
|
||||||
|
* Some tests
|
||||||
|
** 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
|
||||||
|
addpath('./src/')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Simscape Model
|
||||||
|
#+begin_src matlab
|
||||||
|
open('stewart_platform_dynamics.slx')
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** test
|
||||||
|
#+begin_src matlab
|
||||||
|
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
|
||||||
|
stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
|
||||||
|
stewart = computeJointsPose(stewart);
|
||||||
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||||
|
stewart = computeJacobian(stewart);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/J-T'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||||
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
G_cart = minreal(G*inv(stewart.J'));
|
||||||
|
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
figure; bode(G, G_cart)
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
Gd = linearize(mdl, io, options);
|
||||||
|
Gd.InputName = {'Fex', 'Fey', 'Fez', 'Mex', 'Mey', 'Mez'};
|
||||||
|
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
freqs = logspace(0, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
bode(Gd, G)
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
** Compare external forces and forces applied by the actuators
|
||||||
|
Initialization of the Stewart platform.
|
||||||
|
#+begin_src matlab
|
||||||
|
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
|
||||||
|
stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
|
||||||
|
stewart = computeJointsPose(stewart);
|
||||||
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||||
|
stewart = computeJacobian(stewart);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Estimation of the transfer function from $\mathcal{\bm{F}}_{d}$ to $\mathcal{\bm{X}}$:
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
Gd = linearize(mdl, io, options);
|
||||||
|
Gd.InputName = {'Fex', 'Fey', 'Fez', 'Mex', 'Mey', 'Mez'};
|
||||||
|
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Comparison of the two transfer function matrices.
|
||||||
|
#+begin_src matlab
|
||||||
|
freqs = logspace(0, 4, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
bode(Gd, G, freqs)
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_important
|
||||||
|
Seems quite similar.
|
||||||
|
#+end_important
|
||||||
|
|
||||||
|
** Comparison of the static transfer function and the Compliance matrix
|
||||||
|
Initialization of the Stewart platform.
|
||||||
|
#+begin_src matlab
|
||||||
|
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
|
||||||
|
stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
|
||||||
|
stewart = computeJointsPose(stewart);
|
||||||
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||||
|
stewart = computeJacobian(stewart);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||||
|
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Let's first look at the low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$.
|
||||||
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||||
|
data2orgtable(real(freqresp(G, 0.1)), {}, {}, ' %.1e ');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+RESULTS:
|
||||||
|
| 2.0e-06 | -9.1e-19 | -5.3e-12 | 7.3e-18 | 1.7e-05 | 1.3e-18 |
|
||||||
|
| -1.7e-18 | 2.0e-06 | 8.6e-19 | -1.7e-05 | -1.5e-17 | 6.7e-12 |
|
||||||
|
| 3.6e-13 | 3.2e-19 | 5.0e-07 | -2.5e-18 | 8.1e-12 | -1.5e-19 |
|
||||||
|
| 1.0e-17 | -1.7e-05 | -5.0e-18 | 1.9e-04 | 9.1e-17 | -3.5e-11 |
|
||||||
|
| 1.7e-05 | -6.9e-19 | -5.3e-11 | 6.9e-18 | 1.9e-04 | 4.8e-18 |
|
||||||
|
| -3.5e-18 | -4.5e-12 | 1.5e-18 | 7.1e-11 | -3.4e-17 | 4.6e-05 |
|
||||||
|
|
||||||
|
And now at the Compliance matrix.
|
||||||
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||||
|
data2orgtable(stewart.C, {}, {}, ' %.1e ');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+RESULTS:
|
||||||
|
| 2.0e-06 | 2.9e-22 | 2.8e-22 | -3.2e-21 | 1.7e-05 | 1.5e-37 |
|
||||||
|
| -2.1e-22 | 2.0e-06 | -1.8e-23 | -1.7e-05 | -2.3e-21 | 1.1e-22 |
|
||||||
|
| 3.1e-22 | -1.6e-23 | 5.0e-07 | 1.7e-22 | 2.2e-21 | -8.1e-39 |
|
||||||
|
| 2.1e-21 | -1.7e-05 | 2.0e-22 | 1.9e-04 | 2.3e-20 | -8.7e-21 |
|
||||||
|
| 1.7e-05 | 2.5e-21 | 2.0e-21 | -2.8e-20 | 1.9e-04 | 1.3e-36 |
|
||||||
|
| 3.7e-23 | 3.1e-22 | -6.0e-39 | -1.0e-20 | 3.1e-22 | 4.6e-05 |
|
||||||
|
|
||||||
|
#+begin_important
|
||||||
|
The low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$ corresponds to the compliance matrix of the Stewart platform.
|
||||||
|
#+end_important
|
||||||
|
|
||||||
|
** Transfer function from forces applied in the legs to the displacement of the legs
|
||||||
|
Initialization of the Stewart platform.
|
||||||
|
#+begin_src matlab
|
||||||
|
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
|
||||||
|
stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3);
|
||||||
|
stewart = computeJointsPose(stewart);
|
||||||
|
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||||
|
stewart = computeJacobian(stewart);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
Estimation of the transfer function from $\bm{\tau}$ to $\bm{L}$:
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Options for Linearized
|
||||||
|
options = linearizeOptions;
|
||||||
|
options.SampleTime = 0;
|
||||||
|
|
||||||
|
%% Name of the Simulink File
|
||||||
|
mdl = 'stewart_platform_dynamics';
|
||||||
|
|
||||||
|
%% Input/Output definition
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/J-T'], 1, 'openinput'); io_i = io_i + 1;
|
||||||
|
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
|
||||||
|
|
||||||
|
%% Run the linearization
|
||||||
|
G = linearize(mdl, io, options);
|
||||||
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||||
|
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
freqs = logspace(1, 3, 1000);
|
||||||
|
figure; bode(G, 2*pi*freqs)
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
bodeFig({G(1,1), G(1,2)}, freqs, struct('phase', true));
|
||||||
|
#+end_src
|
||||||
BIN
figs/control_measure_rotating_2dof.png
Normal file
BIN
figs/control_measure_rotating_2dof.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.0 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 21 KiB After Width: | Height: | Size: 23 KiB |
@ -61,6 +61,7 @@ For Simscape, we need:
|
|||||||
- The position of the frame $\{A\}$ with respect to the frame $\{F\}$: ${}^{F}\bm{O}_{A}$
|
- The position of the frame $\{A\}$ with respect to the frame $\{F\}$: ${}^{F}\bm{O}_{A}$
|
||||||
- The position of the frame $\{B\}$ with respect to the frame $\{M\}$: ${}^{M}\bm{O}_{B}$
|
- The position of the frame $\{B\}$ with respect to the frame $\{M\}$: ${}^{M}\bm{O}_{B}$
|
||||||
|
|
||||||
|
|
||||||
* Procedure
|
* Procedure
|
||||||
The procedure to define the Stewart platform is the following:
|
The procedure to define the Stewart platform is the following:
|
||||||
1. Define the initial position of frames {A}, {B}, {F} and {M}.
|
1. Define the initial position of frames {A}, {B}, {F} and {M}.
|
||||||
@ -285,6 +286,7 @@ This Matlab function is accessible [[file:src/computeJointsPose.m][here]].
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
** Documentation
|
** Documentation
|
||||||
|
|
||||||
#+name: fig:stewart-struts
|
#+name: fig:stewart-struts
|
||||||
#+caption: Position and orientation of the struts
|
#+caption: Position and orientation of the struts
|
||||||
[[file:figs/stewart-struts.png]]
|
[[file:figs/stewart-struts.png]]
|
||||||
@ -355,7 +357,7 @@ This Matlab function is accessible [[file:src/initializeStrutDynamics.m][here]].
|
|||||||
arguments
|
arguments
|
||||||
stewart
|
stewart
|
||||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
||||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e2*ones(6,1)
|
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||||
end
|
end
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
|
|||||||
@ -16,7 +16,7 @@ function [stewart] = initializeStrutDynamics(stewart, args)
|
|||||||
arguments
|
arguments
|
||||||
stewart
|
stewart
|
||||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
||||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e2*ones(6,1)
|
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||||
end
|
end
|
||||||
|
|
||||||
stewart.Ki = args.Ki;
|
stewart.Ki = args.Ki;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user