diff --git a/autonomy/simulation/Humanoid_Wato/UsdModelAssets/full_stand.usd b/autonomy/simulation/Humanoid_Wato/UsdModelAssets/full_stand.usd new file mode 100644 index 00000000..fc861495 Binary files /dev/null and b/autonomy/simulation/Humanoid_Wato/UsdModelAssets/full_stand.usd differ diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/armDouble.SLDASM_limits.usda b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/armDouble.SLDASM_limits.usda new file mode 100644 index 00000000..5e3c8156 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/armDouble.SLDASM_limits.usda @@ -0,0 +1,109 @@ +#usda 1.0 +( + defaultPrim = "armDouble_SLDASM" + upAxis = "Z" +) + +over "armDouble_SLDASM" ( + prepend references = @./armDouble.SLDASM.usd@ +) +{ + over "joints" + { + over "joint1" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint2" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint3" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint4" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint5" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint6" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint1L" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint2l" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint3l" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint4l" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint5l" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint6l" + { + float physics:lowerLimit = -180 + float physics:upperLimit = 180 + } + + over "joint7" + { + float physics:lowerLimit = -0.05 + float physics:upperLimit = 0 + } + + over "joint8" + { + float physics:lowerLimit = 0 + float physics:upperLimit = 0.05 + } + + over "joint7l" + { + float physics:lowerLimit = -0.05 + float physics:upperLimit = 0 + } + + over "joint8l" + { + float physics:lowerLimit = 0 + float physics:upperLimit = 0.05 + } + } +} diff --git a/autonomy/simulation/quest_isaac_teleop/README.md b/autonomy/simulation/quest_isaac_teleop/README.md new file mode 100644 index 00000000..0f8571fd --- /dev/null +++ b/autonomy/simulation/quest_isaac_teleop/README.md @@ -0,0 +1,41 @@ +# Quest Bimanual Teleop (CloudXR + Pink IK) + +Hand-tracking teleop of the bimanual arm on a Quest, streamed via CloudXR. + +## 1. Start the USB tunnel (gnirehtet) + +The Quest reaches the PC through a USB reverse-tether. Plug the Quest in over USB, +then: + +```bash +cd ~/gnirehtet/gnirehtet-rust-linux64 && ./gnirehtet run +``` + +Approve the **VPN prompt on the headset** (a key icon appears). Leave this +running. + +> If you restart `adb` or replug the Quest, the reverse mapping gnirehtet needs +> gets wiped. Re-add it (then relaunch the client / re-run `./gnirehtet run`): +> +> ```bash +> adb reverse tcp:31416 tcp:31416 +> ``` + +## 2. Run the teleop + +```bash +cd ~/Wato/humanoid/autonomy/simulation/quest_isaac_teleop && ./run_bimanual_teleop.sh +``` + +Wait for `IsaacTeleop teleoperation started`. It runs headless; the view only +appears in the headset. + +Add `--gui` for a local desktop window (no headset/CloudXR). + +## 3. Connect the headset + +In the Quest browser open `https://:48322/`, accept the self-signed cert, +then in the CloudXR client set Server IP ``, Port `48322`, **VR Immersive**, +and **Connect**. + +Move your hands to drive the arms; pinch thumb + index to close each gripper. diff --git a/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/__init__.py b/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/__init__.py index e69de29b..461c79fb 100644 --- a/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/__init__.py +++ b/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/__init__.py @@ -0,0 +1,13 @@ +def register_bimanual_tasks() -> list: + """Register the bimanual teleop task after Isaac Sim starts.""" + import gymnasium as gym + + gym.register( + id="Isaac-WatoBimanualTeleop-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": "quest_isaac_teleop.bimanual_teleop_env_cfg:WatoBimanualTeleopEnvCfg", + }, + ) + return [] diff --git a/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_pink_controller_cfg.py b/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_pink_controller_cfg.py new file mode 100644 index 00000000..aa9fea9e --- /dev/null +++ b/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_pink_controller_cfg.py @@ -0,0 +1,68 @@ +"""Pink IK action config for the bimanual arm.""" + +from isaaclab.controllers.pink_ik import ( + DampingTaskCfg, + LocalFrameTaskCfg, + NullSpacePostureTaskCfg, + PinkIKControllerCfg, +) +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg + +RIGHT_EE_LINK = "link6" +LEFT_EE_LINK = "link6l" +BASE_LINK = "base_link" + +RIGHT_ARM_JOINTS = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] +LEFT_ARM_JOINTS = ["joint1L", "joint2l", "joint3l", "joint4l", "joint5l", "joint6l"] +ARM_JOINTS = RIGHT_ARM_JOINTS + LEFT_ARM_JOINTS + +# Order must match the teleop pipeline output. +GRIPPER_JOINTS = ["joint7", "joint8", "joint7l", "joint8l"] + +GRIPPER_OPEN = {"joint7": 0.0, "joint8": 0.0, "joint7l": 0.0, "joint8l": 0.0} +GRIPPER_CLOSED = {"joint7": -0.05, "joint8": 0.05, "joint7l": -0.05, "joint8l": 0.05} + + +WATO_BIMANUAL_IK_CONTROLLER_CFG = PinkIKControllerCfg( + articulation_name="robot", + base_link_name=BASE_LINK, + num_hand_joints=len(GRIPPER_JOINTS), + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTaskCfg( + frame=RIGHT_EE_LINK, + base_link_frame_name=BASE_LINK, + position_cost=8.0, + orientation_cost=1.0, + lm_damping=12, + gain=0.5, + ), + LocalFrameTaskCfg( + frame=LEFT_EE_LINK, + base_link_frame_name=BASE_LINK, + position_cost=8.0, + orientation_cost=1.0, + lm_damping=12, + gain=0.5, + ), + NullSpacePostureTaskCfg( + cost=0.05, + lm_damping=1, + controlled_frames=[RIGHT_EE_LINK, LEFT_EE_LINK], + controlled_joints=ARM_JOINTS, + gain=0.075, + ), + DampingTaskCfg(cost=0.5), + ], + fixed_input_tasks=[], +) + +WATO_BIMANUAL_IK_ACTION_CFG = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=ARM_JOINTS, + hand_joint_names=GRIPPER_JOINTS, + target_eef_link_names={"right_ee": RIGHT_EE_LINK, "left_ee": LEFT_EE_LINK}, + asset_name="robot", + enable_gravity_compensation=True, + controller=WATO_BIMANUAL_IK_CONTROLLER_CFG, +) diff --git a/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_teleop_env_cfg.py b/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_teleop_env_cfg.py new file mode 100644 index 00000000..304127bd --- /dev/null +++ b/autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_teleop_env_cfg.py @@ -0,0 +1,321 @@ +"""CloudXR + Pink IK teleop env for the bimanual arm.""" + +import importlib.util +import math +import sys +from pathlib import Path + +_SIMULATION_DIR = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(_SIMULATION_DIR / "Humanoid_Wato")) + +from isaaclab_teleop import IsaacTeleopCfg, XrCfg + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.envs.common import ViewerCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils.configclass import configclass + +from quest_isaac_teleop.bimanual_pink_controller_cfg import ( + GRIPPER_CLOSED, + GRIPPER_JOINTS, + GRIPPER_OPEN, + WATO_BIMANUAL_IK_ACTION_CFG, +) + +# The keyboard-teleop config lives in a directory with a space, so load by path. +_BIMANUAL_CFG_PATH = ( + _SIMULATION_DIR / "Teleop" / "keyboard-based teleoperation" / "bimanual_arm_cfg.py" +) +_spec = importlib.util.spec_from_file_location("bimanual_arm_cfg", str(_BIMANUAL_CFG_PATH)) +_bimanual_arm_cfg = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(_bimanual_arm_cfg) +BIMANUAL_ARM_CFG = _bimanual_arm_cfg.BIMANUAL_ARM_CFG + +_BIMANUAL_ROOT = _SIMULATION_DIR / "Humanoid_Wato" / "wato_bimanual_arm" +_URDF_PATH = str(_BIMANUAL_ROOT / "urdf" / "armDouble.SLDASM.urdf") +_MESH_DIR = str(_BIMANUAL_ROOT / "meshes") + +_ROBOT_USD_PATH = str( + _BIMANUAL_ROOT / "urdf" / "armDouble.SLDASM" / "armDouble.SLDASM_limits.usda" +) + +_ROBOT_POS = (0.0, 0.0, 0.5) +_ROBOT_ROT = (0.0, 0.0, 1.0, 0.0) # xyzw, 180 deg about Z + +_VIEWER_EYE = (-0.198, -0.016, 1.329) +_VIEWER_LOOKAT = (0.667, -0.016, 0.828) + +# Flip the z sign if left/right or forward is mirrored in the headset. +_XR_ANCHOR_POS = (0.0, 0.0, 0.0) +_XR_ANCHOR_ROT = (0.0, 0.0, -0.70710678, 0.70710678) + + +def _make_bimanual_gripper_retargeter_cls(): + """Build the retargeter class lazily (isaacteleop only imports under Isaac).""" + import numpy as np + + from isaacteleop.retargeting_engine.interface import BaseRetargeter, RetargeterIOType + from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO + from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, + ) + from isaacteleop.retargeting_engine.tensor_types import ( + FloatType, + HandInput, + HandInputIndex, + HandJointIndex, + ) + + class BimanualGripperRetargeter(BaseRetargeter): + """Map thumb/index pinch to the four gripper joints.""" + + def __init__(self, name: str, close_m: float = 0.03, open_m: float = 0.05) -> None: + super().__init__(name=name) + self._close_m = close_m + self._open_m = open_m + self._closed = {"left": False, "right": False} + + def input_spec(self) -> RetargeterIOType: + return { + "hand_right": OptionalType(HandInput()), + "hand_left": OptionalType(HandInput()), + } + + def output_spec(self) -> RetargeterIOType: + return { + "gripper_joints": TensorGroupType( + "gripper_joints", [FloatType(j) for j in GRIPPER_JOINTS] + ) + } + + def _update_side(self, side: str, hand_group) -> bool: + if hand_group.is_none: + return self._closed[side] + + joint_positions = np.from_dlpack(hand_group[HandInputIndex.JOINT_POSITIONS]) + joint_valid = np.from_dlpack(hand_group[HandInputIndex.JOINT_VALID]) + + if not (joint_valid[HandJointIndex.THUMB_TIP] and joint_valid[HandJointIndex.INDEX_TIP]): + return self._closed[side] + + thumb = joint_positions[HandJointIndex.THUMB_TIP] + index = joint_positions[HandJointIndex.INDEX_TIP] + distance = float(np.linalg.norm(thumb - index)) + + if distance < self._close_m: + self._closed[side] = True + elif distance > self._open_m: + self._closed[side] = False + return self._closed[side] + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + if context.execution_events.reset: + self._closed = {"left": False, "right": False} + + out = outputs["gripper_joints"] + right_targets = GRIPPER_CLOSED if self._update_side("right", inputs["hand_right"]) else GRIPPER_OPEN + left_targets = GRIPPER_CLOSED if self._update_side("left", inputs["hand_left"]) else GRIPPER_OPEN + + out[0] = right_targets["joint7"] + out[1] = right_targets["joint8"] + out[2] = left_targets["joint7l"] + out[3] = left_targets["joint8l"] + + return BimanualGripperRetargeter + + +def _build_bimanual_pipeline(): + """Build the 18D teleop action pipeline.""" + from isaacteleop.retargeters import ( + Se3AbsRetargeter, + Se3RetargeterConfig, + TensorReorderer, + ) + from isaacteleop.retargeting_engine.deviceio_source_nodes import HandsSource + from isaacteleop.retargeting_engine.interface import OutputCombiner, ValueInput + from isaacteleop.retargeting_engine.tensor_types import TransformMatrix + + BimanualGripperRetargeter = _make_bimanual_gripper_retargeter_cls() + + hands = HandsSource(name="hands") + transform_input = ValueInput("world_T_anchor", TransformMatrix()) + transformed_hands = hands.transformed(transform_input.output(ValueInput.VALUE)) + + right_se3 = Se3AbsRetargeter( + Se3RetargeterConfig( + input_device=HandsSource.RIGHT, + zero_out_xy_rotation=False, + use_wrist_rotation=True, + use_wrist_position=True, + target_offset_roll=0.0, + target_offset_pitch=0.0, + target_offset_yaw=0.0, + ), + name="right_ee_pose", + ) + connected_right_se3 = right_se3.connect( + {HandsSource.RIGHT: transformed_hands.output(HandsSource.RIGHT)} + ) + + left_se3 = Se3AbsRetargeter( + Se3RetargeterConfig( + input_device=HandsSource.LEFT, + zero_out_xy_rotation=False, + use_wrist_rotation=True, + use_wrist_position=True, + target_offset_roll=0.0, + target_offset_pitch=0.0, + target_offset_yaw=0.0, + ), + name="left_ee_pose", + ) + connected_left_se3 = left_se3.connect( + {HandsSource.LEFT: transformed_hands.output(HandsSource.LEFT)} + ) + + gripper = BimanualGripperRetargeter(name="gripper") + connected_gripper = gripper.connect( + { + "hand_right": transformed_hands.output(HandsSource.RIGHT), + "hand_left": transformed_hands.output(HandsSource.LEFT), + } + ) + + right_ee_elements = ["r_pos_x", "r_pos_y", "r_pos_z", "r_quat_x", "r_quat_y", "r_quat_z", "r_quat_w"] + left_ee_elements = ["l_pos_x", "l_pos_y", "l_pos_z", "l_quat_x", "l_quat_y", "l_quat_z", "l_quat_w"] + gripper_elements = list(GRIPPER_JOINTS) + + reorderer = TensorReorderer( + input_config={ + "right_ee_pose": right_ee_elements, + "left_ee_pose": left_ee_elements, + "gripper_joints": gripper_elements, + }, + output_order=right_ee_elements + left_ee_elements + gripper_elements, + name="action_reorderer", + input_types={ + "right_ee_pose": "array", + "left_ee_pose": "array", + "gripper_joints": "scalar", + }, + ) + connected_reorderer = reorderer.connect( + { + "right_ee_pose": connected_right_se3.output("ee_pose"), + "left_ee_pose": connected_left_se3.output("ee_pose"), + "gripper_joints": connected_gripper.output("gripper_joints"), + } + ) + + pipeline = OutputCombiner({"action": connected_reorderer.output("output")}) + return pipeline, [right_se3, left_se3] + + +def _high_pd_actuators(): + """Stiffen the arm joints so they track IK targets closely.""" + actuators = dict(BIMANUAL_ARM_CFG.actuators) + for name in ("left_shoulder", "left_elbow", "left_wrist", "right_arm"): + actuators[name] = actuators[name].replace(stiffness=4400.0, damping=40.0) + return actuators + + +@configclass +class BimanualTeleopSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), + ) + robot: ArticulationCfg = BIMANUAL_ARM_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + # The USD overlay already includes the joint limit fixes. + spawn=sim_utils.UsdFileCfg( + usd_path=_ROBOT_USD_PATH, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + activate_contact_sensors=False, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + ), + ), + actuators=_high_pd_actuators(), + init_state=ArticulationCfg.InitialStateCfg( + pos=_ROBOT_POS, + rot=_ROBOT_ROT, + joint_pos=BIMANUAL_ARM_CFG.init_state.joint_pos, + ), + ) + + +@configclass +class ActionsCfg: + bimanual_ik = WATO_BIMANUAL_IK_ACTION_CFG + + +@configclass +class ObservationsCfg: + @configclass + class PolicyCfg(ObsGroup): + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + time_out = DoneTerm(func=base_mdp.time_out, time_out=True) + + +@configclass +class WatoBimanualTeleopEnvCfg(ManagerBasedRLEnvCfg): + scene: BimanualTeleopSceneCfg = BimanualTeleopSceneCfg(num_envs=1, env_spacing=2.5) + viewer: ViewerCfg = ViewerCfg(eye=_VIEWER_EYE, lookat=_VIEWER_LOOKAT, origin_type="world") + actions: ActionsCfg = ActionsCfg() + observations: ObservationsCfg = ObservationsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + commands = None + rewards = None + curriculum = None + + def __post_init__(self): + self.decimation = 4 + self.episode_length_s = 3600.0 + self.sim.dt = 1 / 200 + self.sim.render_interval = 2 + + self.actions.bimanual_ik.controller.usd_path = None + self.actions.bimanual_ik.controller.urdf_path = _URDF_PATH + self.actions.bimanual_ik.controller.mesh_path = _MESH_DIR + + self.xr = XrCfg( + anchor_pos=_XR_ANCHOR_POS, + anchor_rot=_XR_ANCHOR_ROT, + near_plane=0.1, + ) + self.isaac_teleop = IsaacTeleopCfg( + pipeline_builder=lambda: _build_bimanual_pipeline()[0], + sim_device=self.sim.device, + xr_cfg=self.xr, + ) diff --git a/autonomy/simulation/quest_isaac_teleop/run_bimanual_teleop.sh b/autonomy/simulation/quest_isaac_teleop/run_bimanual_teleop.sh new file mode 100755 index 00000000..e80b3e23 --- /dev/null +++ b/autonomy/simulation/quest_isaac_teleop/run_bimanual_teleop.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash +# Launch CloudXR + Pink IK teleop for the bimanual arm. +# +# Use --gui for local debug without CloudXR. + +set -e + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ISAAC_LAB="${ISAAC_LAB:-$HOME/robotics/IsaacLab-3.0}" + +export PYTHONPATH="${SCRIPT_DIR}:${SCRIPT_DIR}/../Humanoid_Wato:${PYTHONPATH}" + +MODE_ARGS=(--xr --headless) +if [[ "${1:-}" == "--gui" || "${WATO_GUI:-0}" == "1" ]]; then + [[ "${1:-}" == "--gui" ]] && shift + MODE_ARGS=(--visualizer kit --cloudxr_env none --no-auto_launch_cloudxr) +fi + +exec "${ISAAC_LAB}/isaaclab.sh" -p \ + "${ISAAC_LAB}/scripts/environments/teleoperation/teleop_se3_agent.py" \ + --task Isaac-WatoBimanualTeleop-v0 \ + --external_callback quest_isaac_teleop.register_bimanual_tasks \ + --device cpu \ + "${MODE_ARGS[@]}" \ + "$@"