diff --git a/docs/figs/hybrid_control_Ex.pdf b/docs/figs/hybrid_control_Ex.pdf new file mode 100644 index 0000000..6f75e06 Binary files /dev/null and b/docs/figs/hybrid_control_Ex.pdf differ diff --git a/docs/figs/hybrid_control_Ex.png b/docs/figs/hybrid_control_Ex.png new file mode 100644 index 0000000..c6ef318 Binary files /dev/null and b/docs/figs/hybrid_control_Ex.png differ diff --git a/docs/figs/hybrid_control_Kl_loop_gain.pdf b/docs/figs/hybrid_control_Kl_loop_gain.pdf new file mode 100644 index 0000000..806dd9b Binary files /dev/null and b/docs/figs/hybrid_control_Kl_loop_gain.pdf differ diff --git a/docs/figs/hybrid_control_Kl_loop_gain.png b/docs/figs/hybrid_control_Kl_loop_gain.png new file mode 100644 index 0000000..dbcfabe Binary files /dev/null and b/docs/figs/hybrid_control_Kl_loop_gain.png differ diff --git a/docs/figs/hybrid_control_Kl_plant_diagonal.pdf b/docs/figs/hybrid_control_Kl_plant_diagonal.pdf new file mode 100644 index 0000000..47b4383 Binary files /dev/null and b/docs/figs/hybrid_control_Kl_plant_diagonal.pdf differ diff --git a/docs/figs/hybrid_control_Kl_plant_diagonal.png b/docs/figs/hybrid_control_Kl_plant_diagonal.png new file mode 100644 index 0000000..00f06c1 Binary files /dev/null and b/docs/figs/hybrid_control_Kl_plant_diagonal.png differ diff --git a/docs/figs/hybrid_control_Kl_plant_off_diagonal.pdf b/docs/figs/hybrid_control_Kl_plant_off_diagonal.pdf new file mode 100644 index 0000000..31b43e7 Binary files /dev/null and b/docs/figs/hybrid_control_Kl_plant_off_diagonal.pdf differ diff --git a/docs/figs/hybrid_control_Kl_plant_off_diagonal.png b/docs/figs/hybrid_control_Kl_plant_off_diagonal.png new file mode 100644 index 0000000..b64d71d Binary files /dev/null and b/docs/figs/hybrid_control_Kl_plant_off_diagonal.png differ diff --git a/docs/figs/hybrid_control_Kx_loop_gain.pdf b/docs/figs/hybrid_control_Kx_loop_gain.pdf new file mode 100644 index 0000000..bae1e32 Binary files /dev/null and b/docs/figs/hybrid_control_Kx_loop_gain.pdf differ diff --git a/docs/figs/hybrid_control_Kx_loop_gain.png b/docs/figs/hybrid_control_Kx_loop_gain.png new file mode 100644 index 0000000..4812dcb Binary files /dev/null and b/docs/figs/hybrid_control_Kx_loop_gain.png differ diff --git a/docs/figs/hybrid_control_Kx_plant.pdf b/docs/figs/hybrid_control_Kx_plant.pdf new file mode 100644 index 0000000..b546c5b Binary files /dev/null and b/docs/figs/hybrid_control_Kx_plant.pdf differ diff --git a/docs/figs/hybrid_control_Kx_plant.png b/docs/figs/hybrid_control_Kx_plant.png new file mode 100644 index 0000000..2582f4d Binary files /dev/null and b/docs/figs/hybrid_control_Kx_plant.png differ diff --git a/docs/figs/hybrid_reference_tracking.pdf b/docs/figs/hybrid_reference_tracking.pdf index 816bd5e..be46a58 100644 Binary files a/docs/figs/hybrid_reference_tracking.pdf and b/docs/figs/hybrid_reference_tracking.pdf differ diff --git a/docs/figs/hybrid_reference_tracking.png b/docs/figs/hybrid_reference_tracking.png index 27b8b83..4b7e233 100644 Binary files a/docs/figs/hybrid_reference_tracking.png and b/docs/figs/hybrid_reference_tracking.png differ diff --git a/matlab/stewart_platform_model.slx b/matlab/stewart_platform_model.slx index 45a190a..7a17f24 100644 Binary files a/matlab/stewart_platform_model.slx and b/matlab/stewart_platform_model.slx differ diff --git a/org/active-damping.org b/org/active-damping.org index ca82745..79143ed 100644 --- a/org/active-damping.org +++ b/org/active-damping.org @@ -772,6 +772,7 @@ The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]]. #+begin_important Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors. #+end_important + * Compliance and Transmissibility Comparison ** Introduction :ignore: ** Matlab Init :noexport:ignore: diff --git a/org/control-study.org b/org/control-study.org index cdcd1cc..b4f1a01 100644 --- a/org/control-study.org +++ b/org/control-study.org @@ -1364,7 +1364,7 @@ The rotation point of the ground is located at the origin of frame $\{A\}$. :END: #+begin_src matlab arguments - args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X'})} = 'open-loop' + args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop' end #+end_src @@ -1396,5 +1396,7 @@ The rotation point of the ground is located at the origin of frame $\{A\}$. controller.type = 5; case 'ref-track-X' controller.type = 6; + case 'ref-track-hac-dvf' + controller.type = 7; end #+end_src diff --git a/org/control-tracking.org b/org/control-tracking.org index ff23f44..5e6eed2 100644 --- a/org/control-tracking.org +++ b/org/control-tracking.org @@ -121,7 +121,7 @@ Then, a diagonal (decentralized) controller $\bm{K}_\mathcal{L}$ is used such th #+end_src ** Identification of the plant -Let's identify the transfer function from $\bm{\tau}$ to $\bm{L}$. +Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{L}}$. #+begin_src matlab %% Name of the Simulink File mdl = 'stewart_platform_model'; @@ -231,7 +231,6 @@ We see that the plant is decoupled at low frequency which indicate that decentra ** Controller Design The controller consists of: - A pure integrator -- A lead around the crossover frequency to increase the phase margin - A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:loop_gain_decentralized_L]]. @@ -428,6 +427,10 @@ However, as $\mathcal{X}$ is not directly measured, it is possible that importan simulinkproject('../'); #+end_src +#+begin_src matlab + open('stewart_platform_model.slx'); +#+end_src + ** Control Schematic The centralized controller takes the position error $\bm{\epsilon}_\mathcal{X}$ as an inputs and generate actuator forces $\bm{\tau}$ (see Figure [[fig:centralized_reference_tracking]]). The signals are: @@ -492,7 +495,7 @@ We can think of two ways to make the plant more diagonal that are described in s #+end_src ** Identification of the plant -Let's identify the transfer function from $\bm{\tau}$ to $\bm{L}$. +Let's identify the transfer function from $\bm{\tau}$ to $\bm{\mathcal{X}}$. #+begin_src matlab %% Name of the Simulink File mdl = 'stewart_platform_model'; @@ -650,7 +653,6 @@ Thus $J \cdot G(\omega = 0) = J \cdot \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\t *** Controller Design The controller consists of: - A pure integrator -- A lead around the crossover frequency to increase the phase margin - A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:loop_gain_centralized_L]]. @@ -877,7 +879,6 @@ This control architecture can also give a dynamically decoupled plant if the Cen *** Controller Design The controller consists of: - A pure integrator -- A lead around the crossover frequency to increase the phase margin - A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin The obtained loop gains corresponding to the diagonal elements are shown in Figure [[fig:loop_gain_centralized_X]]. @@ -1221,6 +1222,7 @@ Thus, this method should be quite robust against parameter variation (e.g. the p * Hybrid Control Architecture - HAC-LAC with relative DVF <> +** 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) <> @@ -1234,7 +1236,26 @@ Thus, this method should be quite robust against parameter variation (e.g. the p simulinkproject('../'); #+end_src +#+begin_src matlab + open('stewart_platform_model.slx'); +#+end_src + ** Control Schematic +Let's consider the control schematic shown in Figure [[fig:hybrid_reference_tracking]]. + +The first loop containing $\bm{K}_\mathcal{L}$ is a Decentralized Direct (Relative) Velocity Feedback. + +A reference $\bm{r}_\mathcal{L}$ is computed using the inverse kinematics and corresponds to the wanted motion of each leg. +The actual length of each leg $\bm{\mathcal{L}}$ is subtracted and then passed trough the controller $\bm{K}_\mathcal{L}$. + +The controller is a diagonal controller with pure derivative action on the diagonal. + +The effect of this loop is: +- it adds damping to the system (the force applied in each actuator is proportional to the relative velocity of the strut) +- it however does not go "against" the reference path $\bm{r}_\mathcal{X}$ thanks to the use of the inverse kinematics + +Then, the second loop containing $\bm{K}_\mathcal{X}$ is designed based on the already damped plant (represented by the gray area). +This second loop is responsible for the reference tracking. #+begin_src latex :file hybrid_reference_tracking.pdf \begin{tikzpicture} @@ -1260,8 +1281,8 @@ Thus, this method should be quite robust against parameter variation (e.g. the p % Connections and labels \draw[<-] (subx.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1.2, 0); \draw[->] ($(subx.west) + (-0.8, 0)$)node[branch]{} |- (invK.west); - \draw[->] (invK.east) -- (subl.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$}; - \draw[->] (subl.south) -- (Kl.north); + \draw[->] (invK.east) -- (subl.west) node[above left]{$\bm{r}_\mathcal{L}$}; + \draw[->] (subl.south) -- (Kl.north) node[above right]{$\bm{\epsilon}_\mathcal{L}$}; \draw[->] (Kl.south) -- (addF.north); \draw[->] (subx.east) -- (Kx.west) node[above left]{$\bm{\epsilon}_\mathcal{X}$}; @@ -1278,11 +1299,418 @@ Thus, this method should be quite robust against parameter variation (e.g. the p #+end_src #+name: fig:hybrid_reference_tracking -#+caption: Centralized Controller +#+caption: Hybrid Control Architecture #+RESULTS: [[file:figs/hybrid_reference_tracking.png]] +** 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); +#+end_src +#+begin_src matlab + ground = initializeGround('type', 'rigid'); + payload = initializePayload('type', 'none'); + controller = initializeController('type', 'open-loop'); +#+end_src + +#+begin_src matlab + disturbances = initializeDisturbances(); + references = initializeReferences(stewart); +#+end_src + +** First Control Loop - $\bm{K}_\mathcal{L}$ +*** Identification +Let's identify the transfer function from $\bm{\tau}$ to $\bm{L}$. +#+begin_src matlab + %% 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] + + %% Run the linearization + Gl = linearize(mdl, io); + Gl.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; + Gl.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'}; +#+end_src + +*** Obtained Plant +The diagonal elements of the plant are shown in Figure [[fig:hybrid_control_Kl_plant_diagonal]] while the off diagonal terms are shown in Figure [[fig:hybrid_control_Kl_plant_off_diagonal]]. + +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + + linkaxes([ax1,ax2],'x'); +#+end_src + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/hybrid_control_Kl_plant_diagonal.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:hybrid_control_Kl_plant_diagonal +#+caption: Diagonal elements of the plant for the design of $\bm{K}_\mathcal{L}$ ([[./figs/hybrid_control_Kl_plant_diagonal.png][png]], [[./figs/hybrid_control_Kl_plant_diagonal.pdf][pdf]]) +[[file:figs/hybrid_control_Kl_plant_diagonal.png]] + +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz')))); + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(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 + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/hybrid_control_Kl_plant_off_diagonal.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:hybrid_control_Kl_plant_off_diagonal +#+caption: Off-diagonal elements of the plant for the design of $\bm{K}_\mathcal{L}$ ([[./figs/hybrid_control_Kl_plant_off_diagonal.png][png]], [[./figs/hybrid_control_Kl_plant_off_diagonal.pdf][pdf]]) +[[file:figs/hybrid_control_Kl_plant_off_diagonal.png]] + +*** Controller Design +We apply a decentralized (diagonal) direct velocity feedback. +Thus, we apply a pure derivative action. +In order to make the controller realizable, we add a low pass filter at high frequency. +The gain of the controller is chosen such that the resonances are critically damped. + +The obtain loop gain is shown in Figure [[fig:hybrid_control_Kl_loop_gain]]. + +#+begin_src matlab + Kl = 1e4 * s / (1 + s/2/pi/1e4) * eye(6); +#+end_src + +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + plot(freqs, abs(squeeze(freqresp(Gl(1, 1)*Kl(1,1), freqs, 'Hz')))); + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1)*Kl(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 + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/hybrid_control_Kl_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:hybrid_control_Kl_loop_gain +#+caption: Obtain Loop Gain for the DVF control loop ([[./figs/hybrid_control_Kl_loop_gain.png][png]], [[./figs/hybrid_control_Kl_loop_gain.pdf][pdf]]) +[[file:figs/hybrid_control_Kl_loop_gain.png]] + +** Second Control Loop - $\bm{K}_\mathcal{X}$ +*** Identification +#+begin_src matlab + Kx = tf(zeros(6)); + + controller = initializeController('type', 'ref-track-hac-dvf'); +#+end_src + +#+begin_src matlab + %% Name of the Simulink File + mdl = 'stewart_platform_model'; + + %% Input/Output definition + clear io; io_i = 1; + io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N] + io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); 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 = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +#+end_src + +*** Obtained Plant +We use the Jacobian matrix to apply forces in the cartesian frame. +#+begin_src matlab + Gx = G*inv(stewart.kinematics.J'); + Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +#+end_src + +The obtained plant is shown in Figure [[fig:hybrid_control_Kx_plant]]. +#+begin_src matlab :exports none + freqs = logspace(1, 4, 1000); + + labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', '$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'}; + + figure; + + ax1 = subplot(2, 2, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Diagonal elements of the Plant'); + + ax2 = subplot(2, 2, 3); + hold on; + for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i}); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + legend(); + + ax3 = subplot(2, 2, 2); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz')))); + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + title('Off-Diagonal elements of the Plant'); + + ax4 = subplot(2, 2, 4); + hold on; + for i = 1:5 + for j = i+1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); + end + end + set(gca,'ColorOrderIndex',1); + plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(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,ax3,ax4],'x'); +#+end_src + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/hybrid_control_Kx_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:hybrid_control_Kx_plant +#+caption: Diagonal and Off-diagonal elements of the plant for the design of $\bm{K}_\mathcal{L}$ ([[./figs/hybrid_control_Kx_plant.png][png]], [[./figs/hybrid_control_Kx_plant.pdf][pdf]]) +[[file:figs/hybrid_control_Kx_plant.png]] + +*** Controller Design +The controller consists of: +- A pure integrator +- A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin + +#+begin_src matlab + wc = 2*pi*200; % Bandwidth Bandwidth [rad/s] + + h = 3; % Lead parameter + + Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc/2 + 1)/(s/wc/2)); + + % Normalization of the gain of have a loop gain of 1 at frequency wc + Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc)))); +#+end_src + +#+begin_src matlab :exports none + freqs = logspace(1, 3, 1000); + + figure; + + ax1 = subplot(2, 1, 1); + hold on; + for i = 1:6 + plot(freqs, abs(squeeze(freqresp(Kx(i,i)*Gx(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); + ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); + + ax2 = subplot(2, 1, 2); + hold on; + for i = 1:6 + plot(freqs, 180/pi*angle(squeeze(freqresp(Kx(i,i)*Gx(i, i), freqs, 'Hz')))); + end + hold off; + set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); + ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); + ylim([-180, 180]); + yticks([-180, -90, 0, 90, 180]); + + linkaxes([ax1,ax2],'x'); +#+end_src + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/hybrid_control_Kx_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:hybrid_control_Kx_loop_gain +#+caption: Obtained Loop Gain for the controller $\bm{K}_\mathcal{X}$ ([[./figs/hybrid_control_Kx_loop_gain.png][png]], [[./figs/hybrid_control_Kx_loop_gain.pdf][pdf]]) +[[file:figs/hybrid_control_Kx_loop_gain.png]] + +Then we include the Jacobian in the controller matrix. +#+begin_src matlab + Kx = inv(stewart.kinematics.J')*Kx; +#+end_src + +** Simulations +We specify the reference path to follow. +#+begin_src matlab + t = linspace(0, 10, 10000); + + r = zeros(6, length(t)); + + r(1, :) = 5e-3*sin(2*pi*t); + + references = initializeReferences(stewart, 't', t, 'r', r); +#+end_src + +We run the simulation and we save the results. +#+begin_src matlab + sim('stewart_platform_model') + simout_H = simout; +#+end_src + +The obtained position error is shown in Figure [[fig:hybrid_control_Ex]]. + +#+begin_src matlab :exports none + figure; + subplot(2, 3, 1); + hold on; + plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(1, 1, :))-simout_H.x.Xr.Data(:, 1)) + hold off; + xlabel('Time [s]'); + ylabel('Dx [m]'); + + subplot(2, 3, 2); + hold on; + plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(2, 1, :))-simout_H.x.Xr.Data(:, 2)) + hold off; + xlabel('Time [s]'); + ylabel('Dy [m]'); + + subplot(2, 3, 3); + hold on; + plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(3, 1, :))-simout_H.x.Xr.Data(:, 3)) + hold off; + xlabel('Time [s]'); + ylabel('Dz [m]'); + + subplot(2, 3, 4); + hold on; + plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(4, 1, :))-simout_H.x.Xr.Data(:, 4)) + hold off; + xlabel('Time [s]'); + ylabel('Rx [rad]'); + + subplot(2, 3, 5); + hold on; + plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(5, 1, :))-simout_H.x.Xr.Data(:, 5)) + hold off; + xlabel('Time [s]'); + ylabel('Ry [rad]'); + + subplot(2, 3, 6); + hold on; + plot(simout_H.r.r.Time, squeeze(simout_H.r.r.Data(6, 1, :))-simout_H.x.Xr.Data(:, 6)) + hold off; + xlabel('Time [s]'); + ylabel('Rz [rad]'); +#+end_src + +#+header: :tangle no :exports results :results none :noweb yes +#+begin_src matlab :var filepath="figs/hybrid_control_Ex.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png") +<> +#+end_src + +#+name: fig:hybrid_control_Ex +#+caption: Obtained position error $\bm{\epsilon}_\mathcal{X}$ ([[./figs/hybrid_control_Ex.png][png]], [[./figs/hybrid_control_Ex.pdf][pdf]]) +[[file:figs/hybrid_control_Ex.png]] + +** Conclusion * Position Error computation <> @@ -1371,3 +1799,112 @@ We have: [Edx, Edy, Edz, Erx, Ery, Erz] #+end_src + +* Tests +#+begin_src matlab + Dxr = 0.1; + Dyr = 0.1; + Dzr = 0; + Rxr = 0; + Ryr = 0; + Rzr = 0.1; + + Dxm = 0.1; + Dym = 0.1; + Dzm = 0.1; + Rxm = 0.1; + Rym = 0.1; + Rzm = 0.1; + + [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm) +#+end_src + +* Function Name +:PROPERTIES: +:header-args:matlab+: :tangle ../src/computePosErrorTest.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: + +#+begin_src matlab + function [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm) +#+end_src + +Let's note: +- $\{W\}$ the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed). + The center of the frame if $O_W$ +- $\{M\}$ is the frame fixed to the measured elements. + $O_M$ is the point where the pose of the element is measured +- $\{R\}$ is a virtual frame corresponding to the wanted pose of the element. + $O_R$ is the origin of this frame where the we want to position the point $O_M$ of the element. +- $\{V\}$ is a frame which its axes are aligned with $\{W\}$ and its origin $O_V$ is coincident with the $O_M$ + +We measure the position and orientation (pose) of the element represented by the frame $\{M\}$ with respect to frame $\{W\}$. +Thus we can compute the Homogeneous transformation matrix ${}^WT_M$. +#+begin_src matlab + %% Measured Pose + WTm = zeros(4,4); + + WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0; + sin(Rzm) cos(Rzm) 0; + 0 0 1] * ... + [cos(Rym) 0 sin(Rym); + 0 1 0; + -sin(Rym) 0 cos(Rym)] * ... + [1 0 0; + 0 cos(Rxm) -sin(Rxm); + 0 sin(Rxm) cos(Rxm)]; + WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1]; +#+end_src + +We can also compute the Homogeneous transformation matrix ${}^WT_R$ corresponding to the transformation required to go from fixed frame $\{W\}$ to the wanted frame $\{R\}$. +#+begin_src matlab + %% Reference Pose + WTr = zeros(4,4); + + WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0; + sin(Rzr) cos(Rzr) 0; + 0 0 1] * ... + [cos(Ryr) 0 sin(Ryr); + 0 1 0; + -sin(Ryr) 0 cos(Ryr)] * ... + [1 0 0; + 0 cos(Rxr) -sin(Rxr); + 0 sin(Rxr) cos(Rxr)]; + WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1]; +#+end_src + +We can also compute ${}^WT_V$. +#+begin_src matlab + WTv = eye(4); + WTv(1:3, 4) = WTm(1:3, 4); +#+end_src + +Now we want to express ${}^MT_R$ which corresponds to the transformation required to go to wanted position expressed in the frame of the measured element. +This homogeneous transformation can be computed from the previously computed matrices: +\[ {}^MT_R = ({{}^WT_M}^{-1}) {}^WT_R \] + +#+begin_src matlab + %% Wanted pose expressed in a frame corresponding to the actual measured pose + MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; +#+end_src + +Now we want to express ${}^VT_R$: +\[ {}^VT_R = ({{}^WT_V}^{-1}) {}^WT_R \] +#+begin_src matlab + %% Wanted pose expressed in a frame coincident with the actual position but with no rotation + VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr; +#+end_src + +#+begin_src matlab + %% Extract Translations and Rotations from the Homogeneous Matrix + T = MTr; + Edx = T(1, 4); + Edy = T(2, 4); + Edz = T(3, 4); + + % The angles obtained are u-v-w Euler angles (rotations in the moving frame) + Ery = atan2( T(1, 3), sqrt(T(1, 1)^2 + T(1, 2)^2)); + Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery)); + Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery)); + end +#+end_src diff --git a/src/computePosErrorTest.m b/src/computePosErrorTest.m new file mode 100644 index 0000000..40fc4f1 --- /dev/null +++ b/src/computePosErrorTest.m @@ -0,0 +1,50 @@ +function [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm) + +%% Measured Pose +WTm = zeros(4,4); + +WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0; + sin(Rzm) cos(Rzm) 0; + 0 0 1] * ... + [cos(Rym) 0 sin(Rym); + 0 1 0; + -sin(Rym) 0 cos(Rym)] * ... + [1 0 0; + 0 cos(Rxm) -sin(Rxm); + 0 sin(Rxm) cos(Rxm)]; +WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1]; + +%% Reference Pose +WTr = zeros(4,4); + +WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0; + sin(Rzr) cos(Rzr) 0; + 0 0 1] * ... + [cos(Ryr) 0 sin(Ryr); + 0 1 0; + -sin(Ryr) 0 cos(Ryr)] * ... + [1 0 0; + 0 cos(Rxr) -sin(Rxr); + 0 sin(Rxr) cos(Rxr)]; +WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1]; + +WTv = eye(4); +WTv(1:3, 4) = WTm(1:3, 4); + +%% Wanted pose expressed in a frame corresponding to the actual measured pose +MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr; + +%% Wanted pose expressed in a frame coincident with the actual position but with no rotation +VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr; + +%% Extract Translations and Rotations from the Homogeneous Matrix + T = MTr; + Edx = T(1, 4); + Edy = T(2, 4); + Edz = T(3, 4); + + % The angles obtained are u-v-w Euler angles (rotations in the moving frame) + Ery = atan2( T(1, 3), sqrt(T(1, 1)^2 + T(1, 2)^2)); + Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery)); + Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery)); +end diff --git a/src/initializeController.m b/src/initializeController.m index 363b43d..dca523e 100644 --- a/src/initializeController.m +++ b/src/initializeController.m @@ -7,7 +7,7 @@ function [controller] = initializeController(args) % - args - Can have the following fields: arguments - args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X'})} = 'open-loop' + args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop' end controller = struct(); @@ -27,4 +27,6 @@ switch args.type controller.type = 5; case 'ref-track-X' controller.type = 6; + case 'ref-track-hac-dvf' + controller.type = 7; end