%%计算工具坐标系下的雅可比矩阵clear,clc,closeall;formatcompactsymsd1d2d3d4d5d6a2a3alp1alp4alp5symsq1q2q3q4q5q6%%建立机器人DH参数，初始状态为竖直状态%连杆偏移d,连杆长度a,连杆扭转角alphaL(1)=RevoluteMDH(‘d’,d1,‘a’,0,‘alpha’,0);L(2)=RevoluteMDH(‘d’,0,‘a’,0,‘alpha’,alp1,‘offset’,0);%-pi/2L(3)=RevoluteMDH(‘d’,0,‘a’,a2,‘alpha’,0);L(4)=RevoluteMDH(‘