99 lines
2.2 KiB
Mathematica
99 lines
2.2 KiB
Mathematica
|
% 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')
|