Move one function

This commit is contained in:
Thomas Dehaeze 2019-12-11 17:34:42 +01:00
parent 6541ee4002
commit ae2f89f3cc
2 changed files with 149 additions and 1 deletions

View File

@ -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 From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
\begin{align*} \begin{align*}
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\ 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*} \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: To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:

View File

@ -41,6 +41,7 @@
#+PROPERTY: header-args:latex+ :output-dir figs #+PROPERTY: header-args:latex+ :output-dir figs
:END: :END:
* Introduction :ignore:
* General Subsystems * General Subsystems
<<sec:helping functions>> <<sec:helping functions>>
** Generate Reference Signals ** Generate Reference Signals
@ -191,6 +192,153 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
end end
#+end_src #+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 * Initialize Elements
:PROPERTIES: :PROPERTIES:
:ID: a0819dea-8d7a-4d55-b961-2b2ca2312344 :ID: a0819dea-8d7a-4d55-b961-2b2ca2312344