Merge pull request #25 from dexsuite/panda_gripper

[add] support panda gripper teleop retargeting

dex_retargeting/configs/teleop/panda_gripper_both.yml

@@ -0,0 +1,16 @@
+retargeting:
+  type: vector
+  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" ]
+  target_origin_link_names: [ "panda_leftfinger" ]
+  target_task_link_names: [ "panda_rightfinger" ]
+  scaling_factor: 1.5
+
+  # 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: [ [ 4 ], [ 8 ] ]
+
+  # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+  low_pass_alpha: 0.2

dex_retargeting/constants.py

@@ -9,6 +9,7 @@ class RobotName(enum.Enum):
     leap = enum.auto()
     ability = enum.auto()
     inspire = enum.auto()
+    panda = enum.auto()
 
 
 class RetargetingType(enum.Enum):
@@ -20,6 +21,7 @@ class RetargetingType(enum.Enum):
 class HandType(enum.Enum):
     right = enum.auto()
     left = enum.auto()
+    both = enum.auto()
 
 
 ROBOT_NAME_MAP = {
@@ -29,6 +31,7 @@ class HandType(enum.Enum):
     RobotName.leap: "leap_hand",
     RobotName.ability: "ability_hand",
     RobotName.inspire: "inspire_hand",
+    RobotName.panda: "panda_gripper",
 }
 
 ROBOT_NAMES = list(ROBOT_NAME_MAP.keys())

example/profiling/profile_online_retargeting.py

@@ -48,7 +48,11 @@ def main():
     # Vector retargeting
     print(f"Being retargeting profiling with a trajectory of {data_len} hand poses.")
     for robot_name in ROBOT_NAMES:
-        config_path = get_default_config_path(robot_name, RetargetingType.vector, HandType.right)
+        config_path = get_default_config_path(
+            robot_name,
+            RetargetingType.vector,
+            HandType.right if "gripper" not in ROBOT_NAME_MAP[robot_name] else HandType.both,
+        )
         retargeting = RetargetingConfig.load_from_file(config_path).build()
         total_time = profile_retargeting(retargeting, joint_data)
         print(
@@ -57,6 +61,9 @@ 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)

example/vector_retargeting/README.md

@@ -41,10 +41,10 @@ This command uses the data saved from the previous step to create a rendered vid
 *The following instructions assume that your computer has a webcam connected.*
 
 ```bash
-python3 capture_webcam.py --video-path example/vector_retargeting/data/my_human_hand_video.mp4
+python3 capture_webcam.py --video-path data/my_human_hand_video.mp4
 ```
 
-This command enables you to use your webcam to record a video saved in MP4 format. To stop recording, press `q` on your
+This command enables you to use your webcam to record a video saved in MP4 format. To stop recording, press `Esc` on your
 keyboard.
 
 ### Real-time Visualization of Hand Retargeting via Webcam

example/vector_retargeting/capture_webcam.py

@@ -30,6 +30,7 @@ def main(video_path: str, video_capture_device: Union[str, int] = 0):
         if cv2.waitKey(1) & 0xFF == 27:
             break
 
+    print('Recording finished')
     cap.release()
     writer.release()
     cv2.destroyAllWindows()

tests/test_optimizer.py

@@ -67,6 +67,10 @@ def generate_position_retargeting_data_gt(robot: RobotWrapper, optimizer: Positi
     def test_position_optimizer(self, robot_name, hand_type):
         config_path = get_default_config_path(robot_name, RetargetingType.position, hand_type)
 
+        if not config_path.exists():
+            print(f"Skip the test for {robot_name.name} as the config file does not exist.")
+            return
+
         # Note: The parameters below are adjusted solely for this test
         # The smoothness penalty is deactivated here, meaning no low pass filter and no continuous joint value
         # This is because the test is focused solely on the efficiency of single step optimization
@@ -97,7 +101,7 @@ def test_position_optimizer(self, robot_name, hand_type):
             computed_target_pos = np.array([robot.get_link_pose(i)[:3, 3] for i in optimizer.target_link_indices])
 
             # Position difference
-            error = np.mean(np.linalg.norm(computed_target_pos - random_target_pos, axis=1))
+            error = np.mean(np.linalg.norm(computed_target_pos - random_target_pos, axis=-1))
             errors["pos"].append(error)
 
         tac = time()
@@ -110,6 +114,10 @@ def test_position_optimizer(self, robot_name, hand_type):
     def test_vector_optimizer(self, robot_name, hand_type):
         config_path = get_default_config_path(robot_name, RetargetingType.vector, hand_type)
 
+        if not config_path.exists():
+            print(f"Skip the test for {robot_name.name} as the config file does not exist.")
+            return
+
         # Note: The parameters below are adjusted solely for this test
         # For retargeting from human to robot, their values should remain the default in the retargeting config
         # The smoothness penalty is deactivated here, meaning no low pass filter and no continuous joint value
@@ -146,7 +154,7 @@ def test_vector_optimizer(self, robot_name, hand_type):
             computed_target_vector = computed_task_pos - computed_origin_pos
 
             # Vector difference
-            error = np.mean(np.linalg.norm(computed_target_vector - random_target_vector, axis=1))
+            error = np.mean(np.linalg.norm(computed_target_vector - random_target_vector, axis=-1))
             errors["pos"].append(error)
 
         tac = time()
@@ -159,6 +167,10 @@ def test_vector_optimizer(self, robot_name, hand_type):
     def test_dexpilot_optimizer(self, robot_name, hand_type):
         config_path = get_default_config_path(robot_name, RetargetingType.dexpilot, hand_type)
 
+        if not config_path.exists():
+            print(f"Skip the test for {robot_name.name} as the config file does not exist.")
+            return
+
         # Note: The parameters below are adjusted solely for this test
         # For retargeting from human to robot, their values should remain the default in the retargeting config
         # The smoothness penalty is deactivated here, meaning no low pass filter and no continuous joint value
@@ -194,7 +206,7 @@ def test_dexpilot_optimizer(self, robot_name, hand_type):
             computed_target_vector = computed_task_pos - computed_origin_pos
 
             # Vector difference
-            error = np.mean(np.linalg.norm(computed_target_vector - random_target_vector, axis=1))
+            error = np.mean(np.linalg.norm(computed_target_vector - random_target_vector, axis=-1))
             errors["pos"].append(error)
 
         tac = time()