1、手写矩阵函数myfkine.m
%改进型D-H建模
function [T06]=myfkine(theta1,theta2,theta3,theta4,theta5,theta6)
% theta d a alpha
% 关节转角 关节距离 连杆长度 连杆扭角
MDH=[theta1 0.4 0.025 pi/2;
theta2 0 0.56 0;
theta3 0 0.035 pi/2;
theta4 0.515 0 pi/2;
theta5 0 0 pi/2;
theta6 0.08 0 0];
T01=[cos(MDH(1,1)) -sin(MDH(1,1)) 0 MDH(1,3);
cos(MDH(1,4))*sin(MDH(1,1)) cos(MDH(1,4))*cos(MDH(1,1)) -sin(MDH(1,4)) -MDH(1,2)*sin(MDH(1,4));
sin(MDH(1,4))*sin(MDH(1,1)) sin(MDH(1,4))*cos(MDH(1,1)) cos(MDH(1,4)) MDH(1,2)*cos(MDH(1,4));
0 0 0 1];
T12=[cos(MDH(2,1)) -sin(MDH(2,1)) 0 MDH(2,3);
cos(MDH(2,4))*sin(MDH(2,1)) cos(MDH(2,4))*