diff --git a/scripts/environments/teleoperation/tele_se3_with_wheel_agent.py b/scripts/environments/teleoperation/tele_se3_with_wheel_agent.py new file mode 100644 index 0000000..26ba152 --- /dev/null +++ b/scripts/environments/teleoperation/tele_se3_with_wheel_agent.py @@ -0,0 +1,204 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to run teleoperation for MindRobot with chassis (wheel) control. + +Extends teleop_se3_agent.py with differential-drive keyboard control: + ↑ / ↓ — chassis forward / backward + ← / → — chassis left turn / right turn (in-place, skid-steer) + W/S/A/D/Q/E — left arm IK: translate X/Y/Z + Z/X T/G C/V — left arm IK: rotate roll/pitch/yaw + K — left gripper toggle open/close + R — reset environment +""" + +import argparse +import weakref +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="MindRobot teleoperation with wheel control.") +parser.add_argument("--num_envs", type=int, default=1) +parser.add_argument("--task", type=str, default=None) +parser.add_argument("--sensitivity", type=float, default=1.0) +parser.add_argument("--wheel_speed", type=float, default=5.0, + help="Wheel velocity in rad/s (default: 5.0)") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +# ---------- Rest everything follows ---------- + +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 + +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 +# ───────────────────────────────────────────────────────────── +def main() -> None: + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs) + env_cfg.env_name = args_cli.task + if not isinstance(env_cfg, ManagerBasedRLEnvCfg): + raise ValueError(f"Unsupported env config type: {type(env_cfg).__name__}") + env_cfg.terminations.time_out = None + + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + + should_reset = False + + def on_reset(): + nonlocal should_reset + should_reset = True + print("[teleop] Reset triggered.") + + # ── 机械臂 IK 控制器(Se3Keyboard,7 维输出)── + arm_keyboard = Se3Keyboard( + Se3KeyboardCfg( + pos_sensitivity=0.05 * args_cli.sensitivity, + rot_sensitivity=0.05 * args_cli.sensitivity, + sim_device=args_cli.device, + ) + ) + arm_keyboard.add_callback("R", on_reset) + + # ── 底盘轮速控制器(WheelKeyboard,4 维输出)── + wheel_keyboard = WheelKeyboard( + wheel_speed=args_cli.wheel_speed, + sim_device=args_cli.device, + ) + + print(arm_keyboard) + print(wheel_keyboard) + print("\nAction layout: [arm_IK(6) | wheel_vel(4) | gripper(1)] = 11 dims") + print("Press R to reset. Press Ctrl+C to exit.\n") + + env.reset() + arm_keyboard.reset() + wheel_keyboard.reset() + + while simulation_app.is_running(): + with torch.inference_mode(): + # ── 1. 获取机械臂 delta pose + gripper (7维) ── + arm_cmd = arm_keyboard.advance() # [dx,dy,dz,rx,ry,rz, gripper] + arm_ik = arm_cmd[:6] # (6,) + gripper = arm_cmd[6:7] # (1,) + + # ── 2. 获取底盘轮速 (4维) ── + wheel_vel = wheel_keyboard.advance() # [right_b, left_b, left_f, right_f] + + # ── 3. 拼接完整 action (11维) ── + # 顺序与 MindRobotTeleopActionsCfg 中的声明顺序一致: + # arm_action(6) → wheel_action(4) → gripper_action(1) + action = torch.cat([arm_ik, wheel_vel, gripper]) # (11,) + actions = action.unsqueeze(0).repeat(env.num_envs, 1) + + env.step(actions) + + if should_reset: + env.reset() + arm_keyboard.reset() + wheel_keyboard.reset() + should_reset = False + print("[teleop] Environment reset complete.") + + env.close() + + +if __name__ == "__main__": + main() + simulation_app.close() \ No newline at end of file diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py index 4ec0acd..398efe7 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py @@ -20,7 +20,7 @@ from mindbot.utils.assets import MINDBOT_ASSETS_DIR MINDBOT_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( - usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot/mindbot_cd2.usd", + usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindrobot/mindrobot_cd2.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, max_depenetration_velocity=5.0, @@ -106,7 +106,13 @@ MINDBOT_CFG = ArticulationCfg( damping=100.0, ), "wheels": ImplicitActuatorCfg( - joint_names_expr=[".*_revolute_Joint"], + # joint_names_expr=[".*_revolute_Joint"], + joint_names_expr=[ + "right_b_revolute_Joint", + "left_b_revolute_Joint", + "left_f_revolute_Joint", + "right_f_revolute_Joint", + ], effort_limit_sim=200.0, stiffness=0.0, damping=100.0, diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py index c1614e6..fc809b5 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py @@ -138,7 +138,13 @@ class MindRobotTeleopActionsCfg: # Action vector: [right_b_vel, left_b_vel, left_f_vel, right_f_vel] in rad/s. wheel_action = JointVelocityActionCfg( asset_name="robot", - joint_names=[".*_revolute_Joint"], + # joint_names=[".*_revolute_Joint"], + joint_names=[ + "right_b_revolute_Joint", + "left_b_revolute_Joint", + "left_f_revolute_Joint", + "right_f_revolute_Joint", + ], scale=1.0, ) diff --git a/source/mindbot/mindbot/utils/assets.py b/source/mindbot/mindbot/utils/assets.py index 9aff58d..ad019cd 100644 --- a/source/mindbot/mindbot/utils/assets.py +++ b/source/mindbot/mindbot/utils/assets.py @@ -20,5 +20,6 @@ import os ## MINDBOT_ASSETS_DIR: str = os.environ.get( "MINDBOT_ASSETS_DIR", - "/home/tangger/DataDisk/maic_usd_assets_moudle", + # "/home/tangger/DataDisk/maic_usd_assets_moudle", + "/home/maic/LYT/maic_usd_assets_moudle", )