matlab仿真UR5机器人动作

clc;
clear;
a =     [0         -0.42500   -0.39225   0          0          0];
alpha = [pi/2      0          0          pi/2       -pi/2      0];
d =     [0.089459  0          0          0.10915    0.09465    0.0823];
mass =  [3.7000    8.3930     2.33       1.2190     1.2190     0.1897];
center_of_mass = [[0-0.02561 0.00193] [0.2125 0 0.11336] [0.15 0 0.0265] [0 -0.0018 0.01634] [0 -0.0018 0.01634] [0 0 -0.001159]];
theta = [-0.5452   -1.1561    1.6150   -2.4635   -0.6130   -1.1233]

%             th    d       a    alpha  
L1 = link（[-0.5452  0.089459 0 pi/2 ]‘standard‘）;

L2 = link（[-1.1561  0  -0.4250 0