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