From 9c059043222e2530b7bd340590c71f95d397af4f Mon Sep 17 00:00:00 2001 From: John Welsh Date: Tue, 2 Dec 2025 13:22:04 -0800 Subject: [PATCH 01/16] add fii mobile manipulator task --- .../devices/openxr/retargeters/__init__.py | 1 + .../humanoid/fii/fii_retargeter.py | 65 +++ .../locomanipulation_fii_env_cfg.py | 418 ++++++++++++++++++ .../locomanipulation/pick_place/swerve_ik.py | 75 ++++ 4 files changed, 559 insertions(+) create mode 100644 source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_fii_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/swerve_ik.py 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..e0dcbae43a1 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py @@ -0,0 +1,65 @@ +import numpy as np +import torch +from dataclasses import dataclass + +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.], dtype=torch.float32, device=self._sim_device) + right_eef_pose = torch.tensor([-0.3, 0.3, 0.72648, 1.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") + + if left_wrist is not None: + left_eef_pose = torch.tensor(torch.from_numpy(left_wrist), dtype=torch.float32, device=self._sim_device) + left_eef_pose[2] = left_eef_pose[2] + if right_wrist is not None: + right_eef_pose = torch.tensor(torch.from_numpy(right_wrist), dtype=torch.float32, device=self._sim_device) + right_eef_pose[2] = right_eef_pose[2] + + 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) + + +@dataclass +class FiiRetargeterCfg(RetargeterCfg): + retargeter_type: type[RetargeterBase] = FiiRetargeter 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..c10cde06590 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_fii_env_cfg.py @@ -0,0 +1,418 @@ +# 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 torch +import os + +from .swerve_ik import swerve_isosceles_ik + +import isaaclab.sim as sim_utils +import isaaclab.controllers.utils as ControllerUtils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import AssetBaseCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import SceneEntityCfg +import isaaclab.envs.mdp as base_mdp +from isaaclab.envs.common import ViewerCfg + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm + +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.assets.articulation import Articulation +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.envs import ManagerBasedRLEnvCfg, ManagerBasedEnv +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +from dataclasses import MISSING + +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + + +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +# +from isaaclab.devices.openxr.retargeters.humanoid.fii.fii_retargeter import FiiRetargeterCfg + + + +#======================================================================= +# PARAMETERS +#======================================================================= + +CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) +FII_USD_PATH = os.path.join(CURRENT_DIR, "Fiibot_W_1_V2_251016_Modified.usd") +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"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd" +FORCE_URDF_BUILD = True + + +#======================================================================= +# SCENE +#======================================================================= + +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.), 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.0), rot=(1, 0, 0, 0)), + spawn=UsdFileCfg( + usd_path=OBJECT_USD_PATH, + scale=(1., 1., 1.), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + + robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/robot", + 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, + } + ), + spawn=sim_utils.UsdFileCfg( + usd_path=FII_USD_PATH, + 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, + ) + ), + actuators={ + "actuators": ImplicitActuatorCfg( + joint_names_expr=[".*"], + damping=None, + stiffness=None + ), + "jack_joint": ImplicitActuatorCfg( + joint_names_expr=["jack_joint"], + damping=5000., + stiffness=500000. + ), + }, + ) + + +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, + }) + + +#======================================================================= +# REWARDS +#======================================================================= +@configclass +class FiibotRewardsCfg: + pass + + +#======================================================================= +# ENVIRONMENT +#======================================================================= +@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.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 + + urdf_path = FII_URDF_PATH + + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( + self.scene.robot.spawn.usd_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..354c56679d4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/swerve_ik.py @@ -0,0 +1,75 @@ +import math +from typing import Dict, Tuple + + +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). :contentReference[oaicite:0]{index=0} + - Rigid-body relation v_P = v_O + ω × r (planar form used above). :contentReference[oaicite:1]{index=1} + - Community derivations / implementation notes (angle optimization, etc.). :contentReference[oaicite:2]{index=2} + """ + + 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 + From 0ea8d230bb3174820ad2dc1afd66c9d0bf4922b2 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Thu, 11 Dec 2025 11:25:14 -0800 Subject: [PATCH 02/16] format code --- .../humanoid/fii/fii_retargeter.py | 21 +- .../locomanipulation_fii_env_cfg.py | 191 ++++++++---------- .../locomanipulation/pick_place/swerve_ik.py | 20 +- 3 files changed, 109 insertions(+), 123 deletions(-) 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 index e0dcbae43a1..fa41b7ff8b3 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py @@ -1,3 +1,8 @@ +# 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 @@ -6,7 +11,6 @@ from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg - class FiiRetargeter(RetargeterBase): def __init__(self, cfg: "FiiRetargeterCfg"): @@ -19,9 +23,12 @@ 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.], dtype=torch.float32, device=self._sim_device) - right_eef_pose = torch.tensor([-0.3, 0.3, 0.72648, 1.0, 0., 0., 0.], 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] @@ -38,8 +45,10 @@ def retarget(self, data: dict) -> torch.Tensor: 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]) - + 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"] 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 index c10cde06590..c73c4c571ee 100644 --- 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 @@ -3,54 +3,43 @@ # # SPDX-License-Identifier: BSD-3-Clause -import torch import os +import torch +from dataclasses import MISSING -from .swerve_ik import swerve_isosceles_ik - -import isaaclab.sim as sim_utils import isaaclab.controllers.utils as ControllerUtils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets import AssetBaseCfg -from isaaclab.assets.articulation import ArticulationCfg -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import SceneEntityCfg import isaaclab.envs.mdp as base_mdp -from isaaclab.envs.common import ViewerCfg - +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.assets.articulation import Articulation, ArticulationCfg from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask -from isaaclab.managers.action_manager import ActionTerm +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.managers import TerminationTermCfg as DoneTerm +# +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.assets.articulation import Articulation -from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp - -from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg -from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg -from isaaclab.envs import ManagerBasedRLEnvCfg, ManagerBasedEnv -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR - -from dataclasses import MISSING - +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg -from isaaclab.utils import configclass - - +from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg -from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg -# -from isaaclab.devices.openxr.retargeters.humanoid.fii.fii_retargeter import FiiRetargeterCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp +from .swerve_ik import swerve_isosceles_ik -#======================================================================= +# ======================================================================= # PARAMETERS -#======================================================================= +# ======================================================================= CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) FII_USD_PATH = os.path.join(CURRENT_DIR, "Fiibot_W_1_V2_251016_Modified.usd") @@ -59,28 +48,22 @@ FORCE_URDF_BUILD = True -#======================================================================= +# ======================================================================= # SCENE -#======================================================================= +# ======================================================================= + class FiibotSceneCfg(InteractiveSceneCfg): - ground = AssetBaseCfg( - prim_path="/World/defaultGroundPlane", - spawn=sim_utils.GroundPlaneCfg() - ) + 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) - ) + 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.), rot=(1.0, 0.0, 0.0, 0.0)), + 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), @@ -92,16 +75,15 @@ class FiibotSceneCfg(InteractiveSceneCfg): init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.75, 1.0), rot=(1, 0, 0, 0)), spawn=UsdFileCfg( usd_path=OBJECT_USD_PATH, - scale=(1., 1., 1.), + scale=(1.0, 1.0, 1.0), rigid_props=sim_utils.RigidBodyPropertiesCfg(), ), ) - robot = ArticulationCfg( prim_path="{ENV_REGEX_NS}/robot", init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), + pos=(0.0, 0.0, 0.0), rot=(0.7071068, 0.0, 0.0, 0.7071068), joint_pos={ "jack_joint": 0.7, @@ -119,7 +101,7 @@ class FiibotSceneCfg(InteractiveSceneCfg): "right_5_joint": 0.0, "right_6_joint": -0.785398, "right_7_joint": 0.0, - } + }, ), spawn=sim_utils.UsdFileCfg( usd_path=FII_USD_PATH, @@ -131,19 +113,11 @@ class FiibotSceneCfg(InteractiveSceneCfg): max_linear_velocity=1000.0, max_angular_velocity=1000.0, max_depenetration_velocity=1.0, - ) + ), ), actuators={ - "actuators": ImplicitActuatorCfg( - joint_names_expr=[".*"], - damping=None, - stiffness=None - ), - "jack_joint": ImplicitActuatorCfg( - joint_names_expr=["jack_joint"], - damping=5000., - stiffness=500000. - ), + "actuators": ImplicitActuatorCfg(joint_names_expr=[".*"], damping=None, stiffness=None), + "jack_joint": ImplicitActuatorCfg(joint_names_expr=["jack_joint"], damping=5000.0, stiffness=500000.0), }, ) @@ -159,7 +133,7 @@ class FiibotLowerBodyAction(ActionTerm): def __init__(self, cfg: "FiibotLowerBodyActionCfg", env: ManagerBasedEnv): super().__init__(cfg, env) - + self._env = env self._joint_names = [ @@ -169,13 +143,10 @@ def __init__(self, cfg: "FiibotLowerBodyActionCfg", env: ManagerBasedEnv): "jack_joint", "front_wheel_joint", "left_wheel_joint", - "right_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_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) @@ -192,7 +163,7 @@ def raw_actions(self) -> torch.Tensor: @property def processed_actions(self) -> torch.Tensor: return self._joint_pos_target - + def process_actions(self, actions: torch.Tensor): ik_out = swerve_isosceles_ik( @@ -202,26 +173,23 @@ def process_actions(self, actions: torch.Tensor): L1=0.30438, d=0.17362, w=0.25, - R=0.06 + 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[:, 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'] + 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 - ) + self._asset.set_joint_position_target(target=self._joint_pos_target, joint_ids=self._joint_ids) @configclass @@ -249,13 +217,13 @@ class FiibotActionsCfg: "right_4_joint", "right_5_joint", "right_6_joint", - "right_7_joint" + "right_7_joint", ], hand_joint_names=[ "left_hand_grip1_joint", "left_hand_grip2_joint", "right_hand_grip1_joint", - "right_hand_grip2_joint" + "right_hand_grip2_joint", ], target_eef_link_names={ "left_wrist": "Fiibot_W_2_V2_left_7_Link", @@ -284,20 +252,18 @@ class FiibotActionsCfg: orientation_cost=1.0, lm_damping=10, gain=0.1, - ) + ), ], fixed_input_tasks=[], - ) + ), ) - lower_body_ik = FiibotLowerBodyActionCfg( - asset_name="robot" - ) + lower_body_ik = FiibotLowerBodyActionCfg(asset_name="robot") @configclass class FiibotObservationsCfg: - + @configclass class PolicyCfg(ObsGroup): """Observations for policy group with state values.""" @@ -318,12 +284,17 @@ class PolicyCfg(ObsGroup): 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" - ]}) + 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, @@ -334,42 +305,44 @@ 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, - }) + 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, + }, + ) -#======================================================================= +# ======================================================================= # REWARDS -#======================================================================= +# ======================================================================= @configclass class FiibotRewardsCfg: pass -#======================================================================= +# ======================================================================= # ENVIRONMENT -#======================================================================= +# ======================================================================= @configclass class FiibotEnvCfg(ManagerBasedRLEnvCfg): @@ -380,13 +353,14 @@ class FiibotEnvCfg(ManagerBasedRLEnvCfg): terminations = FiibotTerminationsCfg() xr: XrCfg = XrCfg( - anchor_pos=(0., 0., 0.25), + 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 @@ -415,4 +389,3 @@ def __post_init__(self): ), } ) - 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 index 354c56679d4..0107d019b0b 100644 --- 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 @@ -1,3 +1,8 @@ +# 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 from typing import Dict, Tuple @@ -10,7 +15,7 @@ def swerve_isosceles_ik( d: float, w: float, R: float, -) -> Dict[str, Dict[str, 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. @@ -47,19 +52,19 @@ def swerve_isosceles_ik( - Community derivations / implementation notes (angle optimization, etc.). :contentReference[oaicite:2]{index=2} """ - def module_state(xi: float, yi: float) -> Tuple[float, float, float, float]: + 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] + 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 ), + "wheel1": (L1, 0.0), + "wheel2": (-d, w), + "wheel3": (-d, -w), } out = {} @@ -72,4 +77,3 @@ def module_state(xi: float, yi: float) -> Tuple[float, float, float, float]: "omega": omega, } return out - From a422f64cbc5d36729586f7b967ad0b16cb81e262 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Thu, 11 Dec 2025 12:37:06 -0800 Subject: [PATCH 03/16] swap object for green block --- .../locomanipulation/pick_place/.gitignore | 1 + .../locomanipulation/pick_place/__init__.py | 11 +++++++++- .../locomanipulation_fii_env_cfg.py | 21 +++---------------- 3 files changed, 14 insertions(+), 19 deletions(-) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore 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..e384909b009 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore @@ -0,0 +1 @@ +Fiibot_W_1_V2_251016_Modified_urdf \ No newline at end of file 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..58bfcce8f3b 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_g1_env_cfg, locomanipulation_fii_env_cfg gym.register( id="Isaac-PickPlace-Locomanipulation-G1-Abs-v0", @@ -29,3 +29,12 @@ }, disable_env_checker=True, ) + +gym.register( + id="Isaac-PickPlace-Locomanipulation-Fii", + 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 index c73c4c571ee..f612cdb8d21 100644 --- 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 @@ -37,22 +37,13 @@ from .swerve_ik import swerve_isosceles_ik -# ======================================================================= -# PARAMETERS -# ======================================================================= - CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) FII_USD_PATH = os.path.join(CURRENT_DIR, "Fiibot_W_1_V2_251016_Modified.usd") 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"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd" +OBJECT_USD_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/green_block.usd" FORCE_URDF_BUILD = True -# ======================================================================= -# SCENE -# ======================================================================= - - class FiibotSceneCfg(InteractiveSceneCfg): ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) @@ -72,10 +63,10 @@ class FiibotSceneCfg(InteractiveSceneCfg): object = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Object", - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.75, 1.0), rot=(1, 0, 0, 0)), + 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.0, 1.0, 1.0), + scale=(1.5, 1.5, 1.5), rigid_props=sim_utils.RigidBodyPropertiesCfg(), ), ) @@ -332,17 +323,11 @@ class FiibotTerminationsCfg: ) -# ======================================================================= -# REWARDS -# ======================================================================= @configclass class FiibotRewardsCfg: pass -# ======================================================================= -# ENVIRONMENT -# ======================================================================= @configclass class FiibotEnvCfg(ManagerBasedRLEnvCfg): From dc0903dafd749003caa80cb33b9ab62604f79faa Mon Sep 17 00:00:00 2001 From: John Welsh Date: Thu, 11 Dec 2025 12:49:32 -0800 Subject: [PATCH 04/16] format code --- .../manager_based/locomanipulation/pick_place/.gitignore | 2 +- .../manager_based/locomanipulation/pick_place/__init__.py | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) 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 index e384909b009..50b04294cfc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/.gitignore @@ -1 +1 @@ -Fiibot_W_1_V2_251016_Modified_urdf \ No newline at end of file +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 58bfcce8f3b..080ccaf7359 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, locomanipulation_fii_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", @@ -33,8 +33,6 @@ gym.register( id="Isaac-PickPlace-Locomanipulation-Fii", entry_point="isaaclab.envs:ManagerBasedRLEnv", - kwargs={ - "env_cfg_entry_point": locomanipulation_fii_env_cfg.FiibotEnvCfg - }, + kwargs={"env_cfg_entry_point": locomanipulation_fii_env_cfg.FiibotEnvCfg}, disable_env_checker=True, ) From f3c2295cd915476a5e6e03577f8444bd5630a983 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Fri, 12 Dec 2025 12:25:06 -0800 Subject: [PATCH 05/16] remove no-op in retargeter --- .../devices/openxr/retargeters/humanoid/fii/fii_retargeter.py | 2 -- 1 file changed, 2 deletions(-) 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 index fa41b7ff8b3..2bdc9d438e3 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py @@ -37,10 +37,8 @@ def retarget(self, data: dict) -> torch.Tensor: if left_wrist is not None: left_eef_pose = torch.tensor(torch.from_numpy(left_wrist), dtype=torch.float32, device=self._sim_device) - left_eef_pose[2] = left_eef_pose[2] if right_wrist is not None: right_eef_pose = torch.tensor(torch.from_numpy(right_wrist), dtype=torch.float32, device=self._sim_device) - right_eef_pose[2] = right_eef_pose[2] 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]) From c43b46dc3dbe36ef579a8711a642d8cc01154395 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Fri, 12 Dec 2025 12:25:45 -0800 Subject: [PATCH 06/16] add v0 suffix to fii env --- .../manager_based/locomanipulation/pick_place/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 080ccaf7359..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 @@ -31,7 +31,7 @@ ) gym.register( - id="Isaac-PickPlace-Locomanipulation-Fii", + 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, From b9162def704d549feb0ab485f7b5119c6b90f806 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Fri, 12 Dec 2025 12:27:00 -0800 Subject: [PATCH 07/16] remove broken content references --- .../manager_based/locomanipulation/pick_place/swerve_ik.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 0107d019b0b..e8eca70892c 100644 --- 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 @@ -47,9 +47,9 @@ def swerve_isosceles_ik( φ̇_i = s_i / R References: - - WPILib swerve kinematics (maps chassis speeds → module angles & speeds). :contentReference[oaicite:0]{index=0} - - Rigid-body relation v_P = v_O + ω × r (planar form used above). :contentReference[oaicite:1]{index=1} - - Community derivations / implementation notes (angle optimization, etc.). :contentReference[oaicite:2]{index=2} + - 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]: From 148090ba729954ebe84b886cf0ab78e2fdb770dd Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 14:22:21 -0800 Subject: [PATCH 08/16] fix retargeter --- .../humanoid/fii/fii_retargeter.py | 50 ++++++++++++++++++- 1 file changed, 48 insertions(+), 2 deletions(-) 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 index 2bdc9d438e3..80abd73ce2d 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py @@ -7,6 +7,8 @@ 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 @@ -35,10 +37,13 @@ def retarget(self, data: dict) -> torch.Tensor: 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.tensor(torch.from_numpy(left_wrist), dtype=torch.float32, device=self._sim_device) + 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.tensor(torch.from_numpy(right_wrist), dtype=torch.float32, device=self._sim_device) + 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]) @@ -65,6 +70,47 @@ def _hand_data_to_gripper_values(self, hand_data): 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) + # combined_quat = torch.tensor([0, 0, 0, 1], dtype=torch.float32) + # combined_quat = torch.tensor([0.5, 0.5, 0.5, 0.5], dtype=torch.float32) + # combined_quat = torch.tensor([0., 1., 0., 0.], dtype=torch.float32) + # 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) + # combined_quat = torch.tensor([0, 0, 0, -1], dtype=torch.float32) + # combined_quat = torch.tensor([0.5, -0.5, -0.5, 0.5], dtype=torch.float32) + # combined_quat = torch.tensor([0., 1., 0., 0.], dtype=torch.float32) + # 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 From 0bf8f4e9f11c5eca91ba8f0b9aaf97f898d99b6e Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 14:30:31 -0800 Subject: [PATCH 09/16] cleanup --- .../openxr/retargeters/humanoid/fii/fii_retargeter.py | 10 ---------- .../pick_place/locomanipulation_fii_env_cfg.py | 11 +++++++---- 2 files changed, 7 insertions(+), 14 deletions(-) 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 index 80abd73ce2d..1fcaf4fcfe0 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py @@ -70,7 +70,6 @@ def _hand_data_to_gripper_values(self, hand_data): 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. @@ -85,23 +84,14 @@ def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: 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) - # combined_quat = torch.tensor([0, 0, 0, 1], dtype=torch.float32) - # combined_quat = torch.tensor([0.5, 0.5, 0.5, 0.5], dtype=torch.float32) - # combined_quat = torch.tensor([0., 1., 0., 0.], dtype=torch.float32) - # 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) - # combined_quat = torch.tensor([0, 0, 0, -1], dtype=torch.float32) - # combined_quat = torch.tensor([0.5, -0.5, -0.5, 0.5], dtype=torch.float32) - # combined_quat = torch.tensor([0., 1., 0., 0.], dtype=torch.float32) - # 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)) 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 index f612cdb8d21..7db9460c8b1 100644 --- 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 @@ -12,13 +12,11 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg -from isaaclab.assets.articulation import Articulation, ArticulationCfg +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 @@ -354,8 +352,13 @@ def __post_init__(self): 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", FII_USD_PATH) + temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf( - self.scene.robot.spawn.usd_path, urdf_path, force_conversion=FORCE_URDF_BUILD + usd_file_path, urdf_path, force_conversion=FORCE_URDF_BUILD ) self.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path From 4338080165f37dcda9ff5cc85c4759dfdb89fd6b Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 14:38:44 -0800 Subject: [PATCH 10/16] fix linting errors --- .../devices/openxr/retargeters/humanoid/fii/fii_retargeter.py | 1 - .../pick_place/locomanipulation_fii_env_cfg.py | 3 +-- .../manager_based/locomanipulation/pick_place/swerve_ik.py | 3 +-- 3 files changed, 2 insertions(+), 5 deletions(-) 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 index 1fcaf4fcfe0..1e431609ece 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fii/fii_retargeter.py @@ -8,7 +8,6 @@ from dataclasses import dataclass import isaaclab.utils.math as PoseUtils - from isaaclab.devices import OpenXRDevice from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg 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 index 7db9460c8b1..15673072a2b 100644 --- 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 @@ -5,7 +5,6 @@ import os import torch -from dataclasses import MISSING import isaaclab.controllers.utils as ControllerUtils import isaaclab.envs.mdp as base_mdp @@ -29,7 +28,7 @@ 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, ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp 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 index e8eca70892c..388c95423d4 100644 --- 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 @@ -4,7 +4,6 @@ # SPDX-License-Identifier: BSD-3-Clause import math -from typing import Dict, Tuple def swerve_isosceles_ik( @@ -49,7 +48,7 @@ def swerve_isosceles_ik( 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.). + - Community derivations / implementation notes (angle optimization, etc.). """ def module_state(xi: float, yi: float) -> tuple[float, float, float, float]: From d3ffa3e87438db2abbdf0afe544909ef3e5de839 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 15:07:21 -0800 Subject: [PATCH 11/16] move fiibot to assets --- .../isaaclab_assets/robots/__init__.py | 1 + .../isaaclab_assets/robots/fiibot.py | 105 ++++++++++++++++++ .../locomanipulation_fii_env_cfg.py | 49 ++------ 3 files changed, 115 insertions(+), 40 deletions(-) create mode 100644 source/isaaclab_assets/isaaclab_assets/robots/fiibot.py 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..9e2f69fbc17 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py @@ -0,0 +1,105 @@ +# 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. +""" + +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 + +# For local debugging, use the local data directory +from isaaclab_assets import ISAACLAB_ASSETS_DATA_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_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 index 15673072a2b..42a1a74559b 100644 --- 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 @@ -30,12 +30,14 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +# Import Fiibot robot configuration from assets +from isaaclab_assets.robots.fiibot import FIIBOT_CFG, FIIBOT_USD_PATH + 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_USD_PATH = os.path.join(CURRENT_DIR, "Fiibot_W_1_V2_251016_Modified.usd") 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 @@ -68,45 +70,9 @@ class FiibotSceneCfg(InteractiveSceneCfg): ), ) - robot = ArticulationCfg( + # Placeholder - will be replaced in __post_init__ with Fiibot config from isaaclab_assets + robot: ArticulationCfg = ArticulationCfg( prim_path="{ENV_REGEX_NS}/robot", - 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, - }, - ), - spawn=sim_utils.UsdFileCfg( - usd_path=FII_USD_PATH, - 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, - ), - ), - actuators={ - "actuators": ImplicitActuatorCfg(joint_names_expr=[".*"], damping=None, stiffness=None), - "jack_joint": ImplicitActuatorCfg(joint_names_expr=["jack_joint"], damping=5000.0, stiffness=500000.0), - }, ) @@ -349,12 +315,15 @@ def __post_init__(self): 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", FII_USD_PATH) + 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 From 42d3f2db4e24ce96944a4dda600ad6b75de7fa4e Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 15:08:31 -0800 Subject: [PATCH 12/16] format --- source/isaaclab_assets/isaaclab_assets/robots/fiibot.py | 7 +++---- .../pick_place/locomanipulation_fii_env_cfg.py | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py b/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py index 9e2f69fbc17..a3dbe49162f 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/fiibot.py @@ -10,14 +10,14 @@ * :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 -# For local debugging, use the local data directory -from isaaclab_assets import ISAACLAB_ASSETS_DATA_DIR - ## # Configuration ## @@ -102,4 +102,3 @@ - Height-adjustable jack mechanism for variable working height - Suitable for locomanipulation and pick-and-place tasks """ - 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 index 42a1a74559b..61590b6c41c 100644 --- 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 @@ -6,10 +6,12 @@ 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.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.assets.articulation import Articulation from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask @@ -30,9 +32,6 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR -# Import Fiibot robot configuration from assets -from isaaclab_assets.robots.fiibot import FIIBOT_CFG, FIIBOT_USD_PATH - from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp from .swerve_ik import swerve_isosceles_ik From f746e86bb3b5220351ffb110462d98921e683ae0 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 16:09:27 -0800 Subject: [PATCH 13/16] add locomanipulation SDG env --- .../imitation-learning/teleop_imitation.rst | 4 + .../locomanipulation_sdg/generate_data.py | 124 ++++++++++-- .../locomanipulation_sdg/envs/__init__.py | 9 + .../envs/fii_locomanipulation_sdg_env.py | 186 ++++++++++++++++++ 4 files changed, 305 insertions(+), 18 deletions(-) create mode 100644 source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/fii_locomanipulation_sdg_env.py 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..2bc938ff160 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", @@ -300,6 +331,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 +342,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 +355,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 +395,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 +406,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 +419,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 @@ -610,6 +674,10 @@ def replay( angle_threshold: float = 0.2, approach_distance: float = 1.0, 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. @@ -634,6 +702,10 @@ def replay( angle_threshold: Angular threshold for orientation control (rad) approach_distance: Buffer distance from final goal (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 @@ -678,12 +750,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: @@ -766,6 +850,10 @@ def replay( angle_threshold=args_cli.angle_threshold, approach_distance=args_cli.approach_distance, 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_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..f3d707076c1 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/fii_locomanipulation_sdg_env.py @@ -0,0 +1,186 @@ +# 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, ISAACLAB_NUCLEUS_DIR, retrieve_file_path +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, + FiibotSceneCfg, + FiibotObservationsCfg, + 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.), + ), + 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 [] From 14f6ad631a7c1024f0dc56e5ff3179985e869470 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 16:18:54 -0800 Subject: [PATCH 14/16] add occ map buffer size to args --- .../locomanipulation_sdg/generate_data.py | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/scripts/imitation_learning/locomanipulation_sdg/generate_data.py b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py index 2bc938ff160..8775497afa8 100644 --- a/scripts/imitation_learning/locomanipulation_sdg/generate_data.py +++ b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py @@ -117,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, @@ -280,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. @@ -288,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: @@ -318,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) @@ -673,6 +681,7 @@ 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", @@ -701,6 +710,7 @@ 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' @@ -724,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 @@ -849,6 +859,7 @@ 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, From 9a90f3be9bdedd39d2c55f9268963670d9ea0efa Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 16:26:30 -0800 Subject: [PATCH 15/16] run format --- .../envs/fii_locomanipulation_sdg_env.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) 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 index f3d707076c1..785c6471d26 100644 --- 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 @@ -22,14 +22,15 @@ from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_fii_env_cfg import ( FiibotEnvCfg, - FiibotSceneCfg, FiibotObservationsCfg, - manip_mdp + FiibotSceneCfg, + manip_mdp, ) from .locomanipulation_sdg_env import LocomanipulationSDGEnv from .locomanipulation_sdg_env_cfg import LocomanipulationSDGEnvCfg, LocomanipulationSDGRecorderManagerCfg + @configclass class FiibotLocomanipSceneCfg(FiibotSceneCfg): @@ -47,7 +48,7 @@ class FiibotLocomanipSceneCfg(FiibotSceneCfg): init_state=AssetBaseCfg.InitialStateCfg( pos=(-17.5, 10.8, 0.0), # rot=[0, 0, 0, 1]), - rot=(1., 0., 0., 0.), + rot=(1.0, 0.0, 0.0, 0.0), ), spawn=UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", @@ -65,6 +66,7 @@ class FiibotLocomanipSceneCfg(FiibotSceneCfg): 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. @@ -91,9 +93,7 @@ class FiibotLocomanipEnvCfg(FiibotEnvCfg, LocomanipulationSDGEnvCfg): ) # Scene settings - scene: FiibotLocomanipSceneCfg = FiibotLocomanipSceneCfg( - num_envs=1, env_spacing=2.5, replicate_physics=True - ) + scene: FiibotLocomanipSceneCfg = FiibotLocomanipSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) recorders: LocomanipulationSDGRecorderManagerCfg = LocomanipulationSDGRecorderManagerCfg() observations: FiibotLocomanipObservationsCfg = FiibotLocomanipObservationsCfg() @@ -128,9 +128,7 @@ def load_input_data(self, episode_data: EpisodeData, step: int) -> Locomanipulat 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. + 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 From 87076feed3414e04f59b1baf97f637e1876d4440 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 17 Dec 2025 16:35:47 -0800 Subject: [PATCH 16/16] fix unused imports --- .../locomanipulation_sdg/envs/fii_locomanipulation_sdg_env.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 785c6471d26..d51d36e6754 100644 --- 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 @@ -14,7 +14,7 @@ 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, ISAACLAB_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.datasets import EpisodeData from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGInputData