Matlab机器人仿真（十一）：机器人jtraj轨迹仿真（带轨迹线）_用jtraj和fkine函数对机器人控制系统进行仿真

matlab中利用jtraj函数自动生成轨迹，如图所示：
初始状态，如图所示：

完成动作的过程图
角度一：

角度二：

matla代码：把桌面一的球搬运到桌面二上，使用Jtraj函数自动规划轨迹。

close all;clear all;
clc;
mdl_puma560;
deg = pi/180;
figure
theta = zeros(1,6);
% p560.base([0,0,-0.3]);
p560.plot(theta);
hold on;
%给工作台建模
draw_box([0.3;-0.265;-0.5],[0.7;0.265;-0.55],'y','mesh','k','alpha',0.3);%画放货平台第一个为桌角点，第二个为其对角点坐标
draw_box([0.3;-0.265;-0.5],[0.35;-0.215;-1],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([0.7;-0.265;-0.5],[0.65;-0.215;-1],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([0.3;0.265;-0.5],[0.35;0.215;-1],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([0.7;0.265;-0.5],[0.65;0.215;-1],'y','mesh','k','alpha',0.3);%画放货平台

draw_box([-0.365;0.3;-0.5],[0.1653;0.7;-0.55],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([-0.365;0.3;-0.5],[-0.315;0.35;-1],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([-0.365;0.7;-0.5],[-0.315;0.65;-1],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([0.1653;0.3;-0.5],[0.1153;0.35;-1],'y','mesh','k','alpha',0.3);%画放货平台
draw_box([0.1653;0.7;-0.5],[0.1153;0.65;-1],'y','mesh','k','alpha',0.3);%画放货平台


[x,y,z] = ellipsoid(0.5,0,-0.4,0.05,0.05,0.05);P_1 = surf(x,y,z);%球1
[x,y,z] = ellipsoid(0.5,0.15,-0.4,0.05,0.05,0.05);P_3 = surf(x,y,z);%球2

% t=[0:0.01:2];%4秒完成轨迹，步长0.1

%获取各个位置的位姿方程
T_00 = p560.fkine(theta);
T_0 = transl(0.5,0,0);
T_st = transl(0.5,0,-0.4);
T_1 = transl(-0.1,0.5,-0.4);
T_f = transl(-0.1,0.5,0.1);

% 运动学正解
q0 = p560.ikine(T_00);
q1 = p560.ikine(T_0);
q2 = p560.ikine(T_st);
q3 = p560.ikine(T_1);
q4 = p560.ikine(T_f);

t=0:.04:2;
sqtraj1=jtraj(q0,q1,t); 
sqtraj2=jtraj(q1,q2,t); 
sqtraj3=jtraj(q2,q3,t); 
sqtraj4=jtraj(q3,q4,t);
sqtraj5=jtraj(q4,q0,t);

hold on
atj=zeros(4,4);
axis square;grid on;
view(-35,40)%视角的转动
% xlim([-40,40])
% ylim([-40,40])
% zlim([0,60])

for i=1:1:51
    atj=p560.fkine(sqtraj1(i,:));
    jta=atj.t(:,1);
    JTA(i,:)=jta';
    jta=JTA;
    plot2(jta(i,:),'r.')
    p560.plot(sqtraj1(i,:))
end
for i=1:1:51
    atj=p560.fkine(sqtraj2(i,:));
    jta=atj.t(:,1);
    JTA(i,:)=jta';
    jta=JTA;
    plot2(jta(i,:),'r.')
    p560.plot(sqtraj2(i,:))
end
pause(0.5);
delete(P_1);
for i=1:1:51
    atj=p560.fkine(sqtraj3(i,:));
    jta=atj.t(:,1);
    JTA(i,:)=jta';
    jta=JTA;
    plot2(jta(i,:),'r.')
    p560.plot(sqtraj3(i,:))
end
[x,y,z] = ellipsoid(-0.1,0.5,-0.4,0.05,0.05,0.05);P_2 = surf(x,y,z);
for i=1:1:51
    atj=p560.fkine(sqtraj4(i,:));
    jta=atj.t(:,1);
    JTA(i,:)=jta';
    jta=JTA;
    plot2(jta(i,:),'r.')
    p560.plot(sqtraj4(i,:))
end
for i=1:1:51
    atj=p560.fkine(sqtraj5(i,:));
    jta=atj.t(:,1);
    JTA(i,:)=jta';
    jta=JTA;
    plot2(jta(i,:),'r.')
    p560.plot(sqtraj5(i,:))
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100