Go to the documentation of this file.00001
00002 import sys
00003 import rospy
00004 from sr_robot_commander.sr_hand_commander import SrHandCommander
00005 from sr_utilities.hand_finder import HandFinder
00006
00007 rospy.init_node("basic_hand_examples", anonymous=True)
00008 hand_finder = HandFinder()
00009 hand_parameters = hand_finder.get_hand_parameters()
00010 if len(hand_parameters.mapping) is 0:
00011 print("No hand detected")
00012 sys.exit("no hand detected")
00013
00014 hand_serial = hand_parameters.mapping.keys()[0]
00015 hand_mapping = hand_parameters.mapping[hand_serial]
00016 prefix = hand_parameters.joint_prefix[hand_serial]
00017 if hand_mapping == 'rh':
00018 hand_commander = SrHandCommander()
00019 else:
00020 hand_commander = SrHandCommander(name="left_hand", prefix="lh")
00021 joints = hand_finder.get_hand_joints()[hand_mapping]
00022 if len(joints) is not 24:
00023 print("Joints are less than 24")
00024 sys.exit("no hand detected")
00025 print("Moving to hand position defined by joint values")
00026
00027 position_values \
00028 = [0.3490658767850654, 0.1747066021773609, 0.3773716583863109,
00029 -0.0353585782262833, 0.349075214851851, 0.1749198992229255,
00030 0.41179341920347934, -0.03698607363380546, 0.350811000266134,
00031 0.1754494541236511, 0.3761746978350553, 0.03538037757354218,
00032 0.3508110105031834, 0.1689616221460435, 0.27430689938959674,
00033 0.02130857476409176, 0.03232033355620789, 0.34905554413970474,
00034 0.19665410818337659, -0.094030693144318, 0.01741447758271608,
00035 -0.0044660151203368414, 2.8093405901152835e-05, 0.020004520938839754,
00036 ]
00037
00038
00039 position_1 = dict(zip(joints, position_values))
00040 hand_commander.move_to_joint_value_target(position_1)
00041
00042 named_target_1 = "pack"
00043 print("Moving to hand named target " + named_target_1)
00044 hand_commander.move_to_named_target(named_target_1)
00045
00046 named_target_2 = "open"
00047 print("Moving to hand named target " + named_target_2)
00048 hand_commander.move_to_named_target(named_target_2)
00049
00050 hand_joints_state = hand_commander.get_joints_position()
00051 hand_joints_velocity = hand_commander.get_joints_velocity()
00052 hand_joints_effort = hand_commander.get_joints_effort()
00053
00054 print("Hand joints position \n " + str(hand_joints_state) + "\n")
00055 print("Hand joints velocity \n " + str(hand_joints_velocity) + "\n")
00056 print("Hand joints effort \n " + str(hand_joints_effort) + "\n")
00057
00058 tactile_type = hand_commander.get_tactile_type()
00059 tactile_state = hand_commander.get_tactile_state()
00060 print("Tactile type \n " + str(tactile_type) + "\n")
00061 print("Tactile state \n " + str(tactile_state) + "\n")