EDA365电子论坛网

标题: #技术风云榜#matlab机器人p560 由关节角求驱动力矩仿真代码 [打印本页]

---

作者: ededewa    时间: 2020-11-20 15:36
标题: #技术风云榜#matlab机器人p560 由关节角求驱动力矩仿真代码

; i: G6 [; m. m- L4 S



* F; ]; q8 c* }7 }1 i

% }# X9 b' D" X

1 n# i4 u2 h1 X7 \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')
L{3} =link([pi/2    0        pi/2     -20    0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
, g/ E: r+ l. q  ?4 v. @5 a. t4 EL{4} =link([-pi/2    433     0       0       0   1 0 0 0 0 0 0 0 0 0 0 0],'sta')
! k6 h/ Z/ D: l, y, r5 f2 \6 bL{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)
- F+ P2 Y' R6 ~) `; c5 t; Uc=[0 0 pi/2 0 0 0]%%%角度改为  [p i/2 0 pi/2 0 0 0]
8 P) U$ V3 _+ ?+ r& A/ |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
$ a& x& n7 ~1 M& E# B$ xT=ctraj(T1,T2,length(t))
p=ikine(p560m,T)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%添加的力矩控制
# e- ]( I; I- u- Pc2=p(length(t),: )%取出到达目标时的各个关节角度
[q qd qdd] =jtraj(c,c2,t)%得到关节空间规划轨迹
F=rne(p560m,q,qd,qdd)%得到控制各个关节的输出力矩
& Y4 `/ V( M# H7 M3 J( y  R%plot(F)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:10:length(t)% 正常轨迹 i=1:length(t)
x(i)=T(1,4,i)
y(i)=T(2,4,i)
9 n) d* E. E' z* o/ f0 nz(i)=T(3,4,i)
5 {" W. X  \' O+ cend
! {: u. x& K  c1 E2 _plot3(x,y,z)%画出运动轨迹
+ C% C$ k5 ]6 V; `0 Yplot(p560m,p)
%plot(p)% 画出各个关节角的变量图
! r7 `8 C4 L! Z* l$ K( q% T%drivebot(p560m)
1 j' R) e$ I- w7 G# B- r" P2 Zt1=t'
B=[t1 F]%关节驱动力矩输出
a1=p(:,1)
a2=p(:,2)  
7 {# O1 n# |# g) S9 c% _5 b8 |" C( aa3=p(:,3)
a4=p(:,4)
a5=p(:,5)  
a6=p(:,6)
%输出到 simulink的变量 [t1,a1]


1 s  }  J7 _2 x

---

作者: NingW    时间: 2020-11-20 16:33
matlab机器人p560 由关节角求驱动力矩仿真代码

---

欢迎光临 EDA365电子论坛网 (https://bbs.eda365.com/)

Powered by Discuz! X3.2