我们的目标是让机器人绘制字母“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
作出路径如图
根据轨迹,我们对机器人基座,末端工具位姿进行变换,底座放在 ,SE3.oa请参考双向量表示法,这里我们将机器人末端向下。
>> Tp = SE3(0.15, 0, 0) * SE3(p) * SE3.oa([0 1 0], [0 0 -1]);
逆运动学:
>> q = p560.ikine6s(Tp);