diff --git a/assets b/assets index f6fb7db..efd7710 160000 --- a/assets +++ b/assets @@ -1 +1 @@ -Subproject commit f6fb7db8498a9ea9dada663c1d35bc957dfeede8 +Subproject commit efd7710730dbc7b08a9c370c535e9663a27c4bd2 diff --git a/dex_retargeting/configs/teleop/panda_gripper_both.yml b/dex_retargeting/configs/teleop/panda_gripper_both.yml new file mode 100644 index 0000000..23d5e98 --- /dev/null +++ b/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 diff --git a/dex_retargeting/constants.py b/dex_retargeting/constants.py index 27f7e87..f969629 100644 --- a/dex_retargeting/constants.py +++ b/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()) diff --git a/example/profiling/profile_online_retargeting.py b/example/profiling/profile_online_retargeting.py index a81d73a..48543c7 100644 --- a/example/profiling/profile_online_retargeting.py +++ b/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) diff --git a/example/vector_retargeting/README.md b/example/vector_retargeting/README.md index d8c7616..987557b 100644 --- a/example/vector_retargeting/README.md +++ b/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 diff --git a/example/vector_retargeting/capture_webcam.py b/example/vector_retargeting/capture_webcam.py index 9b42e65..e3de15d 100644 --- a/example/vector_retargeting/capture_webcam.py +++ b/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() diff --git a/tests/test_optimizer.py b/tests/test_optimizer.py index 005c239..c19e588 100644 --- a/tests/test_optimizer.py +++ b/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()