Merge origin/pro6000_xh: differential drive, record/replay tools, scene assets
Conflicts resolved: - assets.py: keep /home/tangger/LYT path (ours) - mindrobot_cfg.py: keep our initial poses + fix path separator \\ → / - mindrobot_left_arm_ik_env_cfg.py: keep our wheel_action using MINDBOT_WHEEL_JOINTS constant (cleaner than remote's hardcoded list) From origin/pro6000_xh: - scripts/tools/: record_demos.py, replay_demos.py, hdf5 tools (Isaac Lab copy) - assets/: dryingbox.py, lab.py, table.py scene asset definitions - tele_se3_with_wheel_agent.py: WheelKeyboard inlined, differential drive keys Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
143
scripts/environments/teleoperation/mindrobot_keyboard.py
Normal file
143
scripts/environments/teleoperation/mindrobot_keyboard.py
Normal file
@@ -0,0 +1,143 @@
|
||||
# scripts/environments/teleoperation/mindrobot_keyboard.py
|
||||
# Copyright (c) 2022-2026, The Isaac Lab Project Developers.
|
||||
# SPDX-License-Identifier: BSD-3-Clause
|
||||
"""Shared keyboard controllers for MindRobot teleoperation and demo recording.
|
||||
|
||||
This module contains ONLY class definitions (no argparse, no AppLauncher),
|
||||
so it is safe to import from both standalone scripts and from record_demos.py.
|
||||
"""
|
||||
|
||||
import weakref
|
||||
import numpy as np
|
||||
import torch
|
||||
import carb
|
||||
|
||||
from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg
|
||||
|
||||
|
||||
class WheelKeyboard:
|
||||
"""Differential-drive (skid-steer) keyboard controller.
|
||||
|
||||
Listens to arrow keys via Carb and produces a 4-D joint-velocity
|
||||
command: [right_b, left_b, left_f, right_f] (rad/s).
|
||||
|
||||
Key mappings
|
||||
─────────────────────────────────────────────────────
|
||||
↑ (UP) forward [ v, v, v, v]
|
||||
↓ (DOWN) backward [-v, -v, -v, -v]
|
||||
← (LEFT) left turn [ v, -v, -v, v]
|
||||
→ (RIGHT) right turn[-v, v, v, -v]
|
||||
─────────────────────────────────────────────────────
|
||||
"""
|
||||
|
||||
_WHEEL_KEYS = {"UP", "DOWN", "LEFT", "RIGHT"}
|
||||
|
||||
def __init__(self, wheel_speed: float = 5.0, sim_device: str = "cuda:0"):
|
||||
self.wheel_speed = wheel_speed
|
||||
self._sim_device = sim_device
|
||||
self._wheel_vel = np.zeros(4)
|
||||
self._key_map: dict[str, np.ndarray] = {}
|
||||
self._create_bindings()
|
||||
|
||||
import omni
|
||||
self._appwindow = omni.appwindow.get_default_app_window()
|
||||
self._input = carb.input.acquire_input_interface()
|
||||
self._keyboard = self._appwindow.get_keyboard()
|
||||
self._keyboard_sub = self._input.subscribe_to_keyboard_events(
|
||||
self._keyboard,
|
||||
lambda event, *args, obj=weakref.proxy(self): obj._on_keyboard_event(event, *args),
|
||||
)
|
||||
|
||||
def __del__(self):
|
||||
self._input.unsubscribe_to_keyboard_events(self._keyboard, self._keyboard_sub)
|
||||
|
||||
def __str__(self) -> str:
|
||||
return (
|
||||
"WheelKeyboard (skid-steer chassis controller)\n"
|
||||
"\t↑ UP — forward\n"
|
||||
"\t↓ DOWN — backward\n"
|
||||
"\t← LEFT — left turn (in-place)\n"
|
||||
"\t→ RIGHT — right turn (in-place)"
|
||||
)
|
||||
|
||||
def reset(self):
|
||||
self._wheel_vel = np.zeros(4)
|
||||
|
||||
def advance(self) -> torch.Tensor:
|
||||
return torch.tensor(self._wheel_vel.copy(), dtype=torch.float32, device=self._sim_device)
|
||||
|
||||
def _create_bindings(self):
|
||||
v = self.wheel_speed
|
||||
self._key_map = {
|
||||
"UP": np.array([ v, v, v, v]),
|
||||
"DOWN": np.array([-v, -v, -v, -v]),
|
||||
"LEFT": np.array([ v, -v, -v, v]),
|
||||
"RIGHT": np.array([-v, v, v, -v]),
|
||||
}
|
||||
|
||||
def _on_keyboard_event(self, event, *args, **kwargs):
|
||||
if event.type == carb.input.KeyboardEventType.KEY_PRESS:
|
||||
key = event.input.name
|
||||
if key in self._key_map:
|
||||
self._wheel_vel += self._key_map[key]
|
||||
if event.type == carb.input.KeyboardEventType.KEY_RELEASE:
|
||||
key = event.input.name
|
||||
if key in self._key_map:
|
||||
self._wheel_vel -= self._key_map[key]
|
||||
return True
|
||||
|
||||
|
||||
class MindRobotCombinedKeyboard:
|
||||
"""组合遥操作控制器：左臂 IK (Se3Keyboard 7D) + 底盘轮速 (WheelKeyboard 4D)
|
||||
|
||||
输出 11 维 action: [arm_IK(6) | wheel_vel(4) | gripper(1)]
|
||||
与 MindRobotTeleopActionsCfg 中声明顺序一致。
|
||||
|
||||
Key bindings
|
||||
─────────────────────────────────────────────
|
||||
W/S/A/D/Q/E — arm IK translate X/Y/Z
|
||||
Z/X T/G C/V — arm IK rotate roll/pitch/yaw
|
||||
K — gripper toggle open/close
|
||||
↑↓←→ — chassis forward/backward/turn
|
||||
R — reset
|
||||
─────────────────────────────────────────────
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
pos_sensitivity: float = 0.05,
|
||||
rot_sensitivity: float = 0.05,
|
||||
wheel_speed: float = 5.0,
|
||||
sim_device: str = "cuda:0",
|
||||
):
|
||||
self._arm = Se3Keyboard(
|
||||
Se3KeyboardCfg(
|
||||
pos_sensitivity=pos_sensitivity,
|
||||
rot_sensitivity=rot_sensitivity,
|
||||
sim_device=sim_device,
|
||||
)
|
||||
)
|
||||
self._wheel = WheelKeyboard(wheel_speed=wheel_speed, sim_device=sim_device)
|
||||
|
||||
def reset(self):
|
||||
self._arm.reset()
|
||||
self._wheel.reset()
|
||||
|
||||
def add_callback(self, key: str, callback):
|
||||
"""Delegate callbacks to the arm keyboard (Se3Keyboard supports arbitrary key callbacks)."""
|
||||
self._arm.add_callback(key, callback)
|
||||
|
||||
def advance(self) -> torch.Tensor:
|
||||
arm_cmd = self._arm.advance() # (7,): [dx,dy,dz,rx,ry,rz, gripper]
|
||||
arm_ik = arm_cmd[:6] # (6,)
|
||||
gripper = arm_cmd[6:7] # (1,)
|
||||
wheel_vel = self._wheel.advance() # (4,)
|
||||
return torch.cat([arm_ik, wheel_vel, gripper]) # (11,)
|
||||
|
||||
def __str__(self) -> str:
|
||||
return (
|
||||
"MindRobotCombinedKeyboard\n"
|
||||
" Arm (Se3Keyboard): W/S/A/D/Q/E + Z/X/T/G/C/V, K=gripper\n"
|
||||
" Wheel (arrows): ↑↓←→\n"
|
||||
" R = reset | L = save-success-and-reset"
|
||||
)
|
||||
@@ -13,8 +13,6 @@ Extends teleop_se3_agent.py with differential-drive keyboard control:
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import weakref
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
from isaaclab.app import AppLauncher
|
||||
@@ -35,98 +33,20 @@ simulation_app = app_launcher.app
|
||||
|
||||
import logging
|
||||
import gymnasium as gym
|
||||
import carb
|
||||
|
||||
from isaaclab.devices import Se3Keyboard, Se3KeyboardCfg
|
||||
from isaaclab.envs import ManagerBasedRLEnvCfg
|
||||
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||
|
||||
import mindbot.tasks # noqa: F401
|
||||
from isaaclab_tasks.utils import parse_env_cfg
|
||||
|
||||
# mindrobot_keyboard.py lives in the same directory as this script.
|
||||
# Python adds the script's own directory to sys.path, so a direct import works.
|
||||
from mindrobot_keyboard import WheelKeyboard, MindRobotCombinedKeyboard # noqa: E402
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────
|
||||
# WheelKeyboard: 监听方向键，输出 4 维轮速 tensor
|
||||
# ─────────────────────────────────────────────────────────────
|
||||
class WheelKeyboard:
|
||||
"""Differential-drive (skid-steer) keyboard controller.
|
||||
|
||||
Listens to arrow keys via Carb and produces a 4-D joint-velocity
|
||||
command: [right_b, left_b, left_f, right_f] (rad/s).
|
||||
|
||||
Key mappings
|
||||
─────────────────────────────────────────────────────
|
||||
↑ (UP) forward [ v, v, v, v]
|
||||
↓ (DOWN) backward [-v, -v, -v, -v]
|
||||
← (LEFT) left turn [ v, -v, -v, v] right wheels fwd
|
||||
→ (RIGHT) right turn[-v, v, v, -v] left wheels fwd
|
||||
─────────────────────────────────────────────────────
|
||||
"""
|
||||
|
||||
_WHEEL_KEYS = {"UP", "DOWN", "LEFT", "RIGHT"}
|
||||
|
||||
def __init__(self, wheel_speed: float = 5.0, sim_device: str = "cuda:0"):
|
||||
self.wheel_speed = wheel_speed
|
||||
self._sim_device = sim_device
|
||||
# 4 维速度缓冲：[right_b, left_b, left_f, right_f]
|
||||
self._wheel_vel = np.zeros(4)
|
||||
# 按键 → 速度向量映射 (在 _create_bindings 中填充)
|
||||
self._key_map: dict[str, np.ndarray] = {}
|
||||
self._create_bindings()
|
||||
|
||||
# 订阅 Carb 键盘事件
|
||||
import omni
|
||||
self._appwindow = omni.appwindow.get_default_app_window()
|
||||
self._input = carb.input.acquire_input_interface()
|
||||
self._keyboard = self._appwindow.get_keyboard()
|
||||
self._keyboard_sub = self._input.subscribe_to_keyboard_events(
|
||||
self._keyboard,
|
||||
lambda event, *args, obj=weakref.proxy(self): obj._on_keyboard_event(event, *args),
|
||||
)
|
||||
|
||||
def __del__(self):
|
||||
self._input.unsubscribe_to_keyboard_events(self._keyboard, self._keyboard_sub)
|
||||
|
||||
def __str__(self) -> str:
|
||||
return (
|
||||
"WheelKeyboard (skid-steer chassis controller)\n"
|
||||
"\t↑ UP — forward\n"
|
||||
"\t↓ DOWN — backward\n"
|
||||
"\t← LEFT — left turn (in-place)\n"
|
||||
"\t→ RIGHT — right turn (in-place)"
|
||||
)
|
||||
|
||||
def reset(self):
|
||||
self._wheel_vel = np.zeros(4)
|
||||
|
||||
def advance(self) -> torch.Tensor:
|
||||
"""Returns 4-D wheel velocity tensor [right_b, left_b, left_f, right_f]."""
|
||||
return torch.tensor(self._wheel_vel.copy(), dtype=torch.float32, device=self._sim_device)
|
||||
|
||||
def _create_bindings(self):
|
||||
v = self.wheel_speed
|
||||
# [right_b, left_b, left_f, right_f]
|
||||
self._key_map = {
|
||||
"UP": np.array([ v, v, v, v]), # 前进
|
||||
"DOWN": np.array([-v, -v, -v, -v]), # 后退
|
||||
"LEFT": np.array([ v, -v, -v, v]), # 左转
|
||||
"RIGHT": np.array([-v, v, v, -v]), # 右转
|
||||
}
|
||||
|
||||
def _on_keyboard_event(self, event, *args, **kwargs):
|
||||
if event.type == carb.input.KeyboardEventType.KEY_PRESS:
|
||||
key = event.input.name
|
||||
if key in self._key_map:
|
||||
self._wheel_vel += self._key_map[key]
|
||||
if event.type == carb.input.KeyboardEventType.KEY_RELEASE:
|
||||
key = event.input.name
|
||||
if key in self._key_map:
|
||||
self._wheel_vel -= self._key_map[key]
|
||||
return True
|
||||
|
||||
|
||||
# ─────────────────────────────────────────────────────────────
|
||||
# main
|
||||
# ─────────────────────────────────────────────────────────────
|
||||
@@ -201,4 +121,4 @@ def main() -> None:
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
simulation_app.close()
|
||||
simulation_app.close()
|
||||
|
||||
Reference in New Issue
Block a user