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

ededewa



0 N3 t& B7 w& }( _' O
& h8 e  ]6 F, ^* ?/ z3 I6 H& }
; k( k  Q5 z* N* Q

  s7 c; C1 R9 g( G- g5 YL{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')
* \, y8 }* v- X+ X) UL{3} =link([pi/2    0        pi/2     -20    0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
* v0 H( }" I  u& t3 v& pL{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')
' K4 g$ [) J6 KL{6} =link([0        56      0        0        0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
6 ~; ^. D( f3 U+ I; Y) Bp560m=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
T=ctraj(T1,T2,length(t))
p=ikine(p560m,T)
* O8 K  s& D! W& C' H%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
( k. J3 A' e6 Q  M* Cc2=p(length(t),: )%取出到达目标时的各个关节角度
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
& s  u" r/ l" S6 p%plot(F)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:10:length(t)% 正常轨迹 i=1:length(t)
7 G7 I; F. r7 M  o6 ]x(i)=T(1,4,i)
y(i)=T(2,4,i)
- Q1 a# `% {  g% n) p2 E, v; g0 Rz(i)=T(3,4,i)
$ e. Z4 M5 B& [* R5 o3 R4 z- ^8 F) w  Yend
plot3(x,y,z)%画出运动轨迹
3 C( `( U* Z( t- q6 ?. Yplot(p560m,p)
  Y7 B3 ^4 j7 r4 e5 Z4 n# Z%plot(p)% 画出各个关节角的变量图
9 o+ y+ g: U: h1 S+ r%drivebot(p560m)
, O0 @8 J3 Y  J) `( W' F' ~t1=t'
B=[t1 F]%关节驱动力矩输出
9 g) g$ N# v! l4 S0 M. Ea1=p(:,1)
a2=p(:,2)  
1 d% N; V7 g7 S: p8 \/ G7 X- ea3=p(:,3)
a4=p(:,4)
a5=p(:,5)  
a6=p(:,6)
  r2 P* f) p6 U/ Z$ W  t2 i%输出到 simulink的变量 [t1,a1]
( X* {; T+ r9 E4 P
1 [$ g/ z& L6 y1 w( _

NingW

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