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

ededewa



9 O: b; J0 O8 U5 K4 N! t
3 A. N5 b- c' c  b


L{1} =link([-pi/2    0       pi/2    0         0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')% 1是连杆质量
; f" z# r( Z- ~7 O8 j( o! kL{2} =link([0       149      0       432     0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')
$ J5 W) m6 N. q: C3 e  v6 qL{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')
& h! ^2 R' Y& PL{5} =link([pi/2     0       0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
0 A2 y( s3 x7 @7 M! |, fL{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
5 _  N; v% K- t9 ^7 i1 Yp560m=robot(L)
c=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
T1=fkine(p560m,c)
+ v: v6 n; \0 i4 E2 ?' U# PT2=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))
9 i& O( H) C* v0 B4 f8 ^% [p=ikine(p560m,T)
; G5 j2 E6 L+ u$ e' m! P% J%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
c2=p(length(t),: )%取出到达目标时的各个关节角度
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
% o) o* w1 y# i' IF=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
& z( L& v) a: u%plot(F)
6 U- g( L2 [7 j1 k" D0 ]%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:10:length(t)% 正常轨迹 i=1:length(t)
( E, a1 }7 E8 a0 Mx(i)=T(1,4,i)
1 {# h: g4 e9 D% My(i)=T(2,4,i)
" i3 Q  j" q$ E% K- hz(i)=T(3,4,i)
end
. E; {1 z: R% U# j  M1 qplot3(x,y,z)%画出运动轨迹
plot(p560m,p)
! O0 `/ I/ w3 ?7 K%plot(p)% 画出各个关节角的变量图
8 i8 n* R( \! P+ k+ r; a9 m5 y%drivebot(p560m)
t1=t'
4 V! G& x& U- _' ^B=[t1 F]%关节驱动力矩输出
+ n( r; M' R+ R* \5 E2 ga1=p(:,1)
- ?! t8 ~. I0 v! ~, ia2=p(:,2)  
a3=p(:,3)
a4=p(:,4)
a5=p(:,5)  
a6=p(:,6)
%输出到 simulink的变量 [t1,a1]
% t3 x! ~2 l- A( P3 I/ J
5 c2 }& \. W  U' `% D# Q$ V$ w

NingW

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