Files
superspider-rl/scripts/superspider_teleop.py

108 lines
4.4 KiB
Python

"""Superspider simulation keyboard control. W/S: speed, A/D: yaw, Q/E: lateral,
C: center steering, X/Space: zero command, L: toggle steering latch.
Zero command is not a hardware emergency stop.
"""
import sys
from dataclasses import asdict, dataclass
from pathlib import Path
from src.resources import checkpoint_path
import torch
import tyro
from mjlab.envs import ManagerBasedRlEnv
from mjlab.rl import MjlabOnPolicyRunner, RslRlVecEnvWrapper
from mjlab.tasks.registry import list_tasks, load_env_cfg, load_rl_cfg, load_runner_cls
from mjlab.utils.torch import configure_torch_backends
from mjlab.viewer import NativeMujocoViewer
TASK = "Custom-Superspider-Flat"
VX_STEP, VY_STEP, WZ_STEP = 0.05, 0.03, 0.10
VX_RANGE, VY_RANGE, WZ_RANGE = (-0.25, 0.40), (-0.15, 0.15), (-0.8, 0.8)
K_RIGHT, K_LEFT, K_DOWN, K_UP, K_SPACE = 262, 263, 264, 265, 32
DECAY = 0.98 # 转向/横移每策略步(50Hz)乘的衰减 ⇒ 半衰期 ~0.7s
DEADZONE = 0.01 # 衰减到此以下直接归零,不留尾巴
@dataclass(frozen=True)
class Cfg:
checkpoint_file: str = "checkpoints/student_5999.pt"
task: str = "Custom-Superspider-Stairs-Distill"
device: str | None = None
class Teleop:
def __init__(self, policy, env: ManagerBasedRlEnv):
self._p = policy
self._robot = env.scene["robot"]
self._latch = False # False=方向盘回正,True=全部保持
self._n = 0
self._t = env.command_manager.get_term("twist")
self._t.cfg.heading_command = False
self._t.cfg.resampling_time_range = (1e9, 1e9)
self._cmd = torch.zeros(3, device=env.device)
ranges = self._t.cfg.ranges
self.ranges = ((min(0.0, ranges.lin_vel_x[0]), ranges.lin_vel_x[1]), ranges.lin_vel_y, ranges.ang_vel_z)
def _show(self):
vx, vy, wz = self._cmd.tolist()
v = self._robot.data.root_com_lin_vel_b[0, :2].tolist()
w = float(self._robot.data.root_com_ang_vel_b[0, 2])
print(f"\r[{'保持' if self._latch else '回正'}] 指令 vx={vx:+.2f} vy={vy:+.2f} wz={wz:+.2f}"
f" | 实测 vx={v[0]:+.3f} vy={v[1]:+.3f} wz={w:+.2f} ", end="", flush=True)
def key_callback(self, key: int) -> None:
c = chr(key).upper() if 0 <= key < 256 else ""
if key == K_UP or c == "W": self._cmd[0] += VX_STEP
elif key == K_DOWN or c == "S": self._cmd[0] -= VX_STEP
elif key == K_LEFT or c == "A": self._cmd[2] += WZ_STEP
elif key == K_RIGHT or c == "D": self._cmd[2] -= WZ_STEP
elif c == "Q": self._cmd[1] += VY_STEP
elif c == "E": self._cmd[1] -= VY_STEP
elif c == "C": self._cmd[1] = 0.0; self._cmd[2] = 0.0
elif c == "L": self._latch = not self._latch
elif c == "X" or key == K_SPACE: self._cmd[:] = 0.0
else: return
self._cmd[0].clamp_(*self.ranges[0])
self._cmd[1].clamp_(*self.ranges[1])
self._cmd[2].clamp_(*self.ranges[2])
self._show()
def __call__(self, obs) -> torch.Tensor:
if not self._latch: # 方向盘回正
self._cmd[1:] *= DECAY
self._cmd[1:][self._cmd[1:].abs() < DEADZONE] = 0.0
self._n += 1
if self._n % 10 == 0: # 5Hz 刷新,顺带显示实测速度
self._show()
# 每帧都清:这三个 buffer 里任何一个没清干净,指令都会被悄悄改写
self._t.is_standing_env[:] = False # 否则该 env 的指令被强制归零
self._t.is_heading_env[:] = False # 否则 wz 被航向控制器覆写 ⇒ X 停不下来
self._t.vel_command_b[:] = self._cmd
return self._p(obs)
def main() -> None:
import mjlab.tasks, src.tasks, mjlab # noqa: F401
configure_torch_backends()
args = tyro.cli(Cfg, config=mjlab.TYRO_FLAGS)
device = args.device or ("cuda:0" if torch.cuda.is_available() else "cpu")
ck = checkpoint_path(args.checkpoint_file)
if not ck.exists(): raise FileNotFoundError(ck)
env_cfg = load_env_cfg(args.task, play=True); env_cfg.scene.num_envs = 1
env_cfg.curriculum.pop("command_vel", None) # 别让课程再改范围
agent_cfg = load_rl_cfg(args.task)
env = ManagerBasedRlEnv(cfg=env_cfg, device=device)
w = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions)
runner = (load_runner_cls(args.task) or MjlabOnPolicyRunner)(w, asdict(agent_cfg), device=device)
from src.policy import load_policy
tele = Teleop(load_policy(runner, ck, device), env)
print(__doc__)
print(f"ckpt: {ck.name}")
NativeMujocoViewer(w, tele, key_callback=tele.key_callback).run()
env.close()
if __name__ == "__main__":
main()