Matlab机器人工具箱7 搬运动画展示

参考视频：【MATLAB机器人工具箱10.4 机械臂仿真教学（未完结）】 https://www.bilibili.com/video/BV1q44y1x7WC/?p=10\&share_source=copy_web\&vd_source=2c56c6a2645587b49d62e5b12b253dca

前面我们学习了正逆解的简单运用、机械臂模型的加载，接下来我们结合起来做点小实验吧~

展示目标：从原点开始，将小球搬运到指定位置并回到原点

1 加载模型

首先加载模型，这在第六节讲了两种方法。

clear;
clc;

L(1) = Link('revolute','d',0.216,'a',0,'alpha',pi/2);
L(2) = Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);
L(3) = Link('revolute','d',0,'a',sqrt(0.145^2+0.42746^2),'alpha',0, 'offset', -atan(427.46 / 145));
L(4) = Link('revolute','d',0,'a',0,'alpha',pi/2,'offset', atan(427.46/145));
L(5) = Link('revolute', 'd', 0.258, 'a', 0, 'alpha', 0);

Five_dof=SerialLink(L,'name','5-dof');
Five_dof.base=transl(0,0,0.28);

q0=[0 0 0 0 0];
v=[35 20];
w=[-1 1 -1 1 0 2];

Five_dof.plot3d(q0,'tilesize',0.1,'workspace',w,'path','D:\工程\Fdof1','nowrist','view',v)

light('Position',[1 1 1],'color','w');

2 绘制小球

用plot_shpere绘制小球，其中position在三维空间需要输入向量，我们用[0.5, 0.5, 0.5]

plot_sphere(position, radius, color)

3 运行机械臂到小球的位置

指定小球的位置Position，然后设定参数生成小球。

接着要让机械臂移动过来，因此先计算终点的位姿，并让z轴向下（rpy2tr(180, 0, 0)）。

得到终点的位和姿后，用transl和rpy2tr将其转换为变换矩阵T1.

接着用我们熟悉的ikunc函数逆解T1，得到关节空间的解q1。

最后用我们第五章讲的jtraj函数，在关节空间内插值，插60个点，得到关节轨迹q。

%% pick
Position = [0.5 0.5 0.5];
r = 0.04;
plot_sphere(Position, r, 'r');
T1 = transl(Position) * rpy2tr(180, 0, 0);
q1 = Five_dof.ikunc(T1);
q = jtraj(q0, q1, 60);
Five_dof.plot3d(q, 'view', v, 'fps', 60, 'nowrist');

plot3d不支持生成gif，因此你只能自己试一下。

Five_dof.plot(q, 'movie', 'pick_trail.gif');

4 搬运小球

对于机械臂而言，和3一样，也是给定目标点计算变换矩阵，求逆解，然后在关节空间插值计算即可。

问题在于小球的跟随移动如何实现？

这里我们用：

首先对每一个插值时刻的关节变量进行正解，得到机械臂末端的位姿，然后用transl将位姿转化为位置，也就是小球应该在的位置。此时再调用plot_sphere绘制小球，就能实现类似跟随的效果了。

%% place
position2 = [0.5, -0.5, 1];
T2 = transl(position2) * rpy2tr(90, 90, 0);
q2 = Five_dof.ikunc(T2);
t = 30;
q = jtraj(q1, q2, t);

for i = 1:30
    qi = q(i, :);
    Ti = Five_dof.fkine(qi);
    Pi = transl(Ti);
    plot_sphere(Pi, r, 'r');
    Five_dof.plot3d(qi, 'view', v, 'fps', 60, 'nowrist');
    
    if i < 30  % 只在前29次清除，最后一次保留
        cla;
    end
end

首先确定目标点Position2，然后计算目标的位姿并转化为变换矩阵，求逆解，在关节空间内插值，得到q2。

过程中我们插入30个点，通过for对每一个点执行如下计算：

首先获取当前各个关节的变量qi=q(i, :)，"："意味着全选所有关节。

然后用fkine正运动学求解末端位姿Ti，并用transl函数转换为坐标Pi。

最后在这里绘制小球，并执行机械臂的移动。

由于每次计算都是独立的，都会生成一个机械臂和一个小球，因此会产生严重残影影响运算，我们用cla清除前一次计算。（你可以删去cla试试）

到第29次时，保留最后一次的位置。

5 复位

最后让机械臂回到原点。同上。

%% back
plot_sphere(Pi, r, 'r');
q = jtraj(q2, q0, 60);
Five_dof.plot3d(q,'view', v, 'nowrist', 'fps', 60);

6 绘制轨迹

%% traj
q = [jtraj(q0, q1, 60); ...
     jtraj(q1, q2, 60); ...
     jtraj(q2, q0, 60)];

Five_dof.plot3d(q,'view', v, 'nowrist', 'fps', 60,...
                'trail', {'r', 'LineWidth', 1});

完整代码：

clear;
clc;

L(1) = Link('revolute','d',0.216,'a',0,'alpha',pi/2);
L(2) = Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);
L(3) = Link('revolute','d',0,'a',sqrt(0.145^2+0.42746^2),'alpha',0, 'offset', -atan(427.46 / 145));
L(4) = Link('revolute','d',0,'a',0,'alpha',pi/2,'offset', atan(427.46/145));
L(5) = Link('revolute', 'd', 0.258, 'a', 0, 'alpha', 0);

Five_dof=SerialLink(L,'name','5-dof');
Five_dof.base=transl(0,0,0.28);

q0=[0 0 0 0 0];
v=[35 20];
w=[-1 1 -1 1 0 2];

Five_dof.plot3d(q0,'tilesize',0.1,'workspace',w,'path','D:\工程\Fdof1','nowrist','view',v)

light('Position',[1 1 1],'color','w');

%% pick
Position = [0.5 0.5 0.5];
r = 0.04;
plot_sphere(Position, r, 'r');
T1 = transl(Position) * rpy2tr(180, 0, 0);
q1 = Five_dof.ikunc(T1);
q = jtraj(q0, q1, 60);
Five_dof.plot3d(q, 'view', v, 'fps', 60, 'nowrist');

%% place
position2 = [0.5, -0.5, 1];
T2 = transl(position2) * rpy2tr(90, 90, 0);
q2 = Five_dof.ikunc(T2);
t = 30;
q = jtraj(q1, q2, t);

for i = 1:30
    qi = q(i, :);
    Ti = Five_dof.fkine(qi);
    Pi = transl(Ti);
    plot_sphere(Pi, r, 'r');
    Five_dof.plot3d(qi, 'view', v, 'fps', 60, 'nowrist');
    
    if i < 30  % 只在前29次清除，最后一次保留
        cla;
    end
end

%% back
plot_sphere(Pi, r, 'r');
q = jtraj(q2, q0, 60);
Five_dof.plot3d(q,'view', v, 'nowrist', 'fps', 60);

%% traj
q = [jtraj(q0, q1, 60); ...
     jtraj(q1, q2, 60); ...
     jtraj(q2, q0, 60)];

Five_dof.plot3d(q,'view', v, 'nowrist', 'fps', 60,...
                'trail', {'r', 'LineWidth', 1});