function [L,UR5] = UR5_Build_Full()
% definizione dei parametri Denavit-Hartenberg
% unità  di misura: rad, m
%Per plottare usare flag_DH = 0, per simulazioni usare flag_DH = 1
%DH_par = [alfa a d theta]

delta_theta=[0 0 0 0 0 0];
delta_a=[0 0 0 0 0 0];
delta_d=[0 0 0 0 0 0];
delta_alpha=[0 0 0 0 0 0];

%% Parametri reali da usare per le simulazioni
% parametri Denavit-Hartenberg link 1 (0-base)
alfa1 = pi/2 + delta_alpha(1); %rad
a1 =  delta_a(1); %m
d1 =  89.459/1000 + delta_d(1); %m 0
teta10 = delta_theta(1); %rad

% parametri Denavit-Hartenberg link 2 (base-spalla)
alfa2 = delta_alpha(2); %rad
a2 = -425 / 1000 + delta_a(2); %m 
d2 = delta_d(2); %m
teta20 = delta_theta(2); %rad

% parametri Denavit-Hartenberg link 3 (spalla-gomito)
alfa3 = delta_alpha(3); %rad
a3 = -392.25 / 1000 + delta_a(3); %m 
d3 = delta_d(3); %m
teta30 = delta_theta(3); %rad

% parametri Denavit-Hartenberg link 4 (gomito-polso1)
alfa4 = pi/2 + delta_alpha(4); %rad
a4 = delta_a(4); %m
d4 = 109.15 / 1000 + delta_d(4); %m 
teta40 = delta_theta(4); %rad

% parametri Denavit-Hartenberg link 5 (polso1-polso2)
alfa5 = -pi/2 + delta_alpha(5); %rad
a5 = delta_a(5); %m
d5 = 94.65 / 1000 + delta_d(5); %m
teta50 = delta_theta(5); %rad


% parametri Denavit-Hartenberg link 6 (polso2-polso3)
alfa6 = delta_alpha(6); %rad
a6 = delta_a(6); %m
d6 = 82.3 / 1000 + delta_d(6); %m
teta60 = delta_theta(6); %rad

%% Costruzione robot con parametri DH reali
% L = vettore dei parametri dei link 1-6
L(1)=Link('alpha',alfa1,'a',a1,'d',d1,'offset',teta10,'standard','qlim',[-2*pi 2*pi]);     % robot link 1
L(2)=Link('alpha',alfa2,'a',a2,'d',d2,'offset',teta20,'standard','qlim',[-2*pi 2*pi]);     % robot link 2
L(3)=Link('alpha',alfa3,'a',a3,'d',d3,'offset',teta30,'standard','qlim',[-2*pi 2*pi]);     % robot link 3
L(4)=Link('alpha',alfa4,'a',a4,'d',d4,'offset',teta40,'standard','qlim',[-2*pi 2*pi]);     % robot link 4
L(5)=Link('alpha',alfa5,'a',a5,'d',d5,'offset',teta50,'standard','qlim',[-2*pi 2*pi]);     % robot link 5
L(6)=Link('alpha',alfa6,'a',a6,'d',d6,'offset',teta60,'standard','qlim',[-2*pi 2*pi]);     % robot link 6

% Links mass [kg] (Universal Robots website)
L(1).m = 3.7; L(2).m = 8.393; L(3).m = 2.33;
L(4).m = 1.219; L(5).m = 1.219; L(6).m = 0.1879;

% Center of mass [m] (Universal Robots website)
L(1).r = [0, -0.02561, 0.00193]; L(2).r = [0.2125, 0, 0.11336]; L(3).r = [0.15, 0.0, 0.0265];
L(4).r = [0, -0.0018, 0.01634]; L(5).r = [0, 0.0018, 0.01634];  L(6).r = [0, 0, -0.001159];

% Inertia tensors [kg*m^2] (thesis model C)
L(1).I = [0.0067 0 0; 0 0.0064 0; 0 0 0.0067];
L(2).I = [0.0149 0 0; 0 0.3564 0; 0 0 0.3553];
L(3).I = [0.0025 0 0.0034; 0 0.0551 0; 0.0034 0 0.0546];
L(4).I = [0.0012 0 0; 0 0.0012 0; 0 0 0.0009];
L(5).I = [0.0012 0 0; 0 0.0012 0; 0 0 0.0009];
L(6).I = [0.0001 0 0; 0 0.0001 0; 0 0 0.0001];

% Gear ratio (UR5 PoliTO data)
L(1).G = 101; L(2).G = 101; L(3).G = 101;
L(4).G = 101; L(5).G = 101; L(6).G = 101;

% Motor inertia [kg*m^2] (UR5 PoliTO data)
L(1).Jm = 187.740e-6; L(2).Jm = 187.740e-6; L(3).Jm = 187.740e-6;
L(4).Jm = 2.0767e-5; L(5).Jm = 2.0767e-5; L(6).Jm = 2.0767e-5;

G=101; %gear ratio
%Coulomb friction [N*m] (UR5 PoliTO data) (motor referred, scaled up by G)
L(1).Tc=[0.07685 -0.07655]/G; L(2).Tc=[0.08392 -0.08209]/G; L(3).Tc=[0.07835 -0.07774]/G;
L(4).Tc=[0.01430 -0.01474]/G; L(5).Tc=[0.02085 -0.01900]/G; L(6).Tc=[0.02097 -0.021111]/G;

% Viscous friction (motor referred, scaled up by G^2)
L(1).B = 0.0525/G^2; L(2).B = 0.0611/G^2; L(3).B = 0.0603/G^2;
L(4).B = 0.0081/G^2; L(5).B = 0.0179/G^2; L(6).B = 0.0124/G^2;

% costruizone del SerialLink object denominato UR5
UR5 = SerialLink(L,'name','UR5');

clc
