机械臂正运动学程序，两种实现方式，robotics工具箱和转换矩阵

包含两种机械臂建模实现方式，可以求出机械臂正运动学末端坐标系到基坐标系的转换矩阵，有转换矩阵形式实现和robotics toolbox实现

%%
%%Transformation matrix
% clear;
% clc;
% Transformation matrix
% a1 = 0;ar1 = 0;d1 = 0;th1 = 0;
% a2 = 0;ar2 = -pi/2;d2 = 149.09;th2 = pi/2;
% a3 = 431.8;ar3 = 0;d3 = 0;th3 = 0;
% a4 = 20.32;ar4 = -pi/2;d4 = 433.07;th4 = 0;
% a5 = 0;ar5 = pi/2;d5 = 0;th5 = pi/2;
% a6 = 0;ar6 = -pi/2;d6 = 0;th6 = 0;
% 
% T16 = eye（4）;
% 
% for i=1:6
%     t1 = eval（[‘th‘ num2str（i）]）;
%     t2 = eval（[‘ar‘ num2str（i）]）;
%     t3 = eval（[‘d‘ num2str（i）]）;
%     t4 = eval（[‘a‘ num2str（i）]）;
%     
%     T（::i）=[cos（t1）-sin（t1）0t4;
%     sin（t1）*cos（t2）cos（t1）*cos（t2）-sin（t2）-t3*sin（t2）;
%     sin（t1）*sin（t2）cos（t1）*sin（t2）cos（t2）t3*cos（t2）;
%     0001];
% end
% 
% for k=1:6
%    T16 = T16*T（::k）;
% end
% 
% T16



%%
%标准DH建立puma560机器人模型
%显示D-H表
clear;
clc;
%建立机器人模型
% theta d a alpha offset
L1=link（[0 0 0 0 0 ]‘modified‘）; %连杆的D-H参数
L2=link（[0 149.09 0 -pi/2 0 ]‘modified‘）;
L3=link（[0 0 431.8 0 0 ]‘modified‘）;
L4=link（[0 433.07 20.32 -pi/2 0 ]‘modified‘）;
L5=link（[0 0 0 pi/2 0 ]‘modified‘）;
L6=link（[0 0 0 -pi/2 0 ]‘modified‘）;
robot=Seriallink（[L1 L2 L3 L4 L5 L6]‘name‘‘puma560‘‘base‘  ...
transl（0 0 0.62）* trotz（0））; %连接连杆，机器人取名puma560
% robot.plot3d（[0pi/200pi0]）;%输出机器人模型，后面的六个角为输出时的theta姿态
% figure
% robot.teach（） 
% robot.display（）; %显示D-H表

figure
% q=[0pi/200pi/20]
% q=[pi/3pi/3-pi/3pi/4pi/40]
q = [00-pi/2000];
robot.plot（q）
t=robot.fkine（q）%运动学正解

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-06-30 08:56  正运动学\
     文件        1529  2020-07-09 16:53  正运动学\zhengyundong_puma560_modified.m