Guest User

Untitled

a guest
Oct 21st, 2019
90
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. import rospy
  3. from std_msgs.msg import Float64
  4. from control_msgs.msg import JointControllerState
  5. import math # for sin
  6.  
  7. finger5_0_process_value = 0
  8. finger5_1_process_value = 0
  9. finger4_0_process_value = 0
  10. finger4_1_process_value = 0
  11. finger3_0_process_value = 0
  12. finger3_1_process_value = 0
  13. finger2_0_process_value = 0
  14. finger2_1_process_value = 0
  15. finger1_rotate_axis_process_value = 0
  16. finger1_0_base_process_value = 0
  17. finger1_1_process_value = 0
  18.  
  19. def finger5_0_callback(subdata):
  20. #rospy.loginfo("process value1 is %f", subdata.process_value)
  21. global finger5_0_process_value
  22. finger5_0_process_value = subdata.process_value
  23.  
  24. def finger5_1_callback(subdata):
  25. #rospy.loginfo("process value2 is %f", subdata.process_value)
  26. global finger5_1_process_value
  27. finger5_1_process_value = subdata.process_value
  28.  
  29. def finger4_0_callback(subdata):
  30. #rospy.loginfo("process value1 is %f", subdata.process_value)
  31. global finger4_0_process_value
  32. finger4_0_process_value = subdata.process_value
  33.  
  34. def finger4_1_callback(subdata):
  35. #rospy.loginfo("process value2 is %f", subdata.process_value)
  36. global finger4_1_process_value
  37. finger4_1_process_value = subdata.process_value
  38.  
  39. def finger3_0_callback(subdata):
  40. #rospy.loginfo("process value1 is %f", subdata.process_value)
  41. global finger3_0_process_value
  42. finger3_0_process_value = subdata.process_value
  43.  
  44. def finger3_1_callback(subdata):
  45. #rospy.loginfo("process value2 is %f", subdata.process_value)
  46. global finger3_1_process_value
  47. finger3_1_process_value = subdata.process_value
  48.  
  49. def finger2_0_callback(subdata):
  50. #rospy.loginfo("process value1 is %f", subdata.process_value)
  51. global finger2_0_process_value
  52. finger2_0_process_value = subdata.process_value
  53.  
  54. def finger2_1_callback(subdata):
  55. #rospy.loginfo("process value2 is %f", subdata.process_value)
  56. global finger2_1_process_value
  57. finger2_1_process_value = subdata.process_value
  58.  
  59. def finger1_rotate_axis_callback(subdata):
  60. #rospy.loginfo("process value2 is %f", subdata.process_value)
  61. global finger1_rotate_axis_process_value
  62. finger1_rotate_axis_process_value = subdata.process_value
  63.  
  64. def finger1_0_base_callback(subdata):
  65. #rospy.loginfo("process value1 is %f", subdata.process_value)
  66. global finger1_0_base_process_value
  67. finger1_0_base_process_value = subdata.process_value
  68.  
  69. def finger1_1_callback(subdata):
  70. #rospy.loginfo("process value2 is %f", subdata.process_value)
  71. global finger1_1_process_value
  72. finger1_1_process_value = subdata.process_value
  73.  
  74.  
  75. #def hand_initial():
  76. # back every joint angle to zero
  77.  
  78. def check_is_arrive(finger_arg, expect_value, expect_limit):
  79. # check finger's position
  80. difference = finger_arg - expect_value
  81. origin_difference = difference
  82.  
  83. # notice pi's number!!!!!!!!!!!
  84. while difference < 0 or difference >= 6.28318530718:
  85. if difference < 0:
  86. difference = difference + 6.28318530718
  87. if difference >= 6.28318530718:
  88. difference = difference - 6.28318530718
  89.  
  90. bound_difference = difference
  91. final_difference = bound_difference
  92. if difference > 3.14159265359:
  93. final_difference = 6.28318530718 - difference
  94.  
  95. rospy.loginfo("process value is %f, expect is %f, - = %f, 0 ~ 6.28 = %f, final = %f", finger_arg, expect_value, origin_difference, bound_difference, final_difference)
  96.  
  97. if final_difference < expect_limit:
  98. return True
  99. else:
  100. return False
  101.  
  102. def movehand():
  103. rospy.init_node("my_hand_command", anonymous=True)
  104. finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20)
  105. finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20)
  106. finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20)
  107. finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20)
  108. finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20)
  109. finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20)
  110. finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20)
  111. finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20)
  112. finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20)
  113. finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20)
  114. finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20)
  115. move_msg = Float64()
  116.  
  117. rospy.Subscriber('/finger5_0_controller/state', JointControllerState, finger5_0_callback)
  118. rospy.Subscriber('/finger5_1_controller/state', JointControllerState, finger5_1_callback)
  119. rospy.Subscriber('/finger4_0_controller/state', JointControllerState, finger4_0_callback)
  120. rospy.Subscriber('/finger4_1_controller/state', JointControllerState, finger4_1_callback)
  121. rospy.Subscriber('/finger3_0_controller/state', JointControllerState, finger3_0_callback)
  122. rospy.Subscriber('/finger3_1_controller/state', JointControllerState, finger3_1_callback)
  123. rospy.Subscriber('/finger2_0_controller/state', JointControllerState, finger2_0_callback)
  124. rospy.Subscriber('/finger2_1_controller/state', JointControllerState, finger2_1_callback)
  125.  
  126. rospy.Subscriber('/finger1_rotate_axis_controller/state', JointControllerState, finger1_rotate_axis_callback)
  127. rospy.Subscriber('/finger1_0_base_controller/state', JointControllerState, finger1_0_base_callback)
  128. rospy.Subscriber('/finger1_1_controller/state', JointControllerState, finger1_1_callback)
  129.  
  130.  
  131. # all yes_count initial
  132. yes_count1 = 0
  133. yes_count2 = 0
  134. yes_count3 = 0
  135. yes_count4 = 0
  136. yes_count5 = 0
  137. yes_count6 = 0
  138. yes_count7 = 0
  139. yes_count8 = 0
  140. yes_count9 = 0
  141. yes_count10 = 0
  142. yes_count11 = 0
  143.  
  144. # all is_break
  145. is_break1 = False
  146. is_break2 = False
  147. is_break3 = False
  148. is_break4 = False
  149. is_break5 = False
  150. is_break6 = False
  151. is_break7 = False
  152. is_break8 = False
  153. is_break9 = False
  154. is_break10 = False
  155. is_break11 = False
  156.  
  157. # back all joint to zero
  158. count = 0
  159. rate = rospy.Rate(15) #15 hz
  160. while not rospy.is_shutdown():
  161. #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data, process_value_data)
  162. # print do not need global
  163.  
  164. angle = 0
  165. move_msg.data = angle
  166.  
  167. finger5_0_publisher.publish(move_msg)
  168. finger5_1_publisher.publish(move_msg)
  169. finger4_0_publisher.publish(move_msg)
  170. finger4_1_publisher.publish(move_msg)
  171. finger3_0_publisher.publish(move_msg)
  172. finger3_1_publisher.publish(move_msg)
  173. finger2_0_publisher.publish(move_msg)
  174. finger2_1_publisher.publish(move_msg)
  175. finger1_rotate_publisher.publish(move_msg)
  176. finger1_0_publisher.publish(move_msg)
  177. finger1_1_publisher.publish(move_msg)
  178.  
  179. if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and
  180. check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and
  181. check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and
  182. check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and
  183. check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and
  184. check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)):
  185. break
  186.  
  187. rate.sleep()
  188. rospy.loginfo("finish open the hand !!!!!!")
  189.  
  190.  
  191.  
  192. # from finger5 start change, use process_value
  193. # move finger5
  194. count = 0
  195. rate_1 = rospy.Rate(15)
  196. while not rospy.is_shutdown():
  197. angle = 1.7
  198. move_msg.data = angle
  199.  
  200. if not check_is_arrive(finger5_0_process_value, angle, 0.01):
  201. finger5_0_publisher.publish(move_msg)
  202. else:
  203. yes_count1 = yes_count1 + 1
  204. rospy.loginfo("yes_count1 = %f", yes_count1)
  205.  
  206. if yes_count1 == 5:
  207. # break
  208. is_break1 = True
  209.  
  210. angle = 1
  211. move_msg.data = angle
  212.  
  213. if not check_is_arrive(finger5_1_process_value, angle, 0.1):
  214. finger5_1_publisher.publish(move_msg)
  215. else:
  216. yes_count2 = yes_count2 + 1
  217. rospy.loginfo("yes_count2 = %f", yes_count2)
  218.  
  219. if yes_count2 == 15:
  220. # break
  221. is_break2 = True
  222.  
  223. if is_break1 and is_break2:
  224. yes_count1 = 0
  225. yes_count2 = 0
  226. is_break1 = False
  227. is_break2 = False
  228. break
  229. # angle = 1
  230. # move_msg.data = angle
  231. # finger5_1_publisher.publish(move_msg)
  232.  
  233. # count = count + 1
  234. # rospy.loginfo("count = %f ", count)
  235. # if count == 45:
  236. # break
  237. rate_1.sleep()
  238. rospy.loginfo("finger5 finish !!!!!!!!!!")
  239.  
  240. # move finger4
  241. count = 0
  242. rate_2 = rospy.Rate(15)
  243. while not rospy.is_shutdown():
  244. # angle = 1.7
  245. # move_msg.data = angle
  246. # finger4_0_publisher.publish(move_msg)
  247. # angle = 1
  248. # move_msg.data = angle
  249. # finger4_1_publisher.publish(move_msg)
  250.  
  251. # count = count + 1
  252. # rospy.loginfo("count = %f ", count)
  253. # if count == 45:
  254. # break
  255. # rate_2.sleep()
  256. angle = 1.7
  257. move_msg.data = angle
  258.  
  259. if not check_is_arrive(finger4_0_process_value, angle, 0.01):
  260. finger4_0_publisher.publish(move_msg)
  261. else:
  262. yes_count1 = yes_count1 + 1
  263. rospy.loginfo("yes_count1 = %f", yes_count1)
  264.  
  265. if yes_count1 == 5:
  266. # break
  267. is_break1 = True
  268.  
  269. angle = 1
  270. move_msg.data = angle
  271.  
  272. if not check_is_arrive(finger4_1_process_value, angle, 0.1):
  273. finger4_1_publisher.publish(move_msg)
  274. else:
  275. yes_count2 = yes_count2 + 1
  276. rospy.loginfo("yes_count2 = %f", yes_count2)
  277.  
  278. if yes_count2 == 15:
  279. # break
  280. is_break2 = True
  281.  
  282. if is_break1 and is_break2:
  283. yes_count1 = 0
  284. yes_count2 = 0
  285. is_break1 = False
  286. is_break2 = False
  287. break
  288.  
  289. rate_2.sleep()
  290.  
  291. rospy.loginfo("finger4 finish !!!!!!!!!!")
  292.  
  293. # move finger3
  294. count = 0
  295. rate_3 = rospy.Rate(15)
  296. while not rospy.is_shutdown():
  297. angle = 1.7
  298. move_msg.data = angle
  299.  
  300. if not check_is_arrive(finger3_0_process_value, angle, 0.01):
  301. finger3_0_publisher.publish(move_msg)
  302. else:
  303. yes_count1 = yes_count1 + 1
  304. rospy.loginfo("yes_count1 = %f", yes_count1)
  305.  
  306. if yes_count1 == 5:
  307. # break
  308. is_break1 = True
  309.  
  310. angle = 1
  311. move_msg.data = angle
  312.  
  313. if not check_is_arrive(finger3_1_process_value, angle, 0.1):
  314. finger3_1_publisher.publish(move_msg)
  315. else:
  316. yes_count2 = yes_count2 + 1
  317. rospy.loginfo("yes_count2 = %f", yes_count2)
  318.  
  319. if yes_count2 == 15:
  320. # break
  321. is_break2 = True
  322.  
  323. if is_break1 and is_break2:
  324. yes_count1 = 0
  325. yes_count2 = 0
  326. is_break1 = False
  327. is_break2 = False
  328. break
  329.  
  330. rate_3.sleep()
  331.  
  332. rospy.loginfo("finger3 finish !!!!!!!!!!")
  333.  
  334. # move finger2
  335. count = 0
  336. rate_4 = rospy.Rate(15)
  337. while not rospy.is_shutdown():
  338. angle = 1.7
  339. move_msg.data = angle
  340.  
  341. if not check_is_arrive(finger2_0_process_value, angle, 0.01):
  342. finger2_0_publisher.publish(move_msg)
  343. else:
  344. yes_count1 = yes_count1 + 1
  345. rospy.loginfo("yes_count1 = %f", yes_count1)
  346.  
  347. if yes_count1 == 5:
  348. # break
  349. is_break1 = True
  350.  
  351. angle = 1
  352. move_msg.data = angle
  353.  
  354. if not check_is_arrive(finger2_1_process_value, angle, 0.1):
  355. finger2_1_publisher.publish(move_msg)
  356. else:
  357. yes_count2 = yes_count2 + 1
  358. rospy.loginfo("yes_count2 = %f", yes_count2)
  359.  
  360. if yes_count2 == 15:
  361. # break
  362. is_break2 = True
  363.  
  364. if is_break1 and is_break2:
  365. yes_count1 = 0
  366. yes_count2 = 0
  367. is_break1 = False
  368. is_break2 = False
  369. break
  370.  
  371. rate_4.sleep()
  372.  
  373. rospy.loginfo("finger2 finish !!!!!!!!!!")
  374.  
  375.  
  376. # move finger1
  377. count = 0
  378. rate_5 = rospy.Rate(15)
  379. while not rospy.is_shutdown():
  380.  
  381. angle = 1
  382. # may need change -- in .xacro
  383. move_msg.data = angle
  384.  
  385. if not check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01):
  386. finger1_rotate_publisher.publish(move_msg)
  387. else:
  388. yes_count1 = yes_count1 + 1
  389. rospy.loginfo("yes_count1 = %f", yes_count1)
  390.  
  391. if yes_count1 == 15:
  392. # break
  393. is_break1 = True
  394.  
  395. angle = -0.1
  396. move_msg.data = angle
  397.  
  398. if not check_is_arrive(finger1_0_base_process_value, angle, 0.01):
  399. finger1_0_publisher.publish(move_msg)
  400. else:
  401. yes_count2 = yes_count2 + 1
  402. rospy.loginfo("yes_count2 = %f", yes_count2)
  403.  
  404. if yes_count2 == 15:
  405. # break
  406. is_break2 = True
  407.  
  408. angle = 0.5
  409. move_msg.data = angle
  410.  
  411. if not check_is_arrive(finger1_1_process_value, angle, 0.01):
  412. finger1_1_publisher.publish(move_msg)
  413. else:
  414. yes_count3 = yes_count3 + 1
  415. rospy.loginfo("yes_count3 = %f", yes_count3)
  416.  
  417. if yes_count3 == 15:
  418. # break
  419. is_break3 = True
  420.  
  421.  
  422. if is_break1 and is_break2 and is_break3:
  423. yes_count1 = 0
  424. yes_count2 = 0
  425. yes_count3 = 0
  426. is_break1 = False
  427. is_break2 = False
  428. is_break3 = False
  429. break
  430.  
  431. rate_5.sleep()
  432.  
  433. rospy.loginfo("finger1 finish !!!!!!!!!!")
  434.  
  435.  
  436. # back all joint to zero
  437. count = 0
  438. rate_6 = rospy.Rate(15) #15 hz
  439. while not rospy.is_shutdown():
  440. #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data, process_value_data)
  441. # print do not need global
  442.  
  443. angle = 0
  444. move_msg.data = angle
  445.  
  446. finger5_0_publisher.publish(move_msg)
  447. finger5_1_publisher.publish(move_msg)
  448. finger4_0_publisher.publish(move_msg)
  449. finger4_1_publisher.publish(move_msg)
  450. finger3_0_publisher.publish(move_msg)
  451. finger3_1_publisher.publish(move_msg)
  452. finger2_0_publisher.publish(move_msg)
  453. finger2_1_publisher.publish(move_msg)
  454. finger1_rotate_publisher.publish(move_msg)
  455. finger1_0_publisher.publish(move_msg)
  456. finger1_1_publisher.publish(move_msg)
  457.  
  458. if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and
  459. check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and
  460. check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and
  461. check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and
  462. check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and
  463. check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)):
  464. break
  465.  
  466. rate_6.sleep()
  467. rospy.loginfo("finish open the hand !!!!!!")
  468.  
  469.  
  470.  
  471. rospy.spin()
  472.  
  473. if __name__ == '__main__':
  474. try:
  475. movehand()
  476. except rospy.ROSInterruptException: pass
RAW Paste Data