Add Fii mobile manipulator for teleoperation and locomanipulation SDG

docs/source/overview/imitation-learning/teleop_imitation.rst

@@ -722,6 +722,10 @@ To generate the locomanipulation dataset, use the following command:
        --num_runs 1 \
        --lift_step 60 \
        --navigate_step 130 \
+       --grasp_left_hand_relative_to object \
+       --grasp_right_hand_relative_to base \
+       --lift_left_hand_relative_to base \
+       --lift_right_hand_relative_to object \
        --enable_pinocchio \
        --output_file ./datasets/generated_dataset_g1_locomanipulation_sdg.hdf5 \
        --enable_cameras

scripts/imitation_learning/locomanipulation_sdg/generate_data.py

@@ -37,7 +37,38 @@
 parser.add_argument("--demo", type=str, default=None, help="The demo in the input dataset to use.")
 parser.add_argument("--num_runs", type=int, default=1, help="The number of trajectories to generate.")
 parser.add_argument(
-    "--draw_visualization", type=bool, default=False, help="Draw the occupancy map and path planning visualization."
+    "--draw_visualization",
+    action="store_true",
+    default=False,
+    help="Draw the occupancy map and path planning visualization.",
+)
+parser.add_argument(
+    "--grasp_left_hand_relative_to",
+    type=str,
+    default="object",
+    choices=["object", "base"],
+    help="During grasp phase, whether left hand moves relative to 'object' or 'base'.",
+)
+parser.add_argument(
+    "--grasp_right_hand_relative_to",
+    type=str,
+    default="object",
+    choices=["object", "base"],
+    help="During grasp phase, whether right hand moves relative to 'object' or 'base'.",
+)
+parser.add_argument(
+    "--lift_left_hand_relative_to",
+    type=str,
+    default="base",
+    choices=["object", "base"],
+    help="During lift phase, whether left hand moves relative to 'object' or 'base'.",
+)
+parser.add_argument(
+    "--lift_right_hand_relative_to",
+    type=str,
+    default="base",
+    choices=["object", "base"],
+    help="During lift phase, whether right hand moves relative to 'object' or 'base'.",
 )
 parser.add_argument(
     "--angular_gain",
@@ -86,6 +117,12 @@
     default=0.5,
     help="An offset distance added to the destination to allow a buffer zone for reliably approaching the goal.",
 )
+parser.add_argument(
+    "--occupancy_map_buffer",
+    type=float,
+    default=0.15,
+    help="The buffer distance in meters to add to the occupancy map before path planning to ensure safe navigation.",
+)
 parser.add_argument(
     "--randomize_placement",
     type=bool,
@@ -249,6 +286,7 @@ def setup_navigation_scene(
     env: LocomanipulationSDGEnv,
     input_episode_data: EpisodeData,
     approach_distance: float,
+    occupancy_map_buffer: float = 0.15,
     randomize_placement: bool = True,
 ) -> tuple[OccupancyMap, ParameterizedPath, RelativePose, RelativePose]:
     """Set up the navigation scene with occupancy map and path planning.
@@ -257,6 +295,7 @@ def setup_navigation_scene(
         env: The locomanipulation SDG environment
         input_episode_data: Input episode data
         approach_distance: Buffer distance from final goal
+        occupancy_map_buffer: Buffer distance to add to occupancy map for path planning
         randomize_placement: Whether to randomize fixture placement
 
     Returns:
@@ -287,7 +326,7 @@ def setup_navigation_scene(
 
     # Plan navigation path
     base_path = plan_path(
-        start=env.get_base(), end=base_goal_approach, occupancy_map=occupancy_map.buffered_meters(0.15)
+        start=env.get_base(), end=base_goal_approach, occupancy_map=occupancy_map.buffered_meters(occupancy_map_buffer)
     )
     base_path_helper = ParameterizedPath(base_path)
 
@@ -300,6 +339,8 @@ def handle_grasp_state(
     recording_step: int,
     lift_step: int,
     output_data: LocomanipulationSDGOutputData,
+    left_hand_relative_to: str = "object",
+    right_hand_relative_to: str = "object",
 ) -> tuple[int, LocomanipulationSDGDataGenerationState]:
     """Handle the GRASP_OBJECT state logic.
 
@@ -309,6 +350,8 @@ def handle_grasp_state(
         recording_step: Current recording step
         lift_step: Step to transition to lift phase
         output_data: Output data to populate
+        left_hand_relative_to: Whether left hand moves relative to 'object' or 'base'
+        right_hand_relative_to: Whether right hand moves relative to 'object' or 'base'
 
     Returns:
         Tuple of (next_recording_step, next_state)
@@ -320,18 +363,30 @@ def handle_grasp_state(
     output_data.recording_step = recording_step
     output_data.base_velocity_target = torch.tensor([0.0, 0.0, 0.0])
 
-    # Transform hand poses relative to object
-    output_data.left_hand_pose_target = transform_relative_pose(
-        recording_item.left_hand_pose_target, recording_item.object_pose, env.get_object().get_pose()
-    )[0]
-    output_data.right_hand_pose_target = transform_relative_pose(
-        recording_item.right_hand_pose_target, recording_item.base_pose, env.get_base().get_pose()
-    )[0]
+    # Transform left hand pose
+    if left_hand_relative_to == "object":
+        output_data.left_hand_pose_target = transform_relative_pose(
+            recording_item.left_hand_pose_target, recording_item.object_pose, env.get_object().get_pose()
+        )[0]
+    else:  # relative to base
+        output_data.left_hand_pose_target = transform_relative_pose(
+            recording_item.left_hand_pose_target, recording_item.base_pose, env.get_base().get_pose()
+        )[0]
+
+    # Transform right hand pose
+    if right_hand_relative_to == "object":
+        output_data.right_hand_pose_target = transform_relative_pose(
+            recording_item.right_hand_pose_target, recording_item.object_pose, env.get_object().get_pose()
+        )[0]
+    else:  # relative to base
+        output_data.right_hand_pose_target = transform_relative_pose(
+            recording_item.right_hand_pose_target, recording_item.base_pose, env.get_base().get_pose()
+        )[0]
+
     output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target
     output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target
 
     # Update state
-
     next_recording_step = recording_step + 1
     next_state = (
         LocomanipulationSDGDataGenerationState.LIFT_OBJECT
@@ -348,6 +403,8 @@ def handle_lift_state(
     recording_step: int,
     navigate_step: int,
     output_data: LocomanipulationSDGOutputData,
+    left_hand_relative_to: str = "base",
+    right_hand_relative_to: str = "base",
 ) -> tuple[int, LocomanipulationSDGDataGenerationState]:
     """Handle the LIFT_OBJECT state logic.
 
@@ -357,6 +414,8 @@ def handle_lift_state(
         recording_step: Current recording step
         navigate_step: Step to transition to navigation phase
         output_data: Output data to populate
+        left_hand_relative_to: Whether left hand moves relative to 'object' or 'base'
+        right_hand_relative_to: Whether right hand moves relative to 'object' or 'base'
 
     Returns:
         Tuple of (next_recording_step, next_state)
@@ -368,13 +427,26 @@ def handle_lift_state(
     output_data.recording_step = recording_step
     output_data.base_velocity_target = torch.tensor([0.0, 0.0, 0.0])
 
-    # Transform hand poses relative to base
-    output_data.left_hand_pose_target = transform_relative_pose(
-        recording_item.left_hand_pose_target, recording_item.base_pose, env.get_base().get_pose()
-    )[0]
-    output_data.right_hand_pose_target = transform_relative_pose(
-        recording_item.right_hand_pose_target, recording_item.object_pose, env.get_object().get_pose()
-    )[0]
+    # Transform left hand pose
+    if left_hand_relative_to == "object":
+        output_data.left_hand_pose_target = transform_relative_pose(
+            recording_item.left_hand_pose_target, recording_item.object_pose, env.get_object().get_pose()
+        )[0]
+    else:  # relative to base
+        output_data.left_hand_pose_target = transform_relative_pose(
+            recording_item.left_hand_pose_target, recording_item.base_pose, env.get_base().get_pose()
+        )[0]
+
+    # Transform right hand pose
+    if right_hand_relative_to == "object":
+        output_data.right_hand_pose_target = transform_relative_pose(
+            recording_item.right_hand_pose_target, recording_item.object_pose, env.get_object().get_pose()
+        )[0]
+    else:  # relative to base
+        output_data.right_hand_pose_target = transform_relative_pose(
+            recording_item.right_hand_pose_target, recording_item.base_pose, env.get_base().get_pose()
+        )[0]
+
     output_data.left_hand_joint_positions_target = recording_item.left_hand_joint_positions_target
     output_data.right_hand_joint_positions_target = recording_item.right_hand_joint_positions_target
 
@@ -609,7 +681,12 @@ def replay(
     following_offset: float = 0.6,
     angle_threshold: float = 0.2,
     approach_distance: float = 1.0,
+    occupancy_map_buffer: float = 0.15,
     randomize_placement: bool = True,
+    grasp_left_hand_relative_to: str = "object",
+    grasp_right_hand_relative_to: str = "object",
+    lift_left_hand_relative_to: str = "base",
+    lift_right_hand_relative_to: str = "base",
 ) -> None:
     """Replay a locomanipulation SDG episode with state machine control.
 
@@ -633,7 +710,12 @@ def replay(
         following_offset: Look-ahead distance for path following (m)
         angle_threshold: Angular threshold for orientation control (rad)
         approach_distance: Buffer distance from final goal (m)
+        occupancy_map_buffer: Buffer distance to add to occupancy map for path planning (m)
         randomize_placement: Whether to randomize obstacle placement
+        grasp_left_hand_relative_to: During grasp, whether left hand moves relative to 'object' or 'base'
+        grasp_right_hand_relative_to: During grasp, whether right hand moves relative to 'object' or 'base'
+        lift_left_hand_relative_to: During lift, whether left hand moves relative to 'object' or 'base'
+        lift_right_hand_relative_to: During lift, whether right hand moves relative to 'object' or 'base'
     """
 
     # Initialize environment to starting state
@@ -652,7 +734,7 @@ def replay(
 
     # Set up navigation scene and path planning
     occupancy_map, base_path_helper, base_goal, base_goal_approach = setup_navigation_scene(
-        env, input_episode_data, approach_distance, randomize_placement
+        env, input_episode_data, approach_distance, occupancy_map_buffer, randomize_placement
     )
 
     # Visualize occupancy map and path if requested
@@ -678,12 +760,24 @@ def replay(
         # Execute state-specific logic using helper functions
         if current_state == LocomanipulationSDGDataGenerationState.GRASP_OBJECT:
             recording_step, current_state = handle_grasp_state(
-                env, input_episode_data, recording_step, lift_step, output_data
+                env,
+                input_episode_data,
+                recording_step,
+                lift_step,
+                output_data,
+                grasp_left_hand_relative_to,
+                grasp_right_hand_relative_to,
             )
 
         elif current_state == LocomanipulationSDGDataGenerationState.LIFT_OBJECT:
             recording_step, current_state = handle_lift_state(
-                env, input_episode_data, recording_step, navigate_step, output_data
+                env,
+                input_episode_data,
+                recording_step,
+                navigate_step,
+                output_data,
+                lift_left_hand_relative_to,
+                lift_right_hand_relative_to,
             )
 
         elif current_state == LocomanipulationSDGDataGenerationState.NAVIGATE:
@@ -765,7 +859,12 @@ def replay(
                 following_offset=args_cli.following_offset,
                 angle_threshold=args_cli.angle_threshold,
                 approach_distance=args_cli.approach_distance,
+                occupancy_map_buffer=args_cli.occupancy_map_buffer,
                 randomize_placement=args_cli.randomize_placement,
+                grasp_left_hand_relative_to=args_cli.grasp_left_hand_relative_to,
+                grasp_right_hand_relative_to=args_cli.grasp_right_hand_relative_to,
+                lift_left_hand_relative_to=args_cli.lift_left_hand_relative_to,
+                lift_right_hand_relative_to=args_cli.lift_right_hand_relative_to,
             )
 
         env.reset()  # FIXME: hack to handle missing final recording

source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py

@@ -4,6 +4,7 @@
 # SPDX-License-Identifier: BSD-3-Clause
 """Retargeters for mapping input device data to robot commands."""
 
+from .humanoid.fii.fii_retargeter import FiiRetargeter, FiiRetargeterCfg
 from .humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter, GR1T2RetargeterCfg
 from .humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeter, G1LowerBodyStandingRetargeterCfg
 from .humanoid.unitree.g1_motion_controller_locomotion import (

source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py

@@ -0,0 +1,107 @@
+# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+import numpy as np
+import torch
+from dataclasses import dataclass
+
+import isaaclab.utils.math as PoseUtils
+from isaaclab.devices import OpenXRDevice
+from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg
+
+
+class FiiRetargeter(RetargeterBase):
+
+    def __init__(self, cfg: "FiiRetargeterCfg"):
+        """Initialize the retargeter."""
+        self.cfg = cfg
+        self._sim_device = cfg.sim_device
+
+    def retarget(self, data: dict) -> torch.Tensor:
+
+        base_vel = torch.tensor([0.0, 0.0, 0.0], dtype=torch.float32, device=self._sim_device)
+        base_height = torch.tensor([0.7], dtype=torch.float32, device=self._sim_device)
+
+        left_eef_pose = torch.tensor(
+            [-0.3, 0.3, 0.72648, 1.0, 0.0, 0.0, 0.0], dtype=torch.float32, device=self._sim_device
+        )
+        right_eef_pose = torch.tensor(
+            [-0.3, 0.3, 0.72648, 1.0, 0.0, 0.0, 0.0], dtype=torch.float32, device=self._sim_device
+        )
+
+        left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT]
+        right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT]
+        left_wrist = left_hand_poses.get("wrist")
+        right_wrist = right_hand_poses.get("wrist")
+
+        left_eef_pose_np = self._retarget_abs(left_wrist, is_left=True)
+        right_eef_pose_np = self._retarget_abs(right_wrist, is_left=False)
+
+        if left_wrist is not None:
+            left_eef_pose = torch.from_numpy(left_eef_pose_np).to(dtype=torch.float32, device=self._sim_device)
+        if right_wrist is not None:
+            right_eef_pose = torch.from_numpy(right_eef_pose_np).to(dtype=torch.float32, device=self._sim_device)
+
+        gripper_value_left = self._hand_data_to_gripper_values(data[OpenXRDevice.TrackingTarget.HAND_LEFT])
+        gripper_value_right = self._hand_data_to_gripper_values(data[OpenXRDevice.TrackingTarget.HAND_RIGHT])
+
+        return torch.cat(
+            [left_eef_pose, right_eef_pose, gripper_value_left, gripper_value_right, base_vel, base_height]
+        )
+
+    def _hand_data_to_gripper_values(self, hand_data):
+        thumb_tip = hand_data["thumb_tip"]
+        index_tip = hand_data["index_tip"]
+
+        distance = np.linalg.norm(thumb_tip[:3] - index_tip[:3])
+
+        finger_dist_closed = 0.00
+        finger_dist_open = 0.06
+
+        gripper_value_closed = 0.06
+        gripper_value_open = 0.00
+
+        t = np.clip((distance - finger_dist_closed) / (finger_dist_open - finger_dist_closed), 0, 1)
+        # t = 1 -> open
+        # t = 0 -> closed
+        gripper_joint_value = (1.0 - t) * gripper_value_closed + t * gripper_value_open
+
+        return torch.tensor([gripper_joint_value, gripper_joint_value], dtype=torch.float32, device=self._sim_device)
+
+    def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray:
+        """Handle absolute pose retargeting.
+
+        Args:
+            wrist: Wrist pose data from OpenXR.
+            is_left: True for the left hand, False for the right hand.
+
+        Returns:
+            Retargeted wrist pose in USD control frame.
+        """
+        wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32)
+        wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32)
+
+        openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat))
+
+        if is_left:
+            # Corresponds to a rotation of (0, 90, 90) in euler angles (x,y,z)
+            combined_quat = torch.tensor([0, 0.7071, 0, 0.7071], dtype=torch.float32)
+        else:
+            # Corresponds to a rotation of (0, -90, -90) in euler angles (x,y,z)
+            combined_quat = torch.tensor([0, -0.7071, 0, 0.7071], dtype=torch.float32)
+
+        offset = torch.tensor([0.0, 0.25, -0.15])
+        transform_pose = PoseUtils.make_pose(offset, PoseUtils.matrix_from_quat(combined_quat))
+
+        result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose)
+        pos, rot_mat = PoseUtils.unmake_pose(result_pose)
+        quat = PoseUtils.quat_from_matrix(rot_mat)
+
+        return np.concatenate([pos.numpy(), quat.numpy()])
+
+
+@dataclass
+class FiiRetargeterCfg(RetargeterCfg):
+    retargeter_type: type[RetargeterBase] = FiiRetargeter

source/isaaclab_assets/isaaclab_assets/robots/__init__.py

@@ -15,6 +15,7 @@
 from .cart_double_pendulum import *
 from .cartpole import *
 from .cassie import *
+from .fiibot import *
 from .fourier import *
 from .franka import *
 from .galbot import *