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

ededewa

" ^. r4 j. g5 }+ S6 J


5 `% R2 }' f; `0 d' y2 ^+ T* {
! {; \8 K( s' _2 L, I

% n; @+ o$ [8 i; @0 I; Z4 B+ z
" v; W2 Q8 C; `
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{2} =link([0       149      0       432     0  1 0 0 0 0 0 0 0 0 0 0 0],'sta')
, C% a' b# y1 n* P, DL{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')
5 m" k& Q' ?: {- b3 E4 ~7 `L{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
9 |, g' F9 k/ F" Ep560m=robot(L)
c=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
T1=fkine(p560m,c)
4 }: ^# W. S+ D9 @* F% h7 L5 @T2=T1-[0 0 0 500;0 0 0 -200;0 0 0 0;0 0 0 0]
. Y! p7 P0 O( S- ]# Q6 s' I3 ct=0:0.056:10
% [( o' v4 p% k1 f; l$ j2 CT=ctraj(T1,T2,length(t))
p=ikine(p560m,T)
' r  m! ^& N; G3 r: M%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
c2=p(length(t),: )%取出到达目标时的各个关节角度
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
% ]. |. o: p! Z6 C# @& V) RF=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
%plot(F)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:10:length(t)% 正常轨迹 i=1:length(t)
x(i)=T(1,4,i)
y(i)=T(2,4,i)
/ X$ }9 x  O( p3 T8 |0 b. }" `z(i)=T(3,4,i)
end
plot3(x,y,z)%画出运动轨迹
plot(p560m,p)
%plot(p)% 画出各个关节角的变量图
) l; b2 L1 g% F# t9 D7 C7 O4 R  i! n5 c%drivebot(p560m)
/ D+ d/ c4 I( [( Mt1=t'
1 y3 o0 N+ E3 o- c; e# KB=[t1 F]%关节驱动力矩输出
5 x+ @% \7 A1 ta1=p(:,1)
" W2 ]' E  C: C, b% ?8 Na2=p(:,2)  
a3=p(:,3)
" x* N+ @- R8 V( l4 a. `) Ta4=p(:,4)
( q% C( k+ r3 u9 T1 Ya5=p(:,5)  
a6=p(:,6)
" Z! G* H+ }$ q/ m& w%输出到 simulink的变量 [t1,a1]
" S+ ?' @  r! q$ e8 `7 {
: `8 G# T: K! ^) o2 h+ x4 U3 Y7 O

NingW

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