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];


%使用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就好了
  之后的效果如下图

02 五自由度双臂的工作空间

• 使用蒙特卡洛法
• 一个臂是蓝色点，一个臂是红色点

% RoboticToolbox v10
% 双臂工作空间求解
clc;
clear;
%% 准备
    %保留精度
    format short;
    
    %角度转换
    du=pi/180;  %度
    radian=180/pi; %弧度
    
    %模型导入（有offset版）
    mdl_double_arm
 
%% 参数
    %关节角限位
    q1_s=-170;     q1_end=170;
    q2_s=60-70;    q2_end=60+70;
    q3_s=-70-70;   q3_end=-70+70;
    q4_s=-70;      q4_end=70;
    q5_s=-170;     q5_end=170;
    
    
    %计算点数
%     num=10000;
     num=10000;
 
%% 求取工作空间
    %设置轴关节随机分布,轴5不对工作范围产生影响，设置为0
    q1_rand = q1_s + rand(num,1)*(q1_end - q1_s);
    q2_rand = q2_s + rand(num,1)*(q2_end - q2_s);
    q3_rand = q3_s + rand(num,1)*(q3_end - q3_s);
    q4_rand = q4_s + rand(num,1)*(q4_end - q4_s);
    q5_rand = rand(num,1)*0;
    q = [q1_rand q2_rand q3_rand q4_rand q5_rand]*du;
    
    %正运动学计算工作空间
    tic;
    %臂1
    T1_cell = cell(num,1);
    for i=1:1:num
        [T1_cell{i}]=bot1.fkine(q(i,:));%正向运动学仿真函数
    end
    
    %臂2
    T2_cell = cell(num,1);
    for i=1:1:num
        [T2_cell{i}]=bot2.fkine(q(i,:));%正向运动学仿真函数
    end
    
    disp(['运行时间：',num2str(toc)]);
 
 %% 分析结果
    %绘制工作空间
    t1=clock;
    figure('name','机械臂工作空间')
    hold on
    plotopt = {'noraise', 'nowrist', 'nojaxes', 'delay',0};
    
    %绘制机械臂1
    %绘制机械臂位置
    bot1.plot([0 20*du -70*du 20*du 0], plotopt{:});
     figure1_x=zeros(num,1);
     figure1_y=zeros(num,1);
     figure1_z=zeros(num,1);
     for cout=1:1:num
         tt1=T1_cell{cout}.t;%使用机器人工具箱v10，保存为se3格式，而不是矩阵
         figure1_x(cout,1)=tt1(1);
         figure1_y(cout,1)=tt1(2);
         figure1_z(cout,1)=tt1(3);
     end
     plot3(figure1_x,figure1_y,figure1_z,'r.','MarkerSize',3);
     
     %绘制机械臂2
     %绘制机械臂位置
    bot2.plot([0 20*du -70*du 20*du 0], plotopt{:});
     figure2_x=zeros(num,1);
     figure2_y=zeros(num,1);
     figure2_z=zeros(num,1);
     for cout=1:1:num
         tt2=T2_cell{cout}.t;%使用机器人工具箱v10，保存为se3格式，而不是矩阵
         figure2_x(cout,1)=tt2(1);
         figure2_y(cout,1)=tt2(2);
         figure2_z(cout,1)=tt2(3);
     end
     plot3(figure2_x,figure2_y,figure2_z,'b.','MarkerSize',3);
     
     hold off
     disp(['绘制工作空间运行时间：',num2str(etime(clock,t1))]);  
     
     %获取X,Y,Z空间坐标范围(y,z二者一样；x去p1的最小，p2的最大）
     Point_range=[min(figure1_x) max(figure2_x) min(figure1_y) max(figure1_y) min(figure1_z) max(figure1_z)];
     
%% 投影视图
figure('name','三维图')
	plot3(figure1_x,figure1_y,figure1_z,'r.','MarkerSize',4);%设置3有点小
	hold on;
	plot3(figure2_x,figure2_y,figure2_z,'b.','MarkerSize',4);%设置3有点小
	hold off;
	set(gca,'XTick',-500:100:500)%设置坐标刻度
	xlabel('x轴/mm');ylabel('y轴/mm');zlabel('z轴/mm');%先plot再label和view

figure('name','左视图')
	plot3(figure1_x,figure1_y,figure1_z,'r.','MarkerSize',4);
	hold on;
	plot3(figure2_x,figure2_y,figure2_z,'b.','MarkerSize',4);%设置3有点小
	hold off;
	xlabel('x轴/mm');ylabel('y轴/mm');zlabel('z轴/mm');
	view(-90,0);


figure('name','正视图')
	plot3(figure1_x,figure1_y,figure1_z,'r.','MarkerSize',4);
	hold on;
	plot3(figure2_x,figure2_y,figure2_z,'b.','MarkerSize',4);%设置3有点小
	hold off;
	view([0 0]);
	set(gca,'XTick',-500:100:500)
	xlabel('x轴/mm');ylabel('y轴/mm');zlabel('z轴/mm');


figure('name','顶视图')
	plot3(figure1_x,figure1_y,figure1_z,'r.','MarkerSize',4);
	hold on;
	plot3(figure2_x,figure2_y,figure2_z,'b.','MarkerSize',4);%设置3有点小
	hold off;
	xlabel('x轴/mm');ylabel('y轴/mm');zlabel('z轴/mm');
	set(gca,'XTick',-500:100:500)
	view(0, 90);

03 最后是我参考的一个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);