Change all indentation
This commit is contained in:
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -40,111 +40,111 @@ In this section, we wish to compare the effect of forces/torques applied by the
|
||||
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Comparison with fixed support
|
||||
Let's generate a Stewart platform.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
#+end_src
|
||||
|
||||
We don't put any flexibility below the Stewart platform such that *its base is fixed to an inertial frame*.
|
||||
We also don't put any payload on top of the Stewart platform.
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
The transfer function from actuator forces $\bm{\tau}$ to the relative displacement of the mobile platform $\mathcal{\bm{X}}$ is extracted.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% 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
|
||||
|
||||
Using the Jacobian matrix, we compute the transfer function from force/torques applied by the actuators on the frame $\{B\}$ fixed to the mobile platform:
|
||||
#+begin_src matlab
|
||||
Gc = minreal(G*inv(stewart.kinematics.J'));
|
||||
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
Gc = minreal(G*inv(stewart.kinematics.J'));
|
||||
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
#+end_src
|
||||
|
||||
We also extract the transfer function from external forces $\bm{\mathcal{F}}_{\text{ext}}$ on the frame $\{B\}$ fixed to the mobile platform to the relative displacement $\mathcal{\bm{X}}$ of $\{B\}$ with respect to frame $\{A\}$:
|
||||
#+begin_src matlab
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'F_ext'); io_i = io_i + 1; % External forces/torques applied on {B}
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'F_ext'); io_i = io_i + 1; % External forces/torques applied on {B}
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
|
||||
%% Run the linearization
|
||||
Gd = linearize(mdl, io, options);
|
||||
Gd.InputName = {'Fex', 'Fey', 'Fez', 'Mex', 'Mey', 'Mez'};
|
||||
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% 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
|
||||
|
||||
The comparison of the two transfer functions is shown in Figure [[fig:comparison_Fext_F_fixed_base]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 4, 1000);
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd(1,1), freqs, 'Hz'))), '--');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd(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(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(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]);
|
||||
legend({'$\mathcal{X}_{x}/\mathcal{F}_{x}$', '$\mathcal{X}_{x}/\mathcal{F}_{x,ext}$'});
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(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]);
|
||||
legend({'$\mathcal{X}_{x}/\mathcal{F}_{x}$', '$\mathcal{X}_{x}/\mathcal{F}_{x,ext}$'});
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
@@ -159,19 +159,19 @@ The comparison of the two transfer functions is shown in Figure [[fig:comparison
|
||||
This can be understood from figure [[fig:1dof_actuator_external_forces]] where $\mathcal{F}_{x}$ and $\mathcal{F}_{x,\text{ext}}$ have clearly the same effect on $\mathcal{X}_{x}$.
|
||||
|
||||
#+begin_src latex :file 1dof_actuator_external_forces.pdf
|
||||
\begin{tikzpicture}
|
||||
\draw[ground] (-1, 0) -- (1, 0);
|
||||
\begin{tikzpicture}
|
||||
\draw[ground] (-1, 0) -- (1, 0);
|
||||
|
||||
\draw[spring] (-0.6, 0) -- (-0.6, 1.5) node[midway, left=0.1]{$k$};
|
||||
\draw[actuator] ( 0.6, 0) -- ( 0.6, 1.5) node[midway, left=0.1](F){$\mathcal{F}_{x}$};
|
||||
\draw[spring] (-0.6, 0) -- (-0.6, 1.5) node[midway, left=0.1]{$k$};
|
||||
\draw[actuator] ( 0.6, 0) -- ( 0.6, 1.5) node[midway, left=0.1](F){$\mathcal{F}_{x}$};
|
||||
|
||||
\draw[fill=white] (-1, 1.5) rectangle (1, 2) node[pos=0.5]{$m$};
|
||||
\draw[fill=white] (-1, 1.5) rectangle (1, 2) node[pos=0.5]{$m$};
|
||||
|
||||
\draw[dashed] (1, 2) -- ++(0.5, 0);
|
||||
\draw[->] (1.5, 2) -- ++(0, 0.5) node[right]{$\mathcal{X}_{x}$};
|
||||
\draw[dashed] (1, 2) -- ++(0.5, 0);
|
||||
\draw[->] (1.5, 2) -- ++(0, 0.5) node[right]{$\mathcal{X}_{x}$};
|
||||
|
||||
\draw[->] (0, 2) node[]{$\bullet$} -- (0, 2.8) node[below right]{$\mathcal{F}_{x,\text{ext}}$};
|
||||
\end{tikzpicture}
|
||||
\draw[->] (0, 2) node[]{$\bullet$} -- (0, 2.8) node[below right]{$\mathcal{F}_{x,\text{ext}}$};
|
||||
\end{tikzpicture}
|
||||
#+end_src
|
||||
|
||||
#+name: fig:1dof_actuator_external_forces
|
||||
@@ -182,62 +182,62 @@ This can be understood from figure [[fig:1dof_actuator_external_forces]] where $
|
||||
** Comparison with a flexible support
|
||||
We now add a flexible support under the Stewart platform.
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'flexible');
|
||||
ground = initializeGround('type', 'flexible');
|
||||
#+end_src
|
||||
|
||||
And we perform again the identification.
|
||||
#+begin_src matlab
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
|
||||
Gc = minreal(G*inv(stewart.kinematics.J'));
|
||||
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
Gc = minreal(G*inv(stewart.kinematics.J'));
|
||||
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'F_ext'); io_i = io_i + 1; % External forces/torques applied on {B}
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances'], 1, 'openinput', [], 'F_ext'); io_i = io_i + 1; % External forces/torques applied on {B}
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
|
||||
%% Run the linearization
|
||||
Gd = linearize(mdl, io, options);
|
||||
Gd.InputName = {'Fex', 'Fey', 'Fez', 'Mex', 'Mey', 'Mez'};
|
||||
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% 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
|
||||
|
||||
The comparison between the obtained transfer functions is shown in Figure [[fig:comparison_Fext_F_flexible_base]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 4, 1000);
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd(1,1), freqs, 'Hz'))), '--');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, abs(squeeze(freqresp(Gd(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(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(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]);
|
||||
legend({'$\mathcal{X}_{x}/\mathcal{F}_{x}$', '$\mathcal{X}_{x}/\mathcal{F}_{x,ext}$'});
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc(1,1), freqs, 'Hz'))), '-');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd(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]);
|
||||
legend({'$\mathcal{X}_{x}/\mathcal{F}_{x}$', '$\mathcal{X}_{x}/\mathcal{F}_{x,ext}$'});
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
@@ -254,22 +254,22 @@ We see that $\mathcal{F}_{x}$ applies a force both on $m$ and $m^{\prime}$ where
|
||||
And thus $\mathcal{F}_{x}$ and $\mathcal{F}_{x,\text{ext}}$ have clearly *not* the same effect on $\mathcal{X}_{x}$.
|
||||
|
||||
#+begin_src latex :file 2dof_actuator_external_forces.pdf
|
||||
\begin{tikzpicture}
|
||||
\draw[ground] (-1, 0) -- (1, 0);
|
||||
\begin{tikzpicture}
|
||||
\draw[ground] (-1, 0) -- (1, 0);
|
||||
|
||||
\draw[spring] (0, 0) -- (0, 1.5) node[midway, left=0.1]{$k^{\prime}$};
|
||||
\draw[fill=white] (-1, 1.5) rectangle (1, 2) node[pos=0.5]{$m^{\prime}$};
|
||||
\draw[spring] (0, 0) -- (0, 1.5) node[midway, left=0.1]{$k^{\prime}$};
|
||||
\draw[fill=white] (-1, 1.5) rectangle (1, 2) node[pos=0.5]{$m^{\prime}$};
|
||||
|
||||
\draw[spring] (-0.6, 2) -- (-0.6, 3.5) node[midway, left=0.1]{$k$};
|
||||
\draw[actuator] ( 0.6, 2) -- ( 0.6, 3.5) node[midway, left=0.1](F){$\mathcal{F}_{x}$};
|
||||
\draw[spring] (-0.6, 2) -- (-0.6, 3.5) node[midway, left=0.1]{$k$};
|
||||
\draw[actuator] ( 0.6, 2) -- ( 0.6, 3.5) node[midway, left=0.1](F){$\mathcal{F}_{x}$};
|
||||
|
||||
\draw[fill=white] (-1, 3.5) rectangle (1, 4) node[pos=0.5]{$m$};
|
||||
\draw[fill=white] (-1, 3.5) rectangle (1, 4) node[pos=0.5]{$m$};
|
||||
|
||||
\draw[dashed] (1, 4) -- ++(0.5, 0);
|
||||
\draw[->] (1.5, 4) -- ++(0, 0.5) node[right]{$\mathcal{X}_{x}$};
|
||||
\draw[dashed] (1, 4) -- ++(0.5, 0);
|
||||
\draw[->] (1.5, 4) -- ++(0, 0.5) node[right]{$\mathcal{X}_{x}$};
|
||||
|
||||
\draw[->] (0, 4) node[]{$\bullet$} -- (0, 4.8) node[below right]{$\mathcal{F}_{x,\text{ext}}$};
|
||||
\end{tikzpicture}
|
||||
\draw[->] (0, 4) node[]{$\bullet$} -- (0, 4.8) node[below right]{$\mathcal{F}_{x,\text{ext}}$};
|
||||
\end{tikzpicture}
|
||||
#+end_src
|
||||
|
||||
#+name: fig:2dof_actuator_external_forces
|
||||
@@ -289,67 +289,67 @@ In this section, we see how the Compliance matrix of the Stewart platform is lin
|
||||
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
Initialization of the Stewart platform.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
||||
#+end_src
|
||||
|
||||
No flexibility below the Stewart platform and no payload.
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
#+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;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% 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
|
||||
Gc = minreal(G*inv(stewart.kinematics.J'));
|
||||
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
Gc = minreal(G*inv(stewart.kinematics.J'));
|
||||
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
#+end_src
|
||||
|
||||
Let's first look at the low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$.
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -46,66 +46,66 @@ In this document, we discuss the various methods to identify the behavior of the
|
||||
** Introduction :ignore:
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Initialize the Stewart Platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 2, 'openoutput'); io_i = io_i + 1; % Velocity of {B} w.r.t. {A}
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Position/Orientation of {B} w.r.t. {A}
|
||||
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 2, 'openoutput'); io_i = io_i + 1; % Velocity of {B} w.r.t. {A}
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
% G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
|
||||
% G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
% G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
|
||||
% G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
|
||||
#+end_src
|
||||
|
||||
Let's check the size of =G=:
|
||||
#+begin_src matlab :results replace output
|
||||
size(G)
|
||||
size(G)
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -117,7 +117,7 @@ Let's check the size of =G=:
|
||||
|
||||
We expect to have only 12 states (corresponding to the 6dof of the mobile platform).
|
||||
#+begin_src matlab :results replace output
|
||||
Gm = minreal(G);
|
||||
Gm = minreal(G);
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -129,7 +129,7 @@ And indeed, we obtain 12 states.
|
||||
** Coordinate transformation
|
||||
We can perform the following transformation using the =ss2ss= command.
|
||||
#+begin_src matlab
|
||||
Gt = ss2ss(Gm, Gm.C);
|
||||
Gt = ss2ss(Gm, Gm.C);
|
||||
#+end_src
|
||||
|
||||
Then, the =C= matrix of =Gt= is the unity matrix which means that the states of the state space model are equal to the measurements $\bm{Y}$.
|
||||
@@ -138,29 +138,29 @@ The measurements are the 6 displacement and 6 velocities of mobile platform with
|
||||
|
||||
We could perform the transformation by hand:
|
||||
#+begin_src matlab
|
||||
At = Gm.C*Gm.A*pinv(Gm.C);
|
||||
At = Gm.C*Gm.A*pinv(Gm.C);
|
||||
|
||||
Bt = Gm.C*Gm.B;
|
||||
Bt = Gm.C*Gm.B;
|
||||
|
||||
Ct = eye(12);
|
||||
Dt = zeros(12, 6);
|
||||
Ct = eye(12);
|
||||
Dt = zeros(12, 6);
|
||||
|
||||
Gt = ss(At, Bt, Ct, Dt);
|
||||
Gt = ss(At, Bt, Ct, Dt);
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
[V,D] = eig(Gt.A);
|
||||
[V,D] = eig(Gt.A);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
ws = imag(diag(D))/2/pi;
|
||||
[ws,I] = sort(ws)
|
||||
ws = imag(diag(D))/2/pi;
|
||||
[ws,I] = sort(ws)
|
||||
|
||||
xi = 100*real(diag(D))./imag(diag(D));
|
||||
xi = xi(I);
|
||||
xi = 100*real(diag(D))./imag(diag(D));
|
||||
xi = xi(I);
|
||||
|
||||
data2orgtable([[1:length(ws(ws>0))]', ws(ws>0), xi(xi>0)], {}, {'Mode Number', 'Resonance Frequency [Hz]', 'Damping Ratio [%]'}, ' %.1f ');
|
||||
data2orgtable([[1:length(ws(ws>0))]', ws(ws>0), xi(xi>0)], {}, {'Mode Number', 'Resonance Frequency [Hz]', 'Damping Ratio [%]'}, ' %.1f ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -180,54 +180,54 @@ To visualize the i'th mode, we may excite the system using the inputs $U_i$ such
|
||||
|
||||
Let's first sort the modes and just take the modes corresponding to a eigenvalue with a positive imaginary part.
|
||||
#+begin_src matlab
|
||||
ws = imag(diag(D));
|
||||
[ws,I] = sort(ws)
|
||||
ws = ws(7:end); I = I(7:end);
|
||||
ws = imag(diag(D));
|
||||
[ws,I] = sort(ws)
|
||||
ws = ws(7:end); I = I(7:end);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
for i = 1:length(ws)
|
||||
for i = 1:length(ws)
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
i_mode = I(i); % the argument is the i'th mode
|
||||
i_mode = I(i); % the argument is the i'th mode
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
lambda_i = D(i_mode, i_mode);
|
||||
xi_i = V(:,i_mode);
|
||||
lambda_i = D(i_mode, i_mode);
|
||||
xi_i = V(:,i_mode);
|
||||
|
||||
a_i = real(lambda_i);
|
||||
b_i = imag(lambda_i);
|
||||
a_i = real(lambda_i);
|
||||
b_i = imag(lambda_i);
|
||||
#+end_src
|
||||
|
||||
Let do 10 periods of the mode.
|
||||
#+begin_src matlab
|
||||
t = linspace(0, 10/(imag(lambda_i)/2/pi), 1000);
|
||||
U_i = pinv(Gt.B) * real(xi_i * lambda_i * (cos(b_i * t) + 1i*sin(b_i * t)));
|
||||
t = linspace(0, 10/(imag(lambda_i)/2/pi), 1000);
|
||||
U_i = pinv(Gt.B) * real(xi_i * lambda_i * (cos(b_i * t) + 1i*sin(b_i * t)));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
U = timeseries(U_i, t);
|
||||
U = timeseries(U_i, t);
|
||||
#+end_src
|
||||
|
||||
Simulation:
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', num2str(t(end)));
|
||||
sim(mdl);
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', num2str(t(end)));
|
||||
sim(mdl);
|
||||
#+end_src
|
||||
|
||||
Save the movie of the mode shape.
|
||||
#+begin_src matlab
|
||||
smwritevideo(mdl, sprintf('figs/mode%i', i), ...
|
||||
'PlaybackSpeedRatio', 1/(b_i/2/pi), ...
|
||||
'FrameRate', 30, ...
|
||||
'FrameSize', [800, 400]);
|
||||
smwritevideo(mdl, sprintf('figs/mode%i', i), ...
|
||||
'PlaybackSpeedRatio', 1/(b_i/2/pi), ...
|
||||
'FrameRate', 30, ...
|
||||
'FrameSize', [800, 400]);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+name: fig:mode1
|
||||
@@ -247,83 +247,83 @@ Save the movie of the mode shape.
|
||||
** Introduction :ignore:
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Initialize the Stewart platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
||||
#+end_src
|
||||
|
||||
We set the rotation point of the ground to be at the same point at frames $\{A\}$ and $\{B\}$.
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
|
||||
payload = initializePayload('type', 'rigid');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
|
||||
payload = initializePayload('type', 'rigid');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
** Transmissibility
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
|
||||
%% Run the linearization
|
||||
T = linearize(mdl, io, options);
|
||||
T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'};
|
||||
T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% Run the linearization
|
||||
T = linearize(mdl, io, options);
|
||||
T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'};
|
||||
T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = logspace(1, 4, 1000);
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
for ix = 1:6
|
||||
figure;
|
||||
for ix = 1:6
|
||||
for iy = 1:6
|
||||
subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-5, 10]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-5, 10]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
From cite:preumont07_six_axis_singl_stage_activ, one can use the Frobenius norm of the transmissibility matrix to obtain a scalar indicator of the transmissibility performance of the system:
|
||||
@@ -333,26 +333,26 @@ From cite:preumont07_six_axis_singl_stage_activ, one can use the Frobenius norm
|
||||
\end{align*}
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = logspace(1, 4, 1000);
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
T_norm = zeros(length(freqs), 1);
|
||||
T_norm = zeros(length(freqs), 1);
|
||||
|
||||
for i = 1:length(freqs)
|
||||
for i = 1:length(freqs)
|
||||
T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')'));
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
And we normalize by a factor $\sqrt{6}$ to obtain a performance metric comparable to the transmissibility of a one-axis isolator:
|
||||
\[ \Gamma(\omega) = \|\bm{T}(\omega)\| / \sqrt{6} \]
|
||||
|
||||
#+begin_src matlab
|
||||
Gamma = T_norm/sqrt(6);
|
||||
Gamma = T_norm/sqrt(6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
plot(freqs, Gamma)
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
figure;
|
||||
plot(freqs, Gamma)
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
#+end_src
|
||||
|
||||
* Compliance Analysis
|
||||
@@ -360,101 +360,101 @@ And we normalize by a factor $\sqrt{6}$ to obtain a performance metric comparabl
|
||||
** Introduction :ignore:
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Initialize the Stewart platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
||||
#+end_src
|
||||
|
||||
We set the rotation point of the ground to be at the same point at frames $\{A\}$ and $\{B\}$.
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'rigid');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'rigid');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
** Compliance
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
|
||||
%% Run the linearization
|
||||
C = linearize(mdl, io, options);
|
||||
C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
|
||||
C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% Run the linearization
|
||||
C = linearize(mdl, io, options);
|
||||
C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
|
||||
C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = logspace(1, 4, 1000);
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
for ix = 1:6
|
||||
figure;
|
||||
for ix = 1:6
|
||||
for iy = 1:6
|
||||
subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-10, 1e-3]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylim([1e-10, 1e-3]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
We can try to use the Frobenius norm to obtain a scalar value representing the 6-dof compliance of the Stewart platform.
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = logspace(1, 4, 1000);
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
C_norm = zeros(length(freqs), 1);
|
||||
C_norm = zeros(length(freqs), 1);
|
||||
|
||||
for i = 1:length(freqs)
|
||||
for i = 1:length(freqs)
|
||||
C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')'));
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure;
|
||||
plot(freqs, C_norm)
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
figure;
|
||||
plot(freqs, C_norm)
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
#+end_src
|
||||
|
||||
* Functions
|
||||
@@ -470,20 +470,20 @@ We can try to use the Frobenius norm to obtain a scalar value representing the 6
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [T, T_norm, freqs] = computeTransmissibility(args)
|
||||
% computeTransmissibility -
|
||||
%
|
||||
% Syntax: [T, T_norm, freqs] = computeTransmissibility(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm
|
||||
% - freqs [] - Frequency vector to estimate the Frobenius norm
|
||||
%
|
||||
% Outputs:
|
||||
% - T [6x6 ss] - Transmissibility matrix
|
||||
% - T_norm [length(freqs)x1] - Frobenius norm of the Transmissibility matrix
|
||||
% - freqs [length(freqs)x1] - Frequency vector in [Hz]
|
||||
function [T, T_norm, freqs] = computeTransmissibility(args)
|
||||
% computeTransmissibility -
|
||||
%
|
||||
% Syntax: [T, T_norm, freqs] = computeTransmissibility(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm
|
||||
% - freqs [] - Frequency vector to estimate the Frobenius norm
|
||||
%
|
||||
% Outputs:
|
||||
% - T [6x6 ss] - Transmissibility matrix
|
||||
% - T_norm [length(freqs)x1] - Frobenius norm of the Transmissibility matrix
|
||||
% - freqs [length(freqs)x1] - Frequency vector in [Hz]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
@@ -491,14 +491,14 @@ We can try to use the Frobenius norm to obtain a scalar value representing the 6
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
args.plots logical {mustBeNumericOrLogical} = false
|
||||
args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000)
|
||||
end
|
||||
arguments
|
||||
args.plots logical {mustBeNumericOrLogical} = false
|
||||
args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = args.freqs;
|
||||
freqs = args.freqs;
|
||||
#+end_src
|
||||
|
||||
*** Identification of the Transmissibility Matrix
|
||||
@@ -506,43 +506,43 @@ We can try to use the Frobenius norm to obtain a scalar value representing the 6
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'output'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/D_w'], 1, 'openinput'); io_i = io_i + 1; % Base Motion [m, rad]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'output'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
|
||||
%% Run the linearization
|
||||
T = linearize(mdl, io, options);
|
||||
T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'};
|
||||
T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% Run the linearization
|
||||
T = linearize(mdl, io, options);
|
||||
T.InputName = {'Wdx', 'Wdy', 'Wdz', 'Wrx', 'Wry', 'Wrz'};
|
||||
T.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
#+begin_src matlab
|
||||
p_handle = zeros(6*6,1);
|
||||
p_handle = zeros(6*6,1);
|
||||
|
||||
if args.plots
|
||||
if args.plots
|
||||
fig = figure;
|
||||
for ix = 1:6
|
||||
for iy = 1:6
|
||||
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
for iy = 1:6
|
||||
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(T(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
linkaxes(p_handle, 'xy')
|
||||
@@ -554,7 +554,7 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
han.YLabel.Visible = 'on';
|
||||
xlabel(han, 'Frequency [Hz]');
|
||||
ylabel(han, 'Transmissibility [m/m]');
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Computation of the Frobenius norm
|
||||
@@ -562,25 +562,25 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
T_norm = zeros(length(freqs), 1);
|
||||
T_norm = zeros(length(freqs), 1);
|
||||
|
||||
for i = 1:length(freqs)
|
||||
for i = 1:length(freqs)
|
||||
T_norm(i) = sqrt(trace(freqresp(T, freqs(i), 'Hz')*freqresp(T, freqs(i), 'Hz')'));
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
T_norm = T_norm/sqrt(6);
|
||||
T_norm = T_norm/sqrt(6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
if args.plots
|
||||
if args.plots
|
||||
figure;
|
||||
plot(freqs, T_norm)
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Transmissibility - Frobenius Norm');
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Compute the Compliance
|
||||
@@ -595,20 +595,20 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [C, C_norm, freqs] = computeCompliance(args)
|
||||
% computeCompliance -
|
||||
%
|
||||
% Syntax: [C, C_norm, freqs] = computeCompliance(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm
|
||||
% - freqs [] - Frequency vector to estimate the Frobenius norm
|
||||
%
|
||||
% Outputs:
|
||||
% - C [6x6 ss] - Compliance matrix
|
||||
% - C_norm [length(freqs)x1] - Frobenius norm of the Compliance matrix
|
||||
% - freqs [length(freqs)x1] - Frequency vector in [Hz]
|
||||
function [C, C_norm, freqs] = computeCompliance(args)
|
||||
% computeCompliance -
|
||||
%
|
||||
% Syntax: [C, C_norm, freqs] = computeCompliance(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - plots [true/false] - Should plot the transmissilibty matrix and its Frobenius norm
|
||||
% - freqs [] - Frequency vector to estimate the Frobenius norm
|
||||
%
|
||||
% Outputs:
|
||||
% - C [6x6 ss] - Compliance matrix
|
||||
% - C_norm [length(freqs)x1] - Frobenius norm of the Compliance matrix
|
||||
% - freqs [length(freqs)x1] - Frequency vector in [Hz]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
@@ -616,14 +616,14 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
args.plots logical {mustBeNumericOrLogical} = false
|
||||
args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000)
|
||||
end
|
||||
arguments
|
||||
args.plots logical {mustBeNumericOrLogical} = false
|
||||
args.freqs double {mustBeNumeric, mustBeNonnegative} = logspace(1,4,1000)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = args.freqs;
|
||||
freqs = args.freqs;
|
||||
#+end_src
|
||||
|
||||
*** Identification of the Compliance Matrix
|
||||
@@ -631,43 +631,43 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % External forces [N, N*m]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'output'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Disturbances/F_ext'], 1, 'openinput'); io_i = io_i + 1; % External forces [N, N*m]
|
||||
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'output'); io_i = io_i + 1; % Absolute Motion [m, rad]
|
||||
|
||||
%% Run the linearization
|
||||
C = linearize(mdl, io, options);
|
||||
C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
|
||||
C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
%% Run the linearization
|
||||
C = linearize(mdl, io, options);
|
||||
C.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
|
||||
C.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
#+begin_src matlab
|
||||
p_handle = zeros(6*6,1);
|
||||
p_handle = zeros(6*6,1);
|
||||
|
||||
if args.plots
|
||||
if args.plots
|
||||
fig = figure;
|
||||
for ix = 1:6
|
||||
for iy = 1:6
|
||||
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
for iy = 1:6
|
||||
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(C(ix, iy), freqs, 'Hz'))), 'k-');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
if ix < 6
|
||||
xticklabels({});
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
end
|
||||
if iy > 1
|
||||
yticklabels({});
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
linkaxes(p_handle, 'xy')
|
||||
@@ -678,7 +678,7 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
han.YLabel.Visible = 'on';
|
||||
xlabel(han, 'Frequency [Hz]');
|
||||
ylabel(han, 'Compliance [m/N, rad/(N*m)]');
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Computation of the Frobenius norm
|
||||
@@ -686,21 +686,21 @@ If wanted, the 6x6 transmissibility matrix is plotted.
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
freqs = args.freqs;
|
||||
freqs = args.freqs;
|
||||
|
||||
C_norm = zeros(length(freqs), 1);
|
||||
C_norm = zeros(length(freqs), 1);
|
||||
|
||||
for i = 1:length(freqs)
|
||||
for i = 1:length(freqs)
|
||||
C_norm(i) = sqrt(trace(freqresp(C, freqs(i), 'Hz')*freqresp(C, freqs(i), 'Hz')'));
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
if args.plots
|
||||
if args.plots
|
||||
figure;
|
||||
plot(freqs, C_norm)
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]');
|
||||
ylabel('Compliance - Frobenius Norm');
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
@@ -234,30 +234,30 @@ This will also gives us the range for which the approximate forward kinematic is
|
||||
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
** Stewart architecture definition
|
||||
We first define some general Stewart architecture.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
|
||||
** Comparison for "pure" translations
|
||||
@@ -267,31 +267,31 @@ We compute the approximate and exact required strut stroke to have the wanted mo
|
||||
The estimate required strut stroke for both the approximate and exact solutions are shown in Figure [[fig:inverse_kinematics_approx_validity_x_translation]].
|
||||
The relative strut length displacement is shown in Figure [[fig:inverse_kinematics_approx_validity_x_translation_relative]].
|
||||
#+begin_src matlab
|
||||
Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]
|
||||
Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]
|
||||
|
||||
Ls_approx = zeros(6, length(Xrs));
|
||||
Ls_exact = zeros(6, length(Xrs));
|
||||
Ls_approx = zeros(6, length(Xrs));
|
||||
Ls_exact = zeros(6, length(Xrs));
|
||||
|
||||
for i = 1:length(Xrs)
|
||||
for i = 1:length(Xrs)
|
||||
Xr = Xrs(i);
|
||||
L_approx(:, i) = stewart.kinematics.J*[Xr; 0; 0; 0; 0; 0;];
|
||||
[~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xr; 0; 0]);
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(Xrs, abs(L_approx(i, :)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(Xrs, abs(L_exact(i, :)), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Wanted $x$ displacement [m]');
|
||||
ylabel('Estimated required stroke');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Wanted $x$ displacement [m]');
|
||||
ylabel('Estimated required stroke');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
@@ -304,15 +304,15 @@ The relative strut length displacement is shown in Figure [[fig:inverse_kinemati
|
||||
[[file:figs/inverse_kinematics_approx_validity_x_translation.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(Xrs, abs(L_approx(i, :) - L_exact(i, :))./abs(L_approx(i, :) + L_exact(i, :)), 'k-');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Wanted $x$ displacement [m]');
|
||||
ylabel('Relative Stroke Error');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Wanted $x$ displacement [m]');
|
||||
ylabel('Relative Stroke Error');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
@@ -349,41 +349,41 @@ This is what is analyzed in this section.
|
||||
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
** Stewart architecture definition
|
||||
Let's first define the Stewart platform architecture that we want to study.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
|
||||
** Wanted translations and rotations
|
||||
Let's now define the wanted extreme translations and rotations.
|
||||
#+begin_src matlab
|
||||
Tx_max = 50e-6; % Translation [m]
|
||||
Ty_max = 50e-6; % Translation [m]
|
||||
Tz_max = 50e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
Tx_max = 50e-6; % Translation [m]
|
||||
Ty_max = 50e-6; % Translation [m]
|
||||
Tz_max = 50e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
#+end_src
|
||||
|
||||
** Needed stroke for "pure" rotations or translations
|
||||
@@ -391,17 +391,17 @@ As a first estimation, we estimate the needed actuator stroke for "pure" rotatio
|
||||
We do that using either the Inverse Kinematic solution or the Jacobian matrix as an approximation.
|
||||
|
||||
#+begin_src matlab
|
||||
LTx = stewart.kinematics.J*[Tx_max 0 0 0 0 0]';
|
||||
LTy = stewart.kinematics.J*[0 Ty_max 0 0 0 0]';
|
||||
LTz = stewart.kinematics.J*[0 0 Tz_max 0 0 0]';
|
||||
LRx = stewart.kinematics.J*[0 0 0 Rx_max 0 0]';
|
||||
LRy = stewart.kinematics.J*[0 0 0 0 Ry_max 0]';
|
||||
LRz = stewart.kinematics.J*[0 0 0 0 0 Rz_max]';
|
||||
LTx = stewart.kinematics.J*[Tx_max 0 0 0 0 0]';
|
||||
LTy = stewart.kinematics.J*[0 Ty_max 0 0 0 0]';
|
||||
LTz = stewart.kinematics.J*[0 0 Tz_max 0 0 0]';
|
||||
LRx = stewart.kinematics.J*[0 0 0 Rx_max 0 0]';
|
||||
LRy = stewart.kinematics.J*[0 0 0 0 Ry_max 0]';
|
||||
LRz = stewart.kinematics.J*[0 0 0 0 0 Rz_max]';
|
||||
#+end_src
|
||||
|
||||
The obtain required stroke is:
|
||||
#+begin_src matlab :results value replace :exports results
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', min(min([LTx,LTy,LTz,LRx,LRy])), max(max([LTx,LTy,LTz,LRx,LRy])), 1e6*(max(max([LTx,LTy,LTz,LRx,LRy]))-min(min([LTx,LTy,LTz,LRx,LRy]))))
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', min(min([LTx,LTy,LTz,LRx,LRy])), max(max([LTx,LTy,LTz,LRx,LRy])), 1e6*(max(max([LTx,LTy,LTz,LRx,LRy]))-min(min([LTx,LTy,LTz,LRx,LRy]))))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -416,11 +416,11 @@ To do so, we may estimate the required actuator stroke for all possible combinat
|
||||
|
||||
Let's first generate all the possible combination of maximum translation and rotations.
|
||||
#+begin_src matlab
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable(Ps, {}, {'*Tx [m]*', '*Ty [m]*', '*Tz [m]*', '*Rx [rad]*', '*Ry [rad]*', '*Rz [rad]*'}, ' %.1e ');
|
||||
data2orgtable(Ps, {}, {'*Tx [m]*', '*Ty [m]*', '*Tz [m]*', '*Rx [rad]*', '*Ry [rad]*', '*Rz [rad]*'}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -454,17 +454,17 @@ Let's first generate all the possible combination of maximum translation and rot
|
||||
|
||||
For all possible combination, we compute the required actuator stroke using the inverse kinematic solution.
|
||||
#+begin_src matlab
|
||||
L_min = 0;
|
||||
L_max = 0;
|
||||
L_min = 0;
|
||||
L_max = 0;
|
||||
|
||||
for i = 1:size(Ps,1)
|
||||
for i = 1:size(Ps,1)
|
||||
Rx = [1 0 0;
|
||||
0 cos(Ps(i, 4)) -sin(Ps(i, 4));
|
||||
0 sin(Ps(i, 4)) cos(Ps(i, 4))];
|
||||
|
||||
Ry = [ cos(Ps(i, 5)) 0 sin(Ps(i, 5));
|
||||
0 1 0;
|
||||
-sin(Ps(i, 5)) 0 cos(Ps(i, 5))];
|
||||
0 1 0;
|
||||
-sin(Ps(i, 5)) 0 cos(Ps(i, 5))];
|
||||
|
||||
Rz = [cos(Ps(i, 6)) -sin(Ps(i, 6)) 0;
|
||||
sin(Ps(i, 6)) cos(Ps(i, 6)) 0;
|
||||
@@ -474,17 +474,17 @@ For all possible combination, we compute the required actuator stroke using the
|
||||
[~, Ls] = inverseKinematics(stewart, 'AP', Ps(i, 1:3)', 'ARB', ARB);
|
||||
|
||||
if min(Ls) < L_min
|
||||
L_min = min(Ls)
|
||||
L_min = min(Ls)
|
||||
end
|
||||
if max(Ls) > L_max
|
||||
L_max = max(Ls)
|
||||
L_max = max(Ls)
|
||||
end
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
We obtain the required actuator stroke:
|
||||
#+begin_src matlab :results value replace :exports results
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', L_min, L_max, 1e6*(L_max-L_min))
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', L_min, L_max, 1e6*(L_max-L_min))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -513,36 +513,36 @@ However, for small displacements, we can use the Jacobian as an approximate solu
|
||||
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
** Stewart architecture definition
|
||||
Let's first define the Stewart platform architecture that we want to study.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
|
||||
Let's now define the actuator stroke.
|
||||
#+begin_src matlab
|
||||
L_min = -50e-6; % [m]
|
||||
L_max = 50e-6; % [m]
|
||||
L_min = -50e-6; % [m]
|
||||
L_max = 50e-6; % [m]
|
||||
#+end_src
|
||||
|
||||
** Pure translations
|
||||
@@ -557,21 +557,21 @@ To obtain the mobility "volume" attainable by the Stewart platform when it's ori
|
||||
|
||||
For each possible value of $(\theta, \phi)$, we compute the maximum radius $r$ attainable with the constraint that the stroke of each actuator should be between =L_min= and =L_max=.
|
||||
#+begin_src matlab
|
||||
thetas = linspace(0, pi, 50);
|
||||
phis = linspace(0, 2*pi, 50);
|
||||
rs = zeros(length(thetas), length(phis));
|
||||
thetas = linspace(0, pi, 50);
|
||||
phis = linspace(0, 2*pi, 50);
|
||||
rs = zeros(length(thetas), length(phis));
|
||||
|
||||
for i = 1:length(thetas)
|
||||
for i = 1:length(thetas)
|
||||
for j = 1:length(phis)
|
||||
Tx = sin(thetas(i))*cos(phis(j));
|
||||
Ty = sin(thetas(i))*sin(phis(j));
|
||||
Tz = cos(thetas(i));
|
||||
Tx = sin(thetas(i))*cos(phis(j));
|
||||
Ty = sin(thetas(i))*sin(phis(j));
|
||||
Tz = cos(thetas(i));
|
||||
|
||||
dL = stewart.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
|
||||
dL = stewart.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
|
||||
|
||||
rs(i, j) = max([dL(dL<0)*L_min; dL(dL>0)*L_max]);
|
||||
rs(i, j) = max([dL(dL<0)*L_min; dL(dL>0)*L_max]);
|
||||
end
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
|
||||
@@ -588,13 +588,13 @@ data2orgtable([1e6*L_min, 1e6*L_max, 1e6*(min(min(rs)))], {}, {'=L_min= [$\mu m$
|
||||
| -50.0 | 50.0 | 31.5 |
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
plot3(reshape(rs.*(sin(thetas)'*cos(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(sin(thetas)'*sin(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(cos(thetas)'*ones(1, length(phis))), [1, length(thetas)*length(phis)]))
|
||||
xlabel('X Translation [m]');
|
||||
ylabel('Y Translation [m]');
|
||||
zlabel('Z Translation [m]');
|
||||
figure;
|
||||
plot3(reshape(rs.*(sin(thetas)'*cos(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(sin(thetas)'*sin(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(cos(thetas)'*ones(1, length(phis))), [1, length(thetas)*length(phis)]))
|
||||
xlabel('X Translation [m]');
|
||||
ylabel('Y Translation [m]');
|
||||
zlabel('Z Translation [m]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
@@ -613,93 +613,93 @@ using this function https://fr.mathworks.com/help/matlab/ref/contour3.html
|
||||
** Introduction :ignore:
|
||||
** 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>>
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
** Estimation with an analytical model
|
||||
Let's first define the Stewart Platform Geometry.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 150e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart, 'AP', [0; 0; 0]);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 150e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart, 'AP', [0; 0; 0]);
|
||||
|
||||
As_init = stewart.geometry.As;
|
||||
As_init = stewart.geometry.As;
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Tx_max = 60e-6; % Translation [m]
|
||||
Ty_max = 60e-6; % Translation [m]
|
||||
Tz_max = 60e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
Tx_max = 60e-6; % Translation [m]
|
||||
Ty_max = 60e-6; % Translation [m]
|
||||
Tz_max = 60e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
flex_ang = zeros(size(Ps, 1), 6);
|
||||
Rxs = zeros(size(Ps, 1), 6)
|
||||
Rys = zeros(size(Ps, 1), 6)
|
||||
Rzs = zeros(size(Ps, 1), 6)
|
||||
flex_ang = zeros(size(Ps, 1), 6);
|
||||
Rxs = zeros(size(Ps, 1), 6)
|
||||
Rys = zeros(size(Ps, 1), 6)
|
||||
Rzs = zeros(size(Ps, 1), 6)
|
||||
|
||||
for Ps_i = 1:size(Ps, 1)
|
||||
Rx = [1 0 0;
|
||||
0 cos(Ps(Ps_i, 4)) -sin(Ps(Ps_i, 4));
|
||||
0 sin(Ps(Ps_i, 4)) cos(Ps(Ps_i, 4))];
|
||||
for Ps_i = 1:size(Ps, 1)
|
||||
Rx = [1 0 0;
|
||||
0 cos(Ps(Ps_i, 4)) -sin(Ps(Ps_i, 4));
|
||||
0 sin(Ps(Ps_i, 4)) cos(Ps(Ps_i, 4))];
|
||||
|
||||
Ry = [ cos(Ps(Ps_i, 5)) 0 sin(Ps(Ps_i, 5));
|
||||
0 1 0;
|
||||
-sin(Ps(Ps_i, 5)) 0 cos(Ps(Ps_i, 5))];
|
||||
Ry = [ cos(Ps(Ps_i, 5)) 0 sin(Ps(Ps_i, 5));
|
||||
0 1 0;
|
||||
-sin(Ps(Ps_i, 5)) 0 cos(Ps(Ps_i, 5))];
|
||||
|
||||
Rz = [cos(Ps(Ps_i, 6)) -sin(Ps(Ps_i, 6)) 0;
|
||||
sin(Ps(Ps_i, 6)) cos(Ps(Ps_i, 6)) 0;
|
||||
0 0 1];
|
||||
Rz = [cos(Ps(Ps_i, 6)) -sin(Ps(Ps_i, 6)) 0;
|
||||
sin(Ps(Ps_i, 6)) cos(Ps(Ps_i, 6)) 0;
|
||||
0 0 1];
|
||||
|
||||
ARB = Rz*Ry*Rx;
|
||||
ARB = Rz*Ry*Rx;
|
||||
|
||||
stewart = computeJointsPose(stewart, 'AP', Ps(Ps_i, 1:3)', 'ARB', ARB);
|
||||
stewart = computeJointsPose(stewart, 'AP', Ps(Ps_i, 1:3)', 'ARB', ARB);
|
||||
|
||||
flex_ang(Ps_i, :) = acos(sum(As_init.*stewart.geometry.As));
|
||||
|
||||
for l_i = 1:6
|
||||
MRf = stewart.platform_M.MRb(:,:,l_i)*(ARB')*(stewart.platform_F.FRa(:,:,l_i)');
|
||||
|
||||
Rys(Ps_i, l_i) = atan2(MRf(1,3), sqrt(MRf(1,1)^2 + MRf(2,2)^2));
|
||||
Rxs(Ps_i, l_i) = atan2(-MRf(2,3)/cos(Rys(Ps_i, l_i)), MRf(3,3)/cos(Rys(Ps_i, l_i)));
|
||||
Rzs(Ps_i, l_i) = atan2(-MRf(1,2)/cos(Rys(Ps_i, l_i)), MRf(1,1)/cos(Rys(Ps_i, l_i)));
|
||||
end
|
||||
end
|
||||
flex_ang(Ps_i, :) = acos(sum(As_init.*stewart.geometry.As));
|
||||
|
||||
for l_i = 1:6
|
||||
MRf = stewart.platform_M.MRb(:,:,l_i)*(ARB')*(stewart.platform_F.FRa(:,:,l_i)');
|
||||
|
||||
Rys(Ps_i, l_i) = atan2(MRf(1,3), sqrt(MRf(1,1)^2 + MRf(2,2)^2));
|
||||
Rxs(Ps_i, l_i) = atan2(-MRf(2,3)/cos(Rys(Ps_i, l_i)), MRf(3,3)/cos(Rys(Ps_i, l_i)));
|
||||
Rzs(Ps_i, l_i) = atan2(-MRf(1,2)/cos(Rys(Ps_i, l_i)), MRf(1,1)/cos(Rys(Ps_i, l_i)));
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
And the maximum bending of the flexible joints is: (in [mrad])
|
||||
#+begin_src matlab :results replace value
|
||||
1e3*max(max(abs(flex_ang)))
|
||||
1e3*max(max(abs(flex_ang)))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
: 1.1704
|
||||
|
||||
#+begin_src matlab :results replace value
|
||||
1e3*max(max(sqrt(Rxs.^2 + Rys.^2)))
|
||||
1e3*max(max(sqrt(Rxs.^2 + Rys.^2)))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
: 0.075063
|
||||
|
||||
#+begin_src matlab :results replace value
|
||||
1e3*max(max(Rzs))
|
||||
1e3*max(max(Rzs))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -708,89 +708,89 @@ And the maximum bending of the flexible joints is: (in [mrad])
|
||||
** Estimation using the Simscape Model
|
||||
*** Model Init
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'rigid');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
disturbances = initializeDisturbances();
|
||||
references = initializeReferences(stewart);
|
||||
ground = initializeGround('type', 'rigid');
|
||||
payload = initializePayload('type', 'none');
|
||||
controller = initializeController('type', 'open-loop');
|
||||
disturbances = initializeDisturbances();
|
||||
references = initializeReferences(stewart);
|
||||
#+end_src
|
||||
|
||||
*** Controller design to be close to the wanted displacement
|
||||
#+begin_src matlab
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
wc = 2*pi*30;
|
||||
Kl = diag(1./diag(abs(freqresp(G, wc)))) * wc/s * 1/(1 + s/3/wc);
|
||||
wc = 2*pi*30;
|
||||
Kl = diag(1./diag(abs(freqresp(G, wc)))) * wc/s * 1/(1 + s/3/wc);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
controller = initializeController('type', 'ref-track-L');
|
||||
controller = initializeController('type', 'ref-track-L');
|
||||
#+end_src
|
||||
|
||||
*** Simulations
|
||||
#+begin_src matlab
|
||||
Tx_max = 60e-6; % Translation [m]
|
||||
Ty_max = 60e-6; % Translation [m]
|
||||
Tz_max = 60e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
Tx_max = 60e-6; % Translation [m]
|
||||
Ty_max = 60e-6; % Translation [m]
|
||||
Tz_max = 60e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
cl_perf = zeros(size(Ps, 1), 1); % Closed loop performance [percentage]
|
||||
Rs = zeros(size(Ps, 1), 5); % Max Flexor angles for the 6 legs [mrad]
|
||||
cl_perf = zeros(size(Ps, 1), 1); % Closed loop performance [percentage]
|
||||
Rs = zeros(size(Ps, 1), 5); % Max Flexor angles for the 6 legs [mrad]
|
||||
|
||||
for Ps_i = 1:size(Ps, 1)
|
||||
fprintf('Experiment %i over %i', Ps_i, size(Ps, 1));
|
||||
|
||||
references = initializeReferences(stewart, 't', 0, 'r', Ps(Ps_i, :)');
|
||||
set_param('stewart_platform_model','StopTime','0.1');
|
||||
sim('stewart_platform_model');
|
||||
for Ps_i = 1:size(Ps, 1)
|
||||
fprintf('Experiment %i over %i', Ps_i, size(Ps, 1));
|
||||
|
||||
cl_perf(Ps_i) = 100*max(abs((simout.y.dLm.Data(end, :) - references.rL.Data(:,1,1)')./simout.y.dLm.Data(end, :)));
|
||||
Rs(Ps_i, :) = [max(abs(1e3*simout.y.Rf.Data(end, 1:2:end))), max(abs(1e3*simout.y.Rf.Data(end, 2:2:end))), max(abs(1e3*simout.y.Rm.Data(end, 1:3:end))), max(abs(1e3*simout.y.Rm.Data(end, 2:3:end))), max(abs(1e3*simout.y.Rm.Data(end, 3:3:end)))]';
|
||||
end
|
||||
references = initializeReferences(stewart, 't', 0, 'r', Ps(Ps_i, :)');
|
||||
set_param('stewart_platform_model','StopTime','0.1');
|
||||
sim('stewart_platform_model');
|
||||
|
||||
cl_perf(Ps_i) = 100*max(abs((simout.y.dLm.Data(end, :) - references.rL.Data(:,1,1)')./simout.y.dLm.Data(end, :)));
|
||||
Rs(Ps_i, :) = [max(abs(1e3*simout.y.Rf.Data(end, 1:2:end))), max(abs(1e3*simout.y.Rf.Data(end, 2:2:end))), max(abs(1e3*simout.y.Rm.Data(end, 1:3:end))), max(abs(1e3*simout.y.Rm.Data(end, 2:3:end))), max(abs(1e3*simout.y.Rm.Data(end, 3:3:end)))]';
|
||||
end
|
||||
#+end_src
|
||||
|
||||
Verify that the simulations are all correct:
|
||||
#+begin_src matlab :results replace value
|
||||
max(cl_perf)
|
||||
max(cl_perf)
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -798,7 +798,7 @@ Verify that the simulations are all correct:
|
||||
|
||||
*** Results
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable(max(Rs)', {'Rx bot', 'Ry bot', 'Rx top', 'Ry top', 'Rz top'}, {'Stroke [mrad]'}, ' %.2f ');
|
||||
data2orgtable(max(Rs)', {'Rx bot', 'Ry bot', 'Rx top', 'Ry top', 'Rz top'}, {'Stroke [mrad]'}, ' %.2f ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
@@ -826,22 +826,22 @@ This Matlab function is accessible [[file:../src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
@@ -849,14 +849,14 @@ This Matlab function is accessible [[file:../src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
#+end_src
|
||||
|
||||
|
||||
@@ -865,7 +865,7 @@ This Matlab function is accessible [[file:../src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
J = [As' , cross(Ab, As)'];
|
||||
J = [As' , cross(Ab, As)'];
|
||||
#+end_src
|
||||
|
||||
*** Compute Stiffness Matrix
|
||||
@@ -873,7 +873,7 @@ This Matlab function is accessible [[file:../src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
K = J'*diag(Ki)*J;
|
||||
K = J'*diag(Ki)*J;
|
||||
#+end_src
|
||||
|
||||
*** Compute Compliance Matrix
|
||||
@@ -881,7 +881,7 @@ This Matlab function is accessible [[file:../src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
C = inv(K);
|
||||
C = inv(K);
|
||||
#+end_src
|
||||
|
||||
*** Populate the =stewart= structure
|
||||
@@ -889,9 +889,9 @@ This Matlab function is accessible [[file:../src/computeJacobian.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
#+end_src
|
||||
|
||||
|
||||
@@ -934,23 +934,23 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
@@ -958,11 +958,11 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
@@ -970,14 +970,14 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
#+end_src
|
||||
|
||||
|
||||
@@ -986,11 +986,11 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
dLi = Li-l;
|
||||
dLi = Li-l;
|
||||
#+end_src
|
||||
|
||||
** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
|
||||
@@ -1007,21 +1007,21 @@ This Matlab function is accessible [[file:../src/forwardKinematicsApprox.m][here
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||||
% the Jacobian Matrix
|
||||
%
|
||||
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||||
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||||
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||||
% the Jacobian Matrix
|
||||
%
|
||||
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||||
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
@@ -1029,10 +1029,10 @@ This Matlab function is accessible [[file:../src/forwardKinematicsApprox.m][here
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
arguments
|
||||
stewart
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
@@ -1040,8 +1040,8 @@ This Matlab function is accessible [[file:../src/forwardKinematicsApprox.m][here
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
||||
J = stewart.kinematics.J;
|
||||
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
||||
J = stewart.kinematics.J;
|
||||
#+end_src
|
||||
|
||||
*** Computation
|
||||
@@ -1052,26 +1052,26 @@ From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
|
||||
position and orientation of {B} with respect to {A} using the following formula:
|
||||
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||||
#+begin_src matlab
|
||||
X = J\args.dL;
|
||||
X = J\args.dL;
|
||||
#+end_src
|
||||
|
||||
The position vector corresponds to the first 3 elements.
|
||||
#+begin_src matlab
|
||||
P = X(1:3);
|
||||
P = X(1:3);
|
||||
#+end_src
|
||||
|
||||
The next 3 elements are the orientation of {B} with respect to {A} expressed
|
||||
using the screw axis.
|
||||
#+begin_src matlab
|
||||
theta = norm(X(4:6));
|
||||
s = X(4:6)/theta;
|
||||
theta = norm(X(4:6));
|
||||
s = X(4:6)/theta;
|
||||
#+end_src
|
||||
|
||||
We then compute the corresponding rotation matrix.
|
||||
#+begin_src matlab
|
||||
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
#+end_src
|
||||
|
||||
* Bibliography :ignore:
|
||||
|
||||
40
org/nass.org
40
org/nass.org
@@ -45,40 +45,40 @@
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('../');
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_model.slx')
|
||||
open('stewart_platform_model.slx')
|
||||
#+end_src
|
||||
|
||||
** Identification of the Dynamics
|
||||
#+begin_src matlab
|
||||
apa = load('./mat/APA300ML.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
|
||||
flex_joint = load('./mat/flexor_ID16.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
|
||||
apa = load('./mat/APA300ML.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
|
||||
flex_joint = load('./mat/flexor_ID16.mat', 'int_xyz', 'int_i', 'n_xyz', 'n_i', 'nodes', 'M', 'K');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 175e-3);
|
||||
stewart = generateGeneralConfiguration(stewart, 'FH', 20e-3, 'MH', 20e-3, 'FR', 228e-3/2, 'MR', 220e-3/2, 'FTh', [-9, 9, 120-9, 120+9, 240-9, 240+9]*(pi/180), 'MTh', [-60+15, 60-15, 60+15, 180-15, 180+15, -60-15]*(pi/180));
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeFlexibleStrutDynamics(stewart, 'H', 0.03, 'K', apa.K, 'M', apa.M, 'n_xyz', apa.n_xyz, 'xi', 0.1, 'step_file', 'mat/APA300ML.STEP');
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'flexible', 'K_F', flex_joint.K, 'M_F', flex_joint.M, 'n_xyz_F', flex_joint.n_xyz, 'xi_F', 0.1, 'step_file_F', 'mat/flexor_ID16.STEP', 'type_M', 'flexible', 'K_M', flex_joint.K, 'M_M', flex_joint.M, 'n_xyz_M', flex_joint.n_xyz, 'xi_M', 0.1, 'step_file_M', 'mat/flexor_ID16.STEP');
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 125e-3);
|
||||
stewart = initializeCylindricalStruts(stewart, 'type_F', 'none', 'type_M', 'none');
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart);
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 175e-3);
|
||||
stewart = generateGeneralConfiguration(stewart, 'FH', 20e-3, 'MH', 20e-3, 'FR', 228e-3/2, 'MR', 220e-3/2, 'FTh', [-9, 9, 120-9, 120+9, 240-9, 240+9]*(pi/180), 'MTh', [-60+15, 60-15, 60+15, 180-15, 180+15, -60-15]*(pi/180));
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeFlexibleStrutDynamics(stewart, 'H', 0.03, 'K', apa.K, 'M', apa.M, 'n_xyz', apa.n_xyz, 'xi', 0.1, 'step_file', 'mat/APA300ML.STEP');
|
||||
stewart = initializeJointDynamics(stewart, 'type_F', 'flexible', 'K_F', flex_joint.K, 'M_F', flex_joint.M, 'n_xyz_F', flex_joint.n_xyz, 'xi_F', 0.1, 'step_file_F', 'mat/flexor_ID16.STEP', 'type_M', 'flexible', 'K_M', flex_joint.K, 'M_M', flex_joint.M, 'n_xyz_M', flex_joint.n_xyz, 'xi_M', 0.1, 'step_file_M', 'mat/flexor_ID16.STEP');
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 125e-3);
|
||||
stewart = initializeCylindricalStruts(stewart, 'type_F', 'none', 'type_M', 'none');
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeInertialSensor(stewart);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'rigid', 'm', 50);
|
||||
controller = initializeController('type', 'open-loop');
|
||||
ground = initializeGround('type', 'none');
|
||||
payload = initializePayload('type', 'rigid', 'm', 50);
|
||||
controller = initializeController('type', 'open-loop');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
disturbances = initializeDisturbances();
|
||||
references = initializeReferences(stewart);
|
||||
disturbances = initializeDisturbances();
|
||||
references = initializeReferences(stewart);
|
||||
#+end_src
|
||||
|
||||
@@ -65,12 +65,12 @@ This is done using something called "*Configuration Reference*" ([[https://fr.ma
|
||||
Basically, the configuration is stored in a mat file =conf_simscape.mat= and then loaded in the workspace for it to be accessible to all the simulink models.
|
||||
It is automatically loaded when the Simulink project is open. It can be loaded manually with the command:
|
||||
#+begin_src matlab :eval no
|
||||
load('mat/conf_simscape.mat');
|
||||
load('mat/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
It is however possible to modify specific parameters just for one simulation using the =set_param= command:
|
||||
#+begin_src matlab :eval no
|
||||
set_param(conf_simscape, 'StopTime', 1);
|
||||
set_param(conf_simscape, 'StopTime', 1);
|
||||
#+end_src
|
||||
|
||||
* Subsystem Reference
|
||||
@@ -158,29 +158,29 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [payload] = initializePayload(args)
|
||||
% initializePayload - Initialize the Payload that can then be used for simulations and analysis
|
||||
%
|
||||
% Syntax: [payload] = initializePayload(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type - 'none', 'rigid', 'flexible', 'cartesian'
|
||||
% - h [1x1] - Height of the CoM of the payload w.r.t {M} [m]
|
||||
% This also the position where K and C are defined
|
||||
% - K [6x1] - Stiffness of the Payload [N/m, N/rad]
|
||||
% - C [6x1] - Damping of the Payload [N/(m/s), N/(rad/s)]
|
||||
% - m [1x1] - Mass of the Payload [kg]
|
||||
% - I [3x3] - Inertia matrix for the Payload [kg*m2]
|
||||
%
|
||||
% Outputs:
|
||||
% - payload - Struture with the following properties:
|
||||
% - type - 1 (none), 2 (rigid), 3 (flexible)
|
||||
% - h [1x1] - Height of the CoM of the payload w.r.t {M} [m]
|
||||
% - K [6x1] - Stiffness of the Payload [N/m, N/rad]
|
||||
% - C [6x1] - Stiffness of the Payload [N/(m/s), N/(rad/s)]
|
||||
% - m [1x1] - Mass of the Payload [kg]
|
||||
% - I [3x3] - Inertia matrix for the Payload [kg*m2]
|
||||
function [payload] = initializePayload(args)
|
||||
% initializePayload - Initialize the Payload that can then be used for simulations and analysis
|
||||
%
|
||||
% Syntax: [payload] = initializePayload(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type - 'none', 'rigid', 'flexible', 'cartesian'
|
||||
% - h [1x1] - Height of the CoM of the payload w.r.t {M} [m]
|
||||
% This also the position where K and C are defined
|
||||
% - K [6x1] - Stiffness of the Payload [N/m, N/rad]
|
||||
% - C [6x1] - Damping of the Payload [N/(m/s), N/(rad/s)]
|
||||
% - m [1x1] - Mass of the Payload [kg]
|
||||
% - I [3x3] - Inertia matrix for the Payload [kg*m2]
|
||||
%
|
||||
% Outputs:
|
||||
% - payload - Struture with the following properties:
|
||||
% - type - 1 (none), 2 (rigid), 3 (flexible)
|
||||
% - h [1x1] - Height of the CoM of the payload w.r.t {M} [m]
|
||||
% - K [6x1] - Stiffness of the Payload [N/m, N/rad]
|
||||
% - C [6x1] - Stiffness of the Payload [N/(m/s), N/(rad/s)]
|
||||
% - m [1x1] - Mass of the Payload [kg]
|
||||
% - I [3x3] - Inertia matrix for the Payload [kg*m2]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
@@ -188,14 +188,14 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'cartesian'})} = 'none'
|
||||
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(6,1)
|
||||
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.h (1,1) double {mustBeNumeric, mustBeNonnegative} = 100e-3
|
||||
args.m (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
|
||||
args.I (3,3) double {mustBeNumeric, mustBeNonnegative} = 1*eye(3)
|
||||
end
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible', 'cartesian'})} = 'none'
|
||||
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(6,1)
|
||||
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
||||
args.h (1,1) double {mustBeNumeric, mustBeNonnegative} = 100e-3
|
||||
args.m (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
|
||||
args.I (3,3) double {mustBeNumeric, mustBeNonnegative} = 1*eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Add Payload Type
|
||||
@@ -203,16 +203,16 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
switch args.type
|
||||
case 'none'
|
||||
payload.type = 1;
|
||||
case 'rigid'
|
||||
payload.type = 2;
|
||||
case 'flexible'
|
||||
payload.type = 3;
|
||||
case 'cartesian'
|
||||
payload.type = 4;
|
||||
end
|
||||
switch args.type
|
||||
case 'none'
|
||||
payload.type = 1;
|
||||
case 'rigid'
|
||||
payload.type = 2;
|
||||
case 'flexible'
|
||||
payload.type = 3;
|
||||
case 'cartesian'
|
||||
payload.type = 4;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Add Stiffness, Damping and Mass properties of the Payload
|
||||
@@ -220,12 +220,12 @@ This Matlab function is accessible [[file:../src/initializePayload.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
payload.K = args.K;
|
||||
payload.C = args.C;
|
||||
payload.m = args.m;
|
||||
payload.I = args.I;
|
||||
payload.K = args.K;
|
||||
payload.C = args.C;
|
||||
payload.m = args.m;
|
||||
payload.I = args.I;
|
||||
|
||||
payload.h = args.h;
|
||||
payload.h = args.h;
|
||||
#+end_src
|
||||
|
||||
** Ground
|
||||
@@ -242,23 +242,23 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [ground] = initializeGround(args)
|
||||
% initializeGround - Initialize the Ground that can then be used for simulations and analysis
|
||||
%
|
||||
% Syntax: [ground] = initializeGround(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type - 'none', 'solid', 'flexible'
|
||||
% - rot_point [3x1] - Rotation point for the ground motion [m]
|
||||
% - K [3x1] - Translation Stiffness of the Ground [N/m]
|
||||
% - C [3x1] - Translation Damping of the Ground [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - ground - Struture with the following properties:
|
||||
% - type - 1 (none), 2 (rigid), 3 (flexible)
|
||||
% - K [3x1] - Translation Stiffness of the Ground [N/m]
|
||||
% - C [3x1] - Translation Damping of the Ground [N/(m/s)]
|
||||
function [ground] = initializeGround(args)
|
||||
% initializeGround - Initialize the Ground that can then be used for simulations and analysis
|
||||
%
|
||||
% Syntax: [ground] = initializeGround(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - type - 'none', 'solid', 'flexible'
|
||||
% - rot_point [3x1] - Rotation point for the ground motion [m]
|
||||
% - K [3x1] - Translation Stiffness of the Ground [N/m]
|
||||
% - C [3x1] - Translation Damping of the Ground [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - ground - Struture with the following properties:
|
||||
% - type - 1 (none), 2 (rigid), 3 (flexible)
|
||||
% - K [3x1] - Translation Stiffness of the Ground [N/m]
|
||||
% - C [3x1] - Translation Damping of the Ground [N/(m/s)]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
@@ -266,12 +266,12 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
arguments
|
||||
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'none'
|
||||
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.K (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e8*ones(3,1)
|
||||
args.C (3,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(3,1)
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Add Ground Type
|
||||
@@ -279,14 +279,14 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
switch args.type
|
||||
case 'none'
|
||||
ground.type = 1;
|
||||
case 'rigid'
|
||||
ground.type = 2;
|
||||
case 'flexible'
|
||||
ground.type = 3;
|
||||
end
|
||||
switch args.type
|
||||
case 'none'
|
||||
ground.type = 1;
|
||||
case 'rigid'
|
||||
ground.type = 2;
|
||||
case 'flexible'
|
||||
ground.type = 3;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Add Stiffness and Damping properties of the Ground
|
||||
@@ -294,8 +294,8 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
ground.K = args.K;
|
||||
ground.C = args.C;
|
||||
ground.K = args.K;
|
||||
ground.C = args.C;
|
||||
#+end_src
|
||||
|
||||
*** Rotation Point
|
||||
@@ -303,7 +303,7 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
ground.rot_point = args.rot_point;
|
||||
ground.rot_point = args.rot_point;
|
||||
#+end_src
|
||||
* Initialize Disturbances
|
||||
:PROPERTIES:
|
||||
@@ -318,13 +318,13 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [disturbances] = initializeDisturbances(args)
|
||||
% initializeDisturbances - Initialize the disturbances
|
||||
%
|
||||
% Syntax: [disturbances] = initializeDisturbances(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args -
|
||||
function [disturbances] = initializeDisturbances(args)
|
||||
% initializeDisturbances - Initialize the disturbances
|
||||
%
|
||||
% Syntax: [disturbances] = initializeDisturbances(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args -
|
||||
|
||||
#+end_src
|
||||
|
||||
@@ -333,12 +333,12 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
arguments
|
||||
args.Fd double {mustBeNumeric, mustBeReal} = zeros(6,1)
|
||||
args.Fd_t double {mustBeNumeric, mustBeReal} = 0
|
||||
args.Dw double {mustBeNumeric, mustBeReal} = zeros(6,1)
|
||||
args.Dw_t double {mustBeNumeric, mustBeReal} = 0
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
|
||||
@@ -347,7 +347,7 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
disturbances = struct();
|
||||
disturbances = struct();
|
||||
#+end_src
|
||||
|
||||
** Ground Motion
|
||||
@@ -355,7 +355,7 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
disturbances.Dw = timeseries([args.Dw], args.Dw_t);
|
||||
disturbances.Dw = timeseries([args.Dw], args.Dw_t);
|
||||
#+end_src
|
||||
|
||||
** Direct Forces
|
||||
@@ -363,7 +363,7 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
disturbances.Fd = timeseries([args.Fd], args.Fd_t);
|
||||
disturbances.Fd = timeseries([args.Fd], args.Fd_t);
|
||||
#+end_src
|
||||
|
||||
* Initialize References
|
||||
@@ -379,13 +379,13 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [references] = initializeReferences(stewart, args)
|
||||
% initializeReferences - Initialize the references
|
||||
%
|
||||
% Syntax: [references] = initializeReferences(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args -
|
||||
function [references] = initializeReferences(stewart, args)
|
||||
% initializeReferences - Initialize the references
|
||||
%
|
||||
% Syntax: [references] = initializeReferences(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args -
|
||||
|
||||
#+end_src
|
||||
|
||||
@@ -394,31 +394,31 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
arguments
|
||||
stewart
|
||||
args.t double {mustBeNumeric, mustBeReal} = 0
|
||||
args.r double {mustBeNumeric, mustBeReal} = zeros(6, 1)
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Compute the corresponding strut length
|
||||
#+begin_src matlab
|
||||
rL = zeros(6, length(args.t));
|
||||
rL = zeros(6, length(args.t));
|
||||
|
||||
for i = 1:length(args.t)
|
||||
for i = 1:length(args.t)
|
||||
R = [cos(args.r(6,i)) -sin(args.r(6,i)) 0;
|
||||
sin(args.r(6,i)) cos(args.r(6,i)) 0;
|
||||
0 0 1] * ...
|
||||
[cos(args.r(5,i)) 0 sin(args.r(5,i));
|
||||
0 1 0;
|
||||
-sin(args.r(5,i)) 0 cos(args.r(5,i))] * ...
|
||||
-sin(args.r(5,i)) 0 cos(args.r(5,i))] * ...
|
||||
[1 0 0;
|
||||
0 cos(args.r(4,i)) -sin(args.r(4,i));
|
||||
0 sin(args.r(4,i)) cos(args.r(4,i))];
|
||||
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', [args.r(1,i); args.r(2,i); args.r(3,i)], 'ARB', R);
|
||||
rL(:, i) = dLi;
|
||||
end
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', [args.r(1,i); args.r(2,i); args.r(3,i)], 'ARB', R);
|
||||
rL(:, i) = dLi;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** References
|
||||
@@ -426,6 +426,6 @@ This Matlab function is accessible [[file:../src/initializeGround.m][here]].
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
references.r = timeseries(args.r, args.t);
|
||||
references.rL = timeseries(rL, args.t);
|
||||
references.r = timeseries(args.r, args.t);
|
||||
references.rL = timeseries(rL, args.t);
|
||||
#+end_src
|
||||
|
||||
@@ -49,30 +49,30 @@ From the [[https://mathworks.com/products/simulink/projects.html][Simulink proje
|
||||
The project can be opened using the =simulinkproject= function:
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('../');
|
||||
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
|
||||
project = simulinkproject;
|
||||
projectRoot = project.RootFolder;
|
||||
project = simulinkproject;
|
||||
projectRoot = project.RootFolder;
|
||||
|
||||
myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
|
||||
myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
|
||||
myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
|
||||
myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
|
||||
|
||||
Simulink.fileGenControl('set',...
|
||||
'CacheFolder', myCacheFolder,...
|
||||
'CodeGenFolder', myCodeFolder,...
|
||||
'createDir', true);
|
||||
Simulink.fileGenControl('set',...
|
||||
'CacheFolder', myCacheFolder,...
|
||||
'CodeGenFolder', myCodeFolder,...
|
||||
'createDir', true);
|
||||
|
||||
%% Load the Simscape Configuration
|
||||
load('mat/conf_simscape.mat');
|
||||
%% Load the Simscape Configuration
|
||||
load('mat/conf_simscape.mat');
|
||||
#+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');
|
||||
Simulink.fileGenControl('reset');
|
||||
#+end_src
|
||||
|
||||
The project also permits to automatically add defined folder to the path when the project is opened.
|
||||
|
||||
@@ -38,16 +38,16 @@
|
||||
What causes the coupling from $F_i$ to $X_i$ ?
|
||||
|
||||
#+begin_src latex :file coupling.pdf
|
||||
\begin{tikzpicture}
|
||||
\node[block] (Jt) at (0, 0) {$J^{-T}$};
|
||||
\node[block, right= of Jt] (G) {$G$};
|
||||
\node[block, right= of G] (J) {$J^{-1}$};
|
||||
\begin{tikzpicture}
|
||||
\node[block] (Jt) at (0, 0) {$J^{-T}$};
|
||||
\node[block, right= of Jt] (G) {$G$};
|
||||
\node[block, right= of G] (J) {$J^{-1}$};
|
||||
|
||||
\draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$};
|
||||
\draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$};
|
||||
\draw[->] (G.east) -- (J.west) node[above left]{$q_i$};
|
||||
\draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$};
|
||||
\end{tikzpicture}
|
||||
\draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$};
|
||||
\draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$};
|
||||
\draw[->] (G.east) -- (J.west) node[above left]{$q_i$};
|
||||
\draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$};
|
||||
\end{tikzpicture}
|
||||
#+end_src
|
||||
|
||||
#+name: fig:block_diag_coupling
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user