From b615da94985bbb8f07975ecff4a24a9492f02123 Mon Sep 17 00:00:00 2001 From: Dingry Date: Mon, 29 Jul 2024 21:02:37 +0800 Subject: [PATCH] [add] support DexPilot teleop for panda gripper --- .../configs/teleop/panda_gripper_dexpilot.yml | 12 ++++++++ dex_retargeting/constants.py | 8 ++--- dex_retargeting/optimizer.py | 29 +++++++++++++------ .../profiling/profile_online_retargeting.py | 3 -- 4 files changed, 36 insertions(+), 16 deletions(-) create mode 100644 dex_retargeting/configs/teleop/panda_gripper_dexpilot.yml diff --git a/dex_retargeting/configs/teleop/panda_gripper_dexpilot.yml b/dex_retargeting/configs/teleop/panda_gripper_dexpilot.yml new file mode 100644 index 0000000..0bc6c64 --- /dev/null +++ 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 index 532eed4..9983a75 100644 --- 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 index eba648b..fdc606e 100644 --- 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 index e2ce7e9..9ee6bbe 100644 --- 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)