From bb31d29d44aca93bdad50e88a3ec80c502b8a2d4 Mon Sep 17 00:00:00 2001 From: Louis Le Lay Date: Thu, 18 Sep 2025 17:21:33 +0200 Subject: [PATCH 01/16] adds wip implementation of phone se3 --- .../isaaclab/devices/phone/__init__.py | 8 + .../isaaclab/devices/phone/se3_phone.py | 203 ++++++++++++++++++ 2 files changed, 211 insertions(+) create mode 100644 source/isaaclab/isaaclab/devices/phone/__init__.py create mode 100644 source/isaaclab/isaaclab/devices/phone/se3_phone.py diff --git a/source/isaaclab/isaaclab/devices/phone/__init__.py b/source/isaaclab/isaaclab/devices/phone/__init__.py new file mode 100644 index 00000000000..43fb3ee8e37 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/phone/__init__.py @@ -0,0 +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 + +"""Phone device for SE(2) and SE(3) control.""" + +from .se3_phone import Se3Phone, Se3PhoneCfg \ No newline at end of file diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py new file mode 100644 index 00000000000..229d0c51699 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -0,0 +1,203 @@ +# IsaacLab/source/isaaclab/isaaclab/devices/phone/se3_phone.py + +from __future__ import annotations + +import threading +import time +from typing import Any, Callable, Optional + +import numpy as np +import torch + +from ..device_base import DeviceBase, DeviceCfg + +try: + from teleop import Teleop +except Exception as exc: # pragma: no cover + Teleop = None + _IMPORT_ERR = exc +else: + _IMPORT_ERR = None +from dataclasses import dataclass + +@dataclass +class Se3PhoneCfg(DeviceCfg): + """Configuration for SE3 space mouse devices.""" + + # pos_scale: float = 1.0 # scale factor (m / raw meter) + # rot_scale: float = 1.0 # scale factor (rad / raw rad) + # max_step_pos: float = 0.10 # clamp per tick (m) + # max_step_rot: float = 0.50 # clamp per tick (rad) + # use_euler: bool = True # convert orientation via RPY (simple) + # gate_key: str = "move" # message boolean that gates motion + # gripper_key: str = "scale" # message float in [-1 1] for gripper + # open_threshold: float = 0.0 # >= -> open (+1) < -> close (-1) + # start_server: bool = True # start Teleop server automatically + # server_kwargs: Optional[dict] = None # forwarded to Teleop(...) + gripper_term: bool = True + pos_sensitivity: float = 1.0 + rot_sensitivity: float = 1.0 + retargeters: None = None + +class Se3Phone(DeviceBase): + """Phone-based SE(3) teleop device. + + Returns a 7D tensor on `advance()`: + [dx, dy, dz, droll, dpitch, dyaw, gripper] + where the first 6 are *relative* deltas since the last frame (meters, radians) + and `gripper` is in {-1.0, +1.0} (close/open). + + Notes + ----- + - The device listens to a background `teleop.Teleop` server, which streams a 4x4 + end-effector target pose (in some chosen reference frame) and a message dict. + - When the message indicates the move gate is OFF, the deltas are zeroed but + the gripper command is still emitted from the message. + """ + + def __init__(self, cfg: Se3PhoneCfg): + + if Teleop is None: + raise ImportError( + "teleop is not available. Install it first (e.g., `pip install teleop`)." + ) from _IMPORT_ERR + + # self._pos_scale = float(cfg.pos_scale) + # self._rot_scale = float(cfg.rot_scale) + # self._max_step_pos = float(cfg.max_step_pos) + # self._max_step_rot = float(cfg.max_step_rot) + # self._use_euler = bool(cfg.use_euler) + # self._gate_key = cfg.gate_key + # self._gripper_key = cfg.gripper_key + # self._open_threshold = float(cfg.open_threshold) + # store inputs + self._pos_sensitivity = cfg.pos_sensitivity + self._rot_sensitivity = cfg.rot_sensitivity + self._gripper_term = cfg.gripper_term + self._sim_device = cfg.sim_device + # latest data (written by callback thread) + self._latest_pose: Optional[np.ndarray] = None # 4x4 + self._latest_msg: dict[str, Any] = {} + self._enabled: bool = False + + # previous pose (read on main thread to compute deltas) + self._prev_pose: Optional[np.ndarray] = None + + # spin Teleop server in the background so `advance()` is non-blocking + self._teleop: Optional[Teleop] = None + self._thread: Optional[threading.Thread] = None + self._server_kwargs: Optional[dict] = None + self._start_server(self._server_kwargs or {}) + + # --------------------------------------------------------------------- # + # DeviceBase required API + # --------------------------------------------------------------------- # + + def reset(self) -> None: + """Reset the device internals (clears reference).""" + self._prev_pose = None + # keep latest pose so user can re-enable without reconnect + + def add_callback(self, key: Any, func: Callable) -> None: + """Optional: bind a callback (unused for phone device).""" + # We could forward callbacks to Teleop if needed; noop for now. + return + + def advance(self) -> torch.Tensor: + """Return SE(3) delta + gripper as a 7D tensor. + + Contract matches other Isaac Lab SE(3) devices: + first 6 entries are [dx, dy, dz, droll, dpitch, dyaw] and last is gripper. :contentReference[oaicite:1]{index=1} + """ + pose = self._latest_pose + msg = self._latest_msg + + # default zeros if no data yet + if pose is None: + return torch.zeros(7, dtype=torch.float32, device=self._sim_device) + + # compute relative motion wrt previous pose + if self._prev_pose is None: + self._prev_pose = pose.copy() + return torch.tensor( + [0, 0, 0, 0, 0, 0, self._gripper_from_msg(msg)], + dtype=torch.float32, device=self._sim_device + ) + + dp = self._delta_pose(self._prev_pose, pose) + self._prev_pose = pose.copy() + + # scale & clamp + dp[:3] *= self._pos_sensitivity + dp[3:6] *= self._rot_sensitivity + # dp[:3] = np.clip(dp[:3], -self._max_step_pos, self._max_step_pos) + # dp[3:6] = np.clip(dp[3:6], -self._max_step_rot, self._max_step_rot) + + command = np.append(dp, self._gripper_from_msg(msg)) + return torch.tensor(command, dtype=torch.float32, device=self._sim_device) + + # --------------------------------------------------------------------- # + # Teleop plumbing + # --------------------------------------------------------------------- # + + def _start_server(self, server_kwargs: dict) -> None: + self._teleop = Teleop(**server_kwargs) + + def _cb(pose: np.ndarray, message: dict) -> None: + # Expect pose: (4, 4), message: dict with keys like "move", "scale" + if not isinstance(pose, np.ndarray) or pose.shape != (4, 4): + return + self._latest_pose = pose.astype(np.float64, copy=True) + self._latest_msg = dict("scale") + print + + + self._teleop.subscribe(_cb) + + self._thread = threading.Thread( + target=self._teleop.run, name="TeleopServer", daemon=True + ) + self._thread.start() + + # give server a moment to boot + time.sleep(0.1) + + # --------------------------------------------------------------------- # + # Helpers + # --------------------------------------------------------------------- # + + def _gripper_from_msg(self, msg: dict) -> float: + # Simple mapping: value >= threshold => open (+1), else close (-1) + val = float(msg.get("scale", 0.0)) + print(val) + return 1.0 if val >= 1.0 else -1.0 + + def _delta_pose(self, T_prev: np.ndarray, T_curr: np.ndarray) -> np.ndarray: + """Compute [dx, dy, dz, droll, dpitch, dyaw] from two 4x4 transforms.""" + dT = np.linalg.inv(T_prev) @ T_curr + t = dT[:3, 3] + # if self._use_euler: + rpy = self._mat_to_rpy(dT[:3, :3]) + # else: + # # axis-angle small-angle approx + # rpy = self._rot_to_small_rpy(dT[:3, :3]) + return np.concatenate([t, rpy], axis=0) + + @staticmethod + def _mat_to_rpy(R: np.ndarray) -> np.ndarray: + """ZYX (roll-pitch-yaw) from rotation matrix, numerically safe.""" + # yaw (z) + yaw = float(np.arctan2(R[1, 0], R[0, 0])) + # pitch (y) + sp = -R[2, 0] + sp = float(np.clip(sp, -1.0, 1.0)) + pitch = float(np.arcsin(sp)) + # roll (x) + roll = float(np.arctan2(R[2, 1], R[2, 2])) + return np.array([roll, pitch, yaw], dtype=np.float64) + + @staticmethod + def _rot_to_small_rpy(R: np.ndarray) -> np.ndarray: + """Small-angle rpy from rotation matrix (approx via vee(logR)).""" + w = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) * 0.5 + return w.astype(np.float64) From 17cfa10608589f86b5ac2c01a486348c5d003e1b Mon Sep 17 00:00:00 2001 From: Louis Le Lay Date: Fri, 19 Sep 2025 02:26:12 +0200 Subject: [PATCH 02/16] adds working se3 phone configuration --- .../teleoperation/teleop_se3_agent.py | 6 +- source/isaaclab/isaaclab/devices/__init__.py | 1 + .../isaaclab/devices/keyboard/se3_keyboard.py | 2 +- .../isaaclab/devices/phone/se3_phone.py | 194 ++++++++---------- .../isaaclab/devices/teleop_device_factory.py | 2 + .../agibot/place_toy2box_rmp_rel_env_cfg.py | 1 + .../place_upright_mug_rmp_rel_env_cfg.py | 1 + .../manipulation/reach/reach_env_cfg.py | 1 + 8 files changed, 96 insertions(+), 112 deletions(-) diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index 021ee5ff80f..159c95941d2 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -56,7 +56,7 @@ import omni.log -from isaaclab.devices import Se3Gamepad, Se3GamepadCfg, Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices import Se3Gamepad, Se3GamepadCfg, Se3Phone, Se3PhoneCfg, Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg from isaaclab.devices.openxr import remove_camera_configs from isaaclab.devices.teleop_device_factory import create_teleop_device from isaaclab.managers import TerminationTermCfg as DoneTerm @@ -193,6 +193,10 @@ def stop_teleoperation() -> None: teleop_interface = Se3Gamepad( Se3GamepadCfg(pos_sensitivity=0.1 * sensitivity, rot_sensitivity=0.1 * sensitivity) ) + elif args_cli.teleop_device.lower() == "phone": + teleop_interface = Se3Phone( + Se3PhoneCfg(pos_sensitivity=0.1 * sensitivity, rot_sensitivity=0.1 * sensitivity) + ) else: omni.log.error(f"Unsupported teleop device: {args_cli.teleop_device}") omni.log.error("Supported devices: keyboard, spacemouse, gamepad, handtracking") diff --git a/source/isaaclab/isaaclab/devices/__init__.py b/source/isaaclab/isaaclab/devices/__init__.py index 718695e3503..144db8e6e66 100644 --- a/source/isaaclab/isaaclab/devices/__init__.py +++ b/source/isaaclab/isaaclab/devices/__init__.py @@ -25,4 +25,5 @@ from .openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg from .retargeter_base import RetargeterBase, RetargeterCfg from .spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from .phone import Se3Phone, Se3PhoneCfg from .teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py index 64e398ad14e..3c5e66034a3 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py @@ -145,7 +145,7 @@ def advance(self) -> torch.Tensor: if self.gripper_term: gripper_value = -1.0 if self._close_gripper else 1.0 command = np.append(command, gripper_value) - + print(command[:3]) return torch.tensor(command, dtype=torch.float32, device=self._sim_device) """ diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index 229d0c51699..51189614a25 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -1,16 +1,21 @@ -# IsaacLab/source/isaaclab/isaaclab/devices/phone/se3_phone.py +# 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 + +"""Phone controller for SE(3) control.""" from __future__ import annotations import threading import time from typing import Any, Callable, Optional - +from isaaclab.utils.math import axis_angle_from_quat import numpy as np import torch from ..device_base import DeviceBase, DeviceCfg - +import os try: from teleop import Teleop except Exception as exc: # pragma: no cover @@ -24,19 +29,9 @@ class Se3PhoneCfg(DeviceCfg): """Configuration for SE3 space mouse devices.""" - # pos_scale: float = 1.0 # scale factor (m / raw meter) - # rot_scale: float = 1.0 # scale factor (rad / raw rad) - # max_step_pos: float = 0.10 # clamp per tick (m) - # max_step_rot: float = 0.50 # clamp per tick (rad) - # use_euler: bool = True # convert orientation via RPY (simple) - # gate_key: str = "move" # message boolean that gates motion - # gripper_key: str = "scale" # message float in [-1 1] for gripper - # open_threshold: float = 0.0 # >= -> open (+1) < -> close (-1) - # start_server: bool = True # start Teleop server automatically - # server_kwargs: Optional[dict] = None # forwarded to Teleop(...) gripper_term: bool = True - pos_sensitivity: float = 1.0 - rot_sensitivity: float = 1.0 + pos_sensitivity: float = 0.5 + rot_sensitivity: float = 0.4 retargeters: None = None class Se3Phone(DeviceBase): @@ -62,26 +57,22 @@ def __init__(self, cfg: Se3PhoneCfg): "teleop is not available. Install it first (e.g., `pip install teleop`)." ) from _IMPORT_ERR - # self._pos_scale = float(cfg.pos_scale) - # self._rot_scale = float(cfg.rot_scale) - # self._max_step_pos = float(cfg.max_step_pos) - # self._max_step_rot = float(cfg.max_step_rot) - # self._use_euler = bool(cfg.use_euler) - # self._gate_key = cfg.gate_key - # self._gripper_key = cfg.gripper_key - # self._open_threshold = float(cfg.open_threshold) # store inputs self._pos_sensitivity = cfg.pos_sensitivity self._rot_sensitivity = cfg.rot_sensitivity self._gripper_term = cfg.gripper_term self._sim_device = cfg.sim_device # latest data (written by callback thread) - self._latest_pose: Optional[np.ndarray] = None # 4x4 + self._gripper = 1.0 + self._move_enabled = False + + self._latest_pos: Optional[torch.Tensor] = None # (3,) + self._latest_rot: Optional[torch.Tensor] = None # (3,) (w,x,y,z) self._latest_msg: dict[str, Any] = {} - self._enabled: bool = False - # previous pose (read on main thread to compute deltas) - self._prev_pose: Optional[np.ndarray] = None + # Previous sample used to compute relative deltas + self._prev_pos: Optional[torch.Tensor] = None # (3,) + self._prev_rot: Optional[torch.Tensor] = None # (3,) # spin Teleop server in the background so `advance()` is non-blocking self._teleop: Optional[Teleop] = None @@ -89,14 +80,9 @@ def __init__(self, cfg: Se3PhoneCfg): self._server_kwargs: Optional[dict] = None self._start_server(self._server_kwargs or {}) - # --------------------------------------------------------------------- # - # DeviceBase required API - # --------------------------------------------------------------------- # - def reset(self) -> None: - """Reset the device internals (clears reference).""" - self._prev_pose = None - # keep latest pose so user can re-enable without reconnect + self._prev_pos = None + self._prev_rot = None def add_callback(self, key: Any, func: Callable) -> None: """Optional: bind a callback (unused for phone device).""" @@ -109,47 +95,75 @@ def advance(self) -> torch.Tensor: Contract matches other Isaac Lab SE(3) devices: first 6 entries are [dx, dy, dz, droll, dpitch, dyaw] and last is gripper. :contentReference[oaicite:1]{index=1} """ - pose = self._latest_pose - msg = self._latest_msg - - # default zeros if no data yet - if pose is None: - return torch.zeros(7, dtype=torch.float32, device=self._sim_device) - - # compute relative motion wrt previous pose - if self._prev_pose is None: - self._prev_pose = pose.copy() - return torch.tensor( - [0, 0, 0, 0, 0, 0, self._gripper_from_msg(msg)], - dtype=torch.float32, device=self._sim_device - ) - - dp = self._delta_pose(self._prev_pose, pose) - self._prev_pose = pose.copy() - - # scale & clamp - dp[:3] *= self._pos_sensitivity - dp[3:6] *= self._rot_sensitivity - # dp[:3] = np.clip(dp[:3], -self._max_step_pos, self._max_step_pos) - # dp[3:6] = np.clip(dp[3:6], -self._max_step_rot, self._max_step_rot) - - command = np.append(dp, self._gripper_from_msg(msg)) - return torch.tensor(command, dtype=torch.float32, device=self._sim_device) - - # --------------------------------------------------------------------- # - # Teleop plumbing - # --------------------------------------------------------------------- # + command = torch.zeros(7, dtype=torch.float32, device=self._sim_device) + command[6] = self._gripper + + if self._latest_pos is None: + return command + + # print(self._move_enabled) + if self._prev_pos is None: + # First sample: initialize reference + self._prev_pos = self._latest_pos.clone() + self._prev_rot = self._latest_rot.clone() + return command + + if not self._move_enabled: + # Gate OFF: zero deltas and keep reference synced to current to avoid jumps on re-enable + self._prev_pos = self._latest_pos.clone() + self._prev_rot = self._latest_rot.clone() + return command + + # Gate ON: compute SE(3) delta wrt previous, then update reference + dpos = torch.sub(self._latest_pos, self._prev_pos) + drot = torch.sub(self._latest_rot, self._prev_rot) + print(f"dpos is {dpos}") + print(f"drot is {drot}") + + command[:3] = dpos * self._pos_sensitivity + command[3:6] = drot * self._rot_sensitivity + + return command + # Teleop plumbing def _start_server(self, server_kwargs: dict) -> None: self._teleop = Teleop(**server_kwargs) - def _cb(pose: np.ndarray, message: dict) -> None: - # Expect pose: (4, 4), message: dict with keys like "move", "scale" - if not isinstance(pose, np.ndarray) or pose.shape != (4, 4): + def _cb(_pose_unused: np.ndarray, message: dict) -> None: + # Expect "message" like the example in your comment. + if not isinstance(message, dict): return - self._latest_pose = pose.astype(np.float64, copy=True) - self._latest_msg = dict("scale") - print + self._latest_msg = dict(message) + + # --- Parse position --- + p = message.get("position", {}) + tx = -float(p.get("z", 0.0)) + ty = -float(p.get("x", 0.0)) + tz = float(p.get("y", 0.0)) + + self._latest_pos = torch.tensor([tx, ty, tz], device=self._sim_device, dtype=torch.float32) + + + # --- Parse quaternion (x, y, z, w) and normalize --- + qd = message.get("orientation", {}) + qx = float(qd.get("x", 0.0)) + qy = float(qd.get("y", 0.0)) + qz = float(qd.get("z", 0.0)) + qw = float(qd.get("w", 1.0)) + + quat = torch.tensor([qw, qx, qy, qz], device=self._sim_device, dtype=torch.float32).unsqueeze(0) # (1, 4) + self._latest_rot = axis_angle_from_quat(quat).squeeze(0) # (3,) + self._latest_rot[[0 ,1, 2]] = self._latest_rot[[2, 0, 1]] * torch.tensor([-1, -1, 1], device=self._sim_device, dtype=torch.float32) + + + g = message.get("gripper") + if isinstance(g, str): + s = g.strip().lower() + if s == "open": self._gripper = 1.0 + elif s == "close": self._gripper = -1.0 + + + self._move_enabled = bool(message.get("move", False)) self._teleop.subscribe(_cb) @@ -161,43 +175,3 @@ def _cb(pose: np.ndarray, message: dict) -> None: # give server a moment to boot time.sleep(0.1) - - # --------------------------------------------------------------------- # - # Helpers - # --------------------------------------------------------------------- # - - def _gripper_from_msg(self, msg: dict) -> float: - # Simple mapping: value >= threshold => open (+1), else close (-1) - val = float(msg.get("scale", 0.0)) - print(val) - return 1.0 if val >= 1.0 else -1.0 - - def _delta_pose(self, T_prev: np.ndarray, T_curr: np.ndarray) -> np.ndarray: - """Compute [dx, dy, dz, droll, dpitch, dyaw] from two 4x4 transforms.""" - dT = np.linalg.inv(T_prev) @ T_curr - t = dT[:3, 3] - # if self._use_euler: - rpy = self._mat_to_rpy(dT[:3, :3]) - # else: - # # axis-angle small-angle approx - # rpy = self._rot_to_small_rpy(dT[:3, :3]) - return np.concatenate([t, rpy], axis=0) - - @staticmethod - def _mat_to_rpy(R: np.ndarray) -> np.ndarray: - """ZYX (roll-pitch-yaw) from rotation matrix, numerically safe.""" - # yaw (z) - yaw = float(np.arctan2(R[1, 0], R[0, 0])) - # pitch (y) - sp = -R[2, 0] - sp = float(np.clip(sp, -1.0, 1.0)) - pitch = float(np.arcsin(sp)) - # roll (x) - roll = float(np.arctan2(R[2, 1], R[2, 2])) - return np.array([roll, pitch, yaw], dtype=np.float64) - - @staticmethod - def _rot_to_small_rpy(R: np.ndarray) -> np.ndarray: - """Small-angle rpy from rotation matrix (approx via vee(logR)).""" - w = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) * 0.5 - return w.astype(np.float64) diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index f2a7eed32c6..1a590ca8636 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -26,6 +26,7 @@ ) from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.devices.spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices.phone import Se3Phone, Se3PhoneCfg with contextlib.suppress(ModuleNotFoundError): # May fail if xr is not in use @@ -35,6 +36,7 @@ DEVICE_MAP: dict[type[DeviceCfg], type[DeviceBase]] = { Se3KeyboardCfg: Se3Keyboard, Se3SpaceMouseCfg: Se3SpaceMouse, + Se3PhoneCfg: Se3Phone, Se3GamepadCfg: Se3Gamepad, Se2KeyboardCfg: Se2Keyboard, Se2GamepadCfg: Se2Gamepad, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py index 0fa7fd9cafa..64fbb9b53cd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -10,6 +10,7 @@ from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.devices.phone import Se3PhoneCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg from isaaclab.managers import EventTermCfg as EventTerm diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py index 6689a9cb154..5fdd595072b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py @@ -10,6 +10,7 @@ from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.devices.phone import Se3PhoneCfg from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py index 8890010a71b..aad181a3cd8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py @@ -11,6 +11,7 @@ from isaaclab.devices.gamepad import Se3GamepadCfg from isaaclab.devices.keyboard import Se3KeyboardCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.devices.phone import Se3PhoneCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import ActionTermCfg as ActionTerm from isaaclab.managers import CurriculumTermCfg as CurrTerm From 81ba45e4f1142125fe01e03e61db02a94ee976af Mon Sep 17 00:00:00 2001 From: Louis Le Lay Date: Fri, 19 Sep 2025 02:46:57 +0200 Subject: [PATCH 03/16] formats --- .../teleoperation/teleop_se3_agent.py | 11 ++++- source/isaaclab/isaaclab/devices/__init__.py | 2 +- .../isaaclab/devices/phone/__init__.py | 2 +- .../isaaclab/devices/phone/se3_phone.py | 47 ++++++++++--------- .../isaaclab/devices/teleop_device_factory.py | 2 +- .../agibot/place_toy2box_rmp_rel_env_cfg.py | 2 +- .../place_upright_mug_rmp_rel_env_cfg.py | 2 +- .../manipulation/reach/reach_env_cfg.py | 2 +- 8 files changed, 41 insertions(+), 29 deletions(-) diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index 159c95941d2..ab6b6b55240 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -56,7 +56,16 @@ import omni.log -from isaaclab.devices import Se3Gamepad, Se3GamepadCfg, Se3Phone, Se3PhoneCfg, Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices import ( + Se3Gamepad, + Se3GamepadCfg, + Se3Keyboard, + Se3KeyboardCfg, + Se3Phone, + Se3PhoneCfg, + Se3SpaceMouse, + Se3SpaceMouseCfg, +) from isaaclab.devices.openxr import remove_camera_configs from isaaclab.devices.teleop_device_factory import create_teleop_device from isaaclab.managers import TerminationTermCfg as DoneTerm diff --git a/source/isaaclab/isaaclab/devices/__init__.py b/source/isaaclab/isaaclab/devices/__init__.py index 144db8e6e66..d7d0dcbf4d9 100644 --- a/source/isaaclab/isaaclab/devices/__init__.py +++ b/source/isaaclab/isaaclab/devices/__init__.py @@ -23,7 +23,7 @@ from .gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg from .keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg from .openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg +from .phone import Se3Phone, Se3PhoneCfg from .retargeter_base import RetargeterBase, RetargeterCfg from .spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg -from .phone import Se3Phone, Se3PhoneCfg from .teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/phone/__init__.py b/source/isaaclab/isaaclab/devices/phone/__init__.py index 43fb3ee8e37..d02de17cd03 100644 --- a/source/isaaclab/isaaclab/devices/phone/__init__.py +++ b/source/isaaclab/isaaclab/devices/phone/__init__.py @@ -5,4 +5,4 @@ """Phone device for SE(2) and SE(3) control.""" -from .se3_phone import Se3Phone, Se3PhoneCfg \ No newline at end of file +from .se3_phone import Se3Phone, Se3PhoneCfg diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index 51189614a25..38e06299392 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -7,15 +7,18 @@ from __future__ import annotations +import numpy as np +import os import threading import time -from typing import Any, Callable, Optional -from isaaclab.utils.math import axis_angle_from_quat -import numpy as np import torch +from typing import Any, Optional +from collections.abc import Callable + +from isaaclab.utils.math import axis_angle_from_quat from ..device_base import DeviceBase, DeviceCfg -import os + try: from teleop import Teleop except Exception as exc: # pragma: no cover @@ -25,6 +28,7 @@ _IMPORT_ERR = None from dataclasses import dataclass + @dataclass class Se3PhoneCfg(DeviceCfg): """Configuration for SE3 space mouse devices.""" @@ -34,6 +38,7 @@ class Se3PhoneCfg(DeviceCfg): rot_sensitivity: float = 0.4 retargeters: None = None + class Se3Phone(DeviceBase): """Phone-based SE(3) teleop device. @@ -66,18 +71,18 @@ def __init__(self, cfg: Se3PhoneCfg): self._gripper = 1.0 self._move_enabled = False - self._latest_pos: Optional[torch.Tensor] = None # (3,) - self._latest_rot: Optional[torch.Tensor] = None # (3,) (w,x,y,z) + self._latest_pos: torch.Tensor | None = None # (3,) + self._latest_rot: torch.Tensor | None = None # (3,) (w,x,y,z) self._latest_msg: dict[str, Any] = {} # Previous sample used to compute relative deltas - self._prev_pos: Optional[torch.Tensor] = None # (3,) - self._prev_rot: Optional[torch.Tensor] = None # (3,) + self._prev_pos: torch.Tensor | None = None # (3,) + self._prev_rot: torch.Tensor | None = None # (3,) # spin Teleop server in the background so `advance()` is non-blocking - self._teleop: Optional[Teleop] = None - self._thread: Optional[threading.Thread] = None - self._server_kwargs: Optional[dict] = None + self._teleop: Teleop | None = None + self._thread: threading.Thread | None = None + self._server_kwargs: dict | None = None self._start_server(self._server_kwargs or {}) def reset(self) -> None: @@ -143,7 +148,6 @@ def _cb(_pose_unused: np.ndarray, message: dict) -> None: self._latest_pos = torch.tensor([tx, ty, tz], device=self._sim_device, dtype=torch.float32) - # --- Parse quaternion (x, y, z, w) and normalize --- qd = message.get("orientation", {}) qx = float(qd.get("x", 0.0)) @@ -152,25 +156,24 @@ def _cb(_pose_unused: np.ndarray, message: dict) -> None: qw = float(qd.get("w", 1.0)) quat = torch.tensor([qw, qx, qy, qz], device=self._sim_device, dtype=torch.float32).unsqueeze(0) # (1, 4) - self._latest_rot = axis_angle_from_quat(quat).squeeze(0) # (3,) - self._latest_rot[[0 ,1, 2]] = self._latest_rot[[2, 0, 1]] * torch.tensor([-1, -1, 1], device=self._sim_device, dtype=torch.float32) - + self._latest_rot = axis_angle_from_quat(quat).squeeze(0) # (3,) + self._latest_rot[[0, 1, 2]] = self._latest_rot[[2, 0, 1]] * torch.tensor( + [-1, -1, 1], device=self._sim_device, dtype=torch.float32 + ) g = message.get("gripper") if isinstance(g, str): s = g.strip().lower() - if s == "open": self._gripper = 1.0 - elif s == "close": self._gripper = -1.0 - + if s == "open": + self._gripper = 1.0 + elif s == "close": + self._gripper = -1.0 self._move_enabled = bool(message.get("move", False)) - self._teleop.subscribe(_cb) - self._thread = threading.Thread( - target=self._teleop.run, name="TeleopServer", daemon=True - ) + self._thread = threading.Thread(target=self._teleop.run, name="TeleopServer", daemon=True) self._thread.start() # give server a moment to boot diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index 1a590ca8636..a67260d2ab6 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -24,9 +24,9 @@ Se3RelRetargeter, Se3RelRetargeterCfg, ) +from isaaclab.devices.phone import Se3Phone, Se3PhoneCfg from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.devices.spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg -from isaaclab.devices.phone import Se3Phone, Se3PhoneCfg with contextlib.suppress(ModuleNotFoundError): # May fail if xr is not in use diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py index 64fbb9b53cd..35d11c28f74 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -9,8 +9,8 @@ from isaaclab.assets import AssetBaseCfg, RigidObjectCfg from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg -from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.devices.phone import Se3PhoneCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg from isaaclab.managers import EventTermCfg as EventTerm diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py index 5fdd595072b..1321014125a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py @@ -9,8 +9,8 @@ from isaaclab.assets import AssetBaseCfg, RigidObjectCfg from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg -from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.devices.phone import Se3PhoneCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py index aad181a3cd8..fb67991dfef 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py @@ -10,8 +10,8 @@ from isaaclab.devices import DevicesCfg from isaaclab.devices.gamepad import Se3GamepadCfg from isaaclab.devices.keyboard import Se3KeyboardCfg -from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.devices.phone import Se3PhoneCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import ActionTermCfg as ActionTerm from isaaclab.managers import CurriculumTermCfg as CurrTerm From 2343a230ecad630660a4133692089b38085a2d4d Mon Sep 17 00:00:00 2001 From: Louis Le Lay Date: Fri, 19 Sep 2025 02:50:40 +0200 Subject: [PATCH 04/16] resolves F401 --- source/isaaclab/isaaclab/devices/phone/se3_phone.py | 1 - .../place/config/agibot/place_toy2box_rmp_rel_env_cfg.py | 5 +++++ .../place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py | 5 +++++ .../manager_based/manipulation/reach/reach_env_cfg.py | 4 ++++ 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index 38e06299392..f12013b9cfb 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -8,7 +8,6 @@ from __future__ import annotations import numpy as np -import os import threading import time import torch diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py index 35d11c28f74..baddfd77b02 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_toy2box_rmp_rel_env_cfg.py @@ -332,6 +332,11 @@ def __post_init__(self): rot_sensitivity=0.05, sim_device=self.sim.device, ), + "phone": Se3PhoneCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), } ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py index 1321014125a..5cea8abff29 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/place/config/agibot/place_upright_mug_rmp_rel_env_cfg.py @@ -272,6 +272,11 @@ def __post_init__(self): rot_sensitivity=0.05, sim_device=self.sim.device, ), + "phone": Se3PhoneCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), } ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py index fb67991dfef..d111ffe2e32 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/reach_env_cfg.py @@ -226,5 +226,9 @@ def __post_init__(self): gripper_term=False, sim_device=self.sim.device, ), + "phone": Se3PhoneCfg( + gripper_term=False, + sim_device=self.sim.device, + ), }, ) From 43474d9616b42d93913fe62081733675bab45fdf Mon Sep 17 00:00:00 2001 From: Louis Le Lay Date: Fri, 19 Sep 2025 02:52:14 +0200 Subject: [PATCH 05/16] formats --- source/isaaclab/isaaclab/devices/phone/se3_phone.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index f12013b9cfb..c04312bb445 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -11,8 +11,8 @@ import threading import time import torch -from typing import Any, Optional from collections.abc import Callable +from typing import Any from isaaclab.utils.math import axis_angle_from_quat From e896d083d27cb87662d02ebd4573540571163edb Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 11:09:01 +0200 Subject: [PATCH 06/16] adds wip implementation for se2 phone --- .../isaaclab/devices/phone/se2_phone.py | 183 ++++++++++++++++++ .../isaaclab/devices/phone/se3_phone.py | 46 +++-- 2 files changed, 215 insertions(+), 14 deletions(-) create mode 100644 source/isaaclab/isaaclab/devices/phone/se2_phone.py diff --git a/source/isaaclab/isaaclab/devices/phone/se2_phone.py b/source/isaaclab/isaaclab/devices/phone/se2_phone.py new file mode 100644 index 00000000000..d04044a2e8b --- /dev/null +++ b/source/isaaclab/isaaclab/devices/phone/se2_phone.py @@ -0,0 +1,183 @@ +# 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 + +"""Phone controller for SE(3) control.""" + +from __future__ import annotations + +import numpy as np +import threading +import time +import torch +from collections.abc import Callable +from typing import Any + +from isaaclab.utils.math import axis_angle_from_quat + +from ..device_base import DeviceBase, DeviceCfg + +try: + from teleop import Teleop +except Exception as exc: # pragma: no cover + Teleop = None + _IMPORT_ERR = exc +else: + _IMPORT_ERR = None +from dataclasses import dataclass + + +@dataclass +class Se2PhoneCfg(DeviceCfg): + """Configuration for SE2 space mouse devices.""" + + v_x_sensitivity: float = 0.8 + v_y_sensitivity: float = 0.4 + omega_z_sensitivity: float = 1.0 + + +class Se2Phone(DeviceBase): + r"""A keyboard controller for sending SE(2) commands as velocity commands. + + This class is designed to provide a keyboard controller for mobile base (such as quadrupeds). + It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + task-space commands. + + The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \omega_z)`. + + Key bindings: + ====================== ========================= ======================== + Command Key (+ve axis) Key (-ve axis) + ====================== ========================= ======================== + Move along x-axis Numpad 8 / Arrow Up Numpad 2 / Arrow Down + Move along y-axis Numpad 4 / Arrow Right Numpad 6 / Arrow Left + Rotate along z-axis Numpad 7 / Z Numpad 9 / X + ====================== ========================= ======================== + + .. seealso:: + + The official documentation for the phone interface: `Phone Interface `__. + + """ + def __init__(self, cfg: Se2PhoneCfg): + """Initialize the phone layer. + + Args: + v_x_sensitivity: Magnitude of linear velocity along x-direction scaling. Defaults to 0.8. + v_y_sensitivity: Magnitude of linear velocity along y-direction scaling. Defaults to 0.4. + omega_z_sensitivity: Magnitude of angular velocity along z-direction scaling. Defaults to 1.0. + """ + if Teleop is None: + raise ImportError( + "teleop is not available. Install it first (e.g., `pip install teleop`)." + ) from _IMPORT_ERR + + # store inputs + self._v_x_sensitivity = cfg.v_x_sensitivity + self._v_y_sensitivity = cfg.v_y_sensitivity + self._omega_z_sensitivity = cfg.omega_z_sensitivity + self._sim_device = cfg.sim_device + # latest data (written by callback thread) + self._gripper = 1.0 + self._move_enabled = False + + self._latest_v_x: torch.Tensor | None = None + self._latest_v_y: torch.Tensor | None = None + self._latest_omega_z: torch.Tensor | None = None + self._latest_msg: dict[str, Any] = {} + + # Previous sample used to compute relative deltas + self._prev_v_x: torch.Tensor | None = None + self._prev_v_y: torch.Tensor | None = None + self._prev_omega_z: torch.Tensor | None = None + + # spin Teleop server in the background so `advance()` is non-blocking + self._teleop: Teleop | None = None + self._thread: threading.Thread | None = None + self._server_kwargs: dict | None = None + self._start_server(self._server_kwargs or {}) + + def reset(self) -> None: + self._prev_v_x = None + self._prev_v_y = None + self._prev_omega_z = None + + def add_callback(self, key: Any, func: Callable) -> None: + """Optional: bind a callback (unused for phone device).""" + # We could forward callbacks to Teleop if needed; noop for now. + return + + def advance(self) -> torch.Tensor: + """Provides the result from keyboard event state. + + Returns: + Tensor containing the linear (x,y) and angular velocity (z). + """ + command = torch.zeros(3, dtype=torch.float32, device=self._sim_device) + + if self._latest_v_x is None: + return command + + # print(self._move_enabled) + if self._prev_v_x is None: + # First sample: initialize reference + self._prev_v_x = self._latest_v_x.clone() + self._prev_v_y = self._latest_v_y.clone() + self._prev_omega_z = self._latest_omega_z.clone() + return command + + if not self._move_enabled: + # Gate OFF: zero deltas and keep reference synced to current to avoid jumps on re-enable + self._prev_v_x = self._latest_v_x.clone() + self._prev_v_y = self._latest_v_y.clone() + self._prev_omega_z = self._latest_omega_z.clone() + return command + + # Gate ON: compute SE(2) delta wrt previous, then update reference + dvx = torch.sub(self._latest_v_x, self._prev_v_x) + dvy = torch.sub(self._latest_v_y, self._prev_v_y) + d_omega_z = torch.sub(self._latest_omega_z, self._prev_omega_z) + print(f"dpos is {dvx, dvy}") + print(f"drot is {d_omega_z}") + + command[0] = dvx * self._v_x_sensitivity + command[1] = dvy * self._v_y_sensitivity + command[2] = d_omega_z * self._omega_z_sensitivity + + return command + + # Teleop plumbing + def _start_server(self, server_kwargs: dict) -> None: + self._teleop = Teleop(**server_kwargs) + + def _cb(_pose_unused: np.ndarray, message: dict) -> None: + # Expect "message" like the example in your comment. + if not isinstance(message, dict): + return + self._latest_msg = dict(message) + + # --- Parse position --- + p = message.get("position", {}) + self._latest_v_x = -float(p.get("z", 0.0)) + self._latest_v_y = -float(p.get("x", 0.0)) + + # --- Parse quaternion (x, y, z, w) and normalize --- + qd = message.get("orientation", {}) + qx = float(qd.get("x", 0.0)) + qy = float(qd.get("y", 0.0)) + qz = float(qd.get("z", 0.0)) + qw = float(qd.get("w", 1.0)) + + quat = torch.tensor([qw, qx, qy, qz], device=self._sim_device, dtype=torch.float32).unsqueeze(0) # (1, 4) + self._latest_omega_z = axis_angle_from_quat(quat).squeeze(0)[1] + + self._move_enabled = bool(message.get("move", False)) + + self._teleop.subscribe(_cb) + + self._thread = threading.Thread(target=self._teleop.run, name="TeleopServer", daemon=True) + self._thread.start() + + # give server a moment to boot + time.sleep(0.1) diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index c04312bb445..659daac2510 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -39,23 +39,41 @@ class Se3PhoneCfg(DeviceCfg): class Se3Phone(DeviceBase): - """Phone-based SE(3) teleop device. - - Returns a 7D tensor on `advance()`: - [dx, dy, dz, droll, dpitch, dyaw, gripper] - where the first 6 are *relative* deltas since the last frame (meters, radians) - and `gripper` is in {-1.0, +1.0} (close/open). - - Notes - ----- - - The device listens to a background `teleop.Teleop` server, which streams a 4x4 - end-effector target pose (in some chosen reference frame) and a message dict. - - When the message indicates the move gate is OFF, the deltas are zeroed but - the gripper command is still emitted from the message. - """ + """A keyboard controller for sending SE(3) commands as delta poses and binary command (open/close). + + This class is designed to provide a keyboard controller for a robotic arm with a gripper. + It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + task-space commands. + + The command comprises of two parts: + + * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. + * gripper: a binary command to open or close the gripper. + + Key bindings: + ============================== ================= ================= + Description Key (+ve axis) Key (-ve axis) + ============================== ================= ================= + Toggle gripper (open/close) K + Move along x-axis W S + Move along y-axis A D + Move along z-axis Q E + Rotate along x-axis Z X + Rotate along y-axis T G + Rotate along z-axis C V + ============================== ================= ================= + .. seealso:: + + The official documentation for the keyboard interface: `Carb Keyboard Interface `__. + + """ def __init__(self, cfg: Se3PhoneCfg): + """Initialize the phone layer. + Args: + cfg: Configuration object for keyboard settings. + """ if Teleop is None: raise ImportError( "teleop is not available. Install it first (e.g., `pip install teleop`)." From 32d05ee7d90d4c11d44626e349f24560f601116b Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 11:09:26 +0200 Subject: [PATCH 07/16] formats --- source/isaaclab/isaaclab/devices/phone/se2_phone.py | 1 + source/isaaclab/isaaclab/devices/phone/se3_phone.py | 1 + 2 files changed, 2 insertions(+) diff --git a/source/isaaclab/isaaclab/devices/phone/se2_phone.py b/source/isaaclab/isaaclab/devices/phone/se2_phone.py index d04044a2e8b..0e11a89fbd6 100644 --- a/source/isaaclab/isaaclab/devices/phone/se2_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se2_phone.py @@ -60,6 +60,7 @@ class Se2Phone(DeviceBase): The official documentation for the phone interface: `Phone Interface `__. """ + def __init__(self, cfg: Se2PhoneCfg): """Initialize the phone layer. diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index 659daac2510..6aa036c1768 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -68,6 +68,7 @@ class Se3Phone(DeviceBase): The official documentation for the keyboard interface: `Carb Keyboard Interface `__. """ + def __init__(self, cfg: Se3PhoneCfg): """Initialize the phone layer. From 6c3709ea91d3bb1dd974394ef195be0e69f87771 Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 11:29:05 +0200 Subject: [PATCH 08/16] adds init for se2 --- source/isaaclab/isaaclab/devices/__init__.py | 2 +- source/isaaclab/isaaclab/devices/phone/__init__.py | 1 + source/isaaclab/isaaclab/devices/teleop_device_factory.py | 3 ++- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/devices/__init__.py b/source/isaaclab/isaaclab/devices/__init__.py index d7d0dcbf4d9..08803967b5a 100644 --- a/source/isaaclab/isaaclab/devices/__init__.py +++ b/source/isaaclab/isaaclab/devices/__init__.py @@ -23,7 +23,7 @@ from .gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg from .keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg from .openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg -from .phone import Se3Phone, Se3PhoneCfg +from .phone import Se2Phone, Se2PhoneCfg, Se3Phone, Se3PhoneCfg from .retargeter_base import RetargeterBase, RetargeterCfg from .spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg from .teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/phone/__init__.py b/source/isaaclab/isaaclab/devices/phone/__init__.py index d02de17cd03..21aea4c952e 100644 --- a/source/isaaclab/isaaclab/devices/phone/__init__.py +++ b/source/isaaclab/isaaclab/devices/phone/__init__.py @@ -6,3 +6,4 @@ """Phone device for SE(2) and SE(3) control.""" from .se3_phone import Se3Phone, Se3PhoneCfg +from .se2_phone import Se2Phone, Se2PhoneCfg diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index a67260d2ab6..9e06095bf08 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -24,7 +24,7 @@ Se3RelRetargeter, Se3RelRetargeterCfg, ) -from isaaclab.devices.phone import Se3Phone, Se3PhoneCfg +from isaaclab.devices.phone import Se2Phone, Se2PhoneCfg, Se3Phone, Se3PhoneCfg from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.devices.spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg @@ -41,6 +41,7 @@ Se2KeyboardCfg: Se2Keyboard, Se2GamepadCfg: Se2Gamepad, Se2SpaceMouseCfg: Se2SpaceMouse, + Se2PhoneCfg: Se2Phone, OpenXRDeviceCfg: OpenXRDevice, ManusViveCfg: ManusVive, } From a41b89cb971d6f0230afb75fa061229f63ed559b Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 12:06:11 +0200 Subject: [PATCH 09/16] adds wip implementation for testing --- .../test/devices/test_device_constructors.py | 158 ++++++++++++++++++ 1 file changed, 158 insertions(+) diff --git a/source/isaaclab/test/devices/test_device_constructors.py b/source/isaaclab/test/devices/test_device_constructors.py index ffe44740d01..de32e676236 100644 --- a/source/isaaclab/test/devices/test_device_constructors.py +++ b/source/isaaclab/test/devices/test_device_constructors.py @@ -14,6 +14,7 @@ import importlib import torch +import numpy as np import pytest @@ -27,6 +28,10 @@ Se2KeyboardCfg, Se2SpaceMouse, Se2SpaceMouseCfg, + Se2Phone, + Se2PhoneCfg, + Se3Phone, + Se3PhoneCfg, Se3Gamepad, Se3GamepadCfg, Se3Keyboard, @@ -260,6 +265,68 @@ def test_se3spacemouse_constructors(mock_environment, mocker): assert isinstance(result, torch.Tensor) assert result.shape == (7,) # (pos_x, pos_y, pos_z, rot_x, rot_y, rot_z, gripper) +""" +Test phone devices. +""" + + +def test_se2phone_constructor(mock_environment, mocker): + """Test constructor and delta-output behavior for Se2Phone.""" + + # --- Fake Teleop that captures the callback and allows us to emit messages --- + class _FakeTeleop: + def __init__(self, **kwargs): + self._cb = None + + def subscribe(self, cb): + self._cb = cb + + def run(self): + # No-op: don't start any network/server loop in tests + return + + def emit(self, msg: dict): + assert self._cb is not None, "Callback not registered" + # The device ignores the pose argument; pass a dummy np.array + self._cb(np.zeros(7), msg) + + # Import the device module and patch Teleop with our fake + device_mod = importlib.import_module("isaaclab.devices.phone.se2_phone") + mocker.patch.object(device_mod, "Teleop", _FakeTeleop) + + # Build config with custom sensitivities (we'll verify via output) + cfg = Se2PhoneCfg(v_x_sensitivity=0.9, v_y_sensitivity=0.5, omega_z_sensitivity=1.2) + + # Create the device + phone = Se2Phone(cfg) + assert isinstance(phone, Se2Phone) + + # Grab the fake teleop instance to push messages + fake = phone._teleop + assert isinstance(fake, _FakeTeleop) + + # Helper: orientation kept constant so omega_z delta is zero (easier assertion) + orient = {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + + # 1) First message initializes internal reference; advance() should return zeros. + fake.emit({"move": True, "position": {"x": 0.0, "z": 0.0}, "orientation": orient}) + out1 = phone.advance() + assert isinstance(out1, torch.Tensor) + assert out1.shape == (3,) + assert torch.allclose(out1, torch.zeros(3, dtype=torch.float32, device=out1.device)) + + # 2) Second message changes position (x, z). Se2Phone maps latest_v_x=-z, latest_v_y=-x. + # Deltas: dvx = -(2.0 - 0.0) = -2.0, dvy = -(1.0 - 0.0) = -1.0 + fake.emit({"move": True, "position": {"x": 1.0, "z": 2.0}, "orientation": orient}) + out2 = phone.advance() + + # Expected scaled command: + # vx = dvx * 0.9 = -1.8 + # vy = dvy * 0.5 = -0.5 + # omega_z = 0.0 (orientation unchanged) + expected = torch.tensor([-1.8, -0.5, 0.0], dtype=torch.float32, device=out2.device) + assert out2.shape == (3,) + assert torch.allclose(out2, expected, atol=1e-5) """ Test OpenXR devices. @@ -320,6 +387,97 @@ def test_openxr_constructors(mock_environment, mocker): # Test reset functionality device.reset() +def test_se3phone_constructor(mocker): + """Test constructor and delta-output behavior for Se3Phone.""" + # --- Fake Teleop that captures the callback and lets us emit messages --- + class _FakeTeleop: + def __init__(self, **kwargs): + self._cb = None + + def subscribe(self, cb): + self._cb = cb + + def run(self): + return # no thread loop in tests + + def emit(self, msg: dict): + assert self._cb is not None, "Callback not registered" + self._cb(np.zeros(7), msg) # pose is unused by device + + # Import the device module and patch Teleop with our fake + device_mod = importlib.import_module("isaaclab.devices.phone.se3_phone") + mocker.patch.object(device_mod, "Teleop", _FakeTeleop) + + # Build config with custom sensitivities to verify scaling + cfg = Se3PhoneCfg(pos_sensitivity=0.5, rot_sensitivity=0.4, gripper_term=True) + + # Create the device + phone = Se3Phone(cfg) + assert isinstance(phone, Se3Phone) + + fake = phone._teleop + assert isinstance(fake, _FakeTeleop) + + # Keep gripper explicit; start with OPEN + # Keep orientation constant on first emit to initialize state + orient0 = {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + + # 1) First message initializes internal reference; advance() -> zeros (with gripper) + fake.emit({ + "move": True, + "position": {"x": 0.0, "y": 0.0, "z": 0.0}, + "orientation": orient0, + "gripper": "open", + }) + out1 = phone.advance() + assert isinstance(out1, torch.Tensor) + assert out1.shape == (7,) + # first call yields zeros for deltas; gripper=+1 + expected1 = torch.zeros(7, dtype=torch.float32, device=out1.device) + expected1[6] = 1.0 + assert torch.allclose(out1, expected1, atol=1e-6) + + # 2) Second message changes position & orientation. + # Position mapping inside device: latest_pos = [-z, -x, y] + # Use x=1, y=2, z=3 -> latest_pos = [-3, -1, 2], prev was [0,0,0] + # Orientation: apply yaw rotation by theta about Z: + # quat (w,x,y,z) = (cos(theta/2), 0, 0, sin(theta/2)) + # axis_angle_from_quat -> [0,0,theta] + # device remaps: rot[[0,1,2]] = rot[[2,0,1]] * [-1,-1,1] + # so -> [theta, 0, 0] * [-1,-1,1] = [-theta, 0, 0] + theta = 0.2 # radians + w = float(np.cos(theta / 2.0)) + z = float(np.sin(theta / 2.0)) + orient1 = {"x": 0.0, "y": 0.0, "z": z, "w": w} + + fake.emit({ + "move": True, + "position": {"x": 1.0, "y": 2.0, "z": 3.0}, + "orientation": orient1, + "gripper": "open", + }) + out2 = phone.advance() + + # Expected scaled deltas + dpos = torch.tensor([-3.0, -1.0, 2.0], dtype=torch.float32, device=out2.device) * 0.5 + drot = torch.tensor([-theta, 0.0, 0.0], dtype=torch.float32, device=out2.device) * 0.4 + expected2 = torch.cat([dpos, drot, torch.tensor([1.0], dtype=torch.float32, device=out2.device)], dim=0) + + assert out2.shape == (7,) + assert torch.allclose(out2, expected2, atol=1e-5) + + # 3) Gate OFF should zero deltas and resync reference + fake.emit({ + "move": False, # gate off + "position": {"x": 4.0, "y": 5.0, "z": 6.0}, + "orientation": orient1, # unchanged quaternion + "gripper": "close", + }) + out3 = phone.advance() + # deltas should be zero; gripper should now be -1 + expected3 = torch.zeros(7, dtype=torch.float32, device=out3.device) + expected3[6] = -1.0 + assert torch.allclose(out3, expected3, atol=1e-6) """ Test teleop device factory. From 8b3b751833817d1c026ec86710f88ea4349f3a62 Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 12:06:47 +0200 Subject: [PATCH 10/16] formats --- .../isaaclab/isaaclab/devices/phone/__init__.py | 2 +- .../test/devices/test_device_constructors.py | 15 ++++++++++----- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/devices/phone/__init__.py b/source/isaaclab/isaaclab/devices/phone/__init__.py index 21aea4c952e..9fbb94abcde 100644 --- a/source/isaaclab/isaaclab/devices/phone/__init__.py +++ b/source/isaaclab/isaaclab/devices/phone/__init__.py @@ -5,5 +5,5 @@ """Phone device for SE(2) and SE(3) control.""" -from .se3_phone import Se3Phone, Se3PhoneCfg from .se2_phone import Se2Phone, Se2PhoneCfg +from .se3_phone import Se3Phone, Se3PhoneCfg diff --git a/source/isaaclab/test/devices/test_device_constructors.py b/source/isaaclab/test/devices/test_device_constructors.py index de32e676236..f9f7a9b9c8b 100644 --- a/source/isaaclab/test/devices/test_device_constructors.py +++ b/source/isaaclab/test/devices/test_device_constructors.py @@ -13,8 +13,8 @@ """Rest everything follows.""" import importlib -import torch import numpy as np +import torch import pytest @@ -26,16 +26,16 @@ Se2GamepadCfg, Se2Keyboard, Se2KeyboardCfg, - Se2SpaceMouse, - Se2SpaceMouseCfg, Se2Phone, Se2PhoneCfg, - Se3Phone, - Se3PhoneCfg, + Se2SpaceMouse, + Se2SpaceMouseCfg, Se3Gamepad, Se3GamepadCfg, Se3Keyboard, Se3KeyboardCfg, + Se3Phone, + Se3PhoneCfg, Se3SpaceMouse, Se3SpaceMouseCfg, ) @@ -265,6 +265,7 @@ def test_se3spacemouse_constructors(mock_environment, mocker): assert isinstance(result, torch.Tensor) assert result.shape == (7,) # (pos_x, pos_y, pos_z, rot_x, rot_y, rot_z, gripper) + """ Test phone devices. """ @@ -328,6 +329,7 @@ def emit(self, msg: dict): assert out2.shape == (3,) assert torch.allclose(out2, expected, atol=1e-5) + """ Test OpenXR devices. """ @@ -387,8 +389,10 @@ def test_openxr_constructors(mock_environment, mocker): # Test reset functionality device.reset() + def test_se3phone_constructor(mocker): """Test constructor and delta-output behavior for Se3Phone.""" + # --- Fake Teleop that captures the callback and lets us emit messages --- class _FakeTeleop: def __init__(self, **kwargs): @@ -479,6 +483,7 @@ def emit(self, msg: dict): expected3[6] = -1.0 assert torch.allclose(out3, expected3, atol=1e-6) + """ Test teleop device factory. """ From 73949ef4556f38441c54e1499a414fb07d1faa98 Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 12:29:13 +0200 Subject: [PATCH 11/16] fixes what the test flagged --- .../isaaclab/devices/phone/se2_phone.py | 8 +- .../test/devices/test_device_constructors.py | 120 +++++++++--------- 2 files changed, 66 insertions(+), 62 deletions(-) diff --git a/source/isaaclab/isaaclab/devices/phone/se2_phone.py b/source/isaaclab/isaaclab/devices/phone/se2_phone.py index 0e11a89fbd6..7ba9149ede7 100644 --- a/source/isaaclab/isaaclab/devices/phone/se2_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se2_phone.py @@ -160,8 +160,12 @@ def _cb(_pose_unused: np.ndarray, message: dict) -> None: # --- Parse position --- p = message.get("position", {}) - self._latest_v_x = -float(p.get("z", 0.0)) - self._latest_v_y = -float(p.get("x", 0.0)) + self._latest_v_x = torch.tensor( + [-float(p.get("z", 0.0))], device=self._sim_device, dtype=torch.float32 + ).unsqueeze(0) + self._latest_v_y = torch.tensor( + [-float(p.get("x", 0.0))], device=self._sim_device, dtype=torch.float32 + ).unsqueeze(0) # --- Parse quaternion (x, y, z, w) and normalize --- qd = message.get("orientation", {}) diff --git a/source/isaaclab/test/devices/test_device_constructors.py b/source/isaaclab/test/devices/test_device_constructors.py index f9f7a9b9c8b..b4a9619f827 100644 --- a/source/isaaclab/test/devices/test_device_constructors.py +++ b/source/isaaclab/test/devices/test_device_constructors.py @@ -330,66 +330,6 @@ def emit(self, msg: dict): assert torch.allclose(out2, expected, atol=1e-5) -""" -Test OpenXR devices. -""" - - -def test_openxr_constructors(mock_environment, mocker): - """Test constructor for OpenXRDevice.""" - # Test config-based constructor with custom XrCfg - xr_cfg = XrCfg( - anchor_pos=(1.0, 2.0, 3.0), - anchor_rot=(0.0, 0.1, 0.2, 0.3), - near_plane=0.2, - ) - config = OpenXRDeviceCfg(xr_cfg=xr_cfg) - - # Create mock retargeters - mock_controller_retargeter = mocker.MagicMock() - mock_head_retargeter = mocker.MagicMock() - retargeters = [mock_controller_retargeter, mock_head_retargeter] - - device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") - mocker.patch.dict( - "sys.modules", - { - "carb": mock_environment["carb"], - "omni.kit.xr.core": mock_environment["omni"].kit.xr.core, - "isaacsim.core.prims": mocker.MagicMock(), - }, - ) - mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) - mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) - mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") - - # Configure the mock to return a string for prim_path - mock_instance = mock_single_xform.return_value - mock_instance.prim_path = "/XRAnchor" - - # Create the device using the factory - device = OpenXRDevice(config) - - # Verify the device was created successfully - assert device._xr_cfg == xr_cfg - - # Test with retargeters - device = OpenXRDevice(cfg=config, retargeters=retargeters) - - # Verify retargeters were correctly assigned as a list - assert device._retargeters == retargeters - - # Test with config and retargeters - device = OpenXRDevice(cfg=config, retargeters=retargeters) - - # Verify both config and retargeters were correctly assigned - assert device._xr_cfg == xr_cfg - assert device._retargeters == retargeters - - # Test reset functionality - device.reset() - - def test_se3phone_constructor(mocker): """Test constructor and delta-output behavior for Se3Phone.""" @@ -484,6 +424,66 @@ def emit(self, msg: dict): assert torch.allclose(out3, expected3, atol=1e-6) +""" +Test OpenXR devices. +""" + + +def test_openxr_constructors(mock_environment, mocker): + """Test constructor for OpenXRDevice.""" + # Test config-based constructor with custom XrCfg + xr_cfg = XrCfg( + anchor_pos=(1.0, 2.0, 3.0), + anchor_rot=(0.0, 0.1, 0.2, 0.3), + near_plane=0.2, + ) + config = OpenXRDeviceCfg(xr_cfg=xr_cfg) + + # Create mock retargeters + mock_controller_retargeter = mocker.MagicMock() + mock_head_retargeter = mocker.MagicMock() + retargeters = [mock_controller_retargeter, mock_head_retargeter] + + device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") + mocker.patch.dict( + "sys.modules", + { + "carb": mock_environment["carb"], + "omni.kit.xr.core": mock_environment["omni"].kit.xr.core, + "isaacsim.core.prims": mocker.MagicMock(), + }, + ) + mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) + mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) + mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") + + # Configure the mock to return a string for prim_path + mock_instance = mock_single_xform.return_value + mock_instance.prim_path = "/XRAnchor" + + # Create the device using the factory + device = OpenXRDevice(config) + + # Verify the device was created successfully + assert device._xr_cfg == xr_cfg + + # Test with retargeters + device = OpenXRDevice(cfg=config, retargeters=retargeters) + + # Verify retargeters were correctly assigned as a list + assert device._retargeters == retargeters + + # Test with config and retargeters + device = OpenXRDevice(cfg=config, retargeters=retargeters) + + # Verify both config and retargeters were correctly assigned + assert device._xr_cfg == xr_cfg + assert device._retargeters == retargeters + + # Test reset functionality + device.reset() + + """ Test teleop device factory. """ From 3d72a56619e4852c20f16a0a736574efd90f233a Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 13:35:02 +0200 Subject: [PATCH 12/16] updates comments --- .../isaaclab/devices/phone/se2_phone.py | 34 ++++++----------- .../isaaclab/devices/phone/se3_phone.py | 38 ++++++------------- 2 files changed, 22 insertions(+), 50 deletions(-) diff --git a/source/isaaclab/isaaclab/devices/phone/se2_phone.py b/source/isaaclab/isaaclab/devices/phone/se2_phone.py index 7ba9149ede7..f3360ef2613 100644 --- a/source/isaaclab/isaaclab/devices/phone/se2_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se2_phone.py @@ -30,7 +30,7 @@ @dataclass class Se2PhoneCfg(DeviceCfg): - """Configuration for SE2 space mouse devices.""" + """Configuration for SE2 phone devices.""" v_x_sensitivity: float = 0.8 v_y_sensitivity: float = 0.4 @@ -38,27 +38,19 @@ class Se2PhoneCfg(DeviceCfg): class Se2Phone(DeviceBase): - r"""A keyboard controller for sending SE(2) commands as velocity commands. + r"""A phone controller for sending SE(3) commands as delta poses and binary command (open/close). - This class is designed to provide a keyboard controller for mobile base (such as quadrupeds). - It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + This class is designed to provide a phone controller for a robotic arm with a gripper. + It uses the PyPi teleop package to listen to phone events and map them to robot's task-space commands. The command comprises of the base linear and angular velocity: :math:`(v_x, v_y, \omega_z)`. - Key bindings: - ====================== ========================= ======================== - Command Key (+ve axis) Key (-ve axis) - ====================== ========================= ======================== - Move along x-axis Numpad 8 / Arrow Up Numpad 2 / Arrow Down - Move along y-axis Numpad 4 / Arrow Right Numpad 6 / Arrow Left - Rotate along z-axis Numpad 7 / Z Numpad 9 / X - ====================== ========================= ======================== + See phone controller section in the teleoperation documentation for details: `Teleop `__ .. seealso:: - The official documentation for the phone interface: `Phone Interface `__. - + PyPi teleop package documentation: `Teleop `__. """ def __init__(self, cfg: Se2PhoneCfg): @@ -106,22 +98,21 @@ def reset(self) -> None: def add_callback(self, key: Any, func: Callable) -> None: """Optional: bind a callback (unused for phone device).""" - # We could forward callbacks to Teleop if needed; noop for now. return def advance(self) -> torch.Tensor: - """Provides the result from keyboard event state. + """Provides the result from phone event state. Returns: Tensor containing the linear (x,y) and angular velocity (z). """ command = torch.zeros(3, dtype=torch.float32, device=self._sim_device) - if self._latest_v_x is None: + if self._latest_v_x is None or self._latest_v_y is None or self._latest_omega_z is None: return command # print(self._move_enabled) - if self._prev_v_x is None: + if self._prev_v_x is None or self._prev_v_y is None or self._prev_omega_z is None: # First sample: initialize reference self._prev_v_x = self._latest_v_x.clone() self._prev_v_y = self._latest_v_y.clone() @@ -135,12 +126,10 @@ def advance(self) -> torch.Tensor: self._prev_omega_z = self._latest_omega_z.clone() return command - # Gate ON: compute SE(2) delta wrt previous, then update reference + # Gate ON: compute SE(2) delta wrt previous dvx = torch.sub(self._latest_v_x, self._prev_v_x) dvy = torch.sub(self._latest_v_y, self._prev_v_y) d_omega_z = torch.sub(self._latest_omega_z, self._prev_omega_z) - print(f"dpos is {dvx, dvy}") - print(f"drot is {d_omega_z}") command[0] = dvx * self._v_x_sensitivity command[1] = dvy * self._v_y_sensitivity @@ -153,7 +142,6 @@ def _start_server(self, server_kwargs: dict) -> None: self._teleop = Teleop(**server_kwargs) def _cb(_pose_unused: np.ndarray, message: dict) -> None: - # Expect "message" like the example in your comment. if not isinstance(message, dict): return self._latest_msg = dict(message) @@ -167,7 +155,7 @@ def _cb(_pose_unused: np.ndarray, message: dict) -> None: [-float(p.get("x", 0.0))], device=self._sim_device, dtype=torch.float32 ).unsqueeze(0) - # --- Parse quaternion (x, y, z, w) and normalize --- + # --- Parse quaternion (x, y, z, w) --- qd = message.get("orientation", {}) qx = float(qd.get("x", 0.0)) qy = float(qd.get("y", 0.0)) diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index 6aa036c1768..1acb5d32720 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -30,7 +30,7 @@ @dataclass class Se3PhoneCfg(DeviceCfg): - """Configuration for SE3 space mouse devices.""" + """Configuration for SE3 phone devices.""" gripper_term: bool = True pos_sensitivity: float = 0.5 @@ -39,10 +39,10 @@ class Se3PhoneCfg(DeviceCfg): class Se3Phone(DeviceBase): - """A keyboard controller for sending SE(3) commands as delta poses and binary command (open/close). + """A phone controller for sending SE(3) commands as delta poses and binary command (open/close). - This class is designed to provide a keyboard controller for a robotic arm with a gripper. - It uses the Omniverse keyboard interface to listen to keyboard events and map them to robot's + This class is designed to provide a phone controller for a robotic arm with a gripper. + It uses the PyPi teleop package to listen to phone events and map them to robot's task-space commands. The command comprises of two parts: @@ -50,22 +50,11 @@ class Se3Phone(DeviceBase): * delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians. * gripper: a binary command to open or close the gripper. - Key bindings: - ============================== ================= ================= - Description Key (+ve axis) Key (-ve axis) - ============================== ================= ================= - Toggle gripper (open/close) K - Move along x-axis W S - Move along y-axis A D - Move along z-axis Q E - Rotate along x-axis Z X - Rotate along y-axis T G - Rotate along z-axis C V - ============================== ================= ================= + See phone controller section in the teleoperation documentation for details: `Teleop `__ .. seealso:: - The official documentation for the keyboard interface: `Carb Keyboard Interface `__. + PyPi teleop package documentation: `Teleop `__. """ @@ -73,7 +62,7 @@ def __init__(self, cfg: Se3PhoneCfg): """Initialize the phone layer. Args: - cfg: Configuration object for keyboard settings. + cfg: Configuration object for phone settings. """ if Teleop is None: raise ImportError( @@ -109,7 +98,6 @@ def reset(self) -> None: def add_callback(self, key: Any, func: Callable) -> None: """Optional: bind a callback (unused for phone device).""" - # We could forward callbacks to Teleop if needed; noop for now. return def advance(self) -> torch.Tensor: @@ -121,11 +109,10 @@ def advance(self) -> torch.Tensor: command = torch.zeros(7, dtype=torch.float32, device=self._sim_device) command[6] = self._gripper - if self._latest_pos is None: + if self._latest_pos is None or self._latest_rot is None: return command - # print(self._move_enabled) - if self._prev_pos is None: + if self._prev_pos is None or self._prev_rot is None: # First sample: initialize reference self._prev_pos = self._latest_pos.clone() self._prev_rot = self._latest_rot.clone() @@ -137,11 +124,9 @@ def advance(self) -> torch.Tensor: self._prev_rot = self._latest_rot.clone() return command - # Gate ON: compute SE(3) delta wrt previous, then update reference + # Gate ON: compute SE(3) delta wrt previous dpos = torch.sub(self._latest_pos, self._prev_pos) drot = torch.sub(self._latest_rot, self._prev_rot) - print(f"dpos is {dpos}") - print(f"drot is {drot}") command[:3] = dpos * self._pos_sensitivity command[3:6] = drot * self._rot_sensitivity @@ -153,7 +138,6 @@ def _start_server(self, server_kwargs: dict) -> None: self._teleop = Teleop(**server_kwargs) def _cb(_pose_unused: np.ndarray, message: dict) -> None: - # Expect "message" like the example in your comment. if not isinstance(message, dict): return self._latest_msg = dict(message) @@ -166,7 +150,7 @@ def _cb(_pose_unused: np.ndarray, message: dict) -> None: self._latest_pos = torch.tensor([tx, ty, tz], device=self._sim_device, dtype=torch.float32) - # --- Parse quaternion (x, y, z, w) and normalize --- + # --- Parse quaternion (x, y, z, w) --- qd = message.get("orientation", {}) qx = float(qd.get("x", 0.0)) qy = float(qd.get("y", 0.0)) From b86a7570d8c84f76afb01a2f2e966c82083849f8 Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 13:35:26 +0200 Subject: [PATCH 13/16] implements wip docs --- .../imitation-learning/teleop_imitation.rst | 150 ++++++++++++------ 1 file changed, 104 insertions(+), 46 deletions(-) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 84b2551f6dc..469640bb89b 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -16,79 +16,137 @@ command is a 6-D vector representing the change in pose. Presently, Isaac Lab Mimic is only supported in Linux. -To play inverse kinematics (IK) control with a keyboard device: +.. tab-set:: + :sync-group: keyboard -.. code:: bash + .. tab-item:: Keyboard + :sync: keyboard - ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device keyboard + To play inverse kinematics (IK) control with a keyboard device: -For smoother operation and off-axis operation, we recommend using a SpaceMouse as the input device. Providing smoother demonstrations will make it easier for the policy to clone the behavior. To use a SpaceMouse, simply change the teleop device accordingly: + .. code:: bash -.. code:: bash + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device keyboard - ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device spacemouse + The script prints the teleoperation events configured. For keyboard, these are as follows -.. note:: + .. code:: text - If the SpaceMouse is not detected, you may need to grant additional user permissions by running ``sudo chmod 666 /dev/hidraw<#>`` where ``<#>`` corresponds to the device index - of the connected SpaceMouse. + Keyboard Controller for SE(3): Se3Keyboard + Reset all commands: R + Toggle gripper (open/close): K + Move arm along x-axis: W/S + Move arm along y-axis: A/D + Move arm along z-axis: Q/E + Rotate arm along x-axis: Z/X + Rotate arm along y-axis: T/G + Rotate arm along z-axis: C/V - To determine the device index, list all ``hidraw`` devices by running ``ls -l /dev/hidraw*``. - Identify the device corresponding to the SpaceMouse by running ``cat /sys/class/hidraw/hidraw<#>/device/uevent`` on each of the devices listed - from the prior step. - We recommend using local deployment of Isaac Lab to use the SpaceMouse. If using container deployment (:ref:`deployment-docker`), you must manually mount the SpaceMouse to the ``isaac-lab-base`` container by - adding a ``devices`` attribute with the path to the device in your ``docker-compose.yaml`` file: + .. tab-item:: SpaceMouse + :sync: spacemouse - .. code:: yaml + For smoother operation and off-axis operation, we recommend using a + SpaceMouse as the input device. Providing smoother demonstrations will + make it easier for the policy to clone the behavior. To use a + SpaceMouse, simply change the teleop device accordingly: - devices: - - /dev/hidraw<#>:/dev/hidraw<#> + .. code:: bash - where ``<#>`` is the device index of the connected SpaceMouse. + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device spacemouse - If you are using the IsaacLab + CloudXR container deployment (:ref:`cloudxr-teleoperation`), you can add the ``devices`` attribute under the ``services -> isaac-lab-base`` section of the - ``docker/docker-compose.cloudxr-runtime.patch.yaml`` file. + The script prints the teleoperation events configured. For SpaceMouse, these are as follows: - Isaac Lab is only compatible with the SpaceMouse Wireless and SpaceMouse Compact models from 3Dconnexion. + .. code:: text + SpaceMouse Controller for SE(3): Se3SpaceMouse + Reset all commands: Right click + Toggle gripper (open/close): Click the left button on the SpaceMouse + Move arm along x/y-axis: Tilt the SpaceMouse + Move arm along z-axis: Push or pull the SpaceMouse + Rotate arm: Twist the SpaceMouse -For tasks that benefit from the use of an extended reality (XR) device with hand tracking, Isaac Lab supports using NVIDIA CloudXR to immersively stream the scene to compatible XR devices for teleoperation. Note that when using hand tracking we recommend using the absolute variant of the task (``Isaac-Stack-Cube-Franka-IK-Abs-v0``), which requires the ``handtracking`` device: + .. note:: -.. code:: bash + If the SpaceMouse is not detected, you may need to grant additional + user permissions by running ``sudo chmod 666 /dev/hidraw<#>`` where + ``<#>`` corresponds to the device index of the connected SpaceMouse. - ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Abs-v0 --teleop_device handtracking --device cpu + To determine the device index, list all ``hidraw`` devices by + running ``ls -l /dev/hidraw*``. -.. note:: + Identify the device corresponding to the SpaceMouse by running + ``cat /sys/class/hidraw/hidraw<#>/device/uevent`` on each of + the devices listed from the prior step. + + We recommend using local deployment of Isaac Lab to use the SpaceMouse. + If using container deployment (:ref:`deployment-docker`), you must + manually mount the SpaceMouse to the ``isaac-lab-base`` container by + adding a ``devices`` attribute with the path to the device in your + ``docker-compose.yaml`` file: + + .. code:: yaml + + devices: + - /dev/hidraw<#>:/dev/hidraw<#> + + where ``<#>`` is the device index of the connected SpaceMouse. + + If you are using the IsaacLab + CloudXR container deployment + (:ref:`cloudxr-teleoperation`), you can add the ``devices`` + attribute under the ``services -> isaac-lab-base`` section + of the ``docker/docker-compose.cloudxr-runtime.patch.yaml`` + file. - See :ref:`cloudxr-teleoperation` to learn more about using CloudXR with Isaac Lab. + Isaac Lab is only compatible with the SpaceMouse Wireless and + SpaceMouse Compact models from 3Dconnexion. -The script prints the teleoperation events configured. For keyboard, -these are as follows: + .. tab-item:: Phone + :sync: phone -.. code:: text + As an alternative to the keyboard and SpaceMouse, it is also possible + to use your phone as a teleoperation device. It is more instinctive to + use than a keyboard, and more accessible than a SpaceMouse (but less + precise). To use your phone as a teleoperation device, use the + following command: + + .. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device phone + + The script prints the teleoperation events configured. For phone, these are as follows + + .. code:: text + + Phone Controller for SE(3): Se3Phone + + Reset all commands: R + Toggle gripper (open/close): K + Move arm along x-axis: W/S + Move arm along y-axis: A/D + Move arm along z-axis: Q/E + Rotate arm along x-axis: Z/X + Rotate arm along y-axis: T/G + Rotate arm along z-axis: C/V + + .. note:: + + Please note that only android devices are supported at this time. + + .. tab-item:: CloudXR Hand Tracking + :sync: cloudxr + + For tasks that benefit from the use of an extended reality (XR) device with hand tracking, Isaac Lab supports using NVIDIA CloudXR to immersively stream the scene to compatible XR devices for teleoperation. Note that when using hand tracking we recommend using the absolute variant of the task (``Isaac-Stack-Cube-Franka-IK-Abs-v0``), which requires the ``handtracking`` device: + + .. code:: bash - Keyboard Controller for SE(3): Se3Keyboard - Reset all commands: R - Toggle gripper (open/close): K - Move arm along x-axis: W/S - Move arm along y-axis: A/D - Move arm along z-axis: Q/E - Rotate arm along x-axis: Z/X - Rotate arm along y-axis: T/G - Rotate arm along z-axis: C/V + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Abs-v0 --teleop_device handtracking --device cpu -For SpaceMouse, these are as follows: + .. note:: -.. code:: text + See :ref:`cloudxr-teleoperation` to learn more about using CloudXR with Isaac Lab. - SpaceMouse Controller for SE(3): Se3SpaceMouse - Reset all commands: Right click - Toggle gripper (open/close): Click the left button on the SpaceMouse - Move arm along x/y-axis: Tilt the SpaceMouse - Move arm along z-axis: Push or pull the SpaceMouse - Rotate arm: Twist the SpaceMouse The next section describes how teleoperation devices can be used for data collection for imitation learning. From fcd2cb53ca5dc495070bb9453330553ed038f828 Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 14:08:21 +0200 Subject: [PATCH 14/16] updates docs --- .../imitation-learning/teleop_imitation.rst | 62 +++++++++---------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 469640bb89b..0719014898d 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -46,9 +46,9 @@ command is a 6-D vector representing the change in pose. .. tab-item:: SpaceMouse :sync: spacemouse - For smoother operation and off-axis operation, we recommend using a - SpaceMouse as the input device. Providing smoother demonstrations will - make it easier for the policy to clone the behavior. To use a + For smoother operation and off-axis operation, we recommend using a + SpaceMouse as the input device. Providing smoother demonstrations will + make it easier for the policy to clone the behavior. To use a SpaceMouse, simply change the teleop device accordingly: .. code:: bash @@ -68,21 +68,21 @@ command is a 6-D vector representing the change in pose. .. note:: - If the SpaceMouse is not detected, you may need to grant additional - user permissions by running ``sudo chmod 666 /dev/hidraw<#>`` where + If the SpaceMouse is not detected, you may need to grant additional + user permissions by running ``sudo chmod 666 /dev/hidraw<#>`` where ``<#>`` corresponds to the device index of the connected SpaceMouse. - To determine the device index, list all ``hidraw`` devices by + To determine the device index, list all ``hidraw`` devices by running ``ls -l /dev/hidraw*``. - Identify the device corresponding to the SpaceMouse by running - ``cat /sys/class/hidraw/hidraw<#>/device/uevent`` on each of + Identify the device corresponding to the SpaceMouse by running + ``cat /sys/class/hidraw/hidraw<#>/device/uevent`` on each of the devices listed from the prior step. - We recommend using local deployment of Isaac Lab to use the SpaceMouse. - If using container deployment (:ref:`deployment-docker`), you must + We recommend using local deployment of Isaac Lab to use the SpaceMouse. + If using container deployment (:ref:`deployment-docker`), you must manually mount the SpaceMouse to the ``isaac-lab-base`` container by - adding a ``devices`` attribute with the path to the device in your + adding a ``devices`` attribute with the path to the device in your ``docker-compose.yaml`` file: .. code:: yaml @@ -92,43 +92,43 @@ command is a 6-D vector representing the change in pose. where ``<#>`` is the device index of the connected SpaceMouse. - If you are using the IsaacLab + CloudXR container deployment - (:ref:`cloudxr-teleoperation`), you can add the ``devices`` - attribute under the ``services -> isaac-lab-base`` section - of the ``docker/docker-compose.cloudxr-runtime.patch.yaml`` + If you are using the IsaacLab + CloudXR container deployment + (:ref:`cloudxr-teleoperation`), you can add the ``devices`` + attribute under the ``services -> isaac-lab-base`` section + of the ``docker/docker-compose.cloudxr-runtime.patch.yaml`` file. - Isaac Lab is only compatible with the SpaceMouse Wireless and + Isaac Lab is only compatible with the SpaceMouse Wireless and SpaceMouse Compact models from 3Dconnexion. .. tab-item:: Phone :sync: phone - As an alternative to the keyboard and SpaceMouse, it is also possible - to use your phone as a teleoperation device. It is more instinctive to - use than a keyboard, and more accessible than a SpaceMouse (but less - precise). To use your phone as a teleoperation device, use the + As an alternative to the keyboard and SpaceMouse, it is also possible + to use your phone as a teleoperation device. It is more instinctive to + use than a keyboard, and more accessible than a SpaceMouse (but less + precise). To use your phone as a teleoperation device, use the following command: .. code:: bash ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --num_envs 1 --teleop_device phone - The script prints the teleoperation events configured. For phone, these are as follows + The script prints the teleoperation events configured. For keyboard, these are as follows .. code:: text - Phone Controller for SE(3): Se3Phone - - Reset all commands: R - Toggle gripper (open/close): K - Move arm along x-axis: W/S - Move arm along y-axis: A/D - Move arm along z-axis: Q/E - Rotate arm along x-axis: Z/X - Rotate arm along y-axis: T/G - Rotate arm along z-axis: C/V + Keyboard Controller for SE(3): Se3Keyboard + Keep the phone upright. + Toggle gripper (open/close): Enabled Gripper button + To move or rotate the arm: Press hold and do the corresponding action + Move arm along x-axis: Push/Pull your phone forward/backward + Move arm along y-axis: Pull/Pull your phone left/right + Move arm along z-axis: Push/Pull your phone up/down + Rotate arm along x-axis: Rotate your phone left/right + Rotate arm along y-axis: Tilt your phone forward/backward + Rotate arm along z-axis: Twist your phone left/right .. note:: From 53ae247c47a83bb48fd2c2398e067a8fcb6a018f Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 14:08:40 +0200 Subject: [PATCH 15/16] adds comments and integrations --- .../teleoperation/teleop_se3_agent.py | 2 +- scripts/tools/record_demos.py | 4 +++- .../isaaclab/devices/keyboard/se3_keyboard.py | 1 - .../isaaclab/isaaclab/devices/phone/se2_phone.py | 11 +++++++++++ .../isaaclab/isaaclab/devices/phone/se3_phone.py | 15 +++++++++++++++ .../config/ur10_gripper/stack_ik_rel_env_cfg.py | 7 +++++++ 6 files changed, 37 insertions(+), 3 deletions(-) diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index ab6b6b55240..a762366eae5 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -208,7 +208,7 @@ def stop_teleoperation() -> None: ) else: omni.log.error(f"Unsupported teleop device: {args_cli.teleop_device}") - omni.log.error("Supported devices: keyboard, spacemouse, gamepad, handtracking") + omni.log.error("Supported devices: keyboard, phone, spacemouse, gamepad, handtracking") env.close() simulation_app.close() return diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index ec01ffaaf8d..a23f9e3cf1b 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -89,7 +89,7 @@ import omni.log import omni.ui as ui -from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg, Se3Phone from isaaclab.devices.openxr import remove_camera_configs from isaaclab.devices.teleop_device_factory import create_teleop_device @@ -272,6 +272,8 @@ def setup_teleop_device(callbacks: dict[str, Callable]) -> object: teleop_interface = Se3Keyboard(Se3KeyboardCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) elif args_cli.teleop_device.lower() == "spacemouse": teleop_interface = Se3SpaceMouse(Se3SpaceMouseCfg(pos_sensitivity=0.2, rot_sensitivity=0.5)) + elif args_cli.teleop_device.lower() == "phone": + teleop_interface = Se3Phone() else: omni.log.error(f"Unsupported teleop device: {args_cli.teleop_device}") omni.log.error("Supported devices: keyboard, spacemouse, handtracking") diff --git a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py index 3c5e66034a3..b18fc3ef8b6 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py @@ -145,7 +145,6 @@ def advance(self) -> torch.Tensor: if self.gripper_term: gripper_value = -1.0 if self._close_gripper else 1.0 command = np.append(command, gripper_value) - print(command[:3]) return torch.tensor(command, dtype=torch.float32, device=self._sim_device) """ diff --git a/source/isaaclab/isaaclab/devices/phone/se2_phone.py b/source/isaaclab/isaaclab/devices/phone/se2_phone.py index f3360ef2613..b5e24c226f9 100644 --- a/source/isaaclab/isaaclab/devices/phone/se2_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se2_phone.py @@ -91,6 +91,17 @@ def __init__(self, cfg: Se2PhoneCfg): self._server_kwargs: dict | None = None self._start_server(self._server_kwargs or {}) + def __str__(self) -> str: + """Returns: A string containing the information of phone.""" + msg = f"Phone Controller for SE(2): {self.__class__.__name__}\n" + msg += "\t----------------------------------------------\n" + msg += "\tKeep the phone upright.\n" + msg += "\tTo move or rotate the arm: Press hold and do the corresponding action\n" + msg += "\tMove in X plane: Push/Pull your phone forward/backward\n" + msg += "\tMove in Y plane: Pull/Pull your phone left/right\n" + msg += "\tRotate in Z axis: Twist your phone left/right" + return msg + def reset(self) -> None: self._prev_v_x = None self._prev_v_y = None diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index 1acb5d32720..b9986912916 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -92,6 +92,21 @@ def __init__(self, cfg: Se3PhoneCfg): self._server_kwargs: dict | None = None self._start_server(self._server_kwargs or {}) + def __str__(self) -> str: + """Returns: A string containing the information of phone.""" + msg = f"Phone Controller for SE(3): {self.__class__.__name__}\n" + msg += "\t----------------------------------------------\n" + msg += "\tKeep the phone upright.\n" + msg += "\tToggle gripper (open/close): Enabled Gripper button\n" + msg += "\tTo move or rotate the arm: Press hold and do the corresponding action\n" + msg += "\tMove arm along x-axis: Push/Pull your phone forward/backward\n" + msg += "\tMove arm along y-axis: Pull/Pull your phone left/right\n" + msg += "\tMove arm along z-axis: Push/Pull your phone up/down\n" + msg += "\tRotate arm along x-axis: Rotate your phone left/right\n" + msg += "\tRotate arm along y-axis: Tilt your phone forward/backward\n" + msg += "\tRotate arm along z-axis: Twist your phone left/right" + return msg + def reset(self) -> None: self._prev_pos = None self._prev_rot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_ik_rel_env_cfg.py index 00c379ef19f..d53e46b0d73 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/ur10_gripper/stack_ik_rel_env_cfg.py @@ -7,6 +7,7 @@ from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.phone import Se3PhoneCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg from isaaclab.utils import configclass @@ -43,6 +44,9 @@ def __post_init__(self): rot_sensitivity=0.05, sim_device=self.sim.device, ), + "phone": Se3PhoneCfg( + sim_device=self.sim.device, + ), } ) @@ -76,5 +80,8 @@ def __post_init__(self): rot_sensitivity=0.05, sim_device=self.sim.device, ), + "phone": Se3PhoneCfg( + sim_device=self.sim.device, + ), } ) From 3cac315dc2f9078cc619e996f5ca7ff74474ef1f Mon Sep 17 00:00:00 2001 From: louislelay Date: Fri, 19 Sep 2025 14:09:08 +0200 Subject: [PATCH 16/16] formats --- scripts/tools/record_demos.py | 2 +- source/isaaclab/isaaclab/devices/phone/se2_phone.py | 2 +- source/isaaclab/isaaclab/devices/phone/se3_phone.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index a23f9e3cf1b..e7f29dc511c 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -89,7 +89,7 @@ import omni.log import omni.ui as ui -from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg, Se3Phone +from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg, Se3Phone, Se3SpaceMouse, Se3SpaceMouseCfg from isaaclab.devices.openxr import remove_camera_configs from isaaclab.devices.teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/phone/se2_phone.py b/source/isaaclab/isaaclab/devices/phone/se2_phone.py index b5e24c226f9..971890ed1c6 100644 --- a/source/isaaclab/isaaclab/devices/phone/se2_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se2_phone.py @@ -101,7 +101,7 @@ def __str__(self) -> str: msg += "\tMove in Y plane: Pull/Pull your phone left/right\n" msg += "\tRotate in Z axis: Twist your phone left/right" return msg - + def reset(self) -> None: self._prev_v_x = None self._prev_v_y = None diff --git a/source/isaaclab/isaaclab/devices/phone/se3_phone.py b/source/isaaclab/isaaclab/devices/phone/se3_phone.py index b9986912916..1584c40075d 100644 --- a/source/isaaclab/isaaclab/devices/phone/se3_phone.py +++ b/source/isaaclab/isaaclab/devices/phone/se3_phone.py @@ -106,7 +106,7 @@ def __str__(self) -> str: msg += "\tRotate arm along y-axis: Tilt your phone forward/backward\n" msg += "\tRotate arm along z-axis: Twist your phone left/right" return msg - + def reset(self) -> None: self._prev_pos = None self._prev_rot = None