hand_serial = hand_parameters.mapping.keys()[0]
hand_commander = SrHandCommander(hand_parameters=hand_parameters,
hand_serial=hand_serial)
hand_mapping = hand_parameters.mapping[hand_serial]
# Hand joints are detected
joints = hand_finder.get_hand_joints()[hand_mapping]
position_values = [0.35, 0.18, 0.38]
# Moving to a target determined by the values in position_values.
rospy.loginfo("Hand moving to script specified target")
position_1 = dict(zip(joints, position_values))
hand_commander.move_to_joint_value_target(position_1)
named_target_1 = "pack"
rospy.loginfo("Hand moving to named target: " + named_target_1)
hand_commander.move_to_named_target(named_target_1)
named_target_2 = "open"
rospy.loginfo("Hand moving to named target: " + named_target_2)
hand_commander.move_to_named_target(named_target_2)
# Hand joints state, velocity and effort are read and displayed to screen.
hand_joints_state = hand_commander.get_joints_position()
hand_joints_velocity = hand_commander.get_joints_velocity()
hand_joints_effort = hand_commander.get_joints_effort()
rospy.loginfo("Hand joints position \n " + str(hand_joints_state) + "\n")
if len(s) > 3:
print "s below split", s.split('_')[1]
s = s.split('_')[1]
s = int(s)
p = float(s)
if s > -1:
p = p / 13 * 1.5
print "0.5 p", p
pose = {
'rh_FFJ1': p,
'rh_MFJ1': p,
'rh_RFJ1': p,
'rh_LFJ1': p
}
print "pose", pose
hand_commander.move_to_joint_value_target(pose)
#print "done"
time.sleep(0.01)
a = 3
#position_1 = {'rh_LFJ1': n, 'rh_LFJ2': n, 'rh_LFJ3': n}
#hand_commander.move_to_joint_value_target(position_1)
#position_1 = {'rh_LFJ1': j, 'rh_LFJ2': j, 'rh_LFJ3': j}
#hand_commander.move_to_joint_value_target(position_1)
#while True:
#position_1 = {'rh_LFJ1': 1.0, 'rh_LFJ2': 1.0, 'rh_LFJ3': 1.0}
#hand_commander.move_to_joint_value_target(position_1)
