Compilation of standard DH model positive kinematics solving program

Based on the robot toolbox model built by Peter Croke.
In the standard DH model, the relationship between the j-th joint, the j-th link and the j-th coordinate system is as follows. That is, the j coordinate system is established at joint j+1.

Improve the DH model. The relationship between the j-th joint, the j-th link and the j-th coordinate system is as follows. That is, the j coordinate system is established at the joint j.

The following robot kinematics positive solution program is based on the standard DH model;
according to Peter Croke's "Robotic, Vision and Control—Fundamental Algorithms in MATLAB", we can get the link coordinate system {j-1} to the coordinate system {j The transformation formula of} is as follows;

can be expanded by matrix translation and rotation transformation:

Therefore, the transformation matrix for the sixth joint coordinate system of a six-degree-of-freedom robot relative to the base coordinate system can be written as:
T6-0=T1-0* T2-1* T3-2* T4-3* T5-4* T6-5
T6-1 represents the conversion matrix of the sixth joint coordinate system relative to the base coordinate system.
First, write the robot's standard DH parameters:

SDH=[theta1   0.000   -0.05      pi/2;
     theta2   0.000   0.425    0.000;
     theta3   0.050   0.000    -pi/2;
     theta4   0.425   0.000    pi/2;
     theta5   0.000   0.000    -pi/2;
     theta6   0.000   0.000    0.000];

Then write the transformation matrix:

T01=[cos(SDH(1,1))  -sin(SDH(1,1))*cos(SDH(1,4))   sin(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*cos(SDH(1,1));
      sin(SDH(1,1))   cos(SDH(1,1))*cos(SDH(1,4))  -cos(SDH(1,1))*sin(SDH(1,4))    SDH(1,3)*sin(SDH(1,1));
      0               sin(SDH(1,4))                 cos(SDH(1,4))                  SDH(1,2);
      0               0                             0                              1];

According to the above, write T12, T23, T32, T45, T56 in turn;
finally:
T06=T01 T12 T23 T34 T45*T56; complete the positive kinematics solution of the robot standard DH model.