close all; clear; clc;
%% theoretical model

d1 = 0.089159; d4 = 0.10915; d5 = 0.09465; d6 = 0.0823;
a3 = 0.425; a4 = 0.39225;

Alpha = [0, -pi/2, 0, 0, pi/2, -pi/2];
A = [0, 0, a3, a4, 0, 0];
D = [d1, 0, 0, d4, d5, d6];
Theta = [0, -pi/2, 0, pi/2, 0, 0];
Sigma = [1, 1, 1, 1, 1, 1];

for i = 1:6
    L(i) = Link('alpha',Alpha(i),'a', A(i),'d', D(i));L(i).offset = Theta(i); L(i).qlim=[-pi, pi];L(i).mdh = 1;
end

robot = SerialLink(L, 'name', 'UR');

DH_Para.Alpha = Alpha;
DH_Para.A = A;
DH_Para.D = D;
DH_Para.Theta = Theta;
DH_Para.Sigma = Sigma;

n = 100;
Q = rand(n, 6) * 2 * pi - pi;

model_actual;