SHARE
TWEET

Untitled

a guest Oct 21st, 2019 94 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. import os
  7.  
  8. # for publish
  9. finger5_0_publisher = 0
  10. finger5_1_publisher = 0
  11. finger4_0_publisher = 0
  12. finger4_1_publisher = 0
  13. finger3_0_publisher = 0
  14. finger3_1_publisher = 0
  15. finger2_0_publisher = 0
  16. finger2_1_publisher = 0
  17. finger1_rotate_publisher = 0
  18. finger1_0_publisher = 0
  19. finger1_1_publisher = 0
  20.  
  21. # for global variable
  22. finger5_0_process_value = 0
  23. finger5_1_process_value = 0
  24. finger4_0_process_value = 0
  25. finger4_1_process_value = 0
  26. finger3_0_process_value = 0
  27. finger3_1_process_value = 0
  28. finger2_0_process_value = 0
  29. finger2_1_process_value = 0
  30. finger1_rotate_axis_process_value = 0
  31. finger1_0_base_process_value = 0
  32. finger1_1_process_value = 0
  33.  
  34. def finger5_0_callback(subdata):
  35.     #rospy.loginfo("process value1 is %f", subdata.process_value)
  36.     global finger5_0_process_value
  37.     finger5_0_process_value = subdata.process_value
  38.  
  39. def finger5_1_callback(subdata):
  40.     #rospy.loginfo("process value2 is %f", subdata.process_value)
  41.     global finger5_1_process_value
  42.     finger5_1_process_value = subdata.process_value
  43.  
  44. def finger4_0_callback(subdata):
  45.     #rospy.loginfo("process value1 is %f", subdata.process_value)
  46.     global finger4_0_process_value
  47.     finger4_0_process_value = subdata.process_value
  48.  
  49. def finger4_1_callback(subdata):
  50.     #rospy.loginfo("process value2 is %f", subdata.process_value)
  51.     global finger4_1_process_value
  52.     finger4_1_process_value = subdata.process_value
  53.  
  54. def finger3_0_callback(subdata):
  55.     #rospy.loginfo("process value1 is %f", subdata.process_value)
  56.     global finger3_0_process_value
  57.     finger3_0_process_value = subdata.process_value
  58.  
  59. def finger3_1_callback(subdata):
  60.     #rospy.loginfo("process value2 is %f", subdata.process_value)
  61.     global finger3_1_process_value
  62.     finger3_1_process_value = subdata.process_value
  63.  
  64. def finger2_0_callback(subdata):
  65.     #rospy.loginfo("process value1 is %f", subdata.process_value)
  66.     global finger2_0_process_value
  67.     finger2_0_process_value = subdata.process_value
  68.  
  69. def finger2_1_callback(subdata):
  70.     #rospy.loginfo("process value2 is %f", subdata.process_value)
  71.     global finger2_1_process_value
  72.     finger2_1_process_value = subdata.process_value
  73.  
  74. def finger1_rotate_axis_callback(subdata):
  75.     #rospy.loginfo("process value2 is %f", subdata.process_value)
  76.     global finger1_rotate_axis_process_value
  77.     finger1_rotate_axis_process_value = subdata.process_value
  78.  
  79. def finger1_0_base_callback(subdata):
  80.     #rospy.loginfo("process value1 is %f", subdata.process_value)
  81.     global finger1_0_base_process_value
  82.     finger1_0_base_process_value = subdata.process_value
  83.  
  84. def finger1_1_callback(subdata):
  85.     #rospy.loginfo("process value2 is %f", subdata.process_value)
  86.     global finger1_1_process_value
  87.     finger1_1_process_value = subdata.process_value
  88.  
  89.  
  90. def open_hand():
  91.     # back every joint angle to zero
  92.  
  93.     move_msg = Float64()
  94.  
  95.     rate_0 = rospy.Rate(15)  #15 hz
  96.     while not rospy.is_shutdown():
  97.         #rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data,  process_value_data)
  98.         # print do not need global
  99.         angle = 0
  100.         move_msg.data = angle
  101.  
  102.         finger5_0_publisher.publish(move_msg)
  103.         finger5_1_publisher.publish(move_msg)
  104.         finger4_0_publisher.publish(move_msg)
  105.         finger4_1_publisher.publish(move_msg)
  106.         finger3_0_publisher.publish(move_msg)
  107.         finger3_1_publisher.publish(move_msg)
  108.         finger2_0_publisher.publish(move_msg)
  109.         finger2_1_publisher.publish(move_msg)
  110.         finger1_rotate_publisher.publish(move_msg)
  111.         finger1_0_publisher.publish(move_msg)
  112.         finger1_1_publisher.publish(move_msg)
  113.  
  114.         if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and
  115.             check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and
  116.             check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and
  117.             check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and
  118.             check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and
  119.             check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)):
  120.             break
  121.  
  122.  
  123.         rate_0.sleep()
  124.     rospy.loginfo("finish open the hand !!!!!!")
  125.  
  126. def finger_pub_function(finger_number, angle):
  127.     # use finger_number to decide which finger should move
  128.     move_msg = Float64()
  129.     yes_count1 = 0
  130.     yes_count2 = 0
  131.     is_break1 = False
  132.     is_break2 = False
  133.  
  134.     # finger_number==5 is the finger5, finger_number==4 is the finger4 ...
  135.     # (special) finger_number==0 is the rotate axis(under the finger1)
  136.     if finger_number == 0:
  137.         rate_1 = rospy.Rate(15)
  138.         while not rospy.is_shutdown():
  139.             move_msg.data = angle
  140.  
  141.             if not check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01):
  142.                 finger1_rotate_publisher.publish(move_msg)
  143.             else:
  144.                  break
  145.  
  146.             rate_1.sleep()
  147.  
  148.         # force to end this finger_pub_function
  149.         return 0
  150.        
  151.     if finger_number == 5:
  152.         finger_0_pub = finger5_0_publisher
  153.         finger_1_pub = finger5_1_publisher
  154.  
  155.     if finger_number == 4:
  156.         finger_0_pub = finger4_0_publisher
  157.         finger_1_pub = finger4_1_publisher
  158.  
  159.     if finger_number == 3:
  160.         finger_0_pub = finger3_0_publisher
  161.         finger_1_pub = finger3_1_publisher
  162.  
  163.     if finger_number == 2:
  164.         finger_0_pub = finger2_0_publisher
  165.         finger_1_pub = finger2_1_publisher
  166.  
  167.     if finger_number == 1:
  168.         finger_0_pub = finger1_0_publisher
  169.         finger_1_pub = finger1_1_publisher
  170.  
  171.  
  172.     rate_1 = rospy.Rate(15)
  173.     while not rospy.is_shutdown():
  174.         angle_0 = angle
  175.         move_msg.data = angle_0
  176.  
  177.         # choose the process value, by finger_number
  178.         # finger_number==5 is the finger5, finger_number==4 is the finger4 ......
  179.         if finger_number == 5:
  180.             process_value_0 = finger5_0_process_value
  181.             process_value_1 = finger5_1_process_value
  182.         if finger_number == 4:
  183.             process_value_0 = finger4_0_process_value
  184.             process_value_1 = finger4_1_process_value
  185.         if finger_number == 3:
  186.             process_value_0 = finger3_0_process_value
  187.             process_value_1 = finger3_1_process_value
  188.         if finger_number == 2:
  189.             process_value_0 = finger2_0_process_value
  190.             process_value_1 = finger2_1_process_value
  191.         if finger_number == 1:
  192.             process_value_0 = finger1_0_base_process_value
  193.             process_value_1 = finger1_1_process_value
  194.  
  195.         # check the angle  arrive. if not arrive, publish command.
  196.         if not check_is_arrive(process_value_0, angle_0, 0.01):
  197.             finger_0_pub.publish(move_msg)
  198.         else:
  199.             yes_count1 = yes_count1 + 1
  200.             rospy.loginfo("yes_count1 = %f", yes_count1)
  201.  
  202.         if yes_count1 == 5:
  203.             # break
  204.             is_break1 = True
  205.  
  206.         # remember to use 2.00 , not 2 !!!
  207.         # because of python, 5/2=2, 5/2.00=2.5
  208.         angle_1 = angle / 2.00
  209.  
  210.         expect_error = 0.1
  211.         # special angle for finger1, and special epect_error for finger1
  212.         if finger_number == 1:
  213.             angle_1 = angle * 5
  214.             expect_error = 0.01
  215.  
  216.         move_msg.data = angle_1
  217.  
  218.         if not check_is_arrive(process_value_1, angle_1, expect_error):
  219.             finger_1_pub.publish(move_msg)
  220.         else:
  221.             yes_count2 = yes_count2 + 1
  222.             rospy.loginfo("yes_count2 = %f", yes_count2)
  223.  
  224.         if yes_count2 == 15:
  225.             # break
  226.             is_break2 = True
  227.  
  228.         if is_break1 and is_break2:
  229.             yes_count1 = 0
  230.             yes_count2 = 0
  231.             is_break1 = False
  232.             is_break2 = False
  233.             break
  234.  
  235.         rate_1.sleep()
  236.  
  237.  
  238.  
  239. def finger5_move(angle):
  240.     # finger5 function , need to add limit
  241.     # if angle is not in this range, exit this program
  242.     if angle < 0 or angle > 1.7:
  243.         rospy.loginfo(" [error! error!][finger5] the angle is not between 0 and 1.7 rad ! ")
  244.         os._exit(0)
  245.  
  246.     finger_number = 5
  247.     finger_pub_function(finger_number, angle)
  248.     rospy.loginfo("finger5 finish !!!!!!!!!!")
  249.  
  250.  
  251.  
  252. def finger4_move(angle):
  253.     # finger4 function , need to add limit
  254.     # if angle is not in this range, exit this program
  255.     if angle < 0 or angle > 1.7:
  256.         rospy.loginfo(" [error! error!][finger4] the angle is not between 0 and 1.7 rad ! ")
  257.         os._exit(0)
  258.  
  259.     finger_number = 4
  260.     finger_pub_function(finger_number, angle)
  261.     rospy.loginfo("finger4 finish !!!!!!!!!!")
  262.  
  263. def finger3_move(angle):
  264.     # finger3 function , need to add limit
  265.     # if angle is not in this range, exit this program
  266.     if angle < 0 or angle > 1.7:
  267.         rospy.loginfo(" [error! error!][finger3] the angle is not between 0 and 1.7 rad ! ")
  268.         os._exit(0)
  269.  
  270.     finger_number = 3
  271.     finger_pub_function(finger_number, angle)
  272.     rospy.loginfo("finger3 finish !!!!!!!!!!")
  273.  
  274. def finger2_move(angle):
  275.     # finger2 function , need to add limit
  276.     # if angle is not in this range, exit this program
  277.     if angle < 0 or angle > 1.7:
  278.         rospy.loginfo(" [error! error!][finger2] the angle is not between 0 and 1.7 rad ! ")
  279.         os._exit(0)
  280.  
  281.     finger_number = 2
  282.     finger_pub_function(finger_number, angle)
  283.     rospy.loginfo("finger2 finish !!!!!!!!!!")
  284.  
  285. def finger1_move(angle):
  286.     # not include rotate axis
  287.     # finger1 function , need to add limit
  288.     # if angle is not in this range, exit this program
  289.     if angle < 0 or angle > 1.7:
  290.         rospy.loginfo(" [error! error!][finger1] the angle is not between 0 and 1.7 rad ! ")
  291.         os._exit(0)
  292.  
  293.     finger_number = 1
  294.     finger_pub_function(finger_number, angle)
  295.     rospy.loginfo("finger1 finish !!!!!!!!!!")
  296.  
  297.  
  298. def finger1_rotate_move(angle):
  299.     if angle < 0 or angle > 1.7:
  300.         rospy.loginfo(" [error! error!][finger1_ rotate_axis] the angle is not between 0 and 1.7 rad ! ")
  301.         os._exit(0)
  302.  
  303.     finger_number = 0
  304.     finger_pub_function(finger_number, angle)
  305.     rospy.loginfo("finger1 rotate axis finish !!!!!!!!!!")
  306.  
  307.  
  308. def check_is_arrive(finger_arg, expect_value, expect_limit):
  309.     # check finger's position
  310.     difference = finger_arg - expect_value
  311.     origin_difference = difference
  312.  
  313.     # notice pi's number!!!!!!!!!!!
  314.     while difference < 0 or difference >= 6.28318530718:
  315.         if difference < 0:
  316.             difference = difference + 6.28318530718
  317.         if difference >= 6.28318530718:
  318.             difference = difference - 6.28318530718
  319.  
  320.     bound_difference = difference
  321.     final_difference = bound_difference
  322.     if difference > 3.14159265359:
  323.         final_difference = 6.28318530718 - difference
  324.  
  325.     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)
  326.  
  327.     if final_difference < expect_limit:
  328.         return True
  329.     else:
  330.         return False
  331.  
  332. def movehand():
  333.     rospy.init_node("my_hand_command", anonymous=True)
  334.  
  335.     global finger5_0_publisher
  336.     finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20)
  337.     global finger5_1_publisher
  338.     finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20)
  339.     global finger4_0_publisher
  340.     finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20)
  341.     global finger4_1_publisher
  342.     finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20)
  343.     global finger3_0_publisher
  344.     finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20)
  345.     global finger3_1_publisher
  346.     finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20)
  347.     global finger2_0_publisher
  348.     finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20)
  349.     global finger2_1_publisher
  350.     finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20)
  351.     global finger1_rotate_publisher
  352.     finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20)
  353.     global finger1_0_publisher
  354.     finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20)
  355.     global finger1_1_publisher
  356.     finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20)
  357.     move_msg = Float64()
  358.  
  359.     rospy.Subscriber('/finger5_0_controller/state', JointControllerState, finger5_0_callback)
  360.     rospy.Subscriber('/finger5_1_controller/state', JointControllerState, finger5_1_callback)
  361.     rospy.Subscriber('/finger4_0_controller/state', JointControllerState, finger4_0_callback)
  362.     rospy.Subscriber('/finger4_1_controller/state', JointControllerState, finger4_1_callback)
  363.     rospy.Subscriber('/finger3_0_controller/state', JointControllerState, finger3_0_callback)
  364.     rospy.Subscriber('/finger3_1_controller/state', JointControllerState, finger3_1_callback)
  365.     rospy.Subscriber('/finger2_0_controller/state', JointControllerState, finger2_0_callback)
  366.     rospy.Subscriber('/finger2_1_controller/state', JointControllerState, finger2_1_callback)
  367.  
  368.     rospy.Subscriber('/finger1_rotate_axis_controller/state', JointControllerState, finger1_rotate_axis_callback)    
  369.     rospy.Subscriber('/finger1_0_base_controller/state', JointControllerState, finger1_0_base_callback)
  370.     rospy.Subscriber('/finger1_1_controller/state', JointControllerState, finger1_1_callback)
  371.  
  372.  
  373. #------------ open hand ---------------------------------------
  374.     open_hand()
  375.  
  376. #------------ move finger5 -----------------------------------
  377.     angle = 1.7
  378.     finger5_move(angle)
  379.  
  380. #------------ move finger4  ---------------------------------
  381.     angle = 1.7
  382.     finger4_move(angle)
  383.  
  384. #------------ move finger3 ----------------------------------
  385.     angle = 1.7
  386.     finger3_move(angle)
  387.  
  388. #------------ move finger2 ----------------------------------
  389.     angle = 1.7    
  390.     finger2_move(angle)
  391.  
  392.  
  393. #------------ move finger1 (not include rotate axis )--------
  394.     angle = 0.1    # 0.1 0.5
  395.     finger1_move(angle)
  396.  
  397. #------------ move rotate axis(under finger1) --------------
  398.     angle = 1   # 1
  399.     finger1_rotate_move(angle)
  400.  
  401. #------------- open hand ----------------------------------
  402.     open_hand()
  403.  
  404.  
  405.     rospy.spin()
  406.  
  407. if __name__ == '__main__':
  408.     try:
  409.         movehand()
  410.     except rospy.ROSInterruptException: pass
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top