有一个基于速度的用python编写的碰撞检测程序，可以执行，但是部分代码看不懂
def driverProg():
# end_freedrive_mode()
set_digital_out(5,False)
safetySpeedFlag = 0
thread safetySpeedDaemon():
textmsg(\"safetySpeedDaemon RUNNING\")
while True:
enter_critical
tgtSpeed = get_target_joint_speeds() #获得目标关节的速度？
if(norm(tgtSpeed[0]) > 2.0 or norm(tgtSpeed[1]) > 2.0 or norm(tgtSpeed[2]) > 2.0 or norm(tgtSpeed[3]) > 2.5 or norm(tgtSpeed[4]) > 2.5 or norm(tgtSpeed[5]) > 3):
textmsg(\"STOP cause the huge move speed: \", tgtSpeed )
safetySpeedFlag = 0
# stopj(5)
else:
safetySpeedFlag = 1
end
exit_critical
sync()
end
end
st = run safetySpeedDaemon()
/*cdk*/
textmsg(\"sssss\",\"ttttttttttttttttttttttttttttttttttttttttttt\")
MULT_jointstate = 1000000
FREEDRIVE = 0
STOP_FREEDRIVE=2
SPEEDJ = 1
control_state = SPEEDJ
cmdq = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
speed = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
speed_rec = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
v_rec = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
nq = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
aa = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
timer_rec1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
timer_rec2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
aaaa = 0
timer = [0,0,0,0,0,0]
mass=[0,0,0,0,0,0]
speeddir=[0,0,0,0,0,0]
maxspeed = [0,0,0,0,0,0]
position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
stoptime = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#add
def set_servo_setpoint(pos):
enter_critical
control_state = SPEEDJ
speed = [pos[0],pos[1],pos[2],pos[3],pos[4],pos[5]]
exit_critical
end
#add
thread servoThread():
while True:
if control_state == SPEEDJ:
/*cdk*/
if (1 == safetySpeedFlag ):
speedj(nq,aaaa,t=0.004)
end
# textmsg(\"cmdv=\",nq)
elif control_state == STOP_FREEDRIVE:
end_freedrive_mode()
sync()
control_state = SPEEDJ
else:
# end_freedrive_mode()
freedrive_mode()
sync()
# sleep(0.008)
end
end
end
thread dataThread():
while True:
vvv=get_actual_joint_speeds()
cq=get_actual_joint_positions()
# textmsg(\"before_q=\",vvv)
enter_critical
if control_state == SPEEDJ:
i=3
while i < 5:
if (norm(speed[i] - speed_rec[i]) > 0.12) and (norm(speed_rec[i]) > 0.05):
mass[i] = 1000
end
if (speed[i]*speed_rec[i] <= 0) and (speed[i] != speed_rec[i]): #the direct of speed is changed!
if (timer[i] < 30) and (timer[i] > 10) and maxspeed[i]/timer[i] > 0.01:
if (timer_rec2[i] < 30) and (timer_rec2[i] > 10):
mass[i] = mass[i]*3
end
# if (timer_rec1[i] < 30):
# mass[i]=100
# end
# else:
mass[i] = 1000
end
# if (timer[i] < 30) and (timer_rec1[i] < 30) and (timer_rec2[i] < 30):
# mass[i] = mass[i]+3
# end
timer_rec1[i]=timer_rec2[i]
timer_rec2[i]=timer[i]
timer[i] = 0
maxspeed=[0,0,0,0,0,0]
else:
timer[i] = timer[i]+1
if maxspeed[i] < norm(speed[i]):
maxspeed[i]=norm(speed[i])
end
end
i=i+1
end
i=0
while i < 6:
speed_rec[i]=speed[i]
# if speed[i]*speeddir[i] > 0:
speed[i]=speed[i]/pow(1.5,norm(mass))
# elif norm(mass) > 1.01:
# speed[i]=0
# end
nq[i]=queue1[i]/5+queue2[i]/5+queue3[i]/5+queue4[i]/5+speed[i]/5
mass[i] = mass[i]/1.04
cmdq[i]=cq[i]+nq[i]/125
aa[i]=nq[i]-queue4[i]
i=i+1
end
if norm(mass) > 20:
# control_state = FREEDRIVE
collisiontime=0
position=get_actual_joint_positions()
queue1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
queue4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
end
# textmsg(\"vvvv=\",speed)
# textmsg(\"cmdq=\",cmdq)
# textmsg(\"qqqq=\",cq)
aaaa = norm(aa)*125
queue1 = queue2
queue2 = queue3
queue3 = queue4
queue4 = nq
elif control_state==FREEDRIVE:
ppp=get_actual_joint_positions()
delta_ppp=[position[0]-ppp[0],position[1]-ppp[1],position[2]-ppp[2],position[3]-ppp[3],position[4]-ppp[4],position[5]-ppp[5]]
# textmsg(\"pppp=\",delta_ppp)
if norm(delta_ppp) > 0.001:
control_state = STOP_FREEDRIVE
end
end
exit_critical
sync()
end
end
socket_open(\"172.17.1.90\", 13801)
thread_servo = run servoThread()
thread_data = run dataThread()
keepalive = 1
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1+1,timeout=0.2)
# textmsg(\"keepalive=\",params_mult[7])
# textmsg(\"userdata=\",params_mult[8] / MULT_jointstate)
if params_mult[0] > 0:
pos = [params_mult[1] / MULT_jointstate,
params_mult[2] / MULT_jointstate,
params_mult[3] / MULT_jointstate,
params_mult[4] / MULT_jointstate,
params_mult[5] / MULT_jointstate,
params_mult[6] / MULT_jointstate]
keepalive = params_mult[7]
if keepalive>0:
userdata=params_mult[8] / MULT_jointstate
# textmsg(\"userdata\",userdata)
if userdata<0.5:
set_servo_setpoint(pos)
else:
textmsg(\"Received Mismatched Data, Robot Stay\")
end
else:
# textmsg(\"keepalive=0\")
keepalive=0
end
else:
keepalive=0
end
end
# textmsg(\"quit programe...\")
# sleep(.1)
socket_close()
kill thread_servo
求大神指点
