tdehaeze/phd-test-bench-id31
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove unecesary comments

Browse Source
This commit is contained in:
Thomas Dehaeze 2025-02-04 13:33:29 +01:00
parent d0c4020dee
commit af1e27b4c8
1 changed files with 80 additions and 396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

476
test-bench-id31.org
View File

@ -133,6 +133,21 @@ mat_dir = "/home/thomas/mnt/data_id31/nass"
- Reflectivity
- Diffraction tomography
** TODO [#A] Ask Veijo to give me a short summary (5 lines) for each experiment type with references
- Tomography
- Lateral scans
- "dirty layer scan"
- Reflectivity
- Diffraction tomography
** TODO [#A] Where to discuss the necessity of estimated Rz?
One big advantage of doing the control in the cartesian plane, is that we don't need the estimation of nano-hexapod Rz, therefore we don't need the encoders anymore!
Maybe this should be done in A6 (simscape-nass).
Here it can be reminded when doing the control in the cartesian frame.
** DONE [#A] Make detailed outline
CLOSED: [2025-01-30 Thu 11:34]
@ -186,14 +201,6 @@ CLOSED: [2025-02-01 Sat 18:38]
Some Tomography experiments are presented in ref:sec:test_id31_iff_hac
Maybe it should only be simulations, and put everything in the last "experimental" section.
** TODO [#A] Ask Veijo to give me a short summary (5 lines) for each experiment type with references
- Tomography
- Lateral scans
- "dirty layer scan"
- Reflectivity
- Diffraction tomography
** DONE [#A] Check why simulation gives worst performances than reality
CLOSED: [2025-02-01 Sat 18:38]
@ -201,13 +208,6 @@ CLOSED: [2025-02-01 Sat 18:38]
No but it makes the simulation faster!
- [ ] Maybe because the disturbances where estimated at 60RPM and not at 1RPM !
** TODO [#A] Where to discuss the necessity of estimated Rz?
One big advantage of doing the control in the cartesian plane, is that we don't need the estimation of nano-hexapod Rz, therefore we don't need the encoders anymore!
Maybe this should be done in A6 (simscape-nass).
Here it can be reminded when doing the control in the cartesian frame.
** DONE [#A] Add same specifications for all the curves
CLOSED: [2025-02-01 Sat 10:22]
@ -226,7 +226,8 @@ CLOSED: [2025-02-01 Sat 18:38]
*Dy, Dz, Ry*
** TODO [#C] Should I speak about higher bandwidth controllers even though they give lower performances?
** DONE [#C] Should I speak about higher bandwidth controllers even though they give lower performances?
CLOSED: [2025-02-04 Tue 13:24]
Probably not
@ -279,7 +280,8 @@ Look at =2023-08-10_18-32_identify_spurious_modes.mat=
*Conclusion is not clear*
** TODO [#B] Explain why position error does not converges to zero during tomography experiments
** DONE [#B] Explain why position error does not converges to zero during tomography experiments
CLOSED: [2025-02-04 Tue 13:24]
In closed-loop, the position does not converges to zeros but to a stable position.
@ -353,7 +355,8 @@ CLOSED: [2024-11-15 Fri 18:22]
Explain that this is due to digital conversions using additional electronics, but this is inducing additional delays.
This was latter resolved by directly decoding the correct digital protocol in the Speedgoat.
** TODO [#B] Verify sign of plants
** DONE [#B] Verify sign of plants
CLOSED: [2025-02-04 Tue 13:24]
Should be inverse compare to encoder output
@ -402,7 +405,8 @@ This means that height of nano-hexapod <=> beam is 800 - 530 - 95 = *175mm and n
it seems 150mm was used for the metrology jacobian!
- [X] If something is change, update the previous Simscape models
** TODO [#C] Verify notations
** DONE [#C] Verify notations
CLOSED: [2025-02-04 Tue 13:25]
$\bm{\epsilon\mathcal{L}}$ and not $\bm{e\mathcal{L}}$
$\bm{\epsilon\mathcal{X}}$ and not $\bm{e\mathcal{L}}$
@ -579,7 +583,7 @@ d_1 = D_y - l_2 R_x, \quad d_2 = D_y + l_1 R_x, \quad d_3 = -D_x - l_2 R_y, \qua
\hfill
#+attr_latex: :options [b]{0.48\linewidth}
#+begin_minipage
#+name: fig:align_top_sphere_comparators
#+name: fig:test_id31_align_top_sphere_comparators
#+attr_latex: :width \linewidth :float nil
#+caption: The top sphere is aligned with the rotation axis of the spindle using two probes.
[[file:figs/test_id31_align_top_sphere_comparators.jpg]]
@ -619,7 +623,7 @@ Hm = [ 0 1 0 -l2 0;
<<ssec:test_id31_metrology_sphere_rought_alignment>>
The two reference spheres are aligned with the rotation axis of the spindle.
To do so, two measuring probes are used as shown in Figure ref:fig:align_top_sphere_comparators.
To do so, two measuring probes are used as shown in Figure ref:fig:test_id31_align_top_sphere_comparators.
To not damage the sensitive sphere surface, the probes are instead positioned on the cylinder on which the sphere is mounted.
First, the probes are fixed to the bottom (fixed) cylinder to align the first sphere with the spindle axis.
@ -632,10 +636,10 @@ However, this first alignment should permit to position the two sphere within th
** Tip-Tilt adjustment of the interferometers
<<ssec:test_id31_metrology_alignment>>
The short-stroke metrology system is placed on top of the main granite using a gantry made of granite blocs (Figure ref:fig:short_stroke_metrology_overview).
The short-stroke metrology system is placed on top of the main granite using a gantry made of granite blocs (Figure ref:fig:test_id31_short_stroke_metrology_overview).
Granite is used to have good vibration and thermal stability.
#+name: fig:short_stroke_metrology_overview
#+name: fig:test_id31_short_stroke_metrology_overview
#+caption: Granite gantry used to fix the short-stroke metrology system
#+attr_latex: :width 0.8\linewidth
[[file:figs/test_id31_short_stroke_metrology_overview.jpg]]
@ -1155,9 +1159,9 @@ for i = 1:5
end
end
plot(f, abs(G_int(:, 1, 1)), 'color', [colors(1,:)], ...
'DisplayName', '$-e\mathcal{L}_i/u_i$ meas');
'DisplayName', '$-\epsilon\mathcal{L}_i/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'DisplayName', '$-e\mathcal{L}_i/u_i$ model');
'DisplayName', '$-\epsilon\mathcal{L}_i/u_i$ model');
for i = 2:6
plot(f, abs(G_int(:,i, i)), 'color', [colors(1,:)], ...
'HandleVisibility', 'off');
@ -1165,9 +1169,9 @@ for i = 2:6
'HandleVisibility', 'off');
end
plot(f, abs(G_int(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
'DisplayName', '$-e\mathcal{L}_i/u_j$ meas');
'DisplayName', '$-\epsilon\mathcal{L}_i/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'DisplayName', '$-e\mathcal{L}_i/u_j$ model');
'DisplayName', '$-\epsilon\mathcal{L}_i/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
@ -1407,9 +1411,9 @@ for i = 1:5
end
end
plot(f, abs(G_int_align(:, 1, 1)), 'color', [colors(1,:)], ...
'DisplayName', '$e\mathcal{L}_i/u_i$ meas');
'DisplayName', '$\epsilon\mathcal{L}_i/u_i$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 1)), freqs, 'Hz'))), 'color', [colors(2,:)], ...
'DisplayName', '$e\mathcal{L}_i/u_i$ model');
'DisplayName', '$\epsilon\mathcal{L}_i/u_i$ model');
for i = 2:6
plot(f, abs(G_int_align(:,i, i)), 'color', [colors(1,:)], ...
'HandleVisibility', 'off');
@ -1417,9 +1421,9 @@ for i = 2:6
'HandleVisibility', 'off');
end
plot(f, abs(G_int_align(:, 1, 2)), 'color', [colors(1,:), 0.2], ...
'DisplayName', '$e\mathcal{L}_i/u_j$ meas');
'DisplayName', '$\epsilon\mathcal{L}_i/u_j$ meas');
plot(freqs, abs(squeeze(freqresp(Gm(sprintf('eL%i', 1), sprintf('u%i', 2)), freqs, 'Hz'))), 'color', [colors(2,:), 0.2], ...
'DisplayName', '$e\mathcal{L}_i/u_j$ model');
'DisplayName', '$\epsilon\mathcal{L}_i/u_j$ model');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]');
@ -1771,10 +1775,10 @@ exportFig('figs/test_id31_comp_simscape_iff_diag_masses.pdf', 'width', 'half', '
#+end_src
#+name: fig:test_nhexa_comp_simscape_diag_masses
#+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the dynamics identified from the multi-body model. Both for the dynamics from $u$ to $e\mathcal{L}$ (\subref{fig:test_id31_comp_simscape_int_diag_masses}) and from $u$ to $V_s$ (\subref{fig:test_id31_comp_simscape_iff_diag_masses})
#+caption: Comparison of the diagonal elements (i.e. "direct" terms) of the measured FRF matrix and the dynamics identified from the multi-body model. Both for the dynamics from $u$ to $\epsilon\mathcal{L}$ (\subref{fig:test_id31_comp_simscape_int_diag_masses}) and from $u$ to $V_s$ (\subref{fig:test_id31_comp_simscape_iff_diag_masses})
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_simscape_int_diag_masses}from $u$ to $e\mathcal{L}$}
#+attr_latex: :caption \subcaption{\label{fig:test_id31_comp_simscape_int_diag_masses}from $u$ to $\epsilon\mathcal{L}$}
#+attr_latex: :options {0.49\textwidth}
#+begin_subfigure
#+attr_latex: :width 0.95\linewidth
@ -1793,7 +1797,7 @@ exportFig('figs/test_id31_comp_simscape_iff_diag_masses.pdf', 'width', 'half', '
To verify that all the kinematics in Figure ref:fig:test_id31_block_schematic_plant are correct and to check whether the system dynamics is affected by Spindle rotation of not, three identification experiments were performed: no spindle rotation, spindle rotation at $36\,\text{deg}/s$ and at $180\,\text{deg}/s$.
The comparison of the obtained dynamics from command signal $u$ to estimated strut error $e\mathcal{L}$ is done in Figure ref:fig:test_id31_effect_rotation.
The comparison of the obtained dynamics from command signal $u$ to estimated strut error $\epsilon\mathcal{L}$ is done in Figure ref:fig:test_id31_effect_rotation.
Both direct terms (Figure ref:fig:test_id31_effect_rotation_direct) and coupling terms (Figure ref:fig:test_id31_effect_rotation_coupling) are unaffected by the rotation.
The same can be observed for the dynamics from the command signal to the encoders and to the force sensors.
This confirms that the rotation has no significant effect on the plant dynamics.
@ -1938,7 +1942,7 @@ exportFig('figs/test_id31_effect_rotation_coupling.pdf', 'width', 'half', 'heigh
#+end_src
#+name: fig:test_id31_effect_rotation
#+caption: Effect of the spindle rotation on the plant dynamics from $u$ to $e\mathcal{L}$. Three rotational velocities are tested ($0\,\text{deg}/s$, $36\,\text{deg}/s$ and $180\,\text{deg}/s$). Both direct terms (\subref{fig:test_id31_effect_rotation_direct}) and coupling terms (\subref{fig:test_id31_effect_rotation_coupling}) are displayed.
#+caption: Effect of the spindle rotation on the plant dynamics from $u$ to $\epsilon\mathcal{L}$. Three rotational velocities are tested ($0\,\text{deg}/s$, $36\,\text{deg}/s$ and $180\,\text{deg}/s$). Both direct terms (\subref{fig:test_id31_effect_rotation_direct}) and coupling terms (\subref{fig:test_id31_effect_rotation_coupling}) are displayed.
#+attr_latex: :options [htbp]
#+begin_figure
#+attr_latex: :caption \subcaption{\label{fig:test_id31_effect_rotation_direct}Direct terms}
@ -2494,21 +2498,21 @@ tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
ax1 = nexttile([2,1]);
hold on;
plot(freqs, abs(squeeze(freqresp(Gm_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.3], ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 0 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.3], ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 13 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(3,:), 0.3], ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 26 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', [colors(4,:), 0.3], ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1$ - 39 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1$ - 39 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m0_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(1,:), ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 0 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 0 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m1_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(2,:), ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 13 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 13 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m2_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(3,:), ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 26 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 26 kg');
plot(freqs, abs(squeeze(freqresp(Gm_hac_m3_Wz0('eL1', 'u1'), freqs, 'Hz'))), 'color', colors(4,:), ...
'DisplayName', '$-e\mathcal{L}_{1}/u_1^\prime$ - 39 kg');
'DisplayName', '$-\epsilon\mathcal{L}_{1}/u_1^\prime$ - 39 kg');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
@ -5078,7 +5082,7 @@ For lateral scanning, the system performed well at moderate speeds ($10\,\mu m/s
The most challenging test case - diffraction tomography combining rotation and lateral scanning - demonstrated the system's ability to maintain vertical and angular stability while highlighting some limitations in lateral positioning during rapid accelerations.
These limitations could potentially be addressed through feedforward control or alternative detector triggering strategies.
Overall, the experimental results validate the effectiveness of the developed control architecture and demonstrate that the NASS meets most design specifications across a wide range of operating conditions (summarized in Table ref:tab:id31_experiments_results_summary).
Overall, the experimental results validate the effectiveness of the developed control architecture and demonstrate that the NASS meets most design specifications across a wide range of operating conditions (summarized in Table ref:tab:test_id31_experiments_results_summary).
The identified limitations, primarily related to high-speed lateral scanning and heavy payload handling, provide clear directions for future improvements.
#+begin_src matlab
@ -5106,7 +5110,7 @@ The identified limitations, primarily related to high-speed lateral scanning and
1e9*data_dt_1000ums.Dy_rms_cl, 1e9*data_dt_1000ums.Dz_rms_cl, 1e6*data_dt_1000ums.Ry_rms_cl % Diffraction Tomo - CL - 6deg/s, 1000um/s
#+end_src
#+name: tab:id31_experiments_results_summary
#+name: tab:test_id31_experiments_results_summary
#+caption: Summary of the experimental results performed with the NASS on ID31. Open-loop errors are indicated at the left of the arrows. Closed-loop errors that are out of specifications are indicated by bold number.
#+attr_latex: :environment tabularx :width \linewidth :align Xccc
#+attr_latex: :center t :booktabs t
@ -6889,49 +6893,38 @@ end
:header-args:matlab+: :tangle matlab/src/initializeStewartPlatform.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Documentation
#+name: fig:stewart-frames-position
#+caption: Definition of the position of the frames
[[file:figs/stewart-frames-position.png]]
**** Function description
#+begin_src matlab
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
#+end_src
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
**** Initialize the Stewart structure
#+begin_src matlab
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();
#+end_src
*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
@ -6940,13 +6933,7 @@ end
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Documentation
#+name: fig:stewart-frames-position
#+caption: Definition of the position of the frames
[[file:figs/stewart-frames-position.png]]
**** Function description
*** Function description
#+begin_src matlab
function [stewart] = initializeFramesPositions(stewart, args)
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
@ -6964,19 +6951,13 @@ end
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
end
#+end_src
**** Compute the position of each frame
#+begin_src matlab
H = args.H; % Total Height of the Stewart Platform [m]
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
@ -6984,10 +6965,7 @@ end
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.geometry.H = H;
stewart.geometry.FO_M = FO_M;
stewart.platform_M.MO_B = MO_B;
@ -7000,52 +6978,6 @@ end
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Documentation
#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
\begin{tikzpicture}
% Internal and external limit
\draw[fill=white!80!black] (0, 0) circle [radius=3];
% Circle where the joints are located
\draw[dashed] (0, 0) circle [radius=2.5];
% Bullets for the positions of the joints
\node[] (J1) at ( 80:2.5){$\bullet$};
\node[] (J2) at (100:2.5){$\bullet$};
\node[] (J3) at (200:2.5){$\bullet$};
\node[] (J4) at (220:2.5){$\bullet$};
\node[] (J5) at (320:2.5){$\bullet$};
\node[] (J6) at (340:2.5){$\bullet$};
% Name of the points
\node[above right] at (J1) {$a_{1}$};
\node[above left] at (J2) {$a_{2}$};
\node[above left] at (J3) {$a_{3}$};
\node[right ] at (J4) {$a_{4}$};
\node[left ] at (J5) {$a_{5}$};
\node[above right] at (J6) {$a_{6}$};
% First 2 angles
\draw[dashed, ->] (0:1) arc [start angle=0, end angle=80, radius=1] node[below right]{$\theta_{1}$};
\draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left ]{$\theta_{2}$};
% Division of 360 degrees by 3
\draw[dashed] (0, 0) -- ( 80:3.2);
\draw[dashed] (0, 0) -- (100:3.2);
\draw[dashed] (0, 0) -- (200:3.2);
\draw[dashed] (0, 0) -- (220:3.2);
\draw[dashed] (0, 0) -- (320:3.2);
\draw[dashed] (0, 0) -- (340:3.2);
% Radius for the position of the joints
\draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);
\draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
\draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
\end{tikzpicture}
#+end_src
**** Function description
#+begin_src matlab
function [stewart] = generateGeneralConfiguration(stewart, args)
% generateGeneralConfiguration - Generate a Very General Configuration
@ -7065,10 +6997,7 @@ end
% - stewart - updated Stewart structure with the added fields:
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
@ -7078,23 +7007,15 @@ end
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
end
#+end_src
**** Compute the pose
#+begin_src matlab
Fa = zeros(3,6);
Mb = zeros(3,6);
#+end_src
#+begin_src matlab
for i = 1:6
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
end
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;
#+end_src
@ -7105,13 +7026,6 @@ end
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Documentation
#+name: fig:stewart-struts
#+caption: Position and orientation of the struts
[[file:figs/stewart-struts.png]]
**** Function description
#+begin_src matlab
function [stewart] = computeJointsPose(stewart)
% computeJointsPose -
@ -7139,10 +7053,7 @@ end
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
#+end_src
**** Check the =stewart= structure elements
#+begin_src matlab
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;
@ -7157,30 +7068,19 @@ end
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
FO_M = stewart.geometry.FO_M;
#+end_src
**** Compute the position of the Joints
#+begin_src matlab
Aa = Fa - repmat(FO_A, [1, 6]);
Bb = Mb - repmat(MO_B, [1, 6]);
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
#+end_src
**** Compute the strut length and orientation
#+begin_src matlab
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
l = vecnorm(Ab - Aa)';
#+end_src
#+begin_src matlab
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
#+end_src
**** Compute the orientation of the Joints
#+begin_src matlab
FRa = zeros(3,3,6);
MRb = zeros(3,3,6);
@ -7191,10 +7091,7 @@ end
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
end
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.geometry.Aa = Aa;
stewart.geometry.Ab = Ab;
stewart.geometry.Ba = Ba;
@ -7216,7 +7113,6 @@ end
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Function description
#+begin_src matlab
function [stewart] = initializeCylindricalPlatforms(stewart, args)
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
@ -7245,10 +7141,7 @@ end
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
% - H [1x1] - Mobile Platform Height [m]
% - R [1x1] - Mobile Platform Radius [m]
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
@ -7258,32 +7151,22 @@ end
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
end
#+end_src
**** Compute the Inertia matrices of platforms
#+begin_src matlab
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/2 *args.Fpm * args.Fpr^2]);
#+end_src
#+begin_src matlab
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/2 *args.Mpm * args.Mpr^2]);
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.platform_F.type = 1;
stewart.platform_F.I = I_F;
stewart.platform_F.M = args.Fpm;
stewart.platform_F.R = args.Fpr;
stewart.platform_F.H = args.Fph;
#+end_src
#+begin_src matlab
stewart.platform_M.type = 1;
stewart.platform_M.I = I_M;
@ -7298,7 +7181,6 @@ end
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Function description
#+begin_src matlab
function [stewart] = initializeCylindricalStruts(stewart, args)
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
@ -7326,10 +7208,7 @@ end
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
@ -7339,11 +7218,7 @@ end
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
end
#+end_src
**** Compute the properties of the cylindrical struts
#+begin_src matlab
Fsm = ones(6,1).*args.Fsm;
Fsh = ones(6,1).*args.Fsh;
Fsr = ones(6,1).*args.Fsr;
@ -7351,9 +7226,7 @@ end
Msm = ones(6,1).*args.Msm;
Msh = ones(6,1).*args.Msh;
Msr = ones(6,1).*args.Msr;
#+end_src
#+begin_src matlab
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
@ -7366,19 +7239,14 @@ end
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
1/2 * Msm(i) * Msr(i)^2]);
end
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.struts_M.type = 1;
stewart.struts_M.I = I_M;
stewart.struts_M.M = Msm;
stewart.struts_M.R = Msr;
stewart.struts_M.H = Msh;
#+end_src
#+begin_src matlab
stewart.struts_F.type = 1;
stewart.struts_F.I = I_F;
@ -7393,49 +7261,6 @@ end
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Documentation
#+name: fig:piezoelectric_stack
#+attr_html: :width 500px
#+caption: Example of a piezoelectric stach actuator (PI)
[[file:figs/piezoelectric_stack.jpg]]
A simplistic model of such amplified actuator is shown in Figure ref:fig:actuator_model_simple where:
- $K$ represent the vertical stiffness of the actuator
- $C$ represent the vertical damping of the actuator
- $F$ represents the force applied by the actuator
- $F_{m}$ represents the total measured force
- $v_{m}$ represents the absolute velocity of the top part of the actuator
- $d_{m}$ represents the total relative displacement of the actuator
#+begin_src latex :file actuator_model_simple.pdf :tangle no
\begin{tikzpicture}
\draw (-1, 0) -- (1, 0);
% Spring, Damper, and Actuator
\draw[spring] (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
\draw[damper] ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
\draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};
\node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};
\node[left] at (fsens.west) {$F_{m}$};
\draw[dashed] (1, 0) -- ++(0.4, 0);
\draw[dashed] (1, 1.7) -- ++(0.4, 0);
\draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};
\draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
\end{tikzpicture}
#+end_src
#+name: fig:actuator_model_simple
#+caption: Simple model of an Actuator
#+RESULTS:
[[file:figs/actuator_model_simple.png]]
**** Function description
#+begin_src matlab
function [stewart] = initializeStrutDynamics(stewart, args)
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
@ -7452,10 +7277,7 @@ A simplistic model of such amplified actuator is shown in Figure ref:fig:actuato
% - actuators.type = 1
% - actuators.K [6x1] - Stiffness of each strut [N/m]
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
@ -7469,10 +7291,7 @@ A simplistic model of such amplified actuator is shown in Figure ref:fig:actuato
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
end
#+end_src
**** Add Stiffness and Damping properties of each strut
#+begin_src matlab
if strcmp(args.type, 'classical')
stewart.actuators.type = 1;
elseif strcmp(args.type, 'amplified')
@ -7500,7 +7319,6 @@ A simplistic model of such amplified actuator is shown in Figure ref:fig:actuato
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Function description
#+begin_src matlab
function [stewart] = initializeJointDynamics(stewart, args)
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
@ -7530,10 +7348,7 @@ A simplistic model of such amplified actuator is shown in Figure ref:fig:actuato
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
@ -7565,10 +7380,7 @@ A simplistic model of such amplified actuator is shown in Figure ref:fig:actuato
args.xi_F double {mustBeNumeric} = 0.1
args.step_file_F char {} = ''
end
#+end_src
**** Add Actuator Type
#+begin_src matlab
switch args.type_F
case 'universal'
stewart.joints_F.type = 1;
@ -7602,49 +7414,31 @@ A simplistic model of such amplified actuator is shown in Figure ref:fig:actuato
case 'spherical_3dof'
stewart.joints_M.type = 7;
end
#+end_src
**** Add Stiffness and Damping in Translation of each strut
Axial and Radial (shear) Stiffness
#+begin_src matlab
stewart.joints_M.Ka = args.Ka_M;
stewart.joints_M.Kr = args.Kr_M;
stewart.joints_F.Ka = args.Ka_F;
stewart.joints_F.Kr = args.Kr_F;
#+end_src
Translation Damping
#+begin_src matlab
stewart.joints_M.Ca = args.Ca_M;
stewart.joints_M.Cr = args.Cr_M;
stewart.joints_F.Ca = args.Ca_F;
stewart.joints_F.Cr = args.Cr_F;
#+end_src
**** Add Stiffness and Damping in Rotation of each strut
Rotational Stiffness
#+begin_src matlab
stewart.joints_M.Kf = args.Kf_M;
stewart.joints_M.Kt = args.Kt_M;
stewart.joints_F.Kf = args.Kf_F;
stewart.joints_F.Kt = args.Kt_F;
#+end_src
Rotational Damping
#+begin_src matlab
stewart.joints_M.Cf = args.Cf_M;
stewart.joints_M.Ct = args.Ct_M;
stewart.joints_F.Cf = args.Cf_F;
stewart.joints_F.Ct = args.Ct_F;
#+end_src
**** Stiffness and Mass matrices for flexible joint
#+begin_src matlab
stewart.joints_F.M = args.M_F;
stewart.joints_F.K = args.K_F;
stewart.joints_F.n_xyz = args.n_xyz_F;
@ -7665,7 +7459,6 @@ Rotational Damping
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Function description
#+begin_src matlab
function [stewart] = initializeStewartPose(stewart, args)
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
@ -7684,24 +7477,15 @@ Rotational Damping
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - actuators.Leq [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
#+begin_src matlab
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
#+end_src
**** Use the Inverse Kinematic function
#+begin_src matlab
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.actuators.Leq = dLi;
#+end_src
@ -7711,7 +7495,6 @@ Rotational Damping
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Function description
#+begin_src matlab
function [stewart] = computeJacobian(stewart)
% computeJacobian -
@ -7729,10 +7512,7 @@ Rotational Damping
% - 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
#+begin_src matlab
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
As = stewart.geometry.As;
@ -7741,26 +7521,13 @@ Rotational Damping
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
Ki = stewart.actuators.K;
#+end_src
**** Compute Jacobian Matrix
#+begin_src matlab
J = [As' , cross(Ab, As)'];
#+end_src
**** Compute Stiffness Matrix
#+begin_src matlab
K = J'*diag(Ki)*J;
#+end_src
**** Compute Compliance Matrix
#+begin_src matlab
C = inv(K);
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.kinematics.J = J;
stewart.kinematics.K = K;
stewart.kinematics.C = C;
@ -7772,45 +7539,6 @@ Rotational Damping
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Geophone - Working Principle
From the schematic of the Z-axis geophone shown in Figure ref:fig:z_axis_geophone, we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
with:
- $\omega_0 = \sqrt{\frac{k}{m}}$
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
#+name: fig:z_axis_geophone
#+caption: Schematic of a Z-Axis geophone
[[file:figs/inertial_sensor.png]]
We see that at frequencies above $\omega_0$:
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
**** Accelerometer - Working Principle
From the schematic of the Z-axis accelerometer shown in Figure ref:fig:z_axis_accelerometer, we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
with:
- $\omega_0 = \sqrt{\frac{k}{m}}$
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
#+name: fig:z_axis_accelerometer
#+caption: Schematic of a Z-Axis geophone
[[file:figs/inertial_sensor.png]]
We see that at frequencies below $\omega_0$:
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
Note that there is trade-off between:
- the highest measurable acceleration $\omega_0$
- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
**** Function description
#+begin_src matlab
function [stewart] = initializeInertialSensor(stewart, args)
% initializeInertialSensor - Initialize the inertial sensor in each strut
@ -7831,20 +7559,14 @@ Note that there is trade-off between:
% - C [1x1] - Damping [N/(m/s)]
% - M [1x1] - Inertial Mass [kg]
% - G [1x1] - Gain
#+end_src
**** Optional Parameters
#+begin_src matlab
arguments
stewart
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
end
#+end_src
**** Compute the properties of the sensor
#+begin_src matlab
sensor = struct();
switch args.type
@ -7864,10 +7586,7 @@ Note that there is trade-off between:
case 'none'
sensor.type = 3;
end
#+end_src
**** Populate the =stewart= structure
#+begin_src matlab
stewart.sensors.inertial = sensor;
#+end_src
@ -7879,29 +7598,6 @@ Note that there is trade-off between:
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
**** Theory
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
\begin{align*}
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
\end{align*}
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
\begin{equation}
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
\end{equation}
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
\begin{equation}
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
\end{equation}
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
**** Function description
#+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}
@ -7920,19 +7616,13 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
% 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
#+begin_src matlab
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
#+begin_src matlab
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
Aa = stewart.geometry.Aa;
@ -7941,15 +7631,9 @@ Otherwise, when the limbs' lengths derived yield complex numbers, then the posit
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
l = stewart.geometry.l;
#+end_src
**** Compute
#+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));
#+end_src
#+begin_src matlab
dLi = Li-l;
#+end_src