#技术风云榜#matlab机器人p560 由关节角求驱动力矩仿真代码

ededewa

! ?' V+ L( r  V4 ?% w

1 ?" J) h+ c: I& z
/ Y( J; c7 g$ S; y

6 E; I- F4 D  v" G9 r* B) L


% Q; X) a8 ~: z, sL{1} =link([-pi/2    0       pi/2    0         0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')% 1是连杆质量
5 [; R6 t2 u: n: l+ k) CL{2} =link([0       149      0       432     0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{3} =link([pi/2    0        pi/2     -20    0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
3 J9 i( _0 A2 U6 [L{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{5} =link([pi/2     0       0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
0 ?0 d: K1 H) W$ d# _9 ^L{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
% d$ ~) d0 r8 G  |- d: Lp560m=robot(L)
2 T3 Q; N9 ]* F* g7 D* J+ Tc=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
T1=fkine(p560m,c)
( t% m: m5 K9 L& m8 ET2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]
t=0:0.056:10
T=ctraj(T1,T2,length(t))
p=ikine(p560m,T)
7 q1 \7 e) l  D  w) \%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
* C1 S, E% h5 z5 Z, m, Qc2=p(length(t),: )%取出到达目标时的各个关节角度
( g/ l& @1 E& K7 Y! h[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
9 B  z) [% o+ }1 S+ m/ U9 c" \# j7 OF=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
%plot(F)
) Q( p3 R: b* u$ c/ ^$ v%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

. s7 n3 E* Z* g2 [' l/ Q( tfor i=1:10:length(t)% 正常轨迹 i=1:length(t)
x(i)=T(1,4,i)
- Z% o1 D& A* v. [8 Vy(i)=T(2,4,i)
6 e/ f8 S6 ~8 ^& P' H% v! Ez(i)=T(3,4,i)
' F! |' j" f9 p* b8 Qend
& Q1 d9 {' N9 T) H8 wplot3(x,y,z)%画出运动轨迹
plot(p560m,p)
%plot(p)% 画出各个关节角的变量图
%drivebot(p560m)
t1=t'
B=[t1 F]%关节驱动力矩输出
a1=p(:,1)
6 X* E6 }2 o7 Q9 ^- Wa2=p(:,2)  
. T, n+ I( v4 O+ L) Q. _2 V5 ia3=p(:,3)
a4=p(:,4)
a5=p(:,5)  
a6=p(:,6)
%输出到 simulink的变量 [t1,a1]


' I) p) ~! W% u, ~: F) g

NingW

matlab机器人p560 由关节角求驱动力矩仿真代码