Move one function

kinematics/index.org

@@ -115,7 +115,7 @@ For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and
 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
+                         &= {}^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:

simscape_subsystems/index.org

@@ -41,6 +41,7 @@
 #+PROPERTY: header-args:latex+ :output-dir figs
 :END:
 
+* Introduction                                                       :ignore:
 * General Subsystems
 <<sec:helping functions>>
 ** Generate Reference Signals
@@ -191,6 +192,153 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
   end
 #+end_src
 
+** Function that initialize the disturbances
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/initDisturbances.m
+:header-args:matlab+: :comments none :mkdirp yes
+:header-args:matlab+: :eval no :results none
+:END:
+<<sec:initDisturbances>>
+
+This Matlab function is accessible [[file:src/initDisturbances.m][here]].
+
+#+begin_src matlab
+  function [] = initDisturbances(opts_param)
+  % initDisturbances - Initialize the disturbances
+  %
+  % Syntax: [] = initDisturbances(opts_param)
+  %
+  % Inputs:
+  %    - opts_param -
+
+#+end_src
+
+*** Default values for the Options
+#+begin_src matlab
+  %% Default values for opts
+  opts = struct();
+
+  %% Populate opts with input parameters
+  if exist('opts_param','var')
+      for opt = fieldnames(opts_param)'
+          opts.(opt{1}) = opts_param.(opt{1});
+      end
+  end
+#+end_src
+
+*** Load Data
+#+begin_src matlab
+  load('./disturbances/mat/dist_psd.mat', 'dist_f');
+#+end_src
+
+We remove the first frequency point that usually is very large.
+#+begin_src matlab :exports none
+  dist_f.f = dist_f.f(2:end);
+  dist_f.psd_gm = dist_f.psd_gm(2:end);
+  dist_f.psd_ty = dist_f.psd_ty(2:end);
+  dist_f.psd_rz = dist_f.psd_rz(2:end);
+#+end_src
+
+*** Parameters
+We define some parameters that will be used in the algorithm.
+#+begin_src matlab
+  Fs = 2*dist_f.f(end);      % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
+  N  = 2*length(dist_f.f);   % Number of Samples match the one of the wanted PSD
+  T0 = N/Fs;                 % Signal Duration [s]
+  df = 1/T0;                 % Frequency resolution of the DFT [Hz]
+                             % Also equal to (dist_f.f(2)-dist_f.f(1))
+  t = linspace(0, T0, N+1)'; % Time Vector [s]
+  Ts = 1/Fs;                 % Sampling Time [s]
+#+end_src
+
+*** Ground Motion
+#+begin_src matlab
+  phi = dist_f.psd_gm;
+  C = zeros(N/2,1);
+  for i = 1:N/2
+    C(i) = sqrt(phi(i)*df);
+  end
+  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+  Cx = [Cx; flipud(conj(Cx(2:end)))];;
+  u = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
+  % Dwx = struct('time', t, 'signals', struct('values', u));
+  Dwx = u;
+  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+  Cx = [Cx; flipud(conj(Cx(2:end)))];;
+  u = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
+  Dwy = u;
+  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+  Cx = [Cx; flipud(conj(Cx(2:end)))];;
+  u = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
+  Dwz = u;
+#+end_src
+
+*** Translation Stage - X direction
+#+begin_src matlab
+  phi = dist_f.psd_ty; % TODO - we take here the vertical direction which is wrong but approximate
+  C = zeros(N/2,1);
+  for i = 1:N/2
+    C(i) = sqrt(phi(i)*df);
+  end
+  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+  Cx = [Cx; flipud(conj(Cx(2:end)))];;
+  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty x [N]
+  Fty_x = u;
+#+end_src
+
+*** Translation Stage - Z direction
+#+begin_src matlab
+  phi = dist_f.psd_ty;
+  C = zeros(N/2,1);
+  for i = 1:N/2
+    C(i) = sqrt(phi(i)*df);
+  end
+  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+  Cx = [Cx; flipud(conj(Cx(2:end)))];;
+  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Ty z [N]
+  Fty_z = u;
+#+end_src
+
+*** Spindle - Z direction
+#+begin_src matlab
+  phi = dist_f.psd_rz;
+  C = zeros(N/2,1);
+  for i = 1:N/2
+    C(i) = sqrt(phi(i)*df);
+  end
+  theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
+  Cx = [0 ; C.*complex(cos(theta),sin(theta))];
+  Cx = [Cx; flipud(conj(Cx(2:end)))];;
+  u = N/sqrt(2)*ifft(Cx); % Disturbance Force Rz z [N]
+  Frz_z = u;
+#+end_src
+
+*** Direct Forces
+#+begin_src matlab
+  u = zeros(length(t), 6);
+  Fd = u;
+#+end_src
+
+*** Set initial value to zero
+#+begin_src matlab
+  Dwx    = Dwx   - Dwx(1);
+  Dwy    = Dwy   - Dwy(1);
+  Dwz    = Dwz   - Dwz(1);
+  Fty_x  = Fty_x - Fty_x(1);
+  Fty_z  = Fty_z - Fty_z(1);
+  Frz_z  = Frz_z - Frz_z(1);
+#+end_src
+
+*** Save
+#+begin_src matlab
+  save('./mat/nass_disturbances.mat', 'Dwx', 'Dwy', 'Dwz', 'Fty_x', 'Fty_z', 'Frz_z', 'Fd', 'Ts', 't');
+#+end_src
+
 * Initialize Elements
 :PROPERTIES:
 :ID:       a0819dea-8d7a-4d55-b961-2b2ca2312344