From ae2f89f3cca67bafcaf2683f9ebf04b2d5354f26 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 11 Dec 2019 17:34:42 +0100 Subject: [PATCH] Move one function --- kinematics/index.org | 2 +- simscape_subsystems/index.org | 148 ++++++++++++++++++++++++++++++++++ 2 files changed, 149 insertions(+), 1 deletion(-) diff --git a/kinematics/index.org b/kinematics/index.org index f40a63b..f94096a 100644 --- a/kinematics/index.org +++ b/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: diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org index 02069a7..e2c432a 100644 --- a/simscape_subsystems/index.org +++ b/simscape_subsystems/index.org @@ -41,6 +41,7 @@ #+PROPERTY: header-args:latex+ :output-dir figs :END: +* Introduction :ignore: * General Subsystems <> ** 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: +<> + +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