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

ededewa

( Z% t- x4 q5 n) k) ]- e/ f) B

7 O/ n* @7 t7 @- }/ k



0 X  Y9 P$ C7 r! WL{1} =link([-pi/2    0       pi/2    0         0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')% 1是连杆质量
L{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')
L{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
$ q5 n8 K8 S! D; _9 fL{5} =link([pi/2     0       0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
L{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
2 p* D$ t# E4 T! Fp560m=robot(L)
c=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
T1=fkine(p560m,c)
T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]
t=0:0.056:10
" j- Z5 h% n9 \+ K/ p% |/ L- _T=ctraj(T1,T2,length(t))
p=ikine(p560m,T)
7 ~1 R$ \  Z8 j3 ]* @%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
. o! x5 w) W$ s/ l3 B) `c2=p(length(t),: )%取出到达目标时的各个关节角度
4 Q# I# m# V6 V/ p' H0 F[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
0 j1 `" D7 U* H5 _9 F' W2 @F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
0 L. B1 P  T  J( d: a1 Y%plot(F)
5 N' ~3 E% g8 y$ j  r' w  P/ i%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:10:length(t)% 正常轨迹 i=1:length(t)
" \8 c( x$ z0 E5 N% G5 Bx(i)=T(1,4,i)
  T0 J0 u. h5 J3 j. O& jy(i)=T(2,4,i)
z(i)=T(3,4,i)
end
2 V5 f% X+ `0 z6 c; r! c- ]plot3(x,y,z)%画出运动轨迹
- l/ x1 O3 _$ w0 B  D& p  b9 ^plot(p560m,p)
2 l2 `" v6 i% a$ y4 z% |# E( B5 t  w%plot(p)% 画出各个关节角的变量图
%drivebot(p560m)
7 ?5 ^9 E) ~& u, k5 [2 T( Et1=t'
B=[t1 F]%关节驱动力矩输出
; L1 B. T+ g9 Q+ p- K; C# w0 ~a1=p(:,1)
. j- i6 F1 O( i& T' |) V6 B$ e3 Ga2=p(:,2)  
) h: z5 a8 o  k3 O$ ia3=p(:,3)
4 J) e- z7 ?" E+ ja4=p(:,4)
a5=p(:,5)  
" \/ g1 L0 J# G' s  ^4 d6 L& R1 da6=p(:,6)
) Q) y. J! V  ?2 Z0 _%输出到 simulink的变量 [t1,a1]
; W- A2 z: j% W1 c, p- J8 y
5 g, k% X2 x' f- S, b$ v

NingW

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