Merge pull request #29 from dexsuite/panda_gripper

yzqin · web-flow · commit 5c24b3a4f3df · 2024-07-30T10:52:11.000+08:00
[add] support DexPilot teleop for panda gripper
diff --git a/dex_retargeting/configs/teleop/panda_gripper_dexpilot.yml b/dex_retargeting/configs/teleop/panda_gripper_dexpilot.yml
@@ -0,0 +1,12 @@
+retargeting:
+  type: DexPilot
+  urdf_path: panda_gripper/panda_gripper_glb.urdf 
+
+  # Target refers to the retargeting target, which is the robot hand
+  target_joint_names: [ "panda_finger_joint1" ]
+  wrist_link_name: "panda_hand"
+  finger_tip_link_names: [ "panda_leftfinger", "panda_rightfinger" ]
+  scaling_factor: 1.5
+
+  # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+  low_pass_alpha: 0.2
diff --git a/dex_retargeting/constants.py b/dex_retargeting/constants.py
@@ -48,11 +48,11 @@ def get_default_config_path(
 
     robot_name_str = ROBOT_NAME_MAP[robot_name]
     hand_type_str = hand_type.name
-    if "gripper" in robot_name_str:
-        config_name = f"{robot_name_str}.yml"  # For gripper robots, only use gripper config file.
+    if "gripper" in robot_name_str:  # For gripper robots, only use gripper config file.
         if retargeting_type == RetargetingType.dexpilot:
-            print(f"DexPilot retargeting is not supported for gripper.")
-            return None
+            config_name = f"{robot_name_str}_dexpilot.yml"
+        else:
+            config_name = f"{robot_name_str}.yml"
     else:
         if retargeting_type == RetargetingType.dexpilot:
             config_name = f"{robot_name_str}_{hand_type_str}_dexpilot.yml"
diff --git a/dex_retargeting/optimizer.py b/dex_retargeting/optimizer.py
@@ -314,17 +314,21 @@ def __init__(
         eta2=3e-2,
         scaling=1.0,
     ):
-        if len(finger_tip_link_names) < 4 or len(finger_tip_link_names) > 5:
-            raise ValueError(f"DexPilot optimizer can only be applied to hands with four or five fingers")
-        is_four_finger = len(finger_tip_link_names) == 4
-        if is_four_finger:
+        # if len(finger_tip_link_names) < 4 or len(finger_tip_link_names) > 5:
+        #     raise ValueError(f"DexPilot optimizer can only be applied to hands with four or five fingers")
+        self.num_fingers = len(finger_tip_link_names)
+
+        if self.num_fingers == 2:  # For gripper
+            origin_link_index = [2, 0, 0]
+            task_link_index = [1, 1, 2]
+        elif self.num_fingers == 4:
             origin_link_index = [2, 3, 4, 3, 4, 4, 0, 0, 0, 0]
             task_link_index = [1, 1, 1, 2, 2, 3, 1, 2, 3, 4]
-            self.num_fingers = 4
-        else:
+        elif self.num_fingers == 5:
             origin_link_index = [2, 3, 4, 5, 3, 4, 5, 4, 5, 5, 0, 0, 0, 0, 0]
             task_link_index = [1, 1, 1, 1, 2, 2, 2, 3, 3, 4, 1, 2, 3, 4, 5]
-            self.num_fingers = 5
+        else:
+            raise NotImplementedError(f"Unsupported number of fingers: {self.num_fingers}")
 
         if target_link_human_indices is None:
             target_link_human_indices = (np.stack([origin_link_index, task_link_index], axis=0) * 4).astype(int)
@@ -359,16 +363,23 @@ def __init__(
         self.opt.set_ftol_abs(1e-6)
 
         # DexPilot cache
-        if is_four_finger:
+        if self.num_fingers == 2:
+            self.projected = np.zeros(1, dtype=bool)
+            self.s2_project_index_origin = np.array([], dtype=int)
+            self.s2_project_index_task = np.array([], dtype=int)
+            self.projected_dist = np.array([eta1] * 1)
+        elif self.num_fingers == 4:
             self.projected = np.zeros(6, dtype=bool)
             self.s2_project_index_origin = np.array([1, 2, 2], dtype=int)
             self.s2_project_index_task = np.array([0, 0, 1], dtype=int)
             self.projected_dist = np.array([eta1] * 3 + [eta2] * 3)
-        else:
+        elif self.num_fingers == 5:
             self.projected = np.zeros(10, dtype=bool)
             self.s2_project_index_origin = np.array([1, 2, 3, 2, 3, 3], dtype=int)
             self.s2_project_index_task = np.array([0, 0, 0, 1, 1, 2], dtype=int)
             self.projected_dist = np.array([eta1] * 4 + [eta2] * 6)
+        else:
+            raise NotImplementedError(f"Unsupported number of fingers: {self.num_fingers}")
 
     def get_objective_function(self, target_vector: np.ndarray, fixed_qpos: np.ndarray, last_qpos: np.ndarray):
         qpos = np.zeros(self.num_joints)
diff --git a/example/profiling/profile_online_retargeting.py b/example/profiling/profile_online_retargeting.py
@@ -61,9 +61,6 @@ def main():
 
     # DexPilot retargeting
     for robot_name in ROBOT_NAMES:
-        if "gripper" in ROBOT_NAME_MAP[robot_name]:
-            print(f"Skip {ROBOT_NAME_MAP[robot_name]} for DexPilot retargeting.")
-            continue
         config_path = get_default_config_path(robot_name, RetargetingType.dexpilot, HandType.right)
         retargeting = RetargetingConfig.load_from_file(config_path).build()
         total_time = profile_retargeting(retargeting, joint_data)
