1.分两个程序①主函数②function函数
2.main
clear;
clc;
%建立机器人模型
% theta d a alpha offset
SL1=Link([0 0 0.180 -pi/2 0 ],'standard');
SL2=Link([0 0 0.600 0 0 ],'standard');
SL3=Link([0 0 0.130 -pi/2 0 ],'standard');
SL4=Link([0 0.630 0 pi/2 0 ],'standard');
SL5=Link([0 0 0 -pi/2 0 ],'standard');
SL6=Link([0 0.1075 0 0 0 ],'standard');
starobot=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','standard');
stat06=starobot.fkine([0,0,pi/2,0,0,pi/2]) %工具箱正解函数
stamyt06=mystafkine(0,0,pi/2,0,0,pi/2) %手写正解函数
3.function
function [T06]=mystafkine(theta1,theta2,theta3,theta4,theta5,theta6)
SDH=[theta1 0 0.180 -pi/2;
theta2 0 0.60