机器人学（MATLAB机器人工具箱10.x）——3.6-画字母

我们的目标是让机器人绘制字母“B”。机器人工具箱自带了一种 hershey 字体

>> load hershey
>> B = hershey{'B'};

我们可以查看B里面是啥

>> B.stroke
ans =
  列 1 至 9
    0.1600    0.1600       NaN    0.1600    0.5200    0.6400    0.6800    0.7200    0.7200
    0.8400         0       NaN    0.8400    0.8400    0.8000    0.7600    0.6800    0.6000
  列 10 至 18
    0.6800    0.6400    0.5200       NaN    0.1600    0.5200    0.6400    0.6800    0.7200
    0.5200    0.4800    0.4400       NaN    0.4400    0.4400    0.4000    0.3600    0.2800
  列 19 至 23
    0.7200    0.6800    0.6400    0.5200    0.1600
    0.1600    0.0800    0.0400         0         0

行分别对应x轴和y轴坐标，列为NAN代表之前线段的结束，也就是提笔或者放笔。

我们先把字母大小缩放到原来的 0.25，然后把 z 轴加进去，然后把 NAN 替换掉。

>> path = [ 0.25*B.stroke; zeros(1,numcols(B.stroke))];
>> k = find(isnan(path(1,:)));
>> path(:,k) = path(:,k-1);
>> path(3,k) = 0.2;

我们将其转换为一条连续路径：

>> p = mstraj(path(:, 2:end)', [0.5 0.5 0.5], [], path(:,1)', 0.02, 0.2);

其中第二个参数是分别在x、y和z方向上的最大速度，第四个参数中分别是初始坐标、采样间隔和加速时间。插值路径的步数为

>> about(p)
p [double] : 487x3 (11.7 kB)

在 20ms 的采样间隔下，共花费 9.74s 的时间：

>> numrows(p) * 0.02
ans =
    9.7400

作出路径如图

根据轨迹，我们对机器人基座，末端工具位姿进行变换，底座放在 $(0.15,0,0)$，SE3.oa请参考双向量表示法，这里我们将机器人末端向下。

>> Tp = SE3(0.15, 0, 0) * SE3(p) * SE3.oa([0 1 0], [0 0 -1]);

逆运动学：

>> q = p560.ikine6s(Tp);