From 7d91cfb6b89c37e1b754dbf6300785787eeeaf7c Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 26 Jun 2025 20:53:02 +0800 Subject: [PATCH] [update] support dex3 DexPilot algorithm. --- assets/unitree_hand/unitree_dex3.yml | 40 +++++++------- .../dex_retargeting/optimizer.py | 11 ++-- .../dex_retargeting/retargeting_config.py | 52 ++++++++++--------- teleop/robot_control/hand_retargeting.py | 2 + teleop/robot_control/robot_hand_unitree.py | 11 +--- 5 files changed, 62 insertions(+), 54 deletions(-) diff --git a/assets/unitree_hand/unitree_dex3.yml b/assets/unitree_hand/unitree_dex3.yml index d54677a..dc5ec85 100644 --- a/assets/unitree_hand/unitree_dex3.yml +++ b/assets/unitree_hand/unitree_dex3.yml @@ -1,9 +1,8 @@ left: - type: vector + type: DexPilot # or vector urdf_path: unitree_hand/unitree_dex3_left.urdf # Target refers to the retargeting target, which is the robot hand - # target_joint_names: target_joint_names: [ "left_hand_thumb_0_joint", @@ -14,27 +13,26 @@ left: "left_hand_index_0_joint", "left_hand_index_1_joint", ] - wrist_link_name: None + + # for DexPilot type + wrist_link_name: "base_link" + finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_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, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]] + + # for vector type target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"] target_task_link_names: ["thumb_tip","index_tip","middle_tip"] - target_link_human_indices: [[0, 0, 0], [4, 9, 14]] + target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]] - # Currently, the scaling factor for each finger is individually distinguished in the robot_hand_unitree.py file. - # The Unitree Dex3 has three fingers with the same specifications, so the retarget scaling factors need to be adjusted separately. - # The relevant code is as follows: - # ref_left_value[0] = ref_left_value[0] * 1.15 - # ref_left_value[1] = ref_left_value[1] * 1.05 - # ref_left_value[2] = ref_left_value[2] * 0.95 - # ref_right_value[0] = ref_right_value[0] * 1.15 - # ref_right_value[1] = ref_right_value[1] * 1.05 - # ref_right_value[2] = ref_right_value[2] * 0.95 + # 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.0 - # 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: unitree_hand/unitree_dex3_right.urdf # Target refers to the retargeting target, which is the robot hand @@ -48,12 +46,18 @@ right: "right_hand_middle_0_joint", "right_hand_middle_1_joint", ] - wrist_link_name: None + # for DexPilot type + wrist_link_name: "base_link" + finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip"] + target_link_human_indices_dexpilot: [[ 9, 14, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]] + + # for vector type target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"] target_task_link_names: ["thumb_tip", "index_tip", "middle_tip"] - target_link_human_indices: [[0, 0, 0], [4, 9, 14]] + target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]] + # 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.0 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 diff --git a/teleop/robot_control/dex_retargeting/optimizer.py b/teleop/robot_control/dex_retargeting/optimizer.py index 9288523..ed891fe 100644 --- a/teleop/robot_control/dex_retargeting/optimizer.py +++ b/teleop/robot_control/dex_retargeting/optimizer.py @@ -324,7 +324,12 @@ class DexPilotOptimizer(Optimizer): origin_link_index, task_link_index = self.generate_link_indices(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) + logical_indices = np.stack([origin_link_index, task_link_index], axis=0) + target_link_human_indices = np.where( + logical_indices > 0, + logical_indices * 5 - 1, + 0 + ).astype(int) link_names = [wrist_link_name] + finger_tip_link_names target_origin_link_names = [link_names[index] for index in origin_link_index] target_task_link_names = [link_names[index] for index in task_link_index] @@ -370,13 +375,13 @@ class DexPilotOptimizer(Optimizer): origin_link_index = [] task_link_index = [] - # Add indices for connections between fingers + # S1：Add indices for connections between fingers for i in range(1, num_fingers): for j in range(i + 1, num_fingers + 1): origin_link_index.append(j) task_link_index.append(i) - # Add indices for connections to the base (0) + # S2：Add indices for connections to the base (0) for i in range(1, num_fingers + 1): origin_link_index.append(0) task_link_index.append(i) diff --git a/teleop/robot_control/dex_retargeting/retargeting_config.py b/teleop/robot_control/dex_retargeting/retargeting_config.py index d2378c0..f1e8cd8 100644 --- a/teleop/robot_control/dex_retargeting/retargeting_config.py +++ b/teleop/robot_control/dex_retargeting/retargeting_config.py @@ -19,32 +19,34 @@ from .yourdfpy import DUMMY_JOINT_NAMES class RetargetingConfig: type: str urdf_path: str + target_joint_names: Optional[List[str]] = None # Whether to add free joint to the root of the robot. Free joint enable the robot hand move freely in the space add_dummy_free_joint: bool = False - # 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: Optional[np.ndarray] = None - + # DexPilot retargeting related # The link on the robot hand which corresponding to the wrist of human hand wrist_link_name: Optional[str] = None + # DexPilot retargeting link names + finger_tip_link_names: Optional[List[str]] = None + target_link_human_indices_dexpilot: Optional[np.ndarray] = None # Position retargeting link names target_link_names: Optional[List[str]] = None + target_link_human_indices_position: Optional[np.ndarray] = None # Vector retargeting link names - target_joint_names: Optional[List[str]] = None target_origin_link_names: Optional[List[str]] = None target_task_link_names: Optional[List[str]] = None - - # DexPilot retargeting link names - finger_tip_link_names: Optional[List[str]] = None + target_link_human_indices_vector: Optional[np.ndarray] = None # 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: float = 1.0 + # Low pass filter + low_pass_alpha: float = 0.1 + # Optimization parameters normal_delta: float = 4e-3 huber_delta: float = 2e-2 @@ -59,9 +61,6 @@ class RetargetingConfig: # Mimic joint tag ignore_mimic_joint: bool = False - # Low pass filter - low_pass_alpha: float = 0.1 - _TYPE = ["vector", "position", "dexpilot"] _DEFAULT_URDF_DIR = "./" @@ -78,24 +77,24 @@ class RetargetingConfig: raise ValueError(f"Vector retargeting requires: target_origin_link_names + target_task_link_names") if len(self.target_task_link_names) != len(self.target_origin_link_names): raise ValueError(f"Vector retargeting origin and task links dim mismatch") - if self.target_link_human_indices.shape != (2, len(self.target_origin_link_names)): + if self.target_link_human_indices_vector.shape != (2, len(self.target_origin_link_names)): raise ValueError(f"Vector retargeting link names and link indices dim mismatch") - if self.target_link_human_indices is None: - raise ValueError(f"Vector retargeting requires: target_link_human_indices") + if self.target_link_human_indices_vector is None: + raise ValueError(f"Vector retargeting requires: target_link_human_indices_vector") elif self.type == "position": if self.target_link_names is None: raise ValueError(f"Position retargeting requires: target_link_names") - self.target_link_human_indices = self.target_link_human_indices.squeeze() - if self.target_link_human_indices.shape != (len(self.target_link_names),): + self.target_link_human_indices_position = self.target_link_human_indices_position.squeeze() + if self.target_link_human_indices_position.shape != (len(self.target_link_names),): raise ValueError(f"Position retargeting link names and link indices dim mismatch") - if self.target_link_human_indices is None: - raise ValueError(f"Position retargeting requires: target_link_human_indices") + if self.target_link_human_indices_position is None: + raise ValueError(f"Position retargeting requires: target_link_human_indices_position") elif self.type == "dexpilot": if self.finger_tip_link_names is None or self.wrist_link_name is None: raise ValueError(f"Position retargeting requires: finger_tip_link_names + wrist_link_name") - if self.target_link_human_indices is not None: + if self.target_link_human_indices_dexpilot is not None: print( "\033[33m", "Target link human indices is provided in the DexPilot retargeting config, which is uncommon.\n" @@ -132,8 +131,13 @@ class RetargetingConfig: @classmethod def from_dict(cls, cfg: Dict[str, Any], override: Optional[Dict] = None): - if "target_link_human_indices" in cfg: - cfg["target_link_human_indices"] = np.array(cfg["target_link_human_indices"]) + if "target_link_human_indices_position" in cfg: + cfg["target_link_human_indices_position"] = np.array(cfg["target_link_human_indices_position"]) + if "target_link_human_indices_vector" in cfg: + cfg["target_link_human_indices_vector"] = np.array(cfg["target_link_human_indices_vector"]) + if "target_link_human_indices_dexpilot" in cfg: + cfg["target_link_human_indices_dexpilot"] = np.array(cfg["target_link_human_indices_dexpilot"]) + if override is not None: for key, value in override.items(): cfg[key] = value @@ -170,7 +174,7 @@ class RetargetingConfig: robot, joint_names, target_link_names=self.target_link_names, - target_link_human_indices=self.target_link_human_indices, + target_link_human_indices=self.target_link_human_indices_position, norm_delta=self.normal_delta, huber_delta=self.huber_delta, ) @@ -180,7 +184,7 @@ class RetargetingConfig: joint_names, target_origin_link_names=self.target_origin_link_names, target_task_link_names=self.target_task_link_names, - target_link_human_indices=self.target_link_human_indices, + target_link_human_indices=self.target_link_human_indices_vector, scaling=self.scaling_factor, norm_delta=self.normal_delta, huber_delta=self.huber_delta, @@ -191,7 +195,7 @@ class RetargetingConfig: joint_names, finger_tip_link_names=self.finger_tip_link_names, wrist_link_name=self.wrist_link_name, - target_link_human_indices=self.target_link_human_indices, + target_link_human_indices=self.target_link_human_indices_dexpilot, scaling=self.scaling_factor, project_dist=self.project_dist, escape_dist=self.escape_dist, diff --git a/teleop/robot_control/hand_retargeting.py b/teleop/robot_control/hand_retargeting.py index 77c6a18..1c67945 100644 --- a/teleop/robot_control/hand_retargeting.py +++ b/teleop/robot_control/hand_retargeting.py @@ -38,6 +38,8 @@ class HandRetargeting: self.left_retargeting_joint_names = self.left_retargeting.joint_names self.right_retargeting_joint_names = self.right_retargeting.joint_names + self.left_indices = self.left_retargeting.optimizer.target_link_human_indices + self.right_indices = self.right_retargeting.optimizer.target_link_human_indices if hand_type == HandType.UNITREE_DEX3 or hand_type == HandType.UNITREE_DEX3_Unit_Test: # In section "Sort by message structure" of https://support.unitree.com/home/en/G1_developer/dexterous_hand diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 320dc2f..fb04cf1 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -24,7 +24,6 @@ import logging_mp logger_mp = logging_mp.get_logger(__name__) -unitree_tip_indices = [4, 9, 14] # [thumb, index, middle] in OpenXR Dex3_Num_Motors = 7 kTopicDex3LeftCommand = "rt/dex3/left/cmd" kTopicDex3RightCommand = "rt/dex3/right/cmd" @@ -183,14 +182,8 @@ class Dex3_1_Controller: state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) 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[unitree_tip_indices] - ref_right_value = right_hand_data[unitree_tip_indices] - ref_left_value[0] = ref_left_value[0] * 1.15 - ref_left_value[1] = ref_left_value[1] * 1.05 - ref_left_value[2] = ref_left_value[2] * 0.95 - ref_right_value[0] = ref_right_value[0] * 1.15 - ref_right_value[1] = ref_right_value[1] * 1.05 - ref_right_value[2] = ref_right_value[2] * 0.95 + 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.right_dex_retargeting_to_hardware] right_q_target = self.hand_retargeting.right_retargeting.retarget(ref_right_value)[self.hand_retargeting.right_dex_retargeting_to_hardware]