%% UR5_parameters.m
addpath('functions')

%% Trajectory
% We considered five different trajetories and each of them have different
% pick and place time
t_pick = [1.70 1.97 2.16 1.97 1.55]; %[s] istant in which the robot picks the object
t_place = [3.74 4.52 5.70 6.23 4.43]; %[s] istant in which the robot places the object
dt_PP = 0.05; %[s] transient time 

pickTime = t_pick(traj); %[s] to select the pick time of considered trajectory
placeTime = t_place(traj); %[s] to select the place time of considered trajectory

%% Settings and failures
kincor = 0;        %1 = kinematic correction of DH parameters

alpha_TTS = 0;     %0 = no fault | experimental coefficients deriving from Arrhenius law
Beta_TTS = 0;      %0 = no fault | experimental coefficients deriving from Arrhenius law
Wf_TTS_ONOFF = 0;  %0 = no fault | switch to set turn to turn short
Wf_TTS_0 = 0;      %0 = no fault | initial fault ratio of degradation 

%% Simulation and Eviroment Parameters
decimation = 1000;                             %[-] to downsampling with signal logs
Sim.time = Time;                               %[s] time of simulation
Sim.dt = 1e-6;                                 %[s] time step
Sim.g = 9.80665;                               %[m/s^2] gravity acceleration
lowerBound_randT = 10;                         %[°C] Lower bound for randomic temperature
upperBound_randT = 35;                         %[°C] Upper bound for randomic temperature
average_randT = ...
    (lowerBound_randT+upperBound_randT)/2;     %[°C] Average of lower and upper bound for randomic temperature
sigma_randT = 2.35;                            %[°C] Standard deviation
Sim.temp = normrnd(average_randT,sigma_randT); %[°C] enviroment temperature

%Ground is the variable containing the dimnesions of the platform.
Ground.l = 1;                               %[m] length
Ground.w = 1;                               %[m] width
Ground.h = 0.1;                             %[m] height

%% Links Parameters (derived from UR)
%The inertia parameters are defined as follows: InertiaM = [Ixx,Iyy,Izz],
%Inertia P = [Ixy, Ixz, Iyz]

%Link 1
L1.Mass = 3.7000;                        %[kg] base mass
L1.CM = [0, -0.02561, 0.00193];          %[m] base center of mass
L1.InertiaM = [0.0067, 0.0064, 0.0067];  %[kg*m^2] base moment of inertia
L1.InertiaP = [0, 0, 0];                 %[kg*m^2] base product of inertia

%Link 2
L2.Mass = 8.3930;                        %[kg] lower arm mass
L2.CM = [0.2125, 0, 0.11336];            %[m] lower arm center of mass
L2.InertiaM = [0.0149, 0.3564, 0.3553];  %[kg*m^2] lower arm moment of inertia
L2.InertiaP = [0, 0, 0];                 %[kg*m^2] lower arm product of inertia

%Link 3
L3.Mass = 2.3300;                        %[kg] upper arm mass
L3.CM = [0.15, 0.0, 0.0265];             %[m] upper arm center of mass
L3.InertiaM = [0.0025, 0.0551, 0.0546];  %[kg*m^2] upper arm moment of inertia
L3.InertiaP = [0, 0.0034, 0];            %[kg*m^2] upper arm product of inertia

%Link 4
L4.Mass = 1.2190;                        %[kg] wrist 2 mass
L4.CM = [0, -0.0018, 0.01634];           %[m] wrist 2 center of mass
L4.InertiaM = [0.0012, 0.0012, 0.0009];  %[kg*m^2] wrist 2 moment of inertia
L4.InertiaP = [0, 0, 0];                 %[kg*m^2] wrist 2 product of inertia

%Link 5
L5.Mass = 1.2190;                        %[kg] wrist 3 mass
L5.CM = [0, 0.0018,0.01634];             %[m] wrist 3 center of mass
L5.InertiaM = [0.0012, 0.0012, 0.0009];  %[kg*m^2] wrist 3 moment of inertia
L5.InertiaP = [0, 0, 0];                 %[kg*m^2] wrist 3 product of inertia

%Link 6
L6.Mass = 0.1879;                        %[kg] tool flange mass
L6.CM = [0, 0, -0.001159];               %[m] tool flange center of mass
L6.InertiaM = [0.0001, 0.0001, 0.0001];  %[kg*m^2] tool flange moment of inertia
L6.InertiaP = [0, 0, 0];                 %[kg*m^2] tool flange product of inertia

%Payload
lowerBound_randM = 2-0.03;                              %[kg] Lower bound randomic payload 
upperBound_randM = 2+0.03;                              %[kg] upper bound randomic payload 
average_randM = ...                                     %[kg] Average randomic payload
    (lowerBound_randM+upperBound_randM)/2;
sigma_randM = 0.008;                                    %[kg] Standard deviation
payload.m = normrnd(average_randM,sigma_randM);         %[kg] Payload mass
payload.ro = 7880;                                      %[Kg/m^3] Payload density (supposed equal to iron density)
payload.r = 25e-3;                                      %[m] Payload base radius
payload.h = 130e-3;                                     %[m] Payload length
payload.CM = [0 0 payload.h/2];                         %[m] Payload center of mass
payload.Ix = payload.m/12*(3*payload.r^2+payload.h^2);  %[kg*m^2] Payload x axis inertia
payload.Iy = payload.Ix;                                %[kg*m^2] Payload y axis inertia
payload.Iz = payload.m*payload.r^2/2;                   %[kg*m^2] Payload z axis inertia
payload.InertiaM = [payload.Ix payload.Iy payload.Iz];  %[kg*m^2] payload moment of inertia
payload.InertiaP = [0 0 0];                             %[kg*m^2] payload product of inertia

%% DH Table, DH Matrices and Geometric Errors (derived from UR)
%DH Table
DH.table = UR_DHtable('UR5');
if kincor == 1
    load('DH_deltas');
    DH_deltas = DH_deltas';
    DH_deltas(:,1) = DH_deltas(:,1)/1000;
    DH_deltas(:,2) = deg2rad(DH_deltas(:,2));
    DH_deltas(:,3) = DH_deltas(:,3)/1000;
    DH_deltas(:,4) = deg2rad(DH_deltas(:,4));
    DH_deltas = [[0 0 0 0];DH_deltas];
    DH.table = DH.table+DH_deltas;
end

%DH Matrices
DH.theta = DH.table(2:7,1)';
DH.d = DH.table(2:7,2)';
DH.a = DH.table(1:6,3)';
DH.alfa = DH.table(1:6,4)';
A_all = DH_mat(DH.theta,DH.d,DH.a,DH.alfa);

% Geometric errors
E.E01 = eye(4);
E.E12 = eye(4);
E.E23 = eye(4);
E.E34 = eye(4);
E.E45 = eye(4);
E.E56 = eye(4);
A_all(:,:,1) = A_all(:,:,1)*E.E01;
A_all(:,:,2) = A_all(:,:,2)*E.E12;
A_all(:,:,3) = A_all(:,:,3)*E.E23;
A_all(:,:,4) = A_all(:,:,4)*E.E34;
A_all(:,:,5) = A_all(:,:,5)*E.E45;
A_all(:,:,6) = A_all(:,:,6)*E.E56;

% DH Rotation Matrices and DH Translation Vector
A01r = A_all(1:3,1:3,1);
A01t = A_all(1:3,4,1);
A12r = A_all(1:3,1:3,2);
A12t = A_all(1:3,4,2);
A23r = A_all(1:3,1:3,3);
A23t = A_all(1:3,4,3);
A34r = A_all(1:3,1:3,4);
A34t = A_all(1:3,4,4);
A45r = A_all(1:3,1:3,5);
A45t = A_all(1:3,4,5);
A56r = A_all(1:3,1:3,6);
A56t = A_all(1:3,4,6);

%% Gearbox Parameters
Motor.eta = 1;                              %[/] gear box efficiency
Motor.tau = 101;                            %[/] gear ratio

%% Friction Parameters
Joints_T = [Sim.temp, Sim.temp, Sim.temp, Sim.temp, Sim.temp, Sim.temp];
load('pBn_UR5_f_Temp_v2')
load('Temp_v2')
Temp = Temp_v2; 
pBn_UR5_f_Temp = pBn_UR5_f_Temp_v2;

% Coulombian friction coefficient
m1 = polyfit(Temp(:,1),pBn_UR5_f_Temp(:,1),1);
fc_1 = polyval(m1,Joints_T(1));
m2 = polyfit(Temp(:,2),pBn_UR5_f_Temp(:,2),1);
fc_2 = polyval(m2,Joints_T(2));
m3 = polyfit(Temp(:,3),pBn_UR5_f_Temp(:,3),1);
fc_3 = polyval(m3,Joints_T(3));
m4 = polyfit(Temp(:,4),pBn_UR5_f_Temp(:,4),1);
fc_4 = polyval(m4,Joints_T(4));
m5 = polyfit(Temp(:,5),pBn_UR5_f_Temp(:,5),1);
fc_5 = polyval(m5,Joints_T(5));
m6 = polyfit(Temp(:,6),pBn_UR5_f_Temp(:,6),1);
fc_6 = polyval(m6,Joints_T(6));

% Viscous friction coefficient
mb1 = polyfit(Temp(:,7),pBn_UR5_f_Temp(:,7),1);
fv_1 = polyval(mb1,Joints_T(1));
mb2 = polyfit(Temp(:,8),pBn_UR5_f_Temp(:,8),1);
fv_2 = polyval(mb2,Joints_T(2));
mb3 = polyfit(Temp(:,9),pBn_UR5_f_Temp(:,9),1);
fv_3 = polyval(mb3,Joints_T(3));
mb4 = polyfit(Temp(:,10),pBn_UR5_f_Temp(:,10),1);
fv_4 = polyval(mb4,Joints_T(4));
mb5 = polyfit(Temp(:,11),pBn_UR5_f_Temp(:,11),1);
fv_5 = polyval(mb5,Joints_T(5));
mb6 = polyfit(Temp(:,12),pBn_UR5_f_Temp(:,12),1);
fv_6 = polyval(mb6,Joints_T(6));

%Joint 1
J1.al = 360;                            %[deg] angular position limits
J1.B = 18.4819;                         %[N*m*s/rad] viscous friction
J1.Cp = 8.6368;                         %[N*m] positive coulomb friction
J1.Cn = -8.6368;                        %[N*m] negative coulomb friction
%Fric1 = [4.3184 1e1 8.6368 18.4819];    %The friction parameters are [Str=Cou/2 1e1 Cou Vis], NO FUNCTION OF TEMP
Fric1 = [fc_1/2 1e1 fc_1 fv_1];         %The friction parameters are [Str=Cou/2 1e1 Cou Vis], FUNCTION OF TEMP

%Joint 2
J2.al = 360;                            %[deg] angular position limits
J2.B = 11.2806;                         %[N*m*s/rad] viscous friction
J2.Cp = 9.2617;                         %[N*m] positive coulomb friction
J2.Cn = -9.2617;                        %[N*m] negative coulomb friction
%Fric2 = [4.6309 1e1 9.2617 11.2806];    %The friction parameters are [Str=Cou/2 1e1 Cou Vis], NO FUNCTION OF TEMP
Fric2 = [fc_2/2 1e1 fc_2 fv_2];         %The friction parameters are [Str=Cou/2 1e1 Cou Vis], FUNCTION OF TEMP

%Joint 3
J3.al = 360;                            %[deg] angular position limits
J3.B = 12.2784;                         %[N*m*s/rad] viscous friction
J3.Cp = 8.2488;                         %[N*m] positive coulomb friction
J3.Cn = -8.2488;                        %[N*m] negative coulomb friction
%Fric3 = [4.1244 1e1 8.2488 12.2784];    %The friction parameters are [Str=Cou/2 1e1 Cou Vis], NO FUNCTION OF TEMP
Fric3 = [fc_3/2 1e1 fc_3 fv_3];         %The friction parameters are [Str=Cou/2 1e1 Cou Vis], FUNCTION OF TEMP

%Joint 4
J4.al = 360;                            %[deg] angular position limits
J4.B = 2.2887;                          %[N*m*s/rad] viscous friction
J4.Cp = 1.8835;                         %[N*m] positive coulomb friction
J4.Cn = -1.8835;                        %[N*m] negative coulomb friction
%Fric4 = [0.9418 1e1 1.8835 2.2887];     %The friction parameters are [Str=Cou/2 1e1 Cou Vis], NO FUNCTION OF TEMP
Fric4 = [fc_4/2 1e1 fc_4 fv_4];         %The friction parameters are [Str=Cou/2 1e1 Cou Vis], FUNCTION OF TEMP

%Joint 5
J5.al = 360;                            %[deg] angular position limits
J5.B = 3.8798;                          %[N*m*s/rad] viscous friction
J5.Cp = 2.0567;                         %[N*m] positive coulomb friction
J5.Cn = -2.0567;                        %[N*m] negative coulomb friction
%Fric5 = [1.0284 1e1 2.0567 3.8798];     %The friction parameters are [Str=Cou/2 1e1 Cou Vis], NO FUNCTION OF TEMP
Fric5 = [fc_5/2 1e1 fc_5 fv_5];         %The friction parameters are [Str=Cou/2 1e1 Cou Vis], FUNCTION OF TEMP

%Joint 6
J6.al = 360;                            %[deg] angular position limits
J6.B = 2.6838;                          %[N*m*s/rad] viscous friction
J6.Cp = 2.3119;                         %[N*m] positive coulomb friction
J6.Cn = -2.3119;                        %[N*m] negative coulomb friction
%Fric6 = [1.1560 1e1 2.3119 2.6838];     %The friction parameters are [Str=Cou/2 1e1 Cou Vis], NO FUNCTION OF TEMP
Fric6 = [fc_6/2 1e1 fc_6 fv_6];         %The friction parameters are [Str=Cou/2 1e1 Cou Vis], FUNCTION OF TEMP

%% Backlash Parameters

% Joint 1 
J1.clearance = 0;                       %[rad] clearance between motor and reducer
J1.K = 50e3;                             %[Nm/rad] stiffness
J1.C = 1e-2;                            %[Nm*s/rad] damping

% Joint 2
J2.clearance = backlash;                %[rad] clearance between motor and reducer
J2.K = 50e3;                             %[Nm/rad] stiffness
J2.C = 1e-2;                            %[Nm*s/rad] damping
stiff_red =1;

% Joint 3
J3.clearance = 0;                       %[rad] clearance between motor and reducer
J3.K = 50e3;                             %[Nm/rad] stiffness
J3.C = 1e-2;                            %[Nm*s/rad] damping

% Joint 4
J4.clearance = 0;                       %[rad] clearance between motor and reducer
J4.K = 6e3;                             %[Nm/rad] stiffness
J4.C = 1e-2;                            %[Nm*s/rad] damping

% Joint 5
J5.clearance = 0;                       %[rad] clearance between motor and reducer
J5.K = 6e3;                             %[Nm/rad] stiffness
J5.C = 1e-2;                            %[Nm*s/rad] damping

% Joint 6
J6.clearance = 0;                       %[rad] clearance between motor and reducer
J6.K = 6e3;                             %[Nm/rad] stiffness
J6.C = 1e-2;                            %[Nm*s/rad] damping

%% Motor and Driver Parameters
U_s = 48;                                   %[V] DC monophase equivalent supply voltage

%DC Motor: joint 1 and 3
Motor.Im_123 = 187.740e-06;                 %[kg*m^2] first three motors rotor inertia
Motor.R_123 = 0.3;                          %[Ohm] DC motor resistance
Motor.L_123 = 0.83e-3;                      %[H] DC motor inductance
Motor.kc_1 = 0.1350;                        %[Nm/A] DC motor torque constant
Motor.kc_2 = 0.1361;                        %[Nm/A] DC motor torque constant
Motor.kc_3 = 0.1355;                        %[Nm/A] DC motor torque constant
Motor.kv_1 = Motor.kc_1;                    %[V*s/rad] DC motor electrical constant
Motor.kv_3 = Motor.kc_3;                    %[V*s/rad] DC motor electrical constant
Motor.Tmax123 = 150/Motor.tau;              %[N*m] Max motor torque
Motor.imax1 = 9.760;                        %[A] Max current
Motor.imax3 = 10.769;                       %[A] Max current

%AC motor: joint 2 (TBM 7615-A)
Motor.kv_2 = 9.69*60/2/pi/1000;  %[V/(rad/s)=V/kRPM*60/2/pi/1000] Three-phase motor electrical constant               
Motor.imax2 = 10.5;              %[A] Max current
Motor.Im_2 = 3.04e-05;           %[kg*m^2] Three-phase motor rotor inertia    
Motor.R_2 = 0.3;                 %[Ohm] Three-phase motor resistance
Motor.L_2 = 0.4e-3;              %[H] Three-phase motor inductance
Motor.Zp_2 = 10/2;               %[/] Number of pole pair
Motor.ViscDamp_2 = 6.05e-3*60/2/pi/1000; %[Nm/(rad/s)] Viscous Damping
%Temperature effects
T_0 = Sim.temp;                    %[°C] 
Cth_w = 74.20;                     %[J/K] windings thermal capacity
Hth_w = 1.14;                      %[W/K] motor thermal conductance windings-housing
Cth_housing = 153.90;              %[J/K] housing thermal capacity
Hth_housing = 0.30;                %[W/K] motor thermal conductance housing-ext
alpha_Tw_R = 3.9e-3;               %[1/K] Resistance variation on temperature
alpha_Tw_L = -6.9e-4;              %[1/K] Resistance variation on flux linkage
R = Motor.R_2/2;                   %[Ohm] Phase resistance
L = Motor.L_2/2;                   %[H] Phase inductance (both mutual and not)
    
%DC Motor: joint 4,5 and 6
Motor.Im_456 = 20.767e-06;                  %[kg*m^2] last three motors rotor inertia 
Motor.R_456 = 1.65;                         %[Ohm] DC motor resistance
Motor.L_456 = 2.5e-3;                       %[H] DC motor inductance
Motor.kc_4 = 0.0957;                        %[Nm/A] DC motor torque constant
Motor.kc_5 = 0.0865;                        %[Nm/A] DC motor torque constant
Motor.kc_6 = 0.0893;                        %[Nm/A] DC motor torque constant
Motor.kv_4 = Motor.kc_4;                    %[V*s/rad] DC motor electrical constant
Motor.kv_5 = Motor.kc_5;                    %[V*s/rad] DC motor electrical constant
Motor.kv_6 = Motor.kc_6;                    %[V*s/rad] DC motor electrical constant
Motor.Tmax456 = 28/Motor.tau;               %[N*m] Max motor torque
Motor.imax4 = 3.518;                        %[A] Max current
Motor.imax5 = 3.063;                        %[A] Max current
Motor.imax6 = 2.454;                        %[A] Max current

% Driver controls parameters
% P = proportional, I = integrative, p = position, v = velocity,
% i = current, id = direct current, iq = quadrature current.
% Driver 1
Driver1.Pp = 4000;
Driver1.Pv = 1.5;
Driver1.Iv = 1;
Driver1.Pi = 5;
Driver1.Ii = 500;

% Driver 2
Driver2.Pp = 2000; 
Driver2.Pv = 1.5;
Driver2.Iv = 1; 
Driver2.Piq = 250;
Driver2.Iiq = 500;
Driver2.Pid = 250;
Driver2.Iid = 500;

% Driver 3
Driver3.Pp = 7000;
Driver3.Pv = 1.5;
Driver3.Iv = 1;
Driver3.Pi = 1.5;
Driver3.Ii = 500;

% Driver 4
Driver4.Pp = 2500;
Driver4.Pv = 1.5;
Driver4.Iv = 1; 
Driver4.Pi = 50;
Driver4.Ii = 500;

% Driver 5
Driver5.Pp = 7000;
Driver5.Pv = 1.5;
Driver5.Iv = 1;
Driver5.Pi = 50;
Driver5.Ii = 500;

% Driver 6
Driver6.Pp = 7000;
Driver6.Pv = 1.5;
Driver6.Iv = 1;
Driver6.Pi = 50;
Driver6.Ii = 500;

% PWM Control
T_switch =  1e-4;                  %[s] PWM carrier period

%% Sensors Parameters

%A/D conversion
sampleTime = 1e-5;                       %[s] Sampler time

%Current Sensor
K_i = 1;                                        %[V/A] Current sensor static gain
fc_i = 2000;                                    %[Hz] Cutoff frequency
tau_i = 1/(fc_i*2*pi);                          %[s] Current sensor time constant
var_i = 1e-1*[.0167 .0184 .0144 .0016 ...       %Variance between I_actual and I_actual_f--> var_i=var(I_actual_f-I_actual)
    .0011 .0007];  
quant_i = [Motor.imax1 Motor.imax2 ...          %Quantization interval: it was supposed a 16 bit A/D covnerter
    Motor.imax3 Motor.imax4 Motor.imax5 Motor.imax6]/2^16;

%Encoder (Velocity loop)
K_v = 1;                                        %[V/(rad/s)] Encoder static gain
sigma_v = 2*pi*1000;                            %[rad/s] 
zeta_v = 0.8;%1;                                     %[/]
var_v = 1e-4*[.0738 .1452 ...                   %Variance between qd_actual and qd_actual_f--> var_v=var(qd_actual_f-qd_actual)
    .1564 .4623 .1824 .0917];
quant_v = pi*Motor.tau/2^16;                    %Quantization interval: it was supposed a 16 bit A/D covnerter

%Resolver (Position loop)
K_p = 1;                                        %[V/rad] Resolver static gain
sigma_p = 444.29;                               %[rad/s]
zeta_p = 0.8;%1.06074;                               %[/] 
var_p = 1e-6*[.0943 .3877 ...                   %Variance between q_actual and q_actual_f--> var_p=var(q_actual_f-q_actual)
    .3478 .5436 .3400 .2081];
quant_p = pi/2^16;                              %Quantization interval: it was supposed a 16 bit A/D covnerter

%FT 300 (derived from FT data sheet)
Tool.Mass = 0.3;                            %[kg] upper arm mass
Tool.CM = [0.0, 0.0, 0.017];                %[m] tool center of mass
Tool.InertiaM = [0.000262 0.000262 0.000219];%[kg*m^2] tool moment of inertia
Tool.InertiaP = [0 1e-6 1e-6];              %[kg*m^2] tool product of inertia

% Parametri HD e condizioni iniziali
% Giunto 1
rg1=0.028;                               %[m] raggio flexspline
alpha_t=30*pi/180;                       %[rad] angolo del dente
alpha_n=0.98*pi/180;                     %[rad] angolo alpha_n
Kb1=0.51e8;                              %[N/m] rigidezza radiale wave generator
Km1=1.2e9;                               %[N/m] rigidezza denti in presa
Kt1=0.96e5;                              %[Nm/rad] rigidezza torsionale flexspline
Cb1=1.8e3;                               %[Ns/m] coefficiente di smorzamento viscoso lineare
c1=1e-2;                                 %[Nms/rad] coefficiente di smorzamento viscoso
J_FS1=1.62e-7;                           %[Kgm^2] momento d'inerzia della FS
J_WG1=3.56e-5;                           %[Kgm^2] momento d'inerzia del WG
J_CS1=7.12e-5;                           %[Kgm^2] momento d'inerzia della CS
K_torsWG1=9.56e5;                        %[Nm/rad] rigidezza torsionale tra WG e rotore
K_torsCS1=4.78e6;                        %[Nm/rad] rigidezza torsionale tra CS e giunto
% condizioni iniziali
q_01=q0(1);
qm_01=q0(1)*Motor.tau;
A=[-K_torsWG1, -Kb1*rg1*cos(alpha_n)*sin(alpha_n),Kb1*rg1*cos(alpha_n)*sin(alpha_n), 0, 0, 0; -rg1*tan(alpha_n), 1, 0, 0, 0, 0; 0, Kb1, -Kb1-Km1*(sin(alpha_t))^2, Km1*sin(alpha_t)*cos(alpha_t), Km1*sin(alpha_t)*cos(alpha_t), 0; 0, 0, -Km1*sin(alpha_t)*cos(alpha_t), Km1*(cos(alpha_t))^2+Kt1/rg1^2, Km1*(cos(alpha_t))^2, 0; 0, 0, 0, 0, 1, -rg1; 0, 0, Km1*sin(alpha_t)*cos(alpha_t), -Km1*(cos(alpha_t))^2, -Km1*(cos(alpha_t))^2, -K_torsCS1];
b=[-K_torsWG1*qm_01; 0; 0; 0; 0; -K_torsCS1*q_01];
x=inv(A)*b;
q_WG01=x(1);
x1_01=x(2);
x2_01=x(3);
y1_01=x(4);
y2_01=x(5);
q_CS01=x(6);

%Giunto 2
rg2=0.028;                                   %[m] raggio flexspline
alpha_t=30*pi/180;                           %[rad] angolo del dente
alpha_n=0.98*pi/180;                         %[rad] angolo alpha_n
Kb2=0.51e8;                                  %[N/m] rigidezza radiale wave generator
Km2=1.2e9;                                   %[N/m] rigidezza denti in presa
Kt2=0.96e5;                                  %[Nm/rad] rigidezza torsionale flexspline
Cb2=1.8e3;                                   %[Ns/m] coefficiente di smorzamento viscoso lineare
c2=1e-2;                                     %[Nms/rad] coefficiente di smorzamento viscoso
J_FS2=1.62e-7;                               %[Kgm^2] momento d'inerzia della FS
J_WG2=3.56e-5;                               %[Kgm^2] momento d'inerzia del WG
J_CS2=7.12e-5;                               %[Kgm^2] momento d'inerzia della CS
K_torsWG2=9.56e5;                            %[Nm/rad] rigidezza torsionale tra WG e rotore
K_torsCS2=4.78e6;                            %[Nm/rad] rigidezza torsionale tra CS e giunto
% condizioni iniziali
q_02=q0(2);
qm_02=q0(2)*Motor.tau;
A=[-K_torsWG2, -Kb2*rg2*cos(alpha_n)*sin(alpha_n), Kb2*rg2*cos(alpha_n)*sin(alpha_n), 0, 0, 0; -rg2*tan(alpha_n), 1, 0, 0, 0, 0; 0, Kb2, -Kb2-Km2*(sin(alpha_t))^2, Km2*sin(alpha_t)*cos(alpha_t), Km2*sin(alpha_t)*cos(alpha_t), 0; 0, 0, -Km2*sin(alpha_t)*cos(alpha_t), Km2*(cos(alpha_t))^2+Kt2/rg2^2, Km2*(cos(alpha_t))^2, 0; 0, 0, 0, 0, 1, -rg2; 0, 0, Km2*sin(alpha_t)*cos(alpha_t), -Km2*(cos(alpha_t))^2, -Km2*(cos(alpha_t))^2, -K_torsCS2];
b=[-K_torsWG2*qm_02; 0; 0; 0; 0; -K_torsCS2*q_02];
x=inv(A)*b;
q_WG02=x(1);
x1_02=x(2);
x2_02=x(3);
y1_02=x(4);
y2_02=x(5);
q_CS02=x(6);

%Giunto 3
rg3=0.028;                                 %[m] raggio flexspline
alpha_t=30*pi/180;                         %[rad] angolo del dente
alpha_n=0.98*pi/180;                       %[rad] angolo alpha_n
Kb3=0.51e8;                                %[N/m] rigidezza radiale wave generator
Km3=1.2e9;                                 %[N/m] rigidezza denti in presa
Kt3=0.96e5;                                %[Nm/rad] rigidezza torsionale flexspline
Cb3=1.8e3;                                 %[Ns/m] coefficiente di smorzamento viscoso lineare
c3=1e-2;                                   %[Nms/rad] coefficiente di smorzamento viscoso
J_FS3=1.62e-7;                             %[Kgm^2] momento d'inerzia della FS
J_WG3=3.56e-5;                             %[Kgm^2] momento d'inerzia del WG
J_CS3=7.12e-5;                             %[Kgm^2] momento d'inerzia della CS
K_torsWG3=9.56e5;                          %[Nm/rad] rigidezza torsionale tra WG e rotore
K_torsCS3=4.78e6;                          %[Nm/rad] rigidezza torsionale tra CS e giunto
% condizioni iniziali
q_03=q0(3);
qm_03=q0(3)*Motor.tau;
A=[-K_torsWG3, -Kb3*rg3*cos(alpha_n)*sin(alpha_n), Kb3*rg3*cos(alpha_n)*sin(alpha_n), 0, 0, 0; -rg3*tan(alpha_n), 1, 0, 0, 0, 0; 0, Kb3, -Kb3-Km3*(sin(alpha_t))^2, Km3*sin(alpha_t)*cos(alpha_t), Km3*sin(alpha_t)*cos(alpha_t), 0; 0, 0, -Km3*sin(alpha_t)*cos(alpha_t), Km3*(cos(alpha_t))^2+Kt3/rg3^2, Km3*(cos(alpha_t))^2, 0; 0, 0, 0, 0, 1, -rg3; 0, 0, Km3*sin(alpha_t)*cos(alpha_t), -Km3*(cos(alpha_t))^2, -Km3*(cos(alpha_t))^2, -K_torsCS3];
b=[-K_torsWG3*qm_03; 0; 0; 0; 0; -K_torsCS3*q_03];
x=inv(A)*b;
q_WG03=x(1);
x1_03=x(2);
x2_03=x(3);
y1_03=x(4);
y2_03=x(5);
q_CS03=x(6);

%Giunto 4
rg4=0.0155;                                 %[m] raggio flexspline
alpha_t=30*pi/180;                          %[rad] angolo del dente
alpha_n=0.98*pi/180;                        %[rad] angolo alpha_n
Kb4=1.98e7;                                 %[N/m] rigidezza radiale wave generator
Km4=5.55e8;                                 %[N/m] rigidezza denti in presa
Kt4=1.15e4;                                 %[Nm/rad] rigidezza torsionale flexspline
Cb4=1.8e3;                                  %[Ns/m] coefficiente di smorzamento viscoso lineare
c4=1e-2;                                    %[Nms/rad] coefficiente di smorzamento viscoso
J_FS4=4.97e-8;                              %[Kgm^2] momento d'inerzia della FS
J_WG4=3e-6;                                 %[Kgm^2] momento d'inerzia del WG
J_CS4=6e-6;                                 %[Kgm^2] momento d'inerzia della CS
K_torsWG4=1.84e5;                           %[Nm/rad] rigidezza torsionale tra WG e rotore
K_torsCS4=1.06e6;                           %[Nm/rad] rigidezza torsionale tra CS e giunto
% condizioni iniziali
q_04=q0(4);
qm_04=q0(4)*Motor.tau;
A=[-K_torsWG4, -Kb4*rg4*cos(alpha_n)*sin(alpha_n), Kb4*rg4*cos(alpha_n)*sin(alpha_n), 0, 0, 0; -rg4*tan(alpha_n), 1, 0, 0, 0, 0; 0, Kb4, -Kb4-Km4*(sin(alpha_t))^2, Km4*sin(alpha_t)*cos(alpha_t), Km4*sin(alpha_t)*cos(alpha_t), 0; 0, 0, -Km4*sin(alpha_t)*cos(alpha_t), Km4*(cos(alpha_t))^2+Kt4/rg4^2, Km4*(cos(alpha_t))^2, 0; 0, 0, 0, 0, 1, -rg4; 0, 0, Km4*sin(alpha_t)*cos(alpha_t), -Km4*(cos(alpha_t))^2, -Km4*(cos(alpha_t))^2, -K_torsCS4];
b=[-K_torsWG4*qm_04; 0; 0; 0; 0; -K_torsCS4*q_04];
x=inv(A)*b;
q_WG04=x(1);
x1_04=x(2);
x2_04=x(3);
y1_04=x(4);
y2_04=x(5);
q_CS04=x(6);

%Giunto 5
rg5=0.0155;                                  %[m] raggio flexspline
alpha_t=30*pi/180;                           %[rad] angolo del dente
alpha_n=0.98*pi/180;                         %[rad] angolo alpha_n
Kb5=1.98e7;                                  %[N/m] rigidezza radiale wave generator
Km5=5.55e8;                                  %[N/m] rigidezza denti in presa
Kt5=1.15e4;                                  %[Nm/rad] rigidezza torsionale flexspline
Cb5=1.8e3;                                   %[Ns/m] coefficiente di smorzamento viscoso lineare
c5=1e-2;                                     %[Nms/rad] coefficiente di smorzamento viscoso
J_FS5=4.97e-8;                               %[Kgm^2] momento d'inerzia della FS
J_WG5=3e-6;                                  %[Kgm^2] momento d'inerzia del WG
J_CS5=6e-6;                                  %[Kgm^2] momento d'inerzia della CS
K_torsWG5=1.84e5;                            %[Nm/rad] rigidezza torsionale tra WG e rotore
K_torsCS5=1.06e6;                            %[Nm/rad] rigidezza torsionale tra CS e giunto
% condizioni iniziali
q_05=q0(5);
qm_05=q0(5)*Motor.tau;
A=[-K_torsWG5, -Kb5*rg5*cos(alpha_n)*sin(alpha_n), Kb5*rg5*cos(alpha_n)*sin(alpha_n), 0, 0, 0; -rg5*tan(alpha_n), 1, 0, 0, 0, 0; 0, Kb5, -Kb5-Km5*(sin(alpha_t))^2, Km5*sin(alpha_t)*cos(alpha_t), Km5*sin(alpha_t)*cos(alpha_t), 0; 0, 0, -Km5*sin(alpha_t)*cos(alpha_t), Km5*(cos(alpha_t))^2+Kt5/rg5^2, Km5*(cos(alpha_t))^2, 0; 0, 0, 0, 0, 1, -rg5; 0, 0, Km5*sin(alpha_t)*cos(alpha_t), -Km5*(cos(alpha_t))^2, -Km5*(cos(alpha_t))^2, -K_torsCS5];
b=[-K_torsWG5*qm_05; 0; 0; 0; 0; -K_torsCS5*q_05];
x=inv(A)*b;
q_WG05=x(1);
x1_05=x(2);
x2_05=x(3);
y1_05=x(4);
y2_05=x(5);
q_CS05=x(6);

%Giunto 6
rg6=0.0155;                                      %[m] raggio flexspline
alpha_t=30*pi/180;                               %[rad] angolo del dente
alpha_n=0.98*pi/180;                             %[rad] angolo alpha_n
Kb6=1.98e7;                                      %[N/m] rigidezza radiale wave generator
Km6=5.55e8;                                      %[N/m] rigidezza denti in presa
Kt6=1.15e4;                                      %[Nm/rad] rigidezza torsionale flexspline
Cb6=1.8e3;                                       %[Ns/m] coefficiente di smorzamento viscoso lineare
c6=1e-2;                                         %[Nms/rad] coefficiente di smorzamento viscoso
J_FS6=4.97e-8;                                   %[Kgm^2] momento d'inerzia della FS
J_WG6=3e-6;                                      %[Kgm^2] momento d'inerzia del WG
J_CS6=6e-6;                                      %[Kgm^2] momento d'inerzia della CS
K_torsWG6=1.84e5;                                %[Nm/rad] rigidezza torsionale tra WG e rotore
K_torsCS6=1.06e6;                                %[Nm/rad] rigidezza torsionale tra CS e giunto
% condizioni iniziali
q_06=q0(6);
qm_06=q0(6)*Motor.tau;
A=[-K_torsWG6, -Kb6*rg6*cos(alpha_n)*sin(alpha_n), Kb6*rg6*cos(alpha_n)*sin(alpha_n), 0, 0, 0; -rg6*tan(alpha_n), 1, 0, 0, 0, 0; 0, Kb6, -Kb6-Km6*(sin(alpha_t))^2, Km6*sin(alpha_t)*cos(alpha_t), Km6*sin(alpha_t)*cos(alpha_t), 0; 0, 0, -Km6*sin(alpha_t)*cos(alpha_t), Km6*(cos(alpha_t))^2+Kt6/rg6^2, Km6*(cos(alpha_t))^2, 0; 0, 0, 0, 0, 1, -rg6; 0, 0, Km6*sin(alpha_t)*cos(alpha_t), -Km6*(cos(alpha_t))^2, -Km6*(cos(alpha_t))^2, -K_torsCS6];
b=[-K_torsWG6*qm_06; 0; 0; 0; 0; -K_torsCS6*q_06];
x=inv(A)*b;
q_WG06=x(1);
x1_06=x(2);
x2_06=x(3);
y1_06=x(4);
y2_06=x(5);
q_CS06=x(6);
