机械臂绘图（绘制字母E）

以下是代码：

mdl_puma560;   %加载p560模型
%机器人行走的路径
path=[1 0 1;     
      1 0 0;
      0 0 0;
      0 2 0;
      1 2 0;
      1 2 1;
      0 1 1;
      0 1 0;
      1 1 0;
      1 1 1];
plot3(path(:,1),path(:,2),path(:,3),'color','k','LineWidth',2);
%将其转换为一条连续路径
p=mstraj(path,[0.5 0.5 0.3],[],[2 2 2],0.02,0.2);
%mstraj的第一个参数就是要通过的中间点，第二个参数就是每个轴的最大的速度
%所组成的向量，第三个参数是每个段的间隔所组成的向量，第四个参数是初始的轴坐标，第六个参数是取样周期，
%最后一个参数是加速时间。mstraj函数会返回一个矩阵，行表示时间间隔，列表示坐标值。

%已经定义出路径绘制出的字母高为两个单位，宽为一个单位，因此需要将坐标值都缩小为
%原来的1/10(使得字母高200毫米，宽100毫米)
Tp=transl(0.1*p);
%Tp是一系列齐次变换矩阵，代表了路径上每个点的位置，
%字母的起点被设置在工作空间中的(0.4,0,0)
%下面的指令就是将Tp中的每个位置都前乘第一个参数
Tp=homtrans(transl(0.4,0,0),Tp);
%将路径提前画到工作空间中，以便更直观的观察到机械臂的运动的轨迹
tp=transl(Tp);
plot3(tp(:,1),tp(:,2),tp(:,3),'color','k','LineWidth',2);
%然后确定机器人末端执行器书写字体的姿态。（垂直向下）
p560.tool=trotx(pi);

%然后应用逆运动学来确定关节坐标
q=p560.ikine6s(Tp);
%绘制动画
p560.plot(q);

效果如下：（速度有点慢，请耐心观看，hhh）