From 64c01a71eee9d5ff88aaf6f7b2a18ef62de1f380 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 30 Jun 2025 11:32:35 +0800 Subject: [PATCH] [update] support inspire1 DexPilot algorithm. --- assets/inspire_hand/inspire_hand.yml | 63 +++++++++++++++------- teleop/robot_control/robot_hand_inspire.py | 11 ++-- 2 files changed, 48 insertions(+), 26 deletions(-) diff --git a/assets/inspire_hand/inspire_hand.yml b/assets/inspire_hand/inspire_hand.yml index a3aaaf0..8466ce0 100644 --- a/assets/inspire_hand/inspire_hand.yml +++ b/assets/inspire_hand/inspire_hand.yml @@ -1,39 +1,62 @@ left: - type: vector + type: DexPilot # or vector urdf_path: inspire_hand/inspire_hand_left.urdf - wrist_link_name: "L_hand_base_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: ['L_thumb_proximal_yaw_joint', 'L_thumb_proximal_pitch_joint', - 'L_index_proximal_joint', 'L_middle_proximal_joint', - 'L_ring_proximal_joint', 'L_pinky_proximal_joint' ] + target_joint_names: + [ + 'L_thumb_proximal_yaw_joint', + 'L_thumb_proximal_pitch_joint', + 'L_index_proximal_joint', + 'L_middle_proximal_joint', + 'L_ring_proximal_joint', + 'L_pinky_proximal_joint' + ] + + # for DexPilot type + wrist_link_name: "L_hand_base_link" + finger_tip_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] + # If you do not know exactly how it is used, please leave it to None for default. + target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] + + # for vector type target_origin_link_names: [ "L_hand_base_link", "L_hand_base_link", "L_hand_base_link", "L_hand_base_link", "L_hand_base_link"] target_task_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] - scaling_factor: 1.20 - - # Source refers to the retargeting input, which usually corresponds to the human hand - # The joint indices of human hand joint which corresponds to each link in the target_link_names - target_link_human_indices: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] + target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] + # Scaling factor for vector retargeting only + # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 + scaling_factor: 1.20 # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 right: - type: vector + type: DexPilot # or vector urdf_path: inspire_hand/inspire_hand_right.urdf - wrist_link_name: "R_hand_base_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: ['R_thumb_proximal_yaw_joint', 'R_thumb_proximal_pitch_joint', - 'R_index_proximal_joint', 'R_middle_proximal_joint', - 'R_ring_proximal_joint', 'R_pinky_proximal_joint' ] + target_joint_names: + [ + 'R_thumb_proximal_yaw_joint', + 'R_thumb_proximal_pitch_joint', + 'R_index_proximal_joint', + 'R_middle_proximal_joint', + 'R_ring_proximal_joint', + 'R_pinky_proximal_joint' + ] + + # for DexPilot type + wrist_link_name: "R_hand_base_link" + finger_tip_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] + # If you do not know exactly how it is used, please leave it to None for + target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] + target_origin_link_names: [ "R_hand_base_link", "R_hand_base_link", "R_hand_base_link", "R_hand_base_link", "R_hand_base_link"] target_task_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] + target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] + + # Scaling factor for vector retargeting only + # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 scaling_factor: 1.20 - - # Source refers to the retargeting input, which usually corresponds to the human hand - # The joint indices of human hand joint which corresponds to each link in the target_link_names - target_link_human_indices: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 \ No newline at end of file diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 47a1e55..bec0a63 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -13,7 +13,6 @@ from multiprocessing import Process, Array import logging_mp logger_mp = logging_mp.get_logger(__name__) -inspire_tip_indices = [4, 9, 14, 19, 24] Inspire_Num_Motors = 6 kTopicInspireCommand = "rt/inspire/cmd" kTopicInspireState = "rt/inspire/state" @@ -103,16 +102,16 @@ class Inspire_Controller: start_time = time.time() # get dual hand state with left_hand_array.get_lock(): - left_hand_mat = np.array(left_hand_array[:]).reshape(25, 3).copy() + left_hand_data = np.array(left_hand_array[:]).reshape(25, 3).copy() with right_hand_array.get_lock(): - right_hand_mat = np.array(right_hand_array[:]).reshape(25, 3).copy() + right_hand_data = np.array(right_hand_array[:]).reshape(25, 3).copy() # Read left and right q_state from shared arrays state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) - if not np.all(right_hand_mat == 0.0) and not np.all(left_hand_mat[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. - ref_left_value = left_hand_mat[inspire_tip_indices] - ref_right_value = right_hand_mat[inspire_tip_indices] + if not np.all(right_hand_data == 0.0) and not np.all(left_hand_data[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. + ref_left_value = left_hand_data[self.hand_retargeting.left_indices[1,:]] - left_hand_data[self.hand_retargeting.left_indices[0,:]] + ref_right_value = right_hand_data[self.hand_retargeting.right_indices[1,:]] - right_hand_data[self.hand_retargeting.right_indices[0,:]] left_q_target = self.hand_retargeting.left_retargeting.retarget(ref_left_value)[self.hand_retargeting.left_dex_retargeting_to_hardware] right_q_target = self.hand_retargeting.right_retargeting.retarget(ref_right_value)[self.hand_retargeting.right_dex_retargeting_to_hardware]