UR5 modeling

%% UR5 modeling
clear;
clc;


% DH parameters
% alptha, a, d, theta respectly
L = [0.5 * pi,       0,         0.0892,         0,          0;
        0,        -0.425,          0,        -0.5 * pi,     0;
        0,        -0.392,          0,           0,          0;
     0.5 * pi,       0 ,        0.1093       -0.5 * pi,     0;
    -0.5 * pi ,      0,         0.09475,        0,          0;
        0,           0,         0.0825 ,        0,          0];
    
L = L(:,[4,3,2,1,5]);


% Modeling
Li(1) = Link(L(1,:));
Li(2) = Link(L(2,:));
Li(3) = Link(L(3,:));
Li(4) = Link(L(4,:));
Li(5) = Link(L(5,:));
Li(6) = Link(L(6,:));
six_link = SerialLink(Li, 'name', 'ur5');


% plot robot at [0 0 0 0 0 0 ](six joints)
six_link.plot([0 0 0 0 0 0]);


% two pose of manipulator
qz = [0, 0.789, -1.756, 2.278, 0, 0];
qr = [1.6034, -1.234, 0.179,-1.428,2.231,-1.0723];


% Forward Kinematics,plot every joints' motion
t = 0:0.056:3;
q = jtraj(qz,qr,t);
T = fkine(six_link,q);
for i = 1:6
    figure;
    plot(t,q(:,i));
end


% Plot the trajectory of end-effector
position = T(1:3,4,:);
position = reshape(position,3,54);
plot3(position(1,:),position(2,:),position(3,:));