Initial Commit

matlab/dcm_active_damping_iff.m

@@ -0,0 +1,225 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% dcm_active_damping_iff.m
+% Test of Integral Force Feedback Strategy
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Simscape Model - Nano Hexapod
+addpath('./STEPS/')
+
+%% Initialize Parameters for Simscape model
+controller.type = 0; % Open Loop Control
+
+%% Options for Linearization
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Open Simulink Model
+mdl = 'simscape_dcm';
+
+open(mdl)
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(1, 3, 1000);
+
+% Identification
+
+%% Input/Output definition
+clear io; io_i = 1;
+
+%% Inputs
+% Control Input {3x1} [N]
+io(io_i) = linio([mdl, '/control_system'], 1, 'openinput');  io_i = io_i + 1;
+
+%% Outputs
+% Force Sensor {3x1} [m]
+io(io_i) = linio([mdl, '/DCM'], 3, 'openoutput'); io_i = io_i + 1;
+
+%% Extraction of the dynamics
+G_fs = linearize(mdl, io);
+
+G_fs.InputName  = {'u_ur',  'u_uh',  'u_d'};
+G_fs.OutputName = {'fs_ur', 'fs_uh', 'fs_d'};
+
+
+
+% #+RESULTS:
+% | -1.4113e-13 |  1.0339e-13 |   3.774e-14 |
+% |  1.0339e-13 | -1.4113e-13 |   3.774e-14 |
+% |  3.7792e-14 |  3.7792e-14 | -7.5585e-14 |
+
+
+%% Bode plot for the plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))), ...
+     'DisplayName', 'd');
+plot(freqs, abs(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))), ...
+     'DisplayName', 'uh');
+plot(freqs, abs(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))), ...
+     'DisplayName', 'ur');
+plot(freqs, abs(squeeze(freqresp(G_fs(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
+     'DisplayName', 'off-diag');
+for i = 1:2
+    for j = i+1:3
+        plot(freqs, abs(squeeze(freqresp(G_fs(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
+ylim([1e-13, 1e-7]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(1,1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(2,2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_fs(3,3), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+% Controller - Root Locus
+
+Kiff_g1 = eye(3)*1/(1 + s/2/pi/20);
+
+%% Root Locus for IFF
+gains = logspace(9, 12, 200);
+
+figure;
+
+hold on;
+plot(real(pole(G_fs)),  imag(pole(G_fs)),  'x', 'color', colors(1,:), ...
+    'DisplayName', '$g = 0$');
+plot(real(tzero(G_fs)), imag(tzero(G_fs)), 'o', 'color', colors(1,:), ...
+    'HandleVisibility', 'off');
+
+for g = gains
+    clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
+    plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
+        'HandleVisibility', 'off');
+end
+
+% Optimal gain
+g = 8e10;
+clpoles = pole(feedback(G_fs, g*Kiff_g1, +1));
+plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ...
+    'DisplayName', sprintf('$g=%.0e$', g));
+hold off;
+axis square;
+xlim([-2700, 0]); ylim([0, 2700]);
+xlabel('Real Part'); ylabel('Imaginary Part');
+legend('location', 'northwest');
+
+
+
+% #+name: fig:iff_root_locus
+% #+caption: Root Locus plot for the IFF Control strategy
+% #+RESULTS:
+% [[file:figs/iff_root_locus.png]]
+
+
+%% Integral Force Feedback Controller
+Kiff = g*Kiff_g1;
+
+% Damped Plant
+
+%% Input/Output definition
+clear io; io_i = 1;
+
+%% Inputs
+% Control Input {3x1} [N]
+io(io_i) = linio([mdl, '/control_system'], 1, 'input');  io_i = io_i + 1;
+
+%% Outputs
+% Force Sensor {3x1} [m]
+io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
+
+%% DCM Kinematics
+load('mat/dcm_kinematics.mat');
+
+%% Identification of the Open Loop plant
+controller.type = 0; % Open Loop
+G_ol = J_a_111*inv(J_s_111)*linearize(mdl, io);
+G_ol.InputName  = {'u_ur',  'u_uh',  'u_d'};
+G_ol.OutputName = {'d_ur',  'd_uh',  'd_d'};
+
+%% Identification of the damped plant with IFF
+controller.type = 1; % IFF
+G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io);
+G_dp.InputName  = {'u_ur',  'u_uh',  'u_d'};
+G_dp.OutputName = {'d_ur',  'd_uh',  'd_d'};
+
+%% Comparison of the damped and undamped plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ...
+     'DisplayName', 'd - OL');
+plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ...
+     'DisplayName', 'uh - OL');
+plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ...
+     'DisplayName', 'ur - OL');
+set(gca,'ColorOrderIndex',1)
+plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ...
+     'DisplayName', 'd - IFF');
+plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ...
+     'DisplayName', 'uh - IFF');
+plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ...
+     'DisplayName', 'ur - IFF');
+for i = 1:2
+    for j = i+1:3
+        plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
+ylim([1e-12, 1e-6]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))));
+set(gca,'ColorOrderIndex',1)
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--');
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--');
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 0]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+save('mat/Kiff.mat', 'Kiff');

matlab/dcm_active_damping_strain_gauges.m

@@ -0,0 +1,101 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% dcm_active_damping_strain_gauges.m
+% Active Damping using relative motion sensors (strain gauges)
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Simscape Model - Nano Hexapod
+addpath('./STEPS/')
+
+%% Initialize Parameters for Simscape model
+controller.type = 0; % Open Loop Control
+
+%% Options for Linearization
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Open Simulink Model
+mdl = 'simscape_dcm';
+
+open(mdl)
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(1, 3, 1000);
+
+% Identification
+
+%% Input/Output definition
+clear io; io_i = 1;
+
+%% Inputs
+% Control Input {3x1} [N]
+io(io_i) = linio([mdl, '/u'], 1, 'openinput');  io_i = io_i + 1;
+% % Stepper Displacement {3x1} [m]
+% io(io_i) = linio([mdl, '/d'], 1, 'openinput');  io_i = io_i + 1;
+
+%% Outputs
+% Strain Gauges {3x1} [m]
+io(io_i) = linio([mdl, '/sg'], 1, 'openoutput'); io_i = io_i + 1;
+
+%% Extraction of the dynamics
+G_sg = linearize(mdl, io);
+
+G_sg.InputName  = {'u_ur',  'u_uh',  'u_d'};
+G_sg.OutputName = {'sg_ur', 'sg_uh', 'sg_d'};
+
+
+
+% #+RESULTS:
+% | -1.4113e-13 |  1.0339e-13 |   3.774e-14 |
+% |  1.0339e-13 | -1.4113e-13 |   3.774e-14 |
+% |  3.7792e-14 |  3.7792e-14 | -7.5585e-14 |
+
+
+%% Bode plot for the plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))), ...
+     'DisplayName', 'd');
+plot(freqs, abs(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))), ...
+     'DisplayName', 'uh');
+plot(freqs, abs(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))), ...
+     'DisplayName', 'ur');
+for i = 1:2
+    for j = i+1:3
+        plot(freqs, abs(squeeze(freqresp(G_sg(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(1,1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(2,2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_sg(3,3), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);

matlab/dcm_hac_iff.m

@@ -0,0 +1,34 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% dcm_hac_iff.m
+% Development of the HAC-IFF control strategy
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Simscape Model - Nano Hexapod
+addpath('./STEPS/')
+
+%% Initialize Parameters for Simscape model
+controller.type = 0; % Open Loop Control
+
+%% Options for Linearization
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Open Simulink Model
+mdl = 'simscape_dcm';
+
+open(mdl)
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(1, 3, 1000);

matlab/dcm_identification.m

@@ -0,0 +1,202 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% dcm_identification.m
+% Extraction of system dynamics using Simscape model
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Simscape Model - Nano Hexapod
+addpath('./STEPS/')
+
+%% Initialize Parameters for Simscape model
+controller.type = 0; % Open Loop Control
+
+%% Options for Linearization
+options = linearizeOptions;
+options.SampleTime = 0;
+
+%% Open Simulink Model
+mdl = 'simscape_dcm';
+
+open(mdl)
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(1, 3, 1000);
+
+
+
+% #+name: fig:schematic_system_inputs_outputs
+% #+caption: Dynamical system with inputs and outputs
+% #+RESULTS:
+% [[file:figs/schematic_system_inputs_outputs.png]]
+
+% The system is identified from the Simscape model.
+
+
+%% Input/Output definition
+clear io; io_i = 1;
+
+%% Inputs
+% Control Input {3x1} [N]
+io(io_i) = linio([mdl, '/control_system'], 1, 'openinput');  io_i = io_i + 1;
+
+%% Outputs
+% Interferometers {3x1} [m]
+io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1;
+
+%% Extraction of the dynamics
+G = linearize(mdl, io);
+
+%% Input and Output names
+G.InputName  = {'u_ur',  'u_uh',  'u_d'};
+G.OutputName = {'int_111_1', 'int_111_2', 'int_111_3'};
+
+% Plant in the frame of the fastjacks
+
+load('mat/dcm_kinematics.mat');
+
+
+
+% #+name: fig:schematic_jacobian_frame_fastjack
+% #+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks
+% #+RESULTS:
+% [[file:figs/schematic_jacobian_frame_fastjack.png]]
+
+
+
+%% Compute the system in the frame of the fastjacks
+G_pz = J_a_111*inv(J_s_111)*G;
+
+
+
+% #+name: tab:dc_gain_plan_fj
+% #+caption: DC gain of the plant in the frame of the fast jacks $\bm{G}_{\text{fj}}$
+% #+attr_latex: :environment tabularx :width 0.5\linewidth :align ccc
+% #+attr_latex: :center t :booktabs t
+% #+RESULTS:
+% | 4.4407e-09 | 2.7656e-12 | 1.0132e-12 |
+% | 2.7656e-12 | 4.4407e-09 | 1.0132e-12 |
+% | 1.0109e-12 | 1.0109e-12 | 4.4424e-09 |
+
+% The bode plot of $\bm{G}_{\text{fj}}(s)$ is shown in Figure [[fig:bode_plot_plant_fj]].
+
+
+%% Bode plot for the plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))), ...
+     'DisplayName', 'd');
+plot(freqs, abs(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))), ...
+     'DisplayName', 'uh');
+plot(freqs, abs(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))), ...
+     'DisplayName', 'ur');
+for i = 1:2
+    for j = i+1:3
+        plot(freqs, abs(squeeze(freqresp(G_pz(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
+ylim([1e-13, 1e-6]);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(1,1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(2,2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_pz(3,3), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+
+
+% #+name: fig:schematic_jacobian_frame_crystal
+% #+caption: Use of Jacobian matrices to obtain the system in the frame of the crystal
+% #+RESULTS:
+% [[file:figs/schematic_jacobian_frame_crystal.png]]
+
+
+G_mr = inv(J_s_111)*G*inv(J_a_111');
+
+
+
+% #+RESULTS:
+% | 1.9978e-09 |  3.9657e-09 |  7.7944e-09 |
+% | 3.9656e-09 |  8.4979e-08 | -1.5135e-17 |
+% | 7.7944e-09 | -3.9252e-17 |   1.834e-07 |
+
+
+%% Bode plot for the plant
+figure;
+tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+ax1 = nexttile([2,1]);
+hold on;
+plot(freqs, abs(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))), ...
+     'DisplayName', 'd');
+plot(freqs, abs(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))), ...
+     'DisplayName', 'uh');
+plot(freqs, abs(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))), ...
+     'DisplayName', 'ur');
+for i = 1:2
+    for j = i+1:3
+        plot(freqs, abs(squeeze(freqresp(G_mr(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
+             'HandleVisibility', 'off');
+    end
+end
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
+legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
+
+ax2 = nexttile;
+hold on;
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(1,1), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(2,2), freqs, 'Hz'))));
+plot(freqs, 180/pi*angle(squeeze(freqresp(G_mr(3,3), freqs, 'Hz'))));
+hold off;
+set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
+xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
+hold off;
+yticks(-360:90:360);
+ylim([-180, 180]);
+
+linkaxes([ax1,ax2],'x');
+xlim([freqs(1), freqs(end)]);
+
+%% Bode plot for the plant
+fig = figure;
+tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
+
+for i_out = 1:3
+    for i_in = 1:3
+        ax = nexttile;
+        plot(freqs, abs(squeeze(freqresp(G_mr(i_out, i_in), freqs, 'Hz'))));
+        set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
+    end
+end
+
+linkaxes(findall(fig, 'type', 'axes'),'xy');
+xlim([freqs(1), freqs(end)]);

matlab/dcm_kinematics.m

@@ -0,0 +1,98 @@
+% Matlab Init                                              :noexport:ignore:
+
+%% dcm_kinematics.m
+% Computation of the DCM kinematics
+
+%% Clear Workspace and Close figures
+clear; close all; clc;
+
+%% Intialize Laplace variable
+s = zpk('s');
+
+%% Path for functions, data and scripts
+addpath('./mat/'); % Path for data
+
+%% Simscape Model - Nano Hexapod
+addpath('./STEPS/')
+
+%% Colors for the figures
+colors = colororder;
+
+%% Frequency Vector
+freqs = logspace(1, 3, 1000);
+
+% Bragg Angle
+
+%% Tested bragg angles
+bragg = linspace(5, 80, 1000); % Bragg angle [deg]
+d_off = 10.5e-3; % Wanted offset between x-rays [m]
+
+%% Vertical Jack motion as a function of Bragg angle
+dz = d_off./(2*cos(bragg*pi/180));
+
+%% Jack motion as a function of Bragg angle
+figure;
+plot(bragg, 1e3*dz)
+xlabel('Bragg angle [deg]'); ylabel('Jack Motion [mm]');
+
+
+
+% #+name: fig:jack_motion_bragg_angle
+% #+caption: Jack motion as a function of Bragg angle
+% #+RESULTS:
+% [[file:figs/jack_motion_bragg_angle.png]]
+
+
+%% Required Jack stroke
+ans = 1e3*(dz(end) - dz(1))
+
+
+
+% #+name: fig:schematic_sensor_jacobian_inverse_kinematics
+% #+caption: Inverse Kinematics - Interferometers
+% #+RESULTS:
+% [[file:figs/schematic_sensor_jacobian_inverse_kinematics.png]]
+
+
+% From the Figure [[fig:sensor_111_crystal_points]], the inverse kinematics can be solved as follow (for small motion):
+% \begin{equation}
+% \bm{J}_{s,111}
+% =
+% \begin{bmatrix}
+% 1 &  0.07 & -0.015 \\
+% 1 &  0    &  0.015 \\
+% 1 & -0.07 & -0.015
+% \end{bmatrix}
+% \end{equation}
+
+
+%% Sensor Jacobian matrix for 111 crystal
+J_s_111 = [1,  0.07, -0.015
+           1,  0,     0.015
+           1, -0.07, -0.015];
+
+
+
+% #+name: fig:schematic_sensor_jacobian_inverse_kinematics
+% #+caption: Inverse Kinematics - Actuators
+% #+RESULTS:
+% [[file:figs/schematic_actuator_jacobian_inverse_kinematics.png]]
+
+% Based on the geometry in Figure [[fig:actuator_jacobian_111_points]], we obtain:
+% \begin{equation}
+% \bm{J}_{a,111}
+% =
+% \begin{bmatrix}
+% 1 &  0.14 & -0.1525 \\
+% 1 &  0.14 &  0.0675 \\
+% 1 & -0.14 & -0.0425
+% \end{bmatrix}
+% \end{equation}
+
+
+%% Actuator Jacobian - 111 crystal
+J_a_111 = [1,  0.14, -0.1525
+           1,  0.14,  0.0675
+           1, -0.14, -0.0425];
+
+save('mat/dcm_kinematics.mat', 'J_a_111', 'J_s_111')