[update] support inspire1 DexPilot algorithm.

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" ]
-  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

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]