1.分两个程序①主函数②function函数
2.main
clear;
clc;
%建立机器人模型
% theta d a alpha offset
ML1=Link([0 0 0 0 0 ],'modified');
ML2=Link([0 0 0.180 -pi/2 0 ],'modified');
ML3=Link([0 0 0.600 0 0 ],'modified');
ML4=Link([0 0.630 0.130 -pi/2 0 ],'modified');
ML5=Link([0 0 0 pi/2 0 ],'modified');
ML6=Link([0 0 0 -pi/2 0 ],'modified');
modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','PUMA 560');
modt06=modrobot.fkine([0,0,pi/2,0,0,pi/2]) %工具箱正解函数
modmyt06=myfkine(0,0,pi/2,0,0,pi/2) %手写的正解函数
3.function
function [T06]=myfkine(theta1,theta2,theta3,theta4,theta5,theta6)
MDH=[theta1 0 0 0;
theta2 0 0.180 -pi