demo记录

clear

mdl_puma560
mdl_puma560
%字母E的途径点
path = [-2 0 1
-2 0 0
-1 0 0
-1 0 1
-1.5 1 1
-1.5 1 0
-1.5 -1 0
-1.5 -1 1
-1.5 0 1
-1.5 0 0
-2 -1 0
-2 -1 1
-1.5 0 1
-1.5 0 0
-1 -1 0
-1 1 1
-1 1 0
-0.5 1 0
-1 0.2 0
0 0.2 0
0 -1 0
0 -1 1
-1 0.2 1
-1 0.2 0
-1 -1 0
-1 -1 1
-0.5 0.2 1
-0.5 0.2 0
-0.5 -1 0
-0.5 -1 1
0.25 0.5 1
0.25 0.5 0
0.25 0 0
0.25 0 1
0.25 0.5 1
0.25 0.5 0
1.75 0.5 0
1.75 0 0
1.75 0 1
0.25 0 1
0.25 0 0
1.75 0 0
1.75 0 1
1 1 1
1 1 0
1 -0.5 0
1 -0.5 1
0.25 -0.5 1
0.25 -0.5 0
0.25 -1 0
0.25 -1 1
0.5 -0.5 1
0.5 -0.5 0
0.5 -1 0
1.5 -1 0
1.5 -0.5 0
1.5 -0.5 1
1 -0.5 1
1 -0.5 0
1 -0.75 0
1 -0.75 1
1.75 -0.5 1
1.75 -0.5 0
1.75 -1 0
];

%分段路径可绘制如下
figure(1);
plot3(path(:,1), path(:,2), path(:,3), 'color', 'k', 'Linewidth', 2);
hold on
%将其转化为连续路径
p=mstraj(path, [10 10 10], [], [2 2 2], 0.02, 0.01);
%第二个参数规定了xyz三个方向上的最大速度
%第四个参数规定了初始坐标、采样间隔、加速时间

path=0.1*path;

%给出字母的起点
Tp = transl(0.1*p);
Tp = homtrans( transl(0.4, 0, 0), Tp);
%Tp是一系列的齐次变换矩阵，代表了路径上每一个点的位姿
about(Tp)
%通过逆运动学求出一系列关节坐标
p560.tool = trotx(pi); %考虑姿态，保证z轴向下
q = p560.ikine6s(Tp);
%figure(2)
p560.plot(q)  %绘制出动画
about(q)
clf
tranimate(Tp)