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

ededewa

! V$ e( E/ g9 o, @; j( d





% T: V7 G/ |( u5 c/ y

L{1} =link([-pi/2    0       pi/2    0         0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')% 1是连杆质量
  l' Y" _( B: s1 U* X/ k  {" O- BL{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')
L{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')
p560m=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]
' r! B& q" Z( Y  St=0:0.056:10
T=ctraj(T1,T2,length(t))
  |) I6 J: ]0 p' V% xp=ikine(p560m,T)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
c2=p(length(t),: )%取出到达目标时的各个关节角度
! ]7 S4 @) r; c" Z+ S[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
: h: `: ~' F& C7 V%plot(F)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 q3 E5 x, m" m; Z2 d0 u
# }' C" H+ m# z5 D- \: U2 b( Afor i=1:10:length(t)% 正常轨迹 i=1:length(t)
x(i)=T(1,4,i)
y(i)=T(2,4,i)
z(i)=T(3,4,i)
end
plot3(x,y,z)%画出运动轨迹
plot(p560m,p)
%plot(p)% 画出各个关节角的变量图
%drivebot(p560m)
t1=t'
B=[t1 F]%关节驱动力矩输出
a1=p(:,1)
* s2 `- t; G  m, {* [a2=p(:,2)  
a3=p(:,3)
/ C3 Y  D, J; Z9 ga4=p(:,4)
a5=p(:,5)  
a6=p(:,6)
" x; P4 n( }. z& G5 {$ i) t%输出到 simulink的变量 [t1,a1]

NingW

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