正坐标系及矢量知识，点乘与差乘，旋转

clear,clc,close all;
%建立机器人DH参数&＃xff0c;初始状态为竖直状态
L1&＃61;Link(&＃39;d&＃39;,144,&＃39;a&＃39;,0,&＃39;alpha&＃39;,0,&＃39;modified&＃39;);
L2&＃61;Link(&＃39;d&＃39;,0,&＃39;a&＃39;,0,&＃39;alpha&＃39;,pi/2,&＃39;offset&＃39;,-pi/2,&＃39;modified&＃39;);
L3&＃61;Link(&＃39;d&＃39;,0,&＃39;a&＃39;,-264,&＃39;alpha&＃39;,0,&＃39;modified&＃39;);
L4&＃61;Link(&＃39;d&＃39;,106,&＃39;a&＃39;,-236,&＃39;alpha&＃39;,0,&＃39;offset&＃39;,-pi/2,&＃39;modified&＃39;);
L5&＃61;Link(&＃39;d&＃39;,114,&＃39;a&＃39;,0,&＃39;alpha&＃39;,pi/2,&＃39;modified&＃39;);
L6&＃61;Link(&＃39;d&＃39;,67,&＃39;a&＃39;,0,&＃39;alpha&＃39;,-pi/2,&＃39;modified&＃39;);
robot&＃61;SerialLink([L1 L2 L3 L4 L5 L6],&＃39;name&＃39;,&＃39;Arm6&＃39;) ;
Theta&＃61;[0 0 0 0 0 0]; Theta&＃61;Theta/180*pi;%换算成弧度 forwarda&＃61;robot.fkine(Theta)%求正解的齐次变换矩阵 W&＃61;[-1000,&＃43;1000,-1000,&＃43;1000,-1000,&＃43;1000];robot.plot(Theta,&＃39;tilesize&＃39;,150,&＃39;workspace&＃39;,W);%显示三维动画
robot.teach(forwarda,&＃39;rpy&＃39; )
%显示roll/pitch/yaw angles&＃xff0c;GUI可调界面