Created
October 3, 2019 15:40
-
-
Save pythongo1/751710308b92d95f09ae3bd95dcfeb68 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 | |
import os | |
# for publish | |
finger5_0_publisher = 0 | |
finger5_1_publisher = 0 | |
finger4_0_publisher = 0 | |
finger4_1_publisher = 0 | |
finger3_0_publisher = 0 | |
finger3_1_publisher = 0 | |
finger2_0_publisher = 0 | |
finger2_1_publisher = 0 | |
finger1_rotate_publisher = 0 | |
finger1_0_publisher = 0 | |
finger1_1_publisher = 0 | |
# for global variable | |
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 open_hand(): | |
# back every joint angle to zero | |
move_msg = Float64() | |
rate_0 = 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_0.sleep() | |
rospy.loginfo("finish open the hand !!!!!!") | |
def finger_pub_function(finger_number, angle): | |
# use finger_number to decide which finger should move | |
move_msg = Float64() | |
yes_count1 = 0 | |
yes_count2 = 0 | |
is_break1 = False | |
is_break2 = False | |
# finger_number==5 is the finger5, finger_number==4 is the finger4 ... | |
# (special) finger_number==0 is the rotate axis(under the finger1) | |
if finger_number == 0: | |
rate_1 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
move_msg.data = angle | |
if not check_is_arrive(finger1_rotate_axis_process_value, angle, 0.01): | |
finger1_rotate_publisher.publish(move_msg) | |
else: | |
break | |
rate_1.sleep() | |
# force to end this finger_pub_function | |
return 0 | |
if finger_number == 5: | |
finger_0_pub = finger5_0_publisher | |
finger_1_pub = finger5_1_publisher | |
if finger_number == 4: | |
finger_0_pub = finger4_0_publisher | |
finger_1_pub = finger4_1_publisher | |
if finger_number == 3: | |
finger_0_pub = finger3_0_publisher | |
finger_1_pub = finger3_1_publisher | |
if finger_number == 2: | |
finger_0_pub = finger2_0_publisher | |
finger_1_pub = finger2_1_publisher | |
if finger_number == 1: | |
finger_0_pub = finger1_0_publisher | |
finger_1_pub = finger1_1_publisher | |
rate_1 = rospy.Rate(15) | |
while not rospy.is_shutdown(): | |
angle_0 = angle | |
move_msg.data = angle_0 | |
# choose the process value, by finger_number | |
# finger_number==5 is the finger5, finger_number==4 is the finger4 ...... | |
if finger_number == 5: | |
process_value_0 = finger5_0_process_value | |
process_value_1 = finger5_1_process_value | |
if finger_number == 4: | |
process_value_0 = finger4_0_process_value | |
process_value_1 = finger4_1_process_value | |
if finger_number == 3: | |
process_value_0 = finger3_0_process_value | |
process_value_1 = finger3_1_process_value | |
if finger_number == 2: | |
process_value_0 = finger2_0_process_value | |
process_value_1 = finger2_1_process_value | |
if finger_number == 1: | |
process_value_0 = finger1_0_base_process_value | |
process_value_1 = finger1_1_process_value | |
# check the angle arrive. if not arrive, publish command. | |
if not check_is_arrive(process_value_0, angle_0, 0.01): | |
finger_0_pub.publish(move_msg) | |
else: | |
yes_count1 = yes_count1 + 1 | |
rospy.loginfo("yes_count1 = %f", yes_count1) | |
if yes_count1 == 5: | |
# break | |
is_break1 = True | |
# remember to use 2.00 , not 2 !!! | |
# because of python, 5/2=2, 5/2.00=2.5 | |
angle_1 = angle / 2.00 | |
expect_error = 0.1 | |
# special angle for finger1, and special epect_error for finger1 | |
if finger_number == 1: | |
angle_1 = angle * 5 | |
expect_error = 0.01 | |
move_msg.data = angle_1 | |
if not check_is_arrive(process_value_1, angle_1, expect_error): | |
finger_1_pub.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_1.sleep() | |
def finger5_move(angle): | |
# finger5 function , need to add limit | |
# if angle is not in this range, exit this program | |
if angle < 0 or angle > 1.7: | |
rospy.loginfo(" [error! error!][finger5] the angle is not between 0 and 1.7 rad ! ") | |
os._exit(0) | |
finger_number = 5 | |
finger_pub_function(finger_number, angle) | |
rospy.loginfo("finger5 finish !!!!!!!!!!") | |
def finger4_move(angle): | |
# finger4 function , need to add limit | |
# if angle is not in this range, exit this program | |
if angle < 0 or angle > 1.7: | |
rospy.loginfo(" [error! error!][finger4] the angle is not between 0 and 1.7 rad ! ") | |
os._exit(0) | |
finger_number = 4 | |
finger_pub_function(finger_number, angle) | |
rospy.loginfo("finger4 finish !!!!!!!!!!") | |
def finger3_move(angle): | |
# finger3 function , need to add limit | |
# if angle is not in this range, exit this program | |
if angle < 0 or angle > 1.7: | |
rospy.loginfo(" [error! error!][finger3] the angle is not between 0 and 1.7 rad ! ") | |
os._exit(0) | |
finger_number = 3 | |
finger_pub_function(finger_number, angle) | |
rospy.loginfo("finger3 finish !!!!!!!!!!") | |
def finger2_move(angle): | |
# finger2 function , need to add limit | |
# if angle is not in this range, exit this program | |
if angle < 0 or angle > 1.7: | |
rospy.loginfo(" [error! error!][finger2] the angle is not between 0 and 1.7 rad ! ") | |
os._exit(0) | |
finger_number = 2 | |
finger_pub_function(finger_number, angle) | |
rospy.loginfo("finger2 finish !!!!!!!!!!") | |
def finger1_move(angle): | |
# not include rotate axis | |
# finger1 function , need to add limit | |
# if angle is not in this range, exit this program | |
if angle < 0 or angle > 1.7: | |
rospy.loginfo(" [error! error!][finger1] the angle is not between 0 and 1.7 rad ! ") | |
os._exit(0) | |
finger_number = 1 | |
finger_pub_function(finger_number, angle) | |
rospy.loginfo("finger1 finish !!!!!!!!!!") | |
def finger1_rotate_move(angle): | |
if angle < 0 or angle > 1.7: | |
rospy.loginfo(" [error! error!][finger1_ rotate_axis] the angle is not between 0 and 1.7 rad ! ") | |
os._exit(0) | |
finger_number = 0 | |
finger_pub_function(finger_number, angle) | |
rospy.loginfo("finger1 rotate axis finish !!!!!!!!!!") | |
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) | |
global finger5_0_publisher | |
finger5_0_publisher = rospy.Publisher("/finger5_0_controller/command", Float64, queue_size=20) | |
global finger5_1_publisher | |
finger5_1_publisher = rospy.Publisher("/finger5_1_controller/command", Float64, queue_size=20) | |
global finger4_0_publisher | |
finger4_0_publisher = rospy.Publisher("/finger4_0_controller/command", Float64, queue_size=20) | |
global finger4_1_publisher | |
finger4_1_publisher = rospy.Publisher("/finger4_1_controller/command", Float64, queue_size=20) | |
global finger3_0_publisher | |
finger3_0_publisher = rospy.Publisher("/finger3_0_controller/command", Float64, queue_size=20) | |
global finger3_1_publisher | |
finger3_1_publisher = rospy.Publisher("/finger3_1_controller/command", Float64, queue_size=20) | |
global finger2_0_publisher | |
finger2_0_publisher = rospy.Publisher("/finger2_0_controller/command", Float64, queue_size=20) | |
global finger2_1_publisher | |
finger2_1_publisher = rospy.Publisher("/finger2_1_controller/command", Float64, queue_size=20) | |
global finger1_rotate_publisher | |
finger1_rotate_publisher = rospy.Publisher("/finger1_rotate_axis_controller/command", Float64, queue_size=20) | |
global finger1_0_publisher | |
finger1_0_publisher = rospy.Publisher("/finger1_0_base_controller/command", Float64, queue_size=20) | |
global finger1_1_publisher | |
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) | |
#------------ open hand --------------------------------------- | |
open_hand() | |
#------------ move finger5 ----------------------------------- | |
angle = 1.7 | |
finger5_move(angle) | |
#------------ move finger4 --------------------------------- | |
angle = 1.7 | |
finger4_move(angle) | |
#------------ move finger3 ---------------------------------- | |
angle = 1.7 | |
finger3_move(angle) | |
#------------ move finger2 ---------------------------------- | |
angle = 1.7 | |
finger2_move(angle) | |
#------------ move finger1 (not include rotate axis )-------- | |
angle = 0.1 # 0.1 0.5 | |
finger1_move(angle) | |
#------------ move rotate axis(under finger1) -------------- | |
angle = 1 # 1 | |
finger1_rotate_move(angle) | |
#------------- open hand ---------------------------------- | |
open_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