参考视频：【MATLAB机器人工具箱10.4 机械臂仿真教学（未完结）】 https://www.bilibili.com/video/BV1q44y1x7WC/?p=10\&share_source=copy_web\&vd_source=2c56c6a2645587b49d62e5b12b253dca
前面我们学习了正逆解的简单运用、机械臂模型的加载，接下来我们结合起来做点小实验吧~
展示目标：从原点开始，将小球搬运到指定位置并回到原点
1 加载模型
首先加载模型，这在第六节讲了两种方法。
Matlab
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]
Matlab
plot_sphere(position, radius, color)3 运行机械臂到小球的位置
指定小球的位置Position，然后设定参数生成小球。
接着要让机械臂移动过来，因此先计算终点的位姿，并让z轴向下（rpy2tr(180, 0, 0)）。
得到终点的位和姿后，用transl和rpy2tr将其转换为变换矩阵T1.
接着用我们熟悉的ikunc函数逆解T1，得到关节空间的解q1。
最后用我们第五章讲的jtraj函数，在关节空间内插值，插60个点，得到关节轨迹q。
Matlab
%% 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，因此你只能自己试一下。
Matlab
Five_dof.plot(q, 'movie', 'pick_trail.gif');4 搬运小球
对于机械臂而言，和3一样，也是给定目标点计算变换矩阵，求逆解，然后在关节空间插值计算即可。
问题在于小球的跟随移动如何实现？
这里我们用：
首先对每一个插值时刻的关节变量进行正解，得到机械臂末端的位姿，然后用transl将位姿转化为位置，也就是小球应该在的位置。此时再调用plot_sphere绘制小球，就能实现类似跟随的效果了。
Matlab
%% 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 复位
最后让机械臂回到原点。同上。
Matlab
%% back
plot_sphere(Pi, r, 'r');
q = jtraj(q2, q0, 60);
Five_dof.plot3d(q,'view', v, 'nowrist', 'fps', 60);6 绘制轨迹
Matlab
%% 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});
完整代码：
Matlab
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});