Change all indentation
This commit is contained in:
+407
-407
File diff suppressed because it is too large
Load Diff
+870
-870
File diff suppressed because it is too large
Load Diff
+689
-689
File diff suppressed because it is too large
Load Diff
+569
-569
File diff suppressed because it is too large
Load Diff
+153
-153
@@ -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}}$.
|
||||
|
||||
+1034
-1034
File diff suppressed because it is too large
Load Diff
+279
-279
@@ -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
|
||||
|
||||
+283
-283
@@ -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:
|
||||
|
||||
+20
-20
@@ -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
|
||||
|
||||
+107
-107
@@ -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
|
||||
|
||||
+12
-12
@@ -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
|
||||
|
||||
+887
-887
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user