def test_hand_finder_init(self):
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_commander = SrHandCommander(
hand_parameters=hand_parameters,
hand_serial=hand_parameters.mapping.keys()[0])
self.assertGreater(len(hand_commander.get_joints_position()), 0,
"No joints found, init must have failed.")
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")
rospy.loginfo("Hand joints velocity \n " + str(hand_joints_velocity) + "\n")
rospy.loginfo("Hand joints effort \n " + str(hand_joints_effort) + "\n")
# Tactile type and state are read and displayed to screen.
tactile_type = hand_commander.get_tactile_type()
tactile_state = hand_commander.get_tactile_state()
rospy.loginfo("Tactile type \n " + str(tactile_type) + "\n")
rospy.loginfo("Tactile state \n " + str(tactile_state) + "\n")
class Teleoperation():
def __init__(self):
self.bridge = CvBridge()
self.hand_commander = SrHandCommander(name="right_hand")
rospy.sleep(1)
def online_once(self):
while True:
img_data = rospy.wait_for_message(
"/camera/aligned_depth_to_color/image_raw", Image)
rospy.loginfo("Got an image ^_^")
try:
img = self.bridge.imgmsg_to_cv2(img_data,
desired_encoding="passthrough")
except CvBridgeError as e:
rospy.logerr(e)
try:
# preproces
img = seg_hand_depth(img, 500, 1000, 10, 100, 4, 4, 250, True,
300)
img = img.astype(np.float32)
img = img / 255. * 2. - 1
n = cv2.resize(img, (0, 0), fx=2, fy=2)
n = cv2.normalize(n,
n,
alpha=0,
beta=1,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_32F)
cv2.imshow("segmented human hand", n)
cv2.waitKey(1)
# get the clipped joints
goal = tuple(self.joint_cal(img, isbio=True))
hand_pos = self.hand_commander.get_joints_position()
# first finger
hand_pos.update({"rh_FFJ3": goal[3]})
hand_pos.update({"rh_FFJ2": goal[4]})
hand_pos.update({"rh_FFJ4": goal[2]})
# middle finger
hand_pos.update({"rh_MFJ3": goal[12]})
hand_pos.update({"rh_MFJ2": goal[13]})
# ring finger
hand_pos.update({"rh_RFJ3": goal[16]})
hand_pos.update({"rh_RFJ2": goal[17]})
# little finger
hand_pos.update({"rh_LFJ3": goal[8]})
hand_pos.update({"rh_LFJ2": goal[9]})
# thumb
hand_pos.update({"rh_THJ5": goal[19]})
hand_pos.update({"rh_THJ4": goal[20]})
hand_pos.update({"rh_THJ3": goal[21]})
hand_pos.update({"rh_THJ2": goal[22]})
self.hand_commander.move_to_joint_value_target_unsafe(
hand_pos, 0.3, False, angle_degrees=False)
#rospy.sleep(0.5)
rospy.loginfo("Next one please ---->")
except:
rospy.loginfo("no images")
def joint_cal(self, img, isbio=False):
# start = rospy.Time.now().to_sec()
# run the model
feature = test(model, img)
# network_time = rospy.Time.now().to_sec() - start
# print("network_time is ", network_time)
joint = [0.0, 0.0]
joint += feature.tolist()
if isbio:
joint[5] = 0.3498509706185152
joint[10] = 0.3498509706185152
joint[14] = 0.3498509706185152
joint[18] = 0.3498509706185152
joint[23] = 0.3498509706185152
# joints crop
joint[2] = self.clip(joint[2], 0.349, -0.349)
joint[3] = self.clip(joint[3], 1.57, 0)
joint[4] = self.clip(joint[4], 1.57, 0)
joint[5] = self.clip(joint[5], 1.57, 0)
joint[6] = self.clip(joint[6], 0.785, 0)
joint[7] = self.clip(joint[7], 0.349, -0.349)
joint[8] = self.clip(joint[8], 1.57, 0)
joint[9] = self.clip(joint[9], 1.57, 0)
joint[10] = self.clip(joint[10], 1.57, 0)
joint[11] = self.clip(joint[11], 0.349, -0.349)
joint[12] = self.clip(joint[12], 1.57, 0)
joint[13] = self.clip(joint[13], 1.57, 0)
joint[14] = self.clip(joint[14], 1.57, 0)
joint[15] = self.clip(joint[15], 0.349, -0.349)
joint[16] = self.clip(joint[16], 1.57, 0)
joint[17] = self.clip(joint[17], 1.57, 0)
joint[18] = self.clip(joint[18], 1.57, 0)
joint[19] = self.clip(joint[19], 1.047, -1.047)
joint[20] = self.clip(joint[20], 1.222, 0)
joint[21] = self.clip(joint[21], 0.209, -0.209)
joint[22] = self.clip(joint[22], 0.524, -0.524)
joint[23] = self.clip(joint[23], 1.57, 0)
return joint
def clip(self, x, maxv=None, minv=None):
if maxv is not None and x > maxv:
x = maxv
if minv is not None and x < minv:
x = minv
return x
FileObj.write("Arm Joints are in the order of: \n")
for i in range(len(arm_keyID)):
FileObj.write(str(arm_keyID[i]) + "\n")
FileObj.write("************************* \n\n")
FileObj2.write("# Joint Position Dictionaries for Arm and Hand:\n\n")
flag = 1
new_hand = 'y'
new_arm = 'y'
while flag == 1:
input_val = raw_input("Specify Current Joint State Name: ")
all_joints_state = hand_commander.get_joints_position()
hand_joints_state = {k: v for k, v in all_joints_state.items() if k.startswith("rh_")}
arm_joints_state = {k: v for k, v in all_joints_state.items() if k.startswith("ra_")}
print("<" + input_val + ">\n")
print("Hand joints position \n " + str(hand_joints_state) + "\n")
print("Arm joints position \n " + str(arm_joints_state) + "\n")
# Write data to files
# Write to _MAT file:
FileObj.write("<" + input_val + ">\n")
FileObj.write("Hand joints position\n")
# Write Hand joints in _MAT
if new_hand == 'y':
for i in range(len(hand_keyID)):
FileObj.write(str(hand_joints_state[hand_keyID[i]]) + "\n")
class Teleoperation():
def __init__(self):
self.bridge = CvBridge()
self.hand_commander = SrHandCommander(name="right_hand")
# play piano start pos
start_play_pos = {
"rh_THJ1": 20,
"rh_THJ2": 10,
"rh_THJ3": 0,
"rh_THJ4": 0,
"rh_THJ5": 0,
"rh_FFJ1": 45,
"rh_FFJ2": 80,
"rh_FFJ3": 0,
"rh_FFJ4": 0,
"rh_MFJ1": 45,
"rh_MFJ2": 80,
"rh_MFJ3": 0,
"rh_MFJ4": 0,
"rh_RFJ1": 45,
"rh_RFJ2": 80,
"rh_RFJ3": 0,
"rh_RFJ4": 0,
"rh_LFJ1": 45,
"rh_LFJ2": 80,
"rh_LFJ3": 0,
"rh_LFJ4": 0,
"rh_LFJ5": 0,
"rh_WRJ1": -30,
"rh_WRJ2": 0
}
self.hand_commander.move_to_joint_value_target_unsafe(
start_play_pos, 1.5, False, angle_degrees=True)
rospy.sleep(1)
def online_once(self):
while True:
# img_data = rospy.wait_for_message("/camera/depth/image_raw", Image)
img_data = rospy.wait_for_message(
"/camera/aligned_depth_to_color/image_raw", Image)
rospy.loginfo("Got an image ^_^")
try:
img = self.bridge.imgmsg_to_cv2(img_data,
desired_encoding="passthrough")
except CvBridgeError as e:
rospy.logerr(e)
try:
# preproces
img = seg_hand_depth(img, 500, 1000, 10, 100, 4, 4, 250, True,
300)
img = img.astype(np.float32)
img = img / 255. * 2. - 1
print(img.shape)
n = cv2.resize(img, (0, 0), fx=2, fy=2)
n1 = cv2.normalize(n,
n,
alpha=0,
beta=1,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_32F)
cv2.imshow("segmented human hand", n1)
cv2.waitKey(1)
# get the clipped joints
goal = tuple(self.joint_cal(img, isbio=True))
# bug:get_joints_position() return radian joints
hand_pos = self.hand_commander.get_joints_position()
# first finger
hand_pos.update({"rh_FFJ3": goal[3]})
#hand_pos.update({"rh_FFJ2": goal[4]})
# hand_pos.update({"rh_FFJ4": goal[2]})
# middle finger
hand_pos.update({"rh_MFJ3": goal[12]})
#hand_pos.update({"rh_MFJ2": goal[13]})
# ring finger
hand_pos.update({"rh_RFJ3": goal[16]})
#hand_pos.update({"rh_RFJ2": goal[17]})
# little finger
hand_pos.update({"rh_LFJ3": goal[8]})
#hand_pos.update({"rh_LFJ2": goal[9]})
# thumb
#hand_pos.update({"rh_THJ5": goal[19]})
#hand_pos.update({"rh_THJ4": goal[20]})
#hand_pos.update({"rh_THJ3": goal[21]})
#hand_pos.update({"rh_THJ2": goal[22]})
self.hand_commander.move_to_joint_value_target_unsafe(
hand_pos, 0.3, False, angle_degrees=False)
#rospy.sleep(0.5)
# collision check and manipulate
# csl_client = rospy.ServiceProxy('CheckSelfCollision', checkSelfCollision)
# try:
# shadow_pos = csl_client(start, goal)
# if shadow_pos.result:
# rospy.loginfo("Move Done!")
# else:
# rospy.logwarn("Failed to move!")
# except rospy.ServiceException as exc:
# rospy.logwarn("Service did not process request: " + str(exc))
rospy.loginfo("Next one please ---->")
except:
rospy.loginfo("no images")
def joint_cal(self, img, isbio=False):
# start = rospy.Time.now().to_sec()
# run the model
feature = test(model, img)
# network_time = rospy.Time.now().to_sec() - start
# print("network_time is ", network_time)
joint = [0.0, 0.0]
joint += feature.tolist()
if isbio:
joint[5] = 0.3498509706185152
joint[10] = 0.3498509706185152
joint[14] = 0.3498509706185152
joint[18] = 0.3498509706185152
joint[23] = 0.3498509706185152
# joints crop
joint[2] = self.clip(joint[2], 0.349, -0.349)
joint[3] = self.clip(joint[3], 1.57, 0)
joint[4] = self.clip(joint[4], 1.57, 0)
joint[5] = self.clip(joint[5], 1.57, 0)
joint[6] = self.clip(joint[6], 0.785, 0)
joint[7] = self.clip(joint[7], 0.349, -0.349)
joint[8] = self.clip(joint[8], 1.57, 0)
joint[9] = self.clip(joint[9], 1.57, 0)
joint[10] = self.clip(joint[10], 1.57, 0)
joint[11] = self.clip(joint[11], 0.349, -0.349)
joint[12] = self.clip(joint[12], 1.57, 0)
joint[13] = self.clip(joint[13], 1.57, 0)
joint[14] = self.clip(joint[14], 1.57, 0)
joint[15] = self.clip(joint[15], 0.349, -0.349)
joint[16] = self.clip(joint[16], 1.57, 0)
joint[17] = self.clip(joint[17], 1.57, 0)
joint[18] = self.clip(joint[18], 1.57, 0)
joint[19] = self.clip(joint[19], 1.047, -1.047)
joint[20] = self.clip(joint[20], 1.222, 0)
joint[21] = self.clip(joint[21], 0.209, -0.209)
joint[22] = self.clip(joint[22], 0.524, -0.524)
joint[23] = self.clip(joint[23], 1.57, 0)
return joint
def clip(self, x, maxv=None, minv=None):
if maxv is not None and x > maxv:
x = maxv
if minv is not None and x < minv:
x = minv
return x
