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

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

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

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

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

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

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

%% 左机械臂（图中右）
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];


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

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

%% 注释部分有错，一画第二个图，第一个图就也改变了
% figure(1)
% 就是下面这里，一定要是不一样的名字呀！
% bot1=SerialLink(L1,'name','五*度机械臂');
bot1=SerialLink(L1,'name','P1');
bot1.base= transl(45, 0, 0);
bot1.plot([0,pi/2,0,0,0])


figure(2)
% 就是下面这里，一定要是不一样的名字呀！
% bot2=SerialLink(L2,'name','五*度机械臂');
bot2=SerialLink(L2,'name','P2');
bot2.base= transl(-45, 0, 0);
% bot2.plot([0,pi/2,0,0,0])

说明：

• 一开始最后画图的部分有错，只要bot2一使用plot，bot1的图就马上变成bot2的样子（姿态&Base），明明下面那个底座的棍子还是对的呀（呜呜呜~）
  
• 解决：
  后来参考了别人的代码，万万没想到，是关于name的问题，我以为他只是一个中文的标记而已啊
  换了name就好了
  之后的效果如下图

然后是我参考的一个demo

他是用puma560
先是建立模型
然后是轨迹规划

%% 第一个机械臂
mdl_puma560;
p560b = p560; % duplicate the robot
p560b.name ='P1'; % give it a unique name
p560b.base = transl([0 -0.16 0]); % move its base
qr1 = [2.8 0 3.5 0 0 0];
plot(p560, qr1); % display it at ready position
hold on;

%% 第二个机械臂
mdl_puma560;
p560b1 = p560; % duplicate the robot
p560b1.name ='P2'; % give it a unique name
p560b1.base = transl([0 0.16 0]); % move its base
%qr2 = [2.8 0 3.5 0 0 0];
plot(p560, qr1); % display it at ready position
hold on;
%plot(p560b, qr); % display it at ready position
%plot(p560b1, qr); % display it at ready position

%% 轨迹规划部分
t = [0:0.05:5];
%qr11 = [3.8 0 3.5 0 0 0];
qr11 = [4.8 1.5 3.9 1 0 0];
qz1 = qr1;
jt = jtraj(qz1,qr11, t); % trajectory to stretch pose
rx=[];
ry=[];
for q = jt' % for all points on the path
%??hold on
%plot(p560, q');
%??hold on
plot(p560b,q');
plot(p560b1, q');
% X = sprintf('%d and %d',x(6),y(6));
% disp(X)
% rx=[rx;x(6)]; 
% ry=[ry;y(6)];

end

scatter3(1,1,1);