Created
October 3, 2019 15:26
-
-
Save pythongo1/ddc37436814e7007479a597c64a9930d to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import rospy | |
from std_msgs.msg import Float64 | |
from control_msgs.msg import JointControllerState | |
import math # for sin | |
finger5_0_process_value = 0 | |
finger5_1_process_value = 0 | |
finger4_0_process_value = 0 | |
finger4_1_process_value = 0 | |
finger3_0_process_value = 0 | |
finger3_1_process_value = 0 | |
finger2_0_process_value = 0 | |
finger2_1_process_value = 0 | |
finger1_rotate_axis_process_value = 0 | |
finger1_0_base_process_value = 0 | |
finger1_1_process_value = 0 | |
def finger5_0_callback(subdata): | |
#rospy.loginfo("process value1 is %f", subdata.process_value) | |
global finger5_0_process_value | |
finger5_0_process_value = subdata.process_value | |
def finger5_1_callback(subdata): | |
#rospy.loginfo("process value2 is %f", subdata.process_value) | |
global finger5_1_process_value | |
finger5_1_process_value = subdata.process_value | |
def finger4_0_callback(subdata): | |
#rospy.loginfo("process value1 is %f", subdata.process_value) | |
global finger4_0_process_value | |
finger4_0_process_value = subdata.process_value | |
def finger4_1_callback(subdata): | |
#rospy.loginfo("process value2 is %f", subdata.process_value) | |
global finger4_1_process_value | |
finger4_1_process_value = subdata.process_value | |
def finger3_0_callback(subdata): | |
#rospy.loginfo("process value1 is %f", subdata.process_value) | |
global finger3_0_process_value | |
finger3_0_process_value = subdata.process_value | |
def finger3_1_callback(subdata): | |
#rospy.loginfo("process value2 is %f", subdata.process_value) | |
global finger3_1_process_value | |
finger3_1_process_value = subdata.process_value | |
def finger2_0_callback(subdata): | |
#rospy.loginfo("process value1 is %f", subdata.process_value) | |
global finger2_0_process_value | |
finger2_0_process_value = subdata.process_value | |
def finger2_1_callback(subdata): | |
#rospy.loginfo("process value2 is %f", subdata.process_value) | |
global finger2_1_process_value | |
finger2_1_process_value = subdata.process_value | |
def finger1_rotate_axis_callback(subdata): | |
#rospy.loginfo("process value2 is %f", subdata.process_value) | |
global finger1_rotate_axis_process_value | |
finger1_rotate_axis_process_value = subdata.process_value | |
def finger1_0_base_callback(subdata): | |
#rospy.loginfo("process value1 is %f", subdata.process_value) | |
global finger1_0_base_process_value | |
finger1_0_base_process_value = subdata.process_value | |
def finger1_1_callback(subdata): | |
#rospy.loginfo("process value2 is %f", subdata.process_value) | |
global finger1_1_process_value | |
finger1_1_process_value = subdata.process_value | |
#def hand_initial(): | |
# back every joint angle to zero | |
def check_is_arrive(finger_arg, expect_value, expect_limit): | |
# check finger's position | |
difference = finger_arg - expect_value | |
origin_difference = difference | |
# notice pi's number!!!!!!!!!!! | |
while difference < 0 or difference >= 6.28318530718: | |
if difference < 0: | |
difference = difference + 6.28318530718 | |
if difference >= 6.28318530718: | |
difference = difference - 6.28318530718 | |
bound_difference = difference | |
final_difference = bound_difference | |
if difference > 3.14159265359: | |
final_difference = 6.28318530718 - difference | |
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) | |
if final_difference < expect_limit: | |
return True | |
else: | |
return False | |
def movehand(): | |
rospy.init_node("my_hand_command", anonymous=True) | |
finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20) | |
finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20) | |
finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20) | |
finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20) | |
finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20) | |
finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20) | |
finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20) | |
finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20) | |
finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20) | |
finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20) | |
finger1_1_publisher = rospy.Publisher("/finger1_1_controller/command", Float64, queue_size=20) | |
move_msg = Float64() | |
rospy.Subscriber('/finger5_0_controller/state', JointControllerState, finger5_0_callback) | |
rospy.Subscriber('/finger5_1_controller/state', JointControllerState, finger5_1_callback) | |
rospy.Subscriber('/finger4_0_controller/state', JointControllerState, finger4_0_callback) | |
rospy.Subscriber('/finger4_1_controller/state', JointControllerState, finger4_1_callback) | |
rospy.Subscriber('/finger3_0_controller/state', JointControllerState, finger3_0_callback) | |
rospy.Subscriber('/finger3_1_controller/state', JointControllerState, finger3_1_callback) | |
rospy.Subscriber('/finger2_0_controller/state', JointControllerState, finger2_0_callback) | |
rospy.Subscriber('/finger2_1_controller/state', JointControllerState, finger2_1_callback) | |
rospy.Subscriber('/finger1_rotate_axis_controller/state', JointControllerState, finger1_rotate_axis_callback) | |
rospy.Subscriber('/finger1_0_base_controller/state', JointControllerState, finger1_0_base_callback) | |
rospy.Subscriber('/finger1_1_controller/state', JointControllerState, finger1_1_callback) | |
# all yes_count initial | |
yes_count1 = 0 | |
yes_count2 = 0 | |
yes_count3 = 0 | |
yes_count4 = 0 | |
yes_count5 = 0 | |
yes_count6 = 0 | |
yes_count7 = 0 | |
yes_count8 = 0 | |
yes_count9 = 0 | |
yes_count10 = 0 | |
yes_count11 = 0 | |
# all is_break | |
is_break1 = False | |
is_break2 = False | |
is_break3 = False | |
is_break4 = False | |
is_break5 = False | |
is_break6 = False | |
is_break7 = False | |
is_break8 = False | |
is_break9 = False | |
is_break10 = False | |
is_break11 = False | |
# back all joint to zero | |
count = 0 | |
rate = rospy.Rate(15) #15 hz | |
while not rospy.is_shutdown(): | |
#rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data, process_value_data) | |
# print do not need global | |
angle = 0 | |
move_msg.data = angle | |
finger5_0_publisher.publish(move_msg) | |
finger5_1_publisher.publish(move_msg) | |
finger4_0_publisher.publish(move_msg) | |
finger4_1_publisher.publish(move_msg) | |
finger3_0_publisher.publish(move_msg) | |
finger3_1_publisher.publish(move_msg) | |
finger2_0_publisher.publish(move_msg) | |
finger2_1_publisher.publish(move_msg) | |
finger1_rotate_publisher.publish(move_msg) | |
finger1_0_publisher.publish(move_msg) | |
finger1_1_publisher.publish(move_msg) | |
if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and | |
check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and | |
check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and | |
check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and | |
check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and | |
check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)): | |
break | |
rate.sleep() | |
rospy.loginfo("finish open the hand !!!!!!") | |
# from finger5 start change, use process_value | |
# move finger5 | |
count = 0 | |
rate_1 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
if not check_is_arrive(finger5_0_process_value, angle, 0.01): | |
finger5_0_publisher.publish(move_msg) | |
else: | |
yes_count1 = yes_count1 + 1 | |
rospy.loginfo("yes_count1 = %f", yes_count1) | |
if yes_count1 == 5: | |
# break | |
is_break1 = True | |
angle = 1 | |
move_msg.data = angle | |
if not check_is_arrive(finger5_1_process_value, angle, 0.1): | |
finger5_1_publisher.publish(move_msg) | |
else: | |
yes_count2 = yes_count2 + 1 | |
rospy.loginfo("yes_count2 = %f", yes_count2) | |
if yes_count2 == 15: | |
# break | |
is_break2 = True | |
if is_break1 and is_break2: | |
yes_count1 = 0 | |
yes_count2 = 0 | |
is_break1 = False | |
is_break2 = False | |
break | |
# angle = 1 | |
# move_msg.data = angle | |
# finger5_1_publisher.publish(move_msg) | |
# count = count + 1 | |
# rospy.loginfo("count = %f ", count) | |
# if count == 45: | |
# break | |
rate_1.sleep() | |
rospy.loginfo("finger5 finish !!!!!!!!!!") | |
# move finger4 | |
count = 0 | |
rate_2 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
# angle = 1.7 | |
# move_msg.data = angle | |
# finger4_0_publisher.publish(move_msg) | |
# angle = 1 | |
# move_msg.data = angle | |
# finger4_1_publisher.publish(move_msg) | |
# count = count + 1 | |
# rospy.loginfo("count = %f ", count) | |
# if count == 45: | |
# break | |
# rate_2.sleep() | |
angle = 1.7 | |
move_msg.data = angle | |
if not check_is_arrive(finger4_0_process_value, angle, 0.01): | |
finger4_0_publisher.publish(move_msg) | |
else: | |
yes_count1 = yes_count1 + 1 | |
rospy.loginfo("yes_count1 = %f", yes_count1) | |
if yes_count1 == 5: | |
# break | |
is_break1 = True | |
angle = 1 | |
move_msg.data = angle | |
if not check_is_arrive(finger4_1_process_value, angle, 0.1): | |
finger4_1_publisher.publish(move_msg) | |
else: | |
yes_count2 = yes_count2 + 1 | |
rospy.loginfo("yes_count2 = %f", yes_count2) | |
if yes_count2 == 15: | |
# break | |
is_break2 = True | |
if is_break1 and is_break2: | |
yes_count1 = 0 | |
yes_count2 = 0 | |
is_break1 = False | |
is_break2 = False | |
break | |
rate_2.sleep() | |
rospy.loginfo("finger4 finish !!!!!!!!!!") | |
# move finger3 | |
count = 0 | |
rate_3 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
if not check_is_arrive(finger3_0_process_value, angle, 0.01): | |
finger3_0_publisher.publish(move_msg) | |
else: | |
yes_count1 = yes_count1 + 1 | |
rospy.loginfo("yes_count1 = %f", yes_count1) | |
if yes_count1 == 5: | |
# break | |
is_break1 = True | |
angle = 1 | |
move_msg.data = angle | |
if not check_is_arrive(finger3_1_process_value, angle, 0.1): | |
finger3_1_publisher.publish(move_msg) | |
else: | |
yes_count2 = yes_count2 + 1 | |
rospy.loginfo("yes_count2 = %f", yes_count2) | |
if yes_count2 == 15: | |
# break | |
is_break2 = True | |
if is_break1 and is_break2: | |
yes_count1 = 0 | |
yes_count2 = 0 | |
is_break1 = False | |
is_break2 = False | |
break | |
rate_3.sleep() | |
rospy.loginfo("finger3 finish !!!!!!!!!!") | |
# move finger2 | |
count = 0 | |
rate_4 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1.7 | |
move_msg.data = angle | |
if not check_is_arrive(finger2_0_process_value, angle, 0.01): | |
finger2_0_publisher.publish(move_msg) | |
else: | |
yes_count1 = yes_count1 + 1 | |
rospy.loginfo("yes_count1 = %f", yes_count1) | |
if yes_count1 == 5: | |
# break | |
is_break1 = True | |
angle = 1 | |
move_msg.data = angle | |
if not check_is_arrive(finger2_1_process_value, angle, 0.1): | |
finger2_1_publisher.publish(move_msg) | |
else: | |
yes_count2 = yes_count2 + 1 | |
rospy.loginfo("yes_count2 = %f", yes_count2) | |
if yes_count2 == 15: | |
# break | |
is_break2 = True | |
if is_break1 and is_break2: | |
yes_count1 = 0 | |
yes_count2 = 0 | |
is_break1 = False | |
is_break2 = False | |
break | |
rate_4.sleep() | |
rospy.loginfo("finger2 finish !!!!!!!!!!") | |
# move finger1 | |
count = 0 | |
rate_5 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle = 1 | |
# may need change -- in .xacro | |
move_msg.data = angle | |
if not check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01): | |
finger1_rotate_publisher.publish(move_msg) | |
else: | |
yes_count1 = yes_count1 + 1 | |
rospy.loginfo("yes_count1 = %f", yes_count1) | |
if yes_count1 == 15: | |
# break | |
is_break1 = True | |
angle = -0.1 | |
move_msg.data = angle | |
if not check_is_arrive(finger1_0_base_process_value, angle, 0.01): | |
finger1_0_publisher.publish(move_msg) | |
else: | |
yes_count2 = yes_count2 + 1 | |
rospy.loginfo("yes_count2 = %f", yes_count2) | |
if yes_count2 == 15: | |
# break | |
is_break2 = True | |
angle = 0.5 | |
move_msg.data = angle | |
if not check_is_arrive(finger1_1_process_value, angle, 0.01): | |
finger1_1_publisher.publish(move_msg) | |
else: | |
yes_count3 = yes_count3 + 1 | |
rospy.loginfo("yes_count3 = %f", yes_count3) | |
if yes_count3 == 15: | |
# break | |
is_break3 = True | |
if is_break1 and is_break2 and is_break3: | |
yes_count1 = 0 | |
yes_count2 = 0 | |
yes_count3 = 0 | |
is_break1 = False | |
is_break2 = False | |
is_break3 = False | |
break | |
rate_5.sleep() | |
rospy.loginfo("finger1 finish !!!!!!!!!!") | |
# back all joint to zero | |
count = 0 | |
rate_6 = rospy.Rate(15) #15 hz | |
while not rospy.is_shutdown(): | |
#rospy.loginfo("angle = %f ,command_data = %f , process_value_data = %f", angle, command_data, process_value_data) | |
# print do not need global | |
angle = 0 | |
move_msg.data = angle | |
finger5_0_publisher.publish(move_msg) | |
finger5_1_publisher.publish(move_msg) | |
finger4_0_publisher.publish(move_msg) | |
finger4_1_publisher.publish(move_msg) | |
finger3_0_publisher.publish(move_msg) | |
finger3_1_publisher.publish(move_msg) | |
finger2_0_publisher.publish(move_msg) | |
finger2_1_publisher.publish(move_msg) | |
finger1_rotate_publisher.publish(move_msg) | |
finger1_0_publisher.publish(move_msg) | |
finger1_1_publisher.publish(move_msg) | |
if (check_is_arrive(finger5_0_process_value, angle, 0.01) and check_is_arrive(finger5_1_process_value, angle, 0.1) and | |
check_is_arrive(finger4_0_process_value, angle, 0.01) and check_is_arrive(finger4_1_process_value, angle, 0.1) and | |
check_is_arrive(finger3_0_process_value, angle, 0.01) and check_is_arrive(finger3_1_process_value, angle, 0.1) and | |
check_is_arrive(finger2_0_process_value, angle, 0.01) and check_is_arrive(finger2_1_process_value, angle, 0.1) and | |
check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01) and | |
check_is_arrive(finger1_0_base_process_value, angle, 0.01) and check_is_arrive(finger1_1_process_value, angle, 0.1)): | |
break | |
rate_6.sleep() | |
rospy.loginfo("finish open the hand !!!!!!") | |
rospy.spin() | |
if __name__ == '__main__': | |
try: | |
movehand() | |
except rospy.ROSInterruptException: pass | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment