SHARE
TWEET

Untitled

a guest Oct 21st, 2019 84 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
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top