Rework - New simscape file

This commit is contained in:
2020-02-13 15:19:30 +01:00
parent 2122ecbf74
commit fee3dd4ca3
2 changed files with 308 additions and 385 deletions

View File

@@ -38,8 +38,9 @@
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:
* Some tests
** Matlab Init :noexport:ignore:
* Compare external forces and forces applied by the actuators
** 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>>
#+end_src
@@ -52,57 +53,43 @@
simulinkproject('../');
#+end_src
** Simscape Model
#+begin_src matlab
open('stewart_platform_dynamics.slx')
open('stewart_platform_model.slx')
#+end_src
** test
** Comparison with fixed support
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart);
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
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
#+begin_src matlab
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
#+end_src
Estimation of the transfer function from $\bm{\tau}$ to $\mathcal{\bm{X}}$:
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/J-T'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
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);
@@ -111,26 +98,16 @@ Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}
#+end_src
#+begin_src matlab
G_cart = minreal(G*inv(stewart.J'));
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
Gc = minreal(G*inv(stewart.kinematics.J'));
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
#+end_src
Estimation of the transfer function from $\bm{\mathcal{F}}_{\text{ext}}$ to $\mathcal{\bm{X}}$:
#+begin_src matlab
figure; bode(G, G_cart)
#+end_src
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
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);
@@ -138,60 +115,70 @@ Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
freqs = logspace(0, 3, 1000);
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
bode(Gd, G)
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 [N/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]);
linkaxes([ax1,ax2],'x');
#+end_src
** Compare external forces and forces applied by the actuators
Initialization of the Stewart platform.
** Comparison with a flexible support
We redo the identification for when the Stewart platform is on a flexible support.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
ground = initializeGround('type', 'flexible');
#+end_src
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
Estimation of the transfer function from $\bm{\tau}$ to $\mathcal{\bm{X}}$:
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
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 = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
Estimation of the transfer function from $\mathcal{\bm{F}}_{d}$ to $\mathcal{\bm{X}}$:
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
Gc = minreal(G*inv(stewart.kinematics.J'));
Gc.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
#+end_src
Estimation of the transfer function from $\bm{\mathcal{F}}_{\text{ext}}$ to $\mathcal{\bm{X}}$:
#+begin_src matlab
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
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);
@@ -199,30 +186,75 @@ Estimation of the transfer function from $\mathcal{\bm{F}}_{d}$ to $\mathcal{\bm
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
#+end_src
Comparison of the two transfer function matrices.
#+begin_src matlab
freqs = logspace(0, 4, 1000);
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
bode(Gd, G, freqs)
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 [N/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]);
linkaxes([ax1,ax2],'x');
#+end_src
** Conclusion
#+begin_important
Seems quite similar.
The transfer function from forces/torques applied by the actuators on the payload $\bm{\mathcal{F}} = \bm{J}^T \bm{\tau}$ to the pose of the mobile platform $\bm{\mathcal{X}}$ is the same as the transfer function from external forces/torques to $\bm{\mathcal{X}}$ as long as the Stewart platform's base is fixed.
#+end_important
** Comparison of the static transfer function and the Compliance matrix
* Comparison of the static transfer function and the Compliance matrix
** 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>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
simulinkproject('../');
#+end_src
#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src
** Analysis
Initialization of the Stewart platform.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart);
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
#+begin_src matlab
ground = initializeGround('type', 'none');
payload = initializePayload('type', 'none');
#+end_src
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
@@ -232,88 +264,56 @@ Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
%% 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 = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
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'};
#+end_src
Let's first look at the low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$.
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(real(freqresp(G, 0.1)), {}, {}, ' %.1e ');
data2orgtable(real(freqresp(Gd, 0.1)), {}, {}, ' %.1e ');
#+end_src
#+RESULTS:
| 2.0e-06 | -9.1e-19 | -5.3e-12 | 7.3e-18 | 1.7e-05 | 1.3e-18 |
| -1.7e-18 | 2.0e-06 | 8.6e-19 | -1.7e-05 | -1.5e-17 | 6.7e-12 |
| 3.6e-13 | 3.2e-19 | 5.0e-07 | -2.5e-18 | 8.1e-12 | -1.5e-19 |
| 1.0e-17 | -1.7e-05 | -5.0e-18 | 1.9e-04 | 9.1e-17 | -3.5e-11 |
| 1.7e-05 | -6.9e-19 | -5.3e-11 | 6.9e-18 | 1.9e-04 | 4.8e-18 |
| -3.5e-18 | -4.5e-12 | 1.5e-18 | 7.1e-11 | -3.4e-17 | 4.6e-05 |
| 4.7e-08 | -7.2e-19 | 5.0e-18 | -8.9e-18 | 3.2e-07 | 9.9e-18 |
| 4.7e-18 | 4.7e-08 | -5.7e-18 | -3.2e-07 | -1.6e-17 | -1.7e-17 |
| 3.3e-18 | -6.3e-18 | 2.1e-08 | 4.4e-17 | 6.6e-18 | 7.4e-18 |
| -3.2e-17 | -3.2e-07 | 6.2e-18 | 5.2e-06 | -3.5e-16 | 6.3e-17 |
| 3.2e-07 | 2.7e-17 | 4.8e-17 | -4.5e-16 | 5.2e-06 | -1.2e-19 |
| 4.0e-17 | -9.5e-17 | 8.4e-18 | 4.3e-16 | 5.8e-16 | 1.7e-06 |
And now at the Compliance matrix.
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(stewart.C, {}, {}, ' %.1e ');
data2orgtable(stewart.kinematics.C, {}, {}, ' %.1e ');
#+end_src
#+RESULTS:
| 2.0e-06 | 2.9e-22 | 2.8e-22 | -3.2e-21 | 1.7e-05 | 1.5e-37 |
| -2.1e-22 | 2.0e-06 | -1.8e-23 | -1.7e-05 | -2.3e-21 | 1.1e-22 |
| 3.1e-22 | -1.6e-23 | 5.0e-07 | 1.7e-22 | 2.2e-21 | -8.1e-39 |
| 2.1e-21 | -1.7e-05 | 2.0e-22 | 1.9e-04 | 2.3e-20 | -8.7e-21 |
| 1.7e-05 | 2.5e-21 | 2.0e-21 | -2.8e-20 | 1.9e-04 | 1.3e-36 |
| 3.7e-23 | 3.1e-22 | -6.0e-39 | -1.0e-20 | 3.1e-22 | 4.6e-05 |
| 4.7e-08 | -2.0e-24 | 7.4e-25 | 5.9e-23 | 3.2e-07 | 5.9e-24 |
| -7.1e-25 | 4.7e-08 | 2.9e-25 | -3.2e-07 | -5.4e-24 | -3.3e-23 |
| 7.9e-26 | -6.4e-25 | 2.1e-08 | 1.9e-23 | 5.3e-25 | -6.5e-40 |
| 1.4e-23 | -3.2e-07 | 1.3e-23 | 5.2e-06 | 4.9e-22 | -3.8e-24 |
| 3.2e-07 | 7.6e-24 | 1.2e-23 | 6.9e-22 | 5.2e-06 | -2.6e-22 |
| 7.3e-24 | -3.2e-23 | -1.6e-39 | 9.9e-23 | -3.3e-22 | 1.7e-06 |
** Conclusion
#+begin_important
The low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$ corresponds to the compliance matrix of the Stewart platform.
#+end_important
** Transfer function from forces applied in the legs to the displacement of the legs
Initialization of the Stewart platform.
#+begin_src matlab
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
#+end_src
Estimation of the transfer function from $\bm{\tau}$ to $\bm{L}$:
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_platform_dynamics';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/J-T'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
#+end_src
#+begin_src matlab
freqs = logspace(1, 3, 1000);
figure; bode(G, 2*pi*freqs)
#+end_src
#+begin_src matlab
bodeFig({G(1,1), G(1,2)}, freqs, struct('phase', true));
#+end_src