Matlab机器人工具箱（3）：双臂操作（从模型建立到轨迹规划）

从 Matlab机器人工具箱（3） 开始，使用的机器人工具版本更换到v10版本
他们的区别还是挺大的：

• 一个是单位的问题：从m变为mm
• 还有一个是变量类型的问题，变换矩阵从正常的矩阵形式变为se3类型

所以，这两个版本的代码基本不可以通用

如果网上的代码报错，很可能是因为版本错误
主要修改的地方就是长度的单位，和旋转矩阵每个元素的获取方式
v10版本元素获取可以参考c++的类方法，比如T.t，就是获取旋转矩阵第四列的前三个元素

01 先是自己写的五自由度模型建立

下面是自己建立的一个五自由度机械臂
因为是空中机械臂，所以左右和地面时颠倒的

%% 左机械臂（图中右）
d1=[        0,     0,        0,        0,       0];
a2=[        0,    13,   233.24,   175.64,       0];
alpha2=[    0,  pi/2,        0,        0,    pi/2];


%使用offset
L1(1)=Link('d',d1(1),'a',a2(1),'alpha',alpha2(1),'modified'); 
L1(2)=Link('d',d1(2),'a',a2(2),'alpha',alpha2(2),'offset',pi/2,'modified');
L1(3)=Link('d',d1(3),'a',a2(3),'alpha',alpha2(3),'modified');
L1(4)=Link('d',d1(4),'a',a2(4),'alpha',alpha2(4),'offset',pi/2,'modified');
L1(5)=Link('d',d1(5),'a',a2(5),'alpha',alpha2(5),'modified');

du=pi/180;
%定义关节范围
L1(1).qlim =[-170, 170]*du;
L1(2).qlim =[60-70, 60+70]*du;
L1(3).qlim =[-70-70,-70+70]*du;
L1(4).qlim =[-70,70]*du;
L1(5).qlim =[-170, 170]*du;


%% 右机械臂（图中左）
d2=[        0,     0,        0,        0,       0];
a2=[        0,    13,   233.24,   175.64,       0];
alpha2=[    0,  pi/2,        0,        0,    pi/2];