Matlab机器人工具箱（2-1）：一些demo（正逆运动学）

01 利用teach函数实现示教功能（工具箱 v9.1）

需要注意的是

• 9.1版本的机器人工具箱，其长度是以m为单位的，需要换算成mm，否则会报错内存不足
• 使用qlim函数对角度范围进行限制，这里的角度都是以rad的为单位的，所以30°要写为30*pi/180

%teach 函数使用
%robot toolbox V9.1
%matlab r2016a
%
clc;
clear;
% theta d a alpha
L(1) = Link([pi/2 0 0 0 0]);
L(2) = Link([0 0.15 0 -pi/2 0]);
L(3) = Link([-pi/2 0 0.431 0 0]);
L(4) = Link([0 0.433 0.020 -pi/2 0]);
L(5) = Link([0 0 0 pi/2 0]);
L(6) = Link([0 0 0 -pi/2 0]);

%定义关节范围
L(1).qlim =[-160*pi/180, 160*pi/180];
L(2).qlim =[-225*pi/180, 45*pi/180];
L(3).qlim =[-45*pi/180, 225*pi/180];
L(4).qlim =[-110*pi/180,170*pi/180];
L(5).qlim =[-100*pi/180,100*pi/180];
L(6).qlim =[-222*pi/180,222*pi/180];
p560=SerialLink(L,'name','puma560');

%初始关节角度
theta = [pi/2 0 -pi/2 0 0 0];
p560.plot(theta);
p560.teach

运行效果：

02 利用subplot函数在一张图中同时展现机械臂多个姿态

L(1)=Link([0,0,1,0]); 
L(2)=Link([0,0,.8,0]);
L(3)=Link([0,0,.6,0]);
tree_link=SerialLink(L,'name','tree_link');
T = tree_link.fkine([0 0 0]);

subplot(2,2,1)
title('T')
tree_link.plot([0 0 0])

subplot(2,2,2)
title('T1')
tree_link.plot([pi/2 0 0])

subplot(2,2,3) 
title('T2')
tree_link.plot([pi/2 pi/2 0])

subplot(2,2,4)
title('T3')
tree_link.plot([pi/2 pi/2 pi/2])

效果图：

03 逆运动学

T = transl(0.5, 0.5, 0.7) * rpy2tr(0, 3*pi/4, 0)
T = p560.fkine(qn)
%上面两句都是求出正运动学的位姿，也可以直接自己给出一个期望的位姿T

qi = p560.ikine(T)
qi = p560.ikine6s(T)
%这两句都是求解逆运动学，但是第二句适用于6自由度机械臂

%由于存在多解情况，我们可以人为的限定期望的位型解
qi = p560.ikine6s(T, 'ru')
%'l','r'   左手/右手
%'u','d'   肘部在上/肘部在下
%'f','n'    手腕翻转/手腕不翻转

p560.plot(qi)

详解：

• ikine6s：6自由度的机械臂的逆解
• ikine3：3自由度的机械臂的逆解（没有腕关节）
• ikine：小于6自由度的机械臂的逆解
  
  

04 利用pause函数实现逆运动学慢过程

%逆运动学
mdl_puma560%调出puma560 DH模型作为实验对象
T = p560.fkine([0 0 0 0 0 0]);%生成一个位姿
qi=p560.ikine6s(T);%逆运动学计算，封闭解
i=5;
while(i)%对比结果
    pause(1)
    p560.plot([0 0 0 0 0 0])
    pause(1)
    p560.plot(qi)
    i=i-1;
end