diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml index 9eb19cc11d8..967fe23acd4 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/data/configs/dex-retargeting/fourier_hand_left_dexpilot.yml @@ -24,7 +24,6 @@ retargeting: - L_thumb_proximal_yaw_joint - L_thumb_proximal_pitch_joint - L_thumb_distal_joint - - L_thumb_distal_joint type: DexPilot urdf_path: /tmp/GR1_T2_left_hand.urdf wrist_link_name: l_hand_base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py index c0a7b056e81..8958caf5ede 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1_t2_dex_retargeting_utils.py @@ -74,6 +74,7 @@ def __init__( left_hand_config_filename: str = "fourier_hand_left_dexpilot.yml", left_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/GR1T2_assets/GR1_T2_left_hand.urdf", right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/GR1T2_assets/GR1_T2_right_hand.urdf", + calibrate_scaling_factor: bool = False, ): """Initialize the hand retargeting. @@ -81,6 +82,7 @@ def __init__( hand_joint_names: Names of hand joints in the robot model right_hand_config_filename: Config file for right hand retargeting left_hand_config_filename: Config file for left hand retargeting + calibrate_scaling_factor: If True, calibrate the scaling factor for the robot hands """ data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), "data/")) config_dir = os.path.join(data_dir, "configs/dex-retargeting") @@ -104,6 +106,12 @@ def __init__( self.dof_names = self.left_dof_names + self.right_dof_names self.isaac_lab_hand_joint_names = hand_joint_names + # Hand length measurement + self.hand_length = [] + self.max_measurements = 200 + self.scaling_factor_calibrated = False + self.calibrate_scaling_factor = calibrate_scaling_factor + omni.log.info("[GR1T2DexRetargeter] init done.") def _update_yaml_with_urdf_path(self, yaml_path: str, urdf_path: str): @@ -132,25 +140,23 @@ def _update_yaml_with_urdf_path(self, yaml_path: str, urdf_path: str): except Exception as e: omni.log.error(f"Error updating YAML file {yaml_path}: {e}") - def convert_hand_joints(self, hand_poses: dict[str, np.ndarray], operator2mano: np.ndarray) -> np.ndarray: + def convert_hand_joints( + self, joint_positions: np.ndarray, wrist: np.ndarray, operator2mano: np.ndarray + ) -> np.ndarray: """Prepares the hand joints data for retargeting. Args: - hand_poses: Dictionary containing hand pose data with joint positions and rotations + joint_positions: Array of joint positions from OpenXR + wrist: Wrist pose [x, y, z, qw, qx, qy, qz] operator2mano: Transformation matrix to convert from operator to MANO frame Returns: Joint positions with shape (21, 3) """ - joint_position = np.zeros((21, 3)) - hand_joints = list(hand_poses.values()) - for i in range(len(_HAND_JOINTS_INDEX)): - joint = hand_joints[_HAND_JOINTS_INDEX[i]] - joint_position[i] = joint[:3] - + joint_position = joint_positions[_HAND_JOINTS_INDEX] # Convert hand pose to the canonical frame. - joint_position = joint_position - joint_position[0:1, :] - xr_wrist_quat = hand_poses.get("wrist")[3:] + joint_position -= joint_position[0:1, :] + xr_wrist_quat = wrist[3:] # OpenXR hand uses w,x,y,z order for quaternions but scipy uses x,y,z,w order wrist_rot = R.from_quat([xr_wrist_quat[1], xr_wrist_quat[2], xr_wrist_quat[3], xr_wrist_quat[0]]).as_matrix() @@ -176,19 +182,20 @@ def compute_ref_value(self, joint_position: np.ndarray, indices: np.ndarray, ret return ref_value def compute_one_hand( - self, hand_joints: dict[str, np.ndarray], retargeting: RetargetingConfig, operator2mano: np.ndarray + self, joint_positions: np.ndarray, wrist: np.ndarray, retargeting: RetargetingConfig, operator2mano: np.ndarray ) -> np.ndarray: """Computes retargeted joint angles for one hand. Args: - hand_joints: Dictionary containing hand joint data + joint_positions: Array of joint positions from OpenXR + wrist: Wrist pose [x, y, z, qw, qx, qy, qz] retargeting: Retargeting configuration object operator2mano: Transformation matrix from operator to MANO frame Returns: Retargeted joint angles """ - joint_pos = self.convert_hand_joints(hand_joints, operator2mano) + joint_pos = self.convert_hand_joints(joint_positions, wrist, operator2mano) ref_value = self.compute_ref_value( joint_pos, indices=retargeting.optimizer.target_link_human_indices, @@ -224,32 +231,98 @@ def get_hand_indices(self, robot) -> np.ndarray: """ return np.array([robot.dof_names.index(name) for name in self.dof_names], dtype=np.int64) - def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + def compute_left(self, left_joint_positions: np.ndarray, left_wrist: np.ndarray) -> np.ndarray: """Computes retargeted joints for left hand. Args: - left_hand_poses: Dictionary of left hand joint poses + left_joint_positions: Array of left hand joint positions from OpenXR + left_wrist: Left wrist pose [x, y, z, qw, qx, qy, qz] Returns: Retargeted joint angles for left hand """ - if left_hand_poses is not None: - left_hand_q = self.compute_one_hand(left_hand_poses, self._dex_left_hand, _OPERATOR2MANO_LEFT) + if left_joint_positions is not None and left_wrist is not None: + # Collect hand length measurement for scaling factor calculation + self.collect_hand_length_measurement(left_joint_positions, left_wrist) + left_hand_q = self.compute_one_hand( + left_joint_positions, left_wrist, self._dex_left_hand, _OPERATOR2MANO_LEFT + ) else: left_hand_q = np.zeros(len(_LEFT_HAND_JOINT_NAMES)) return left_hand_q - def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + def compute_right(self, right_joint_positions: np.ndarray, right_wrist: np.ndarray) -> np.ndarray: """Computes retargeted joints for right hand. Args: - right_hand_poses: Dictionary of right hand joint poses + right_joint_positions: Array of right hand joint positions from OpenXR + right_wrist: Right wrist pose [x, y, z, qw, qx, qy, qz] Returns: Retargeted joint angles for right hand """ - if right_hand_poses is not None: - right_hand_q = self.compute_one_hand(right_hand_poses, self._dex_right_hand, _OPERATOR2MANO_RIGHT) + if right_joint_positions is not None and right_wrist is not None: + # Collect hand length measurement for scaling factor calculation + self.collect_hand_length_measurement(right_joint_positions, right_wrist) + right_hand_q = self.compute_one_hand( + right_joint_positions, right_wrist, self._dex_right_hand, _OPERATOR2MANO_RIGHT + ) else: right_hand_q = np.zeros(len(_RIGHT_HAND_JOINT_NAMES)) return right_hand_q + + def collect_hand_length_measurement(self, joint_positions: np.ndarray, wrist: np.ndarray): + """Collect hand length measurement for scaling factor calculation. + + Args: + joint_positions: Array of joint positions from OpenXR + wrist: Wrist pose [x, y, z, qw, qx, qy, qz] + """ + if not self.calibrate_scaling_factor or self.scaling_factor_calibrated: + return + if np.linalg.norm(wrist[:3]) == 0 or len(self.hand_length) >= self.max_measurements: + return + # Calculate hand length (distance from wrist to middle finger tip) + palm_dir = (joint_positions[12] - wrist[:3]) / np.linalg.norm(joint_positions[12] - wrist[:3]) + middle_finger_dir = (joint_positions[15] - joint_positions[12]) / np.linalg.norm( + joint_positions[15] - joint_positions[12] + ) + is_hand_open = np.dot(palm_dir, middle_finger_dir) > 0.9 + hand_length = np.linalg.norm(wrist[:3] - joint_positions[15]) + if is_hand_open and 0.12 < hand_length < 0.27: + self.hand_length.append(hand_length) + if len(self.hand_length) >= self.max_measurements: + self.calibrate_scaling_factors() + + def calibrate_scaling_factors(self, min_measurements: int = 50): + """Update scaling factors directly in retargeting optimizers based on the collected hand length measurements. + + Args: + min_measurements: Minimum number of measurements required before updating scaling factors + """ + # Update hand scaling factor directly in optimizers + if len(self.hand_length) >= min_measurements: + hand_length_array = np.array(self.hand_length) + q25 = np.percentile(hand_length_array, 25) + q75 = np.percentile(hand_length_array, 75) + filtered_data = hand_length_array[(hand_length_array >= q25) & (hand_length_array <= q75)] + hand_length = float(np.mean(filtered_data)) + reference_hand_length = 0.19 # average adult hand length (meters) + scaling_factor = reference_hand_length / hand_length + + # Update hand scaling factor + try: + if hasattr(self._dex_left_hand, "optimizer") and hasattr(self._dex_left_hand.optimizer, "scaling"): + self._dex_left_hand.optimizer.scaling *= scaling_factor + if hasattr(self._dex_right_hand, "optimizer") and hasattr(self._dex_right_hand.optimizer, "scaling"): + self._dex_right_hand.optimizer.scaling *= scaling_factor + omni.log.info(f"Successfully updated hand scaling factor to {scaling_factor:.3f}") + else: + omni.log.warn("Optimizer does not have 'scaling' attribute") + except Exception as e: + omni.log.warn(f"Failed to update scaling factor: {e}") + + self.scaling_factor_calibrated = True + omni.log.info( + f"Calibrated scaling factor to {scaling_factor:.3f} (hand length average: {hand_length:.3f}m)" + ) diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py index 4548c0f99cb..b576765862c 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py @@ -26,6 +26,7 @@ class GR1T2RetargeterCfg(RetargeterCfg): enable_visualization: bool = False num_open_xr_hand_joints: int = 100 hand_joint_names: list[str] | None = None # List of robot hand joint names + calibrate_scaling_factor: bool = False class GR1T2Retargeter(RetargeterBase): @@ -46,10 +47,13 @@ def __init__( num_open_xr_hand_joints: Number of joints tracked by OpenXR device: PyTorch device for computations hand_joint_names: List of robot hand joint names + calibrate_scaling_factor: If True, calibrate the scaling factor for the robot hands """ self._hand_joint_names = cfg.hand_joint_names - self._hands_controller = GR1TR2DexRetargeting(self._hand_joint_names) + self._hands_controller = GR1TR2DexRetargeting( + self._hand_joint_names, calibrate_scaling_factor=cfg.calibrate_scaling_factor + ) # Initialize visualization if enabled self._enable_visualization = cfg.enable_visualization @@ -87,22 +91,25 @@ def retarget(self, data: dict) -> torch.Tensor: left_wrist = left_hand_poses.get("wrist") right_wrist = right_hand_poses.get("wrist") + left_joint_positions = np.array([pose[:3] for pose in left_hand_poses.values()]) + right_joint_positions = np.array([pose[:3] for pose in right_hand_poses.values()]) + if self._enable_visualization: joints_position = np.zeros((self._num_open_xr_hand_joints, 3)) - joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()]) - joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()]) + joints_position[::2] = left_joint_positions + joints_position[1::2] = right_joint_positions self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) # Create array of zeros with length matching number of joint names - left_hands_pos = self._hands_controller.compute_left(left_hand_poses) + left_hands_pos = self._hands_controller.compute_left(left_joint_positions, left_wrist) indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] left_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) left_retargeted_hand_joints[indexes] = left_hands_pos left_hand_joints = left_retargeted_hand_joints - right_hands_pos = self._hands_controller.compute_right(right_hand_poses) + right_hands_pos = self._hands_controller.compute_right(right_joint_positions, right_wrist) indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] right_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) right_retargeted_hand_joints[indexes] = right_hands_pos diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index 0a3cb26b4d3..391d25787e2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py @@ -197,6 +197,7 @@ def __post_init__(self): num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.gr1_action.hand_joint_names, + calibrate_scaling_factor=True, ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index b7e1ff3ddec..d93af2e1124 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py @@ -195,6 +195,7 @@ def __post_init__(self): num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.gr1_action.hand_joint_names, + calibrate_scaling_factor=True, ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index 9343db5ffc5..18adb623d3c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -434,6 +434,7 @@ def __post_init__(self): num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS, sim_device=self.sim.device, hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + calibrate_scaling_factor=True, ), ], sim_device=self.sim.device,