diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 14017e65b5d..0bb02e0559d 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/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 diff --git a/scripts/imitation_learning/locomanipulation_sdg/generate_data.py b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py index b80e0992ce4..8775497afa8 100644 --- a/scripts/imitation_learning/locomanipulation_sdg/generate_data.py +++ b/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 diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py index d4e12bd40ed..01261f29357 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py +++ b/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 ( diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py new file mode 100644 index 00000000000..1e431609ece --- /dev/null +++ b/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 diff --git a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py index b7bb245900d..3fc4fd187ce 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py +++ b/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 * diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py b/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py new file mode 100644 index 00000000000..a3dbe49162f --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py @@ -0,0 +1,104 @@ +# 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 + +"""Configuration for the Fiibot humanoid robot. + +The following configuration parameters are available: + +* :obj:`FIIBOT_CFG`: The Fiibot humanoid robot with wheeled base. +""" + +# For local debugging, use the local data directory +from isaaclab_assets import ISAACLAB_ASSETS_DATA_DIR + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +# Toggle between local (for debugging) and Nucleus (for production) +USE_LOCAL_ASSETS = True + +if USE_LOCAL_ASSETS: + FIIBOT_USD_PATH = f"{ISAACLAB_ASSETS_DATA_DIR}/Robots/Fiibot/Fiibot/fiibot.usd" +else: + FIIBOT_USD_PATH = f"{ISAACLAB_NUCLEUS_DIR}/Robots/Fiibot/Fiibot/fiibot.usd" + + +FIIBOT_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=FIIBOT_USD_PATH, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=4, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + rot=(0.7071068, 0.0, 0.0, 0.7071068), + joint_pos={ + "jack_joint": 0.7, + "left_1_joint": 0.0, + "left_2_joint": 0.785398, + "left_3_joint": 0.0, + "left_4_joint": 1.570796, + "left_5_joint": 0.0, + "left_6_joint": -0.785398, + "left_7_joint": 0.0, + "right_1_joint": 0.0, + "right_2_joint": 0.785398, + "right_3_joint": 0.0, + "right_4_joint": 1.570796, + "right_5_joint": 0.0, + "right_6_joint": -0.785398, + "right_7_joint": 0.0, + }, + ), + actuators={ + "arms": ImplicitActuatorCfg( + joint_names_expr=[".*_[1-7]_joint"], + stiffness=None, + damping=None, + ), + "jack": ImplicitActuatorCfg( + joint_names_expr=["jack_joint"], + stiffness=500000.0, + damping=5000.0, + ), + "hands": ImplicitActuatorCfg( + joint_names_expr=[".*_hand_.*"], + stiffness=None, + damping=None, + ), + "wheels": ImplicitActuatorCfg( + joint_names_expr=[".*_wheel_.*", "walk_.*"], + stiffness=None, + damping=None, + ), + }, +) +"""Configuration for the Fiibot humanoid robot with wheeled base. + +The Fiibot is a wheeled humanoid robot featuring: +- Dual 7-DOF arms with grippers for manipulation +- Three-wheeled swerve drive base for omnidirectional movement +- Height-adjustable jack mechanism for variable working height +- Suitable for locomanipulation and pick-and-place tasks +""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py index d73d89b0b06..e0500dad15a 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -15,3 +15,12 @@ }, disable_env_checker=True, ) + +gym.register( + id="Isaac-Fii-Locomanipulation", + entry_point=f"{__name__}.fii_locomanipulation_sdg_env:FiibotLocomanipEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.fii_locomanipulation_sdg_env:FiibotLocomanipEnvCfg", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/fii_locomanipulation_sdg_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/fii_locomanipulation_sdg_env.py new file mode 100644 index 00000000000..d51d36e6754 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/fii_locomanipulation_sdg_env.py @@ -0,0 +1,184 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.envs.common import ViewerCfg +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import CameraCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.datasets import EpisodeData + +from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGInputData +from isaaclab_mimic.locomanipulation_sdg.scene_utils import HasPose, SceneBody, SceneFixture + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_fii_env_cfg import ( + FiibotEnvCfg, + FiibotObservationsCfg, + FiibotSceneCfg, + manip_mdp, +) + +from .locomanipulation_sdg_env import LocomanipulationSDGEnv +from .locomanipulation_sdg_env_cfg import LocomanipulationSDGEnvCfg, LocomanipulationSDGRecorderManagerCfg + + +@configclass +class FiibotLocomanipSceneCfg(FiibotSceneCfg): + + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.85, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + packing_table_2 = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable2", + init_state=AssetBaseCfg.InitialStateCfg( + pos=(-17.5, 10.8, 0.0), + # rot=[0, 0, 0, 1]), + rot=(1.0, 0.0, 0.0, 0.0), + ), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + robot_pov_cam = CameraCfg( + prim_path="/World/envs/env_.*/robot/head_pitch_Link2", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=8.0, clipping_range=(0.1, 20.0)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.9848078, 0.0, -0.1736482, 0.0), convention="world"), + ) + + +@configclass +class FiibotLocomanipObservationsCfg(FiibotObservationsCfg): + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(FiibotObservationsCfg.PolicyCfg): + + robot_pov_cam = ObsTerm( + func=manip_mdp.image, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class FiibotLocomanipEnvCfg(FiibotEnvCfg, LocomanipulationSDGEnvCfg): + """Configuration for the G1 29DoF environment.""" + + viewer: ViewerCfg = ViewerCfg( + eye=(0.0, 5.0, 2.0), lookat=(0.0, 0.0, 0.7), origin_type="asset_body", asset_name="robot", body_name="base_link" + ) + + # Scene settings + scene: FiibotLocomanipSceneCfg = FiibotLocomanipSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + recorders: LocomanipulationSDGRecorderManagerCfg = LocomanipulationSDGRecorderManagerCfg() + observations: FiibotLocomanipObservationsCfg = FiibotLocomanipObservationsCfg() + + def __post_init__(self): + FiibotEnvCfg.__post_init__(self) + + +class FiibotLocomanipEnv(LocomanipulationSDGEnv): + + def __init__(self, cfg: FiibotLocomanipEnvCfg, **kwargs): + super().__init__(cfg) + self.sim.set_camera_view([10.5, 10.5, 10.5], [0.0, 0.0, 0.5]) + self._upper_body_dim = self.action_manager.get_term("upper_body_ik").action_dim + self._lower_body_dim = self.action_manager.get_term("lower_body_ik").action_dim + + def load_input_data(self, episode_data: EpisodeData, step: int) -> LocomanipulationSDGInputData | None: + dataset_action = episode_data.get_action(step) + dataset_state = episode_data.get_state(step) + + if dataset_action is None: + return None + + if dataset_state is None: + return None + + object_pose = dataset_state["rigid_object"]["object"]["root_pose"] + + data = LocomanipulationSDGInputData( + left_hand_pose_target=dataset_action[0:7], + right_hand_pose_target=dataset_action[7:14], + left_hand_joint_positions_target=dataset_action[14:16], + right_hand_joint_positions_target=dataset_action[16:18], + base_pose=episode_data.get_initial_state()["articulation"]["robot"]["root_pose"], + object_pose=object_pose, + fixture_pose=torch.tensor([0.0, 0.85, 0.0, 1.0, 0.0, 0.0, 0.0]), # Table pose is not recorded for this env. + ) + + return data + + def build_action_vector( + self, + left_hand_pose_target: torch.Tensor, + right_hand_pose_target: torch.Tensor, + left_hand_joint_positions_target: torch.Tensor, + right_hand_joint_positions_target: torch.Tensor, + base_velocity_target: torch.Tensor, + ): + + action = torch.zeros(self.action_space.shape) + action[0, 0:7] = left_hand_pose_target + action[0, 7:14] = right_hand_pose_target + action[0, 14:16] = left_hand_joint_positions_target + action[0, 16:18] = right_hand_joint_positions_target + action[0, 18:21] = base_velocity_target + action[0, 21:22] = 0.7 + + return action + + def get_base(self) -> HasPose: + return SceneBody(self.scene, "robot", "base_link") + + def get_left_hand(self) -> HasPose: + return SceneBody(self.scene, "robot", "left_7_Link") + + def get_right_hand(self) -> HasPose: + return SceneBody(self.scene, "robot", "right_7_Link") + + def get_object(self) -> HasPose: + return SceneBody(self.scene, "object", "Cube") + + def get_start_fixture(self) -> SceneFixture: + return SceneFixture( + self.scene, + "packing_table", + occupancy_map_boundary=np.array([[-1.55, -0.45], [1.55, -0.45], [1.55, 0.45], [-1.55, 0.45]]), + occupancy_map_resolution=0.05, + ) + + def get_end_fixture(self) -> SceneFixture: + return SceneFixture( + self.scene, + "packing_table_2", + occupancy_map_boundary=np.array([[-1.55, -0.45], [1.55, -0.45], [1.55, 0.45], [-1.55, 0.45]]), + occupancy_map_resolution=0.05, + ) + + def get_obstacle_fixtures(self): + return [] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore new file mode 100644 index 00000000000..50b04294cfc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore @@ -0,0 +1 @@ +Fiibot_W_1_V2_251016_Modified_urdf diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py index a3b30988b7f..7722140c0ac 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py @@ -9,7 +9,7 @@ import gymnasium as gym import os -from . import agents, fixed_base_upper_body_ik_g1_env_cfg, locomanipulation_g1_env_cfg +from . import agents, fixed_base_upper_body_ik_g1_env_cfg, locomanipulation_fii_env_cfg, locomanipulation_g1_env_cfg gym.register( id="Isaac-PickPlace-Locomanipulation-G1-Abs-v0", @@ -29,3 +29,10 @@ }, disable_env_checker=True, ) + +gym.register( + id="Isaac-PickPlace-Locomanipulation-Fii-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={"env_cfg_entry_point": locomanipulation_fii_env_cfg.FiibotEnvCfg}, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_fii_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_fii_env_cfg.py new file mode 100644 index 00000000000..61590b6c41c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_fii_env_cfg.py @@ -0,0 +1,346 @@ +# 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 os +import torch + +# Import Fiibot robot configuration from assets +from isaaclab_assets.robots.fiibot import FIIBOT_CFG, FIIBOT_USD_PATH + +import isaaclab.controllers.utils as ControllerUtils +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.assets.articulation import Articulation +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.fii.fii_retargeter import FiiRetargeterCfg +from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnvCfg +from isaaclab.envs.common import ViewerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from .swerve_ik import swerve_isosceles_ik + +CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) +FII_URDF_PATH = os.path.join(CURRENT_DIR, "Fiibot_W_1_V2_251016_Modified_urdf") # will be created if it doesn't exit +OBJECT_USD_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd" +FORCE_URDF_BUILD = True + + +class FiibotSceneCfg(InteractiveSceneCfg): + + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.85, 0.0), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.75, 1.035), rot=(1, 0, 0, 0)), + spawn=UsdFileCfg( + usd_path=OBJECT_USD_PATH, + scale=(1.5, 1.5, 1.5), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Placeholder - will be replaced in __post_init__ with Fiibot config from isaaclab_assets + robot: ArticulationCfg = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/robot", + ) + + +class FiibotLowerBodyAction(ActionTerm): + """Action term that is based on Agile lower body RL policy.""" + + cfg: "FiibotLowerBodyActionCfg" + """The configuration of the action term.""" + + _asset: Articulation + """The articulation asset to which the action term is applied.""" + + def __init__(self, cfg: "FiibotLowerBodyActionCfg", env: ManagerBasedEnv): + super().__init__(cfg, env) + + self._env = env + + self._joint_names = [ + "walk_mid_top_joint", + "walk_left_bottom_joint", + "walk_right_bottom_joint", + "jack_joint", + "front_wheel_joint", + "left_wheel_joint", + "right_wheel_joint", + ] + + self._joint_ids = [self._asset.data.joint_names.index(joint_name) for joint_name in self._joint_names] + + self._joint_pos_target = torch.zeros(self.num_envs, 7, device=self.device) + self._joint_vel_target = torch.zeros(self.num_envs, 3, device=self.device) + + @property + def action_dim(self) -> int: + """Lower Body Action: [vx, vy, wz, jack_joint_height]""" + return 4 + + @property + def raw_actions(self) -> torch.Tensor: + return self._joint_pos_target + + @property + def processed_actions(self) -> torch.Tensor: + return self._joint_pos_target + + def process_actions(self, actions: torch.Tensor): + + ik_out = swerve_isosceles_ik( + vx=float(actions[0, 0]), + vy=float(actions[0, 1]), + wz=float(actions[0, 2]), + L1=0.30438, + d=0.17362, + w=0.25, + R=0.06, + ) + + self._joint_pos_target[:, 0] = ik_out["wheel1"]["angle_rad"] + self._joint_pos_target[:, 1] = ik_out["wheel2"]["angle_rad"] + self._joint_pos_target[:, 2] = ik_out["wheel3"]["angle_rad"] + self._joint_pos_target[:, 3] = float(actions[0, 3]) + + self._joint_vel_target[:, 0] = ik_out["wheel1"]["omega"] + self._joint_vel_target[:, 1] = ik_out["wheel2"]["omega"] + self._joint_vel_target[:, 2] = ik_out["wheel3"]["omega"] + + def apply_actions(self): + + self._joint_pos_target[:, 4:] = self._joint_pos_target[:, 4:] + self._env.physics_dt * self._joint_vel_target + + self._asset.set_joint_position_target(target=self._joint_pos_target, joint_ids=self._joint_ids) + + +@configclass +class FiibotLowerBodyActionCfg(ActionTermCfg): + class_type: type[ActionTerm] = FiibotLowerBodyAction + + +@configclass +class FiibotActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + # "waist_joint", + "left_1_joint", + "left_2_joint", + "left_3_joint", + "left_4_joint", + "left_5_joint", + "left_6_joint", + "left_7_joint", + "right_1_joint", + "right_2_joint", + "right_3_joint", + "right_4_joint", + "right_5_joint", + "right_6_joint", + "right_7_joint", + ], + hand_joint_names=[ + "left_hand_grip1_joint", + "left_hand_grip2_joint", + "right_hand_grip1_joint", + "right_hand_grip2_joint", + ], + target_eef_link_names={ + "left_wrist": "Fiibot_W_2_V2_left_7_Link", + "right_wrist": "Fiibot_W_2_V2_right_7_Link", + }, + asset_name="robot", + controller=PinkIKControllerCfg( + articulation_name="robot", + base_link_name="base_link", + num_hand_joints=4, + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTask( + "Fiibot_W_2_V2_left_7_Link", + base_link_frame_name="Root", + position_cost=1.0, + orientation_cost=1.0, + lm_damping=10, + gain=0.1, + ), + LocalFrameTask( + "Fiibot_W_2_V2_right_7_Link", + base_link_frame_name="Root", + position_cost=1.0, + orientation_cost=1.0, + lm_damping=10, + gain=0.1, + ), + ], + fixed_input_tasks=[], + ), + ) + + lower_body_ik = FiibotLowerBodyActionCfg(asset_name="robot") + + +@configclass +class FiibotObservationsCfg: + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_7_Link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_7_Link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_7_Link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_7_Link"}) + + hand_joint_state = ObsTerm( + func=manip_mdp.get_robot_joint_state, + params={ + "joint_names": [ + "left_hand_grip1_joint", + "left_hand_grip2_joint", + "right_hand_grip1_joint", + "right_hand_grip2_joint", + ] + }, + ) + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_7_Link", "right_eef_link_name": "right_7_Link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class FiibotTerminationsCfg: + + time_out = DoneTerm(func=base_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm( + func=manip_mdp.task_done_pick_place, + params={ + "task_link_name": "right_7_Link", + "right_wrist_max_x": 0.26, + "min_x": 0.40, + "max_x": 0.85, + "min_y": 0.35, + "max_y": 0.8, + "max_height": 1.10, + "min_vel": 0.20, + }, + ) + + +@configclass +class FiibotRewardsCfg: + pass + + +@configclass +class FiibotEnvCfg(ManagerBasedRLEnvCfg): + + scene: FiibotSceneCfg = FiibotSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + actions: FiibotActionsCfg = FiibotActionsCfg() + observations = FiibotObservationsCfg() + rewards = FiibotRewardsCfg() + terminations = FiibotTerminationsCfg() + + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.25), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + viewer: ViewerCfg = ViewerCfg( + eye=(0.0, 3.0, 1.5), lookat=(0.0, 0.0, 0.7), origin_type="asset_body", asset_name="robot", body_name="base_link" + ) + + def __post_init__(self): + self.decimation = 4 + self.episode_length_s = 200.0 + self.sim.dt = 1 / 120 # 200Hz + self.sim.render_interval = 4 + + # Use the Fiibot robot configuration from isaaclab_assets + self.scene.robot = FIIBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/robot") # type: ignore[attr-defined] + + urdf_path = FII_URDF_PATH + + # Get the USD path from the robot spawn configuration + robot_spawn = self.scene.robot.spawn + # Type checker doesn't recognize UsdFileCfg.usd_path, but it exists + usd_file_path = getattr(robot_spawn, "usd_path", FIIBOT_USD_PATH) + + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + usd_file_path, urdf_path, force_conversion=FORCE_URDF_BUILD + ) + + self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + FiiRetargeterCfg( + sim_device=self.sim.device, + ) + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/swerve_ik.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/swerve_ik.py new file mode 100644 index 00000000000..388c95423d4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/swerve_ik.py @@ -0,0 +1,78 @@ +# 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 math + + +def swerve_isosceles_ik( + vx: float, + vy: float, + wz: float, + L1: float, + d: float, + w: float, + R: float, +) -> dict[str, dict[str, float]]: + """ + Inverse kinematics for a 3-module swerve (independent steering + drive) + in an isosceles layout with +x pointing to Wheel 1. + + Geometry (meters): + r1 = [ L1, 0 ] + r2 = [ -d, w ] + r3 = [ -d, -w ] + + Inputs: + vx : chassis linear velocity in x (robot frame) [m/s] + vy : chassis linear velocity in y (robot frame) [m/s] + wz : chassis yaw rate about +z [rad/s] (CCW +) + L1 : distance from center to Wheel 1 along +x [m] + d : x offset magnitude of base wheels (W2/W3) at x = -d [m] + w : half spacing of base wheels (W2/W3) at y = ±w [m] + R : wheel radius [m] (for converting linear speed to wheel angular speed) + + Outputs (per wheel i in {1,2,3}): + angle_rad : steering angle α_i = atan2(v_i_y, v_i_x) [rad] (−π, π] + angle_deg : same, degrees + speed : wheel linear speed s_i = ||v_i|| [m/s] + omega : wheel angular speed φ̇_i = s_i / R [rad/s] + + Equations (standard rigid-body velocity addition): + v_i = [vx, vy]^T + wz * [-y_i, x_i]^T + α_i = atan2(v_i_y, v_i_x) + s_i = sqrt(v_i_x^2 + v_i_y^2) + φ̇_i = s_i / R + + References: + - WPILib swerve kinematics (maps chassis speeds → module angles & speeds). + - Rigid-body relation v_P = v_O + ω × r (planar form used above). + - Community derivations / implementation notes (angle optimization, etc.). + """ + + def module_state(xi: float, yi: float) -> tuple[float, float, float, float]: + vix = vx + wz * (-yi) + viy = vy + wz * (xi) + angle = math.atan2(viy, vix) # [-pi, pi] + speed = math.hypot(vix, viy) + omega = speed / R if R > 0 else float("inf") + return angle, math.degrees(angle), speed, omega + + # Module positions (isosceles) + r = { + "wheel1": (L1, 0.0), + "wheel2": (-d, w), + "wheel3": (-d, -w), + } + + out = {} + for name, (xi, yi) in r.items(): + angle_rad, angle_deg, speed, omega = module_state(xi, yi) + out[name] = { + "angle_rad": angle_rad, + "angle_deg": angle_deg, + "speed": speed, + "omega": omega, + } + return out