From a3fab458fc006da09fb18835f64a7ab8282bb364 Mon Sep 17 00:00:00 2001 From: zilch Date: Wed, 7 Oct 2026 12:43:26 +0800 Subject: [PATCH] feat: unify robot configuration and deployment runtimes --- AGENTS.md | 4 +- README.md | 4 +- README.zh-CN.md | 4 +- .../advanced/input_devices_and_bindings.md | 9 + .../tutorial/advanced/motrix_deploy.md | 225 ++++++++ .../en/user_guide/tutorial/basic_frame.md | 5 +- .../advanced/input_devices_and_bindings.md | 11 +- .../tutorial/advanced/motrix_deploy.md | 204 +++++++ .../zh_CN/user_guide/tutorial/basic_frame.md | 5 +- motrix_deploy/README.md | 51 +- .../tests/test_simulation_runtime.py | 28 + motrix_deploy_mujoco/README.md | 25 +- .../tests/test_mujoco_deploy_backend.py | 4 +- motrix_deploy_tasks/README.md | 31 +- .../tests/test_training_parity.py | 5 +- motrix_deploy_unitree/README.md | 141 +---- motrix_deploy_unitree/pyproject.toml | 2 + .../src/motrix_deploy_unitree/actuation.py | 41 ++ .../src/motrix_deploy_unitree/cli.py | 4 + .../src/motrix_deploy_unitree/config.py | 102 ++-- .../src/motrix_deploy_unitree/direct.py | 88 +-- .../go2_joint_control.py | 8 +- .../src/motrix_deploy_unitree/hardware.py | 88 +++ .../src/motrix_deploy_unitree/interface.py | 144 +++-- .../src/motrix_deploy_unitree/sensor.py | 34 ++ .../tests/test_hardware_contracts.py | 249 ++++++++ .../tests/test_unitree_deploy_backend.py | 531 ++++++++++++++---- .../src/motrix_envs/deploy/go2_walk.py | 4 +- .../motrix_envs/locomotion/quadruped/cfg.py | 8 +- .../locomotion/quadruped/walk_np.py | 24 +- motrix_envs/tests/test_quadruped_walk.py | 109 +++- .../src/motrix_robots/assets/go1/go1.xml | 1 + motrix_robots/src/motrix_robots/unitree.py | 18 +- uv.lock | 4 + wiki/design/deploy-command-input.md | 26 +- wiki/design/index.md | 2 +- wiki/design/motrix-deploy.md | 335 ++++++++--- 37 files changed, 2045 insertions(+), 533 deletions(-) create mode 100644 motrix_deploy_unitree/src/motrix_deploy_unitree/actuation.py create mode 100644 motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py create mode 100644 motrix_deploy_unitree/src/motrix_deploy_unitree/sensor.py create mode 100644 motrix_deploy_unitree/tests/test_hardware_contracts.py diff --git a/AGENTS.md b/AGENTS.md index 69436fb3..0c29214e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -11,7 +11,7 @@ MotrixLab 是构建在 MotrixSim 仿真后端之上的强化学习框架，提 项目使用 UV workspace，包含以下 package： -- `motrix_env_core`：backend 无关的环境框架、配置与机器人 registry，不依赖具体 simulator +- `motrix_env_core`：backend 无关的环境框架、配置与机器人 registry - `motrix_env_motrixsim`：MotrixSim 仿真后端（SimBackend、scene compiler、renderer、torch frontend） - `motrix_env_mujoco`：仅用于编译的 MuJoCo scene 后端 - `motrix_robots`：可复用机器人配置、默认姿态与模型资产；仅依赖 core。框架代码从定义模块导入模型类，例如 `motrix_robots.unitree` @@ -23,7 +23,7 @@ MotrixLab 是构建在 MotrixSim 仿真后端之上的强化学习框架，提 - `motrix_deploy`：框架无关的 artifact、运行时契约与部署 CLI - `motrix_deploy_mujoco`：SceneCfg 支撑的 MuJoCo 后端插件 - `motrix_deploy_motrixsim`：SceneCfg 支撑的原生 MotrixSim 部署后端插件 -- `motrix_deploy_unitree`：Unitree SDK2 DDS 硬件后端插件 +- `motrix_deploy_unitree`：Unitree 硬件配置、sensor/actuation 接线与 SDK2 DDS 硬件后端插件 - `motrix_deploy_tasks`：具体部署任务实现 `scripts/` 提供训练、可视化、评估入口。内置环境由环境 package 注册并通过项目 registry 暴露；simulator backend diff --git a/README.md b/README.md index a3711e49..d3034835 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ _Microduck locomotion policies trained with MotrixLab, rendered in MotrixRender - **Unified Interface**: Provides a concise and unified reinforcement learning training and evaluation interface - **Multi-framework Support**: Supports SKRL (JAX/PyTorch), RSLRL (PyTorch), and the built-in FastSAC implementation - **Rich Environments**: Includes various robot simulation environments such as basic control, locomotion, and manipulation tasks -- **Sim-to-Real Deployment**: The same policy code deploys via the deploy CLI — Sim2Sim to MuJoCo or native MotrixSim, Sim2Real to real hardware +- **Sim-to-Real Deployment**: The same policy code deploys via the deploy CLI — simulation deployment to MuJoCo or native MotrixSim, hardware deployment to Unitree Go2 - **High-precision, High-performance Simulation**: Built on [MotrixSim](https://motrixsim.readthedocs.io/), a high-precision, high-performance physics engine - **Visual Training**: Supports real-time rendering and training process visualization @@ -181,7 +181,7 @@ MotrixLab is a [uv](https://docs.astral.sh/uv/) workspace with the following pac | **motrix_deploy** | `motrix-deploy` | Artifacts, control sessions, runtime contracts, and deployment CLI | | **motrix_deploy_mujoco** | `motrix-deploy-mujoco` | MuJoCo deployment backend plugin | | **motrix_deploy_motrixsim** | `motrix-deploy-motrixsim` | Native MotrixSim deployment backend plugin | -| **motrix_deploy_unitree** | `motrix-deploy-unitree` | Unitree SDK2 DDS hardware backend plugin | +| **motrix_deploy_unitree** | `motrix-deploy-unitree` | Asset-free Unitree hardware configuration, sensor/actuation wiring, and SDK2 DDS backend plugin | | **motrix_deploy_tasks** | `motrix-deploy-tasks` | Walking tasks, deployment scenes, and installed Hydra recipes | | **motrix_env_core** | `motrix-env-core` | Backend-agnostic environment framework, configuration, registry, and rendering | | **motrix_env_motrixsim** | `motrix-env-motrixsim` | Live MotrixSim backend, renderer, and Torch frontend | diff --git a/README.zh-CN.md b/README.zh-CN.md index 22af734f..58953340 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -46,7 +46,7 @@ _使用 MotrixLab 训练的 microduck 行走策略，由 MotrixRender 实时渲 - **统一接口**: 提供简洁统一的强化学习训练和评估接口 - **多框架支持**: 支持 SKRL（JAX/PyTorch）、RSLRL（PyTorch）和内置 FastSAC 实现 - **丰富环境**: 包含基础控制、运动、操作等多种机器人仿真环境 -- **Sim-to-Real 部署**: 同一套策略代码，通过部署 CLI Sim2Sim 到 MuJoCo 或原生 MotrixSim 仿真、Sim2Real 到真实硬件 +- **Sim-to-Real 部署**: 同一套策略代码，通过部署 CLI 部署到 MuJoCo 或原生 MotrixSim 仿真，以及 Unitree Go2 真机 - **高精度高性能仿真**: 基于 [MotrixSim](https://motrixsim.readthedocs.io/) 高精度、高性能物理仿真引擎 - **可视化训练**: 支持实时渲染和训练过程可视化 @@ -181,7 +181,7 @@ MotrixLab 是一个 [uv](https://docs.astral.sh/uv/) workspace，包含以下 pa | **motrix_deploy** | `motrix-deploy` | artifact、控制会话、运行时契约与部署 CLI | | **motrix_deploy_mujoco** | `motrix-deploy-mujoco` | MuJoCo 部署 backend plugin | | **motrix_deploy_motrixsim** | `motrix-deploy-motrixsim` | 原生 MotrixSim 部署 backend plugin | -| **motrix_deploy_unitree** | `motrix-deploy-unitree` | Unitree SDK2 DDS 硬件 backend plugin | +| **motrix_deploy_unitree** | `motrix-deploy-unitree` | 独立于资产的 Unitree 硬件配置、sensor/actuation 接线与 SDK2 DDS backend plugin | | **motrix_deploy_tasks** | `motrix-deploy-tasks` | 行走任务、部署场景与安装的 Hydra 配方 | | **motrix_env_core** | `motrix-env-core` | backend 无关的环境框架、配置、registry 与渲染 | | **motrix_env_motrixsim** | `motrix-env-motrixsim` | MotrixSim 实时仿真 backend、renderer 与 Torch frontend | diff --git a/docs/source/en/user_guide/tutorial/advanced/input_devices_and_bindings.md b/docs/source/en/user_guide/tutorial/advanced/input_devices_and_bindings.md index 2e6a55b8..cff16bf1 100644 --- a/docs/source/en/user_guide/tutorial/advanced/input_devices_and_bindings.md +++ b/docs/source/en/user_guide/tutorial/advanced/input_devices_and_bindings.md @@ -69,6 +69,15 @@ task.validate_command(context.command) observation = task.build_observation(state, context) ``` +Deployment's robot I/O boundary `RobotInterface` and optional `KeyboardDeviceProvider` / `GamePadDeviceProvider` +capability protocols are defined in `motrix_deploy.robot.interface`; import them from that module. The providers expose +input devices whose lifecycle they own, rather than discovering runtime plugins. Runtime discovery remains in +`motrix_deploy.runtime.factory` through `motrix_deploy.backends`, and recipes still select plugins with `runtime.backend`. +The prepared `runtime.robot` is available before opening. The application constructs +`ControlSession(robot=runtime.robot, command_binding=binding, ...)` explicitly, +binds it with `runtime.bind_control_session(control)`, and runs it inside `with runtime`. +The factory does not construct bindings or controllers through callbacks. + For example, MuJoCo deployment obtains a `MujocoKeyboardDevice` from the runtime's `KeyboardDeviceProvider` and constructs a `KeyboardPlanarVelocityBinding`. Training can use a random binding for the same `PlanarVelocityCommand`. Both paths end at the same typed task contract. diff --git a/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md b/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md index f61676c7..96f74b0b 100644 --- a/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md +++ b/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md @@ -122,3 +122,228 @@ motrix-deploy-unitree joint-control enp5s0 FL_thigh_joint 0.9 \ This helper uses the artifact's robot, gain, timing, and limit settings. It waits for Start and A, moves the joint, holds, returns to the default pose, and closes with damping. + +## Advanced usage + +Artifact inspection resolves the versioned task class through `motrix_deploy.tasks` and validates its Pydantic +`spec_type`, without constructing a task or importing training environments or simulator backends. +The JSON wire format remains `{name, config}`. Python uses direct spec attributes such as `spec.kp` and +`spec.action_scale`; action scale and bounds are joint vectors in canonical order. Policy action bounds apply +before scaling and adding the default pose; robot position limits apply to the resulting targets. + +### Load a task spec from YAML + +For a programmatic application, keep the complete Go2 task fields in `task.yaml` (the contents of +`task.config`, not the manifest envelope). OmegaConf loads and merges configuration and resolves interpolations; +Pydantic then validates the plain dictionary: + +```python +from omegaconf import OmegaConf +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec + +cfg = OmegaConf.load("task.yaml") +spec = Go2WalkTaskSpec.model_validate(OmegaConf.to_container(cfg, resolve=True)) +``` + +`Go2WalkTaskSpec` uses field constraints and a model validator for joint-vector lengths and ordered bounds. +No separate `validate()` call is needed. Use the artifact reader for a full deployment manifest. + +### Startup and array validation + +`RobotInterface.enable()` takes no initial command. Unitree owns the transition to +`RobotSpec.default_joint_position`, using zero target velocity/feedforward torque +and the resolved startup gains. `runtime.kp` / `runtime.kd` override artifact gains; +`null` retains the artifact gains. Startup does not call task `process_action()` +with a zero action. Initial-state safety and task-termination checks still run before +enable, and fresh-state checks run afterward; Start/A gating remains unchanged. + +`DeployTask` declares neither `observation_size` nor `action_size`. The policy +input boundary validates actual observations, and the task action-conversion +boundary validates actual policy outputs. CLI assembly does not precheck task +dimensions. Manifest tensor specs remain model metadata; `DeploymentProfile` +dimensions remain export-only checks against the compiled environment. + +### Hardware sensor and motor wiring + +`motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg` owns asset-free hardware poses and wiring. +`HardwareSensorBinding` maps SDK IMU fields, including `wxyz` quaternion conversion to canonical `xyzw`; +`UnitreeMotorBinding` maps canonical joint names to SDK motor indices. These definitions live in the +hardware plugin's `sensor` and `actuation` modules, not in simulation robot models. +The artifact still owns the policy joint order, default pose, and limits. Hardware configuration imports +neither model assets nor a simulator or SDK eagerly, and does not fabricate base position or linear velocity. + +### Select a deployment world + +Select a world with `runtime.deploy_env_id` and a robot with the existing registry ID `runtime.robot_id`. +The CLI calls `motrix_deploy.env.assemble_deploy_scene(deploy_env_id, robot_id)`: it creates the robot-free world, +fills `scene.objs.robot` using `motrix_env_core.registry.make_robot_config(robot_id)`. +The core registry lazily discovers installed `motrix_env_core.robots` entry points, shared by training and deployment. +The deployment worlds `flat` and `rough` live in independent `motrix_deploy_tasks.envs` modules. They use core +floor/height-field configuration without importing a training task, robot model, or training contact setup. + +To keep the same Go2 recipe and artifact but switch its world: + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.deploy_env_id=rough +``` + +A world-only override leaves robot attachment placement unchanged. Adjust `runtime.robot_translation` if the terrain +needs different clearance; the complete `task=go2-walk-rough/sim` recipe supplies its own attachment defaults. + +The installed flat recipe selects the world and robot by ID: + +```yaml +runtime: + kind: simulation + backend: mujoco + deploy_env_id: flat + robot_id: go2 + robot_translation: [0.0, 0.0, -0.114] + robot_rotation: [0.0, 0.0, 0.0, 1.0] +``` + +`runtime.physics` and `runtime.sensor_bindings` remain separate settings. MuJoCo accepts only the completed `SceneCfg`; +it neither selects an environment/robot pair nor assembles the scene or queries a training registry. +Programmatic `SimulationRuntimeConfig.scene` receives the same complete scene with the robot declared only in `scene.objs.robot`. + +Placement has one source: `RobotCfg.translation` (Vec3) and `rotation` (xyzw) are attachment transforms that compose once +with the model's intrinsic base pose, not absolute world root poses. Go2's intrinsic base height is 0.445 m; the flat +recipe's -0.114 m offset gives 0.331 m, and the rough recipe's -0.025 m offset gives 0.42 m with identity rotation. +The runtime preserves these transforms on its copied scene. `mj_resetData` restores the compiled `model.qpos0`, including +the composed root placement; initialization overrides only articulation joints with the bound `RobotSpec` key pose. + +For an external world plugin, declare a zero-argument `SceneCfg` factory in the `motrix_deploy.envs` entry-point group. +`motrix_deploy.env.available_deploy_envs()` lists installed IDs without loading factories; +`create_deploy_env(id)` loads only the selected factory and requires `scene.objs.robot` to be empty. +The core assembly helper inserts the robot selected by the existing robot registry; no generic scene-factory registry +or Hydra scene instantiation is involved. The CLI consumes `deploy_env_id`, `robot_id`, and optional +`robot_translation` / `robot_rotation`, applying attachment overrides to `scene.objs.robot.translation` / `.rotation` +before passing the ready scene to the simulation plugin. These application options are not runtime backend fields. + +`SimulationRuntimeConfig` owns scene, rendering, realtime pacing, and state-source settings. +Its `physics` field uses the shared `motrix_env_core.config.sim.SimCfg` for `dt`, +`solver_iterations`, `solver_tolerance`, and `gravity`, defaulting to +`SimCfg(dt=0.002, solver_iterations=100)`. Both backend compilers consume this field directly: +optional solver/gravity fields set to `None` preserve model-source values; explicit values override them. +The YAML layout remains `runtime.physics`, independent of scene and state sensors. +Set physics values before runtime construction: the runtime reads the shared `SimCfg` used for +compilation; changing `dt` after compilation is not a supported runtime retuning workflow. + +Programmatic applications use the same helper and pass its result to `SimulationRuntimeConfig`: + +```python +from motrix_deploy.env import assemble_deploy_scene +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_env_core.config.sim import SimCfg + +scene = assemble_deploy_scene("flat", "go2") +scene.objs.robot.translation = (0.0, 0.0, -0.114) +scene.objs.robot.rotation = (0.0, 0.0, 0.0, 1.0) +config = SimulationRuntimeConfig( + scene=scene, + physics=SimCfg(dt=0.002, solver_iterations=100), + sensor_bindings=SensorBindings( + base_angular_velocity="gyro", + base_linear_acceleration="accelerometer", + base_linear_velocity="global_linvel", + ), +) +``` + +`SimulationRuntimeConfig.sensor_bindings` uses the frozen `SensorBindings` dataclass, +not a Python dictionary. It supports only `base_angular_velocity`, +`base_linear_acceleration`, and `base_linear_velocity`, each `str | None` with a +`None` default. Names must be nonempty and contain a nonwhitespace character; +unknown roles and non-string values are rejected. YAML `runtime.sensor_bindings` +keeps the same mapping keys and is converted with `SensorBindings(**mapping)` +at the configuration boundary. An absent field or explicit `None` means no binding; +MuJoCo rejects either for its three required roles when opening the robot. +The backend still checks sensor existence, type, dimension, and frame; it does not +synthesize state for other roles or bind every `RobotState` field to a sensor. + +### Assemble the runtime and controller explicitly + +The runtime factory selects the backend and prepares `runtime.robot`; its bound +`RobotSpec` is available before `open()`. The application creates the task, policy, +and command binding, then constructs and binds the control session. Using the +`config` above and your application-owned `task`, `policy`, and `command_binding`: + +```python +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.factory import create_simulation_runtime + +runtime = create_simulation_runtime("mujoco", config) +robot = runtime.robot +control = ControlSession( + robot=robot, + task=task, + policy=policy, + command_binding=command_binding, + period_s=0.02, +) +runtime.bind_control_session(control) +with runtime: + result = runtime.run(steps=50) +``` + +Construct a robot-dependent task from `robot.spec` after runtime creation. Simulation +runtimes derive `RobotSpec` only from the scene; they do not accept an external spec. +For artifact deployment, the CLI validates the artifact's robot contract against +`runtime.robot.spec` before assembling control. +`ControlSession.period_s` is the single control-period setting; `SimulationRuntimeConfig` +does not duplicate it. The simulation runtime validates it against the physics timestep +when binding or resolving the session. Runtime creation does not accept controller-building callbacks. The runtime context owns +world resources and simulation advancement; `ControlSession` owns shared control +execution, and `RobotInterface` owns robot I/O without advancing or closing the world. +Hardware applications use `create_hardware_runtime(name, config, context)` followed +by the same explicit construction and binding. Direct callers can also supply a +session to `runtime.run(control, steps=...)`. + +### Select native MotrixSim deployment + +The installed simulation recipes default to MuJoCo. With `motrix-deploy-motrixsim` +installed, choose the native backend using the same recipe and artifact: + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.backend=motrixsim runtime.viewer=false runtime.realtime=false duration_s=1.0 +``` + +The same override works for `task=go2-walk-rough/sim`; no duplicated task recipe is +needed. Programmatic applications call `create_simulation_runtime("motrixsim", config)` +and follow the explicit session-binding sequence above. The Go2 state mappings remain +`gyro`, `accelerometer`, and `global_linvel`: native sensors validate local angular +velocity/acceleration and world-frame linear velocity. The runtime owns native MSD +compilation, model/data, reset, physics, and optional SDK `RenderApp` rendering, +not a MuJoCo runtime or training environment. Removing `runtime.viewer=false` enables the +native viewer and its keyboard provider in a working graphical environment. + +For the programmatic ONNX example, run +`python examples/deploy_to_sim.py --backend motrixsim --headless --steps 50` +with a matching ONNX policy; see the example's policy-file options. This path loads raw ONNX, not a +`.deploy` manifest: task preprocessing, tensor specs, and control settings are explicit +in the Python example and must match the model. Its scene and controller are shared +with the default MuJoCo path. Headless uses a zero-velocity command; windowed mode uses +the viewer keyboard provider when available. + +### Use an external Hydra config + +The native commands above work with the installed task package, without a checkout config directory. The canonical +built-in runtime recipes live in `motrix_deploy_tasks/config/task/{task}/{sim,hardware}.yaml`; prefer `task=...` to select them. +For a custom application's own configuration, Hydra's standard `--config-path` and `--config-name` remain available. +This illustrative command assumes you have authored `/absolute/path/to/my_app/config/deploy.yaml`; it does not select +or copy a built-in recipe: + +```bash +motrix-deploy \ + --config-path /absolute/path/to/my_app/config \ + --config-name deploy \ + artifact=artifacts/go2-walk-flat.deploy +``` + +Use an absolute config path: Hydra resolves relative paths against the CLI module, not the shell's current directory. +External configs own their defaults and complete runtime/backend configuration, and need not use the installed `task` +group. For simulation, select `runtime=sim` in defaults and provide `runtime.backend`, `runtime.deploy_env_id`, and `runtime.robot_id`, plus all +settings required by the selected backend (including MuJoCo state-sensor mappings). The artifact does not supply these +runtime choices. Use Hydra's normal options; there is no additional `--config` argument. diff --git a/docs/source/en/user_guide/tutorial/basic_frame.md b/docs/source/en/user_guide/tutorial/basic_frame.md index 407a44f0..bcb40ff7 100644 --- a/docs/source/en/user_guide/tutorial/basic_frame.md +++ b/docs/source/en/user_guide/tutorial/basic_frame.md @@ -50,8 +50,9 @@ MotrixLab is a UV workspace. The packages are grouped by ownership rather than t | `configs/`, `scripts/` | Configuration and orchestration | Hydra algorithm configs and Task recipes; train / play / view / export entry points | | `motrix_deploy` | Policy deployment | Framework-independent artifacts, runtime contracts, and deployment CLI | | `motrix_deploy_mujoco` | Policy deployment | MuJoCo deployment backend plugin | -| `motrix_deploy_unitree` | Policy deployment | Unitree SDK2 DDS hardware backend plugin | -| `motrix_deploy_tasks` | Policy deployment | Walking tasks, deployment scenes, and installed Hydra recipes | +| `motrix_deploy_motrixsim` | Policy deployment | MotrixSim deployment backend plugin | +| `motrix_deploy_unitree` | Policy deployment | Unitree hardware configs, sensor/actuation wiring, and SDK2 DDS backend plugin | +| `motrix_deploy_tasks` | Policy deployment | Versioned tasks via `motrix_deploy.tasks`; robot-free worlds via `motrix_deploy.envs`; installed Hydra runtime recipes | A simulation backend (such as `motrix_env_motrixsim`) is isolated behind the `SimBackend` interface — when using an environment you normally do not need to care which one it is. To select or integrate a diff --git a/docs/source/zh_CN/user_guide/tutorial/advanced/input_devices_and_bindings.md b/docs/source/zh_CN/user_guide/tutorial/advanced/input_devices_and_bindings.md index df91c476..615fac54 100644 --- a/docs/source/zh_CN/user_guide/tutorial/advanced/input_devices_and_bindings.md +++ b/docs/source/zh_CN/user_guide/tutorial/advanced/input_devices_and_bindings.md @@ -66,7 +66,16 @@ task.validate_command(context.command) observation = task.build_observation(state, context) ``` -例如 MuJoCo deployment 从运行时的 `KeyboardDeviceProvider` 取得 `MujocoKeyboardDevice`，再创建 +部署的机器人 I/O 边界 `RobotInterface` 与可选的 `KeyboardDeviceProvider` / `GamePadDeviceProvider` +capability protocol 定义在 `motrix_deploy.robot.interface`，使用方从该模块导入。provider 提供由自身管理生命周期的 +输入设备，不负责 runtime 插件发现。runtime discovery 仍由 `motrix_deploy.runtime.factory` 通过 +`motrix_deploy.backends` 完成，recipe 仍用 `runtime.backend` 选择插件。 +准备好的 `runtime.robot` 在打开前即可使用。应用显式构造 +`ControlSession(robot=runtime.robot, command_binding=binding, ...)`， +通过 `runtime.bind_control_session(control)` 绑定，再在 `with runtime` 内运行。 +Factory 不通过回调构造 binding 或控制器。 + +例如 MuJoCo deployment 从 runtime 的 `KeyboardDeviceProvider` 取得 `MujocoKeyboardDevice`，再创建 `KeyboardPlanarVelocityBinding`。训练环境则可以为同一个 `PlanarVelocityCommand` 使用 random binding。两条路径最终 进入同一个 typed task contract。 diff --git a/docs/source/zh_CN/user_guide/tutorial/advanced/motrix_deploy.md b/docs/source/zh_CN/user_guide/tutorial/advanced/motrix_deploy.md index 23374b39..e9723ec7 100644 --- a/docs/source/zh_CN/user_guide/tutorial/advanced/motrix_deploy.md +++ b/docs/source/zh_CN/user_guide/tutorial/advanced/motrix_deploy.md @@ -115,3 +115,207 @@ motrix-deploy-unitree joint-control enp5s0 FL_thigh_joint 0.9 \ 该工具使用 artifact 中的机器人、增益、时序和限位设置。等待 Start 和 A 后，移动关节、保持、返回默认姿态， 最后以阻尼模式关闭。 + +## 进阶说明 + +Artifact 检查通过 `motrix_deploy.tasks` 解析版本化 task 类并校验其 Pydantic `spec_type`， +不构造 task，也不 import 训练环境或仿真后端。JSON wire format 保持 `{name, config}`。 +Python 使用 `spec.kp`、`spec.action_scale` 等直接属性；action scale 与上下界是规范关节顺序的向量。 +Policy action 上下界先于缩放和默认姿态偏移生效；机器人 position limits 约束最终关节目标。 + +### 从 YAML 加载 task spec + +程序化应用可在 `task.yaml` 中保存完整 Go2 任务字段（即 `task.config` 的内容，不包含 manifest envelope）。 +OmegaConf 负责加载、合并配置和解析插值，随后由 Pydantic 校验普通字典： + +```python +from omegaconf import OmegaConf +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec + +cfg = OmegaConf.load("task.yaml") +spec = Go2WalkTaskSpec.model_validate(OmegaConf.to_container(cfg, resolve=True)) +``` + +`Go2WalkTaskSpec` 使用字段约束和 model validator 校验关节向量长度及上下界顺序，无需单独调用 `validate()`。 +完整 deployment manifest 应通过 artifact reader 读取。 + +### 启动与数组校验 + +`RobotInterface.enable()` 不接收初始命令。Unitree 自行过渡到 `RobotSpec.default_joint_position`， +目标速度和 feedforward torque 为零，使用解析后的启动增益。`runtime.kp` / `runtime.kd` 覆盖 artifact 增益； +`null` 保留 artifact 增益。启动不通过 task `process_action()` 转换零动作。 +初始状态 safety 与 task termination 检查仍先于 enable，之后重新检查新状态；Start/A 门控不变。 + +`DeployTask` 不声明 `observation_size` 或 `action_size`。policy 输入边界校验实际 observation， +task action 转换边界校验实际 policy 输出。CLI 组装不提前比较 task 维度。manifest tensor specs +保持模型元数据；`DeploymentProfile` 维度仅用于导出时与编译环境交叉校验。 + +### 真机传感器与电机接线 + +`motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg` 独立持有不依赖资产的硬件姿态与接线。 +`HardwareSensorBinding` 映射 SDK IMU 字段，包括将 `wxyz` 四元数转换为规范 `xyzw`； +`UnitreeMotorBinding` 将规范关节名映射到 SDK 电机索引。这些定义属于硬件插件的 `sensor` 和 +`actuation` 模块，而不是仿真机器人模型。artifact 仍拥有策略关节顺序、默认姿态和限幅。 +导入硬件配置不会提前加载模型资产、仿真器或 SDK，也不伪造 base position 或 linear velocity。 + +### 选择部署世界 + +通过 `runtime.deploy_env_id` 选择世界，通过现有 registry ID `runtime.robot_id` 选择机器人。 +CLI 调用 `motrix_deploy.env.assemble_deploy_scene(deploy_env_id, robot_id)`：创建无机器人世界， +通过 `motrix_env_core.registry.make_robot_config(robot_id)` 填入 `scene.objs.robot`。 +core registry 惰性发现已安装的 `motrix_env_core.robots` entry points，训练与部署复用同一机制。部署世界 `flat` 和 `rough` 位于独立的 +`motrix_deploy_tasks.envs` 模块，使用 core 地面/高度场配置，不 import 训练任务、机器人模型或训练 contact 配置。 + +保留同一 Go2 recipe 和 artifact，仅切换世界： + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.deploy_env_id=rough +``` + +仅覆写世界不会改变机器人 attachment placement。新地形需要不同净空时应调整 `runtime.robot_translation`； +完整的 `task=go2-walk-rough/sim` recipe 自带对应的 attachment 默认值。 + +已安装的平地 recipe 使用 ID 选择世界和机器人： + +```yaml +runtime: + kind: simulation + backend: mujoco + deploy_env_id: flat + robot_id: go2 + robot_translation: [0.0, 0.0, -0.114] + robot_rotation: [0.0, 0.0, 0.0, 1.0] +``` + +`runtime.physics` 与 `runtime.sensor_bindings` 仍是独立设置。MuJoCo 只接收完整 `SceneCfg`， +不选择环境/机器人组合，不组装场景，也不查询训练 registry。程序化 `SimulationRuntimeConfig.scene` 接收同一个完整场景， +机器人只在 `scene.objs.robot` 中声明一次。 + +Placement 只有一个来源：`RobotCfg.translation`（Vec3）与 `rotation`（xyzw）是 attachment transform， +与模型固有 base pose 组合一次，并非绝对世界 root 位姿。Go2 固有 base 高度为 0.445 m；identity rotation 下， +平地 recipe 的 -0.114 m 偏移得到 0.331 m，粗糙地形 recipe 的 -0.025 m 偏移得到 0.42 m。 +runtime 在复制的场景中保留这些变换。`mj_resetData` 恢复编译后的 `model.qpos0`，包括组合后的 root placement； +初始化只用绑定 `RobotSpec` 的关键姿态覆写 articulation 关节。 + +外部世界插件在 `motrix_deploy.envs` entry-point group 中声明零参数 `SceneCfg` factory。 +`motrix_deploy.env.available_deploy_envs()` 无需加载 factory 即可列出已安装 ID； +`create_deploy_env(id)` 只加载选中的 factory，并要求 `scene.objs.robot` 为空。 +core 组装 helper 添加由现有 robot registry 选择的机器人；不引入通用 scene-factory registry，也不通过 +Hydra 实例化场景。CLI 消费 `deploy_env_id`、`robot_id` 与可选 `robot_translation` / `robot_rotation`， +把 attachment overrides 写入 `scene.objs.robot.translation` / `.rotation` 后，再把完整场景传给仿真插件。 +这些应用层选项不是 runtime backend 字段。 + +`SimulationRuntimeConfig` 负责场景、渲染、实时节奏与状态来源设置；其 `physics` 字段使用共享的 +`motrix_env_core.config.sim.SimCfg` 配置 `dt`、`solver_iterations`、`solver_tolerance` 与 `gravity`， +默认值为 `SimCfg(dt=0.002, solver_iterations=100)`。两个 backend compiler 直接消费该字段： +可选 solver/gravity 字段为 `None` 时保留模型来源中的值，显式配置则覆盖。 +YAML 仍使用 `runtime.physics`，与场景、state sensors 保持独立。 +物理参数应在 runtime 构造前设置：runtime 读取编译使用的共享 `SimCfg`；不支持编译后修改 `dt` 来动态调参。 + +程序化应用使用同一 helper，并把结果传给 `SimulationRuntimeConfig`： + +```python +from motrix_deploy.env import assemble_deploy_scene +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_env_core.config.sim import SimCfg + +scene = assemble_deploy_scene("flat", "go2") +scene.objs.robot.translation = (0.0, 0.0, -0.114) +scene.objs.robot.rotation = (0.0, 0.0, 0.0, 1.0) +config = SimulationRuntimeConfig( + scene=scene, + physics=SimCfg(dt=0.002, solver_iterations=100), + sensor_bindings=SensorBindings( + base_angular_velocity="gyro", + base_linear_acceleration="accelerometer", + base_linear_velocity="global_linvel", + ), +) +``` + +`SimulationRuntimeConfig.sensor_bindings` 使用 frozen dataclass `SensorBindings`， +而非 Python 字典。它只支持 `base_angular_velocity`、`base_linear_acceleration` 与 +`base_linear_velocity`，类型均为 `str | None`，默认为 `None`。名称必须非空且至少包含 +一个非空白字符；未知角色与非字符串值均被拒绝。YAML `runtime.sensor_bindings` 保留同样的 +映射键，在配置边界通过 `SensorBindings(**mapping)` 转换。字段缺省或显式 `None` 表示 +没有绑定；MuJoCo 在打开机器人时会拒绝这三个必需角色的任一种未绑定状态。 +Backend 仍校验 sensor 是否存在、类型、维度与坐标系；不会为其他角色合成状态， +也不是每个 `RobotState` 字段都通过 sensor 绑定。 + +### 显式组装 runtime 与控制器 + +Runtime factory 选择 backend 并准备 `runtime.robot`；绑定的 `RobotSpec` 在 `open()` 之前即可读取。 +应用自行创建 task、policy 与 command binding，再显式构造并绑定 control session。 +沿用上面的 `config` 以及应用持有的 `task`、`policy`、`command_binding`： + +```python +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.factory import create_simulation_runtime + +runtime = create_simulation_runtime("mujoco", config) +robot = runtime.robot +control = ControlSession( + robot=robot, + task=task, + policy=policy, + command_binding=command_binding, + period_s=0.02, +) +runtime.bind_control_session(control) +with runtime: + result = runtime.run(steps=50) +``` + +依赖机器人契约的 task 应在 runtime 创建后从 `robot.spec` 构造。仿真 runtime 只从 scene 派生 +`RobotSpec`，不接收外部 spec。Artifact 部署由 CLI 在组装 control 前，将 artifact 的机器人契约 +与 `runtime.robot.spec` 对照校验。 +`ControlSession.period_s` 是控制周期的 +唯一配置位置，`SimulationRuntimeConfig` 不重复声明；仿真 runtime 在绑定或解析 session 时校验该周期与 physics timestep +的关系。Runtime 创建不接收控制器构造回调。 +Runtime context 持有世界资源并推进仿真；`ControlSession` 执行共享控制逻辑；`RobotInterface` +负责机器人 I/O，不推进或关闭世界。硬件应用先调用 `create_hardware_runtime(name, config, context)`， +再使用同样的显式构造与绑定流程。直接调用方也可向 `runtime.run(control, steps=...)` 传入 session。 + +### 选择原生 MotrixSim 部署 + +已安装的仿真 recipe 默认使用 MuJoCo。安装 `motrix-deploy-motrixsim` 后，可用同一 recipe 和 artifact +选择原生 backend： + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.backend=motrixsim runtime.viewer=false runtime.realtime=false duration_s=1.0 +``` + +同样的 override 适用于 `task=go2-walk-rough/sim`，无需复制 task recipe。程序化应用调用 +`create_simulation_runtime("motrixsim", config)`，然后沿用上面的显式 session 绑定流程。 +Go2 state mappings 仍为 `gyro`、`accelerometer` 与 `global_linvel`：原生 sensor 校验 local angular +velocity/acceleration 与 world-frame linear velocity。Runtime 持有原生 MSD 编译、model/data、reset、physics +与可选 SDK `RenderApp` 渲染，不创建 MuJoCo runtime 或训练环境。在可用的图形环境中移除 `runtime.viewer=false` +即可启用原生 viewer 与其 keyboard provider。 + +程序化 ONNX 示例可运行 +`python examples/deploy_to_sim.py --backend motrixsim --headless --steps 50`， +准备匹配的 ONNX 策略；策略文件选项见示例。 +这条路径加载 raw ONNX，不读取 `.deploy` manifest：任务预处理、tensor specs 和控制参数由 Python +示例显式声明，必须与模型匹配。其 scene 与控制器和默认 MuJoCo 路径共用。 +Headless 使用零速度指令；窗口模式在可用时使用 viewer keyboard provider。 + +### 使用外部 Hydra 配置 + +上面的原生命令直接使用已安装的任务包，无需 checkout 中的配置目录。内置 runtime recipe 的唯一维护位置是 +`motrix_deploy_tasks/config/task/{task}/{sim,hardware}.yaml`，优先通过 `task=...` 选择。自定义应用仍可使用 Hydra 标准的 +`--config-path` 与 `--config-name` 管理自己的配置。以下示例假定应用已自行编写 +`/absolute/path/to/my_app/config/deploy.yaml`，不是选择或复制内置 recipe： + +```bash +motrix-deploy \ + --config-path /absolute/path/to/my_app/config \ + --config-name deploy \ + artifact=artifacts/go2-walk-flat.deploy +``` + +请使用绝对配置路径：Hydra 相对于 CLI 模块而非 shell 当前目录解析相对路径。 +外部配置自行声明 defaults 和完整的 runtime/backend 配置，不必使用已安装的 `task` group。仿真配置必须显式提供 +defaults 中的 `runtime=sim`、`runtime.backend`、`runtime.deploy_env_id`、`runtime.robot_id`，以及选中 backend 所需的全部设置（包括 MuJoCo +state-sensor mappings）。artifact 不提供这些 runtime 选择。沿用 Hydra 原生选项，不增加额外的 `--config` 参数。 diff --git a/docs/source/zh_CN/user_guide/tutorial/basic_frame.md b/docs/source/zh_CN/user_guide/tutorial/basic_frame.md index 43b1431e..b5dfb8d4 100644 --- a/docs/source/zh_CN/user_guide/tutorial/basic_frame.md +++ b/docs/source/zh_CN/user_guide/tutorial/basic_frame.md @@ -45,8 +45,9 @@ MotrixLab 是一个 UV workspace。下面按所有权而不是按单一整体运 | `configs/`、`scripts/` | 配置与编排 | Hydra 算法配置与 Task 配方；train / play / view / export 入口 | | `motrix_deploy` | 策略的部署 | 框架无关 artifact、运行时契约与部署 CLI | | `motrix_deploy_mujoco` | 策略的部署 | MuJoCo 部署 backend plugin | -| `motrix_deploy_unitree` | 策略的部署 | Unitree SDK2 DDS 硬件 backend plugin | -| `motrix_deploy_tasks` | 策略的部署 | 行走任务、部署场景与安装的 Hydra 配方 | +| `motrix_deploy_motrixsim` | 策略的部署 | MotrixSim 部署 backend plugin | +| `motrix_deploy_unitree` | 策略的部署 | Unitree 硬件配置、sensor/actuation 接线与 SDK2 DDS backend plugin | +| `motrix_deploy_tasks` | 策略的部署 | 通过 `motrix_deploy.tasks` 发现版本化任务；通过 `motrix_deploy.envs` 发现无机器人世界；已安装 Hydra 运行 recipe | 仿真后端（如 `motrix_env_motrixsim`）通过 `SimBackend` 接口隔离在环境框架之下，使用环境时通常 无需关心它；需要选择或接入仿真后端时，见[SimBackend：与仿真器解耦](building_envs/sim_backend.md)。 diff --git a/motrix_deploy/README.md b/motrix_deploy/README.md index cf1d59f3..d39a1963 100644 --- a/motrix_deploy/README.md +++ b/motrix_deploy/README.md @@ -1,50 +1,15 @@ # Motrix Deploy -`motrix_deploy` runs exported policies without a training framework. It provides deployment artifacts, robot and task -contracts, `ControlSession`, simulation/hardware runtimes, and the `motrix-deploy` CLI. Install a task plugin and the -runtime plugin for your target; the Go2 recipes below come from `motrix_deploy_tasks` and use `motrix_deploy_mujoco`. +`motrix-deploy` runs exported policies without a training framework. It provides deployment artifacts, +robot/task contracts, shared control execution, runtime plugin discovery, and the `motrix-deploy` CLI. +Install the task and runtime plugins needed for your target. -From the repository root, install and activate the environment, then export a trained Go2 flat-terrain run: +With the Go2 task and a simulation backend installed: ```bash -sh install.sh --all -source .venv/bin/activate -python scripts/export_deploy.py env=go2-walk-flat -motrix-deploy inspect artifact=artifacts/go2-walk-flat.deploy -motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy ``` -Export selects the latest metadata-backed run. Use `run= output=` to select a specific run and -output. Export creates a new directory; it does not overwrite an existing artifact. - -The artifact contains the ONNX policy, robot/control contract, and serialized task specification. The installed task -plugin supplies the matching implementation and Hydra recipes, so deployment does not require workspace config files. -Task defaults select the Hydra runtime group (`sim` or `hardware`). All target-specific settings live directly under -`runtime`: `runtime.kind` selects the simulation or hardware path, while `runtime.backend` selects the plugin (`mujoco` -for simulation or `unitree_go2` for hardware). `runtime.backend` is a scalar string selector, not a nested configuration -mapping or Hydra group. The simulation recipe selects the scene and robot -(`runtime.deploy_env_id=flat`, `runtime.robot_id=go2`). - -Omit `runtime.viewer=false duration_s=2.0` to open the MuJoCo viewer. Hold `W/S`, `A/D`, and `Q/E` for forward, lateral, and yaw -commands; Esc, Ctrl-C, or closing the window stops the run. Headless operation uses the recipe's constant zero velocity; -override `command.velocity` to change it. Common settings are `artifact`, `duration_s`, and `command`. -`duration_s` is a control-time budget of `ceil(duration_s / control_period_s)` ticks, not a wall-clock timer. -Simulation-only settings are `runtime.viewer` and `runtime.realtime`; `runtime.realtime=null` follows -`runtime.viewer`. Hardware runs are always real-time internally and expose neither simulation field. - -## Programmatic control - -[`examples/deploy_to_sim.py`](../examples/deploy_to_sim.py) assembles a `SceneCfg`, `SimulationRuntimeConfig`, walking task, -ONNX policy, and command binding, then binds a `ControlSession` to a MuJoCo runtime. It includes a sample policy: - -```bash -python examples/deploy_to_sim.py --headless --steps 100 -``` - -`ControlSession` reads input and state, builds observations, runs inference, and writes robot commands. The runtime owns -physics advancement, scheduling, and the viewer. A raw ONNX model needs explicit task preprocessing and action settings; -use an exported artifact when those settings should travel with the policy. - -For the training-to-hardware workflow and safety checklist, see the -[deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) and -[Unitree plugin guide](../motrix_deploy_unitree/README.md). +See the [deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) +for export, simulation, and hardware workflows, or the [Python example](../examples/deploy_to_sim.py) +for programmatic control. diff --git a/motrix_deploy/tests/test_simulation_runtime.py b/motrix_deploy/tests/test_simulation_runtime.py index 3736ec16..013a2460 100644 --- a/motrix_deploy/tests/test_simulation_runtime.py +++ b/motrix_deploy/tests/test_simulation_runtime.py @@ -58,6 +58,34 @@ def factory(config): assert not runtime.opened +def test_simulation_config_from_mapping_preserves_scene_and_decodes_runtime_recipe(): + scene = SceneCfg() + values = { + "scene": scene, + "physics": {"dt": 0.01, "solver_iterations": 50}, + "sensor_bindings": {"base_angular_velocity": "gyro"}, + } + + config = SimulationRuntimeConfig.from_mapping(values, render=True, realtime=False) + + assert config.scene is scene + assert config.physics.dt == values["physics"]["dt"] + assert config.physics.solver_iterations == values["physics"]["solver_iterations"] + assert config.sensor_bindings.base_angular_velocity == "gyro" + assert config.sensor_bindings.base_linear_velocity is None + assert config.render is True + assert config.realtime is False + assert values["sensor_bindings"] == {"base_angular_velocity": "gyro"} + + +def test_simulation_config_from_mapping_allows_implicit_sensor_bindings(): + config = SimulationRuntimeConfig.from_mapping( + {"scene": SceneCfg(), "physics": {"dt": 0.01, "solver_iterations": 50}} + ) + + assert config.sensor_bindings == SensorBindings() + + def test_simulation_config_uses_independent_typed_sensor_defaults(): first = SimulationRuntimeConfig(scene=SceneCfg()) second = SimulationRuntimeConfig(scene=SceneCfg()) diff --git a/motrix_deploy_mujoco/README.md b/motrix_deploy_mujoco/README.md index 7b9e2040..1174b00f 100644 --- a/motrix_deploy_mujoco/README.md +++ b/motrix_deploy_mujoco/README.md @@ -1,22 +1,15 @@ -# motrix-deploy-mujoco +# Motrix Deploy MuJoCo -`motrix-deploy-mujoco` runs deployment policies in MuJoCo. It compiles a `SceneCfg`, exposes the scene's robot through -`RobotInterface`, and supplies physics stepping, joint control, and an optional keyboard viewer. +`motrix-deploy-mujoco` provides the MuJoCo simulation backend for `motrix-deploy`. +It compiles a complete `SceneCfg`, derives the robot contract, and owns physics stepping, +joint control, and an optional interactive viewer. -With `motrix_deploy_tasks` installed and a matching exported artifact: +With `motrix-deploy-tasks` and a matching exported artifact: ```bash -motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy ``` -Omit the headless overrides for interactive control. The Go2 flat and rough recipes are installed with the task plugin, -not read from workspace config paths. Task defaults select the `sim` Hydra runtime group. Simulation settings are -`runtime.viewer` and `runtime.realtime`; `runtime.realtime=null` follows `runtime.viewer`. MuJoCo options live directly -under `runtime`, including `runtime.backend=mujoco`, `runtime.deploy_env_id`, `runtime.robot_id`, and `runtime.physics`. -`runtime.backend` is a scalar string selector, not a nested configuration mapping or Hydra group. -`duration_s` budgets `ceil(duration_s / control_period_s)` control ticks, not wall-clock time. - -Programmatic callers use `create_simulation_runtime("mujoco", SimulationRuntimeConfig(...))`, assemble a `ControlSession` -with `runtime.robot`, and call `runtime.bind_control_session(control)` before running. The runtime advances physics between -control ticks; the control session handles task observations, policy inference, and robot commands. See -[`examples/deploy_to_sim.py`](../examples/deploy_to_sim.py) for a complete headless or viewer example. +For programmatic use, see [deploy_to_sim.py](../examples/deploy_to_sim.py). +Configuration and deployment workflows are covered in the +[deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md). diff --git a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py index 5c9fdb17..7b97cacd 100644 --- a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py +++ b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py @@ -14,7 +14,7 @@ import numpy as np import pytest -from motrix_robots.unitree import UnitreeGo2Robot +from motrix_robots.unitree import UnitreeGo1Robot, UnitreeGo2Robot from omegaconf import OmegaConf from scene_helpers import AdapterSceneObjsCfg, build_scene @@ -481,7 +481,7 @@ def test_direct_scene_cfg_headless_uses_supplied_robot_floor_and_placement() -> backend.close() -@pytest.mark.parametrize("robot_type", [UnitreeGo2Robot]) +@pytest.mark.parametrize("robot_type", [UnitreeGo1Robot, UnitreeGo2Robot]) def test_robot_description_switch_needs_no_backend_specific_wiring(robot_type) -> None: robot = robot_type() scene = SceneCfg(objs=AdapterSceneObjsCfg(robot=robot, floor=FlatTerrainCfg())) diff --git a/motrix_deploy_tasks/README.md b/motrix_deploy_tasks/README.md index f1655dec..b06db9e5 100644 --- a/motrix_deploy_tasks/README.md +++ b/motrix_deploy_tasks/README.md @@ -1,28 +1,17 @@ # Motrix Deploy Tasks -`motrix_deploy_tasks` supplies the Go2 walking task, flat/rough deployment scenes, and installed Hydra recipes for -`motrix-deploy`. Install it alongside the MuJoCo runtime plugin to run an exported policy: +`motrix-deploy-tasks` supplies the versioned Go2 walking task, flat/rough deployment worlds, +and installed Hydra recipes for `motrix-deploy`. It does not provide a simulation or hardware backend. + +Install a backend plugin and use an artifact exported for the matching task: ```bash -motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy ``` -The recipes are packaged with the plugin; the command works outside the repository without `--config-path`. -`task=go2-walk-rough/sim` selects the rough scene, while `task=go2-walk-flat/hardware` selects the Unitree runtime. -Task defaults choose the `sim` or `hardware` Hydra runtime group. Target settings live directly under `runtime`; -`runtime.backend` is `mujoco` for simulation or `unitree_go2` for hardware. It is a scalar string selector, not a nested -configuration mapping or Hydra group. -Use an artifact exported for the matching task. Common settings are `artifact`, `duration_s`, and `command`; -`duration_s` budgets `ceil(duration_s / control_period_s)` control ticks, not wall-clock time. -Simulation exposes `runtime.viewer` and `runtime.realtime`; `runtime.realtime=null` follows `runtime.viewer`. -Hardware is always real-time internally and has neither field. For hardware networking, override -`runtime.network_interface=`. -Follow the [deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) for physical operator -safety, startup/enable gates, and emergency stop procedures before commanding a robot. - -The artifact's `go2_walk/v1` specification defines observation preprocessing, action conversion, gains, command bounds, -and termination settings. `Go2WalkTaskSpec` and `Go2WalkDeployTaskV1` are defined in -`motrix_deploy_tasks.tasks.go2_walk`. Training profile compilers live in `motrix_envs.deploy`. +Use `go2-walk-rough/sim` for the rough scene, `runtime.backend=motrixsim` for native MotrixSim, +or `go2-walk-flat/hardware` for Unitree hardware. Recipes are packaged with the plugin; +no workspace config path is required. -For a scene/task/policy assembled directly in Python, see -[`examples/deploy_to_sim.py`](../examples/deploy_to_sim.py). +See the [deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) +for export, configuration, and hardware safety procedures. diff --git a/motrix_deploy_tasks/tests/test_training_parity.py b/motrix_deploy_tasks/tests/test_training_parity.py index 00c421c1..985bf19e 100644 --- a/motrix_deploy_tasks/tests/test_training_parity.py +++ b/motrix_deploy_tasks/tests/test_training_parity.py @@ -38,12 +38,11 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: np.testing.assert_allclose(task.command_upper, env.cfg.commands.velocity.upper * command_scale) task.validate_command(context.command) task.reset(robot_state, context) - # Mainline training's compute_transition uses orientation-only up_z <= 0.5; - # these probes reproduce that behavior rather than require new training config. + assert profile.task.termination_min_up_z == env.cfg.termination_min_up_z for up_z in (-1.0, 0.0, 1.0): angle = np.arccos(up_z) robot_state.base_orientation_xyzw = np.array([np.sin(angle / 2), 0.0, 0.0, np.cos(angle / 2)], dtype=np.float32) - assert (task.check_termination(robot_state) is not None) == (up_z <= 0.5) + assert (task.check_termination(robot_state) is not None) == (up_z <= env.cfg.termination_min_up_z) robot_state = _robot_state_from_env(env, env_state, probe) np.testing.assert_allclose( diff --git a/motrix_deploy_unitree/README.md b/motrix_deploy_unitree/README.md index 06152691..2139a995 100644 --- a/motrix_deploy_unitree/README.md +++ b/motrix_deploy_unitree/README.md @@ -1,135 +1,32 @@ -# motrix-deploy-unitree +# Motrix Deploy Unitree -`motrix-deploy-unitree` is the Unitree SDK2 DDS hardware plugin for `motrix-deploy`. It exposes the -`unitree_go2` backend through the `motrix_deploy.backends` entry-point group. SDK symbols remain lazily imported until a -physical backend is opened. +`motrix-deploy-unitree` provides the `unitree_go2` SDK2 DDS hardware backend for `motrix-deploy`, +asset-independent hardware sensor/motor configuration, and direct read/write and diagnostic tools. +The Unitree SDK is imported only when a hardware connection is opened. -The SDK is installed from the pinned Motphys-maintained fork as part of the normal workspace sync: +## Safety -```bash -sh install.sh --all -``` - -The Motphys fork includes the CRC shared libraries in built distributions. The SDK metadata declares -`cyclonedds`, `numpy`, -and `opencv-python`; uv therefore installs them into the environment as transitive dependencies. The DDS control backend does not import -OpenCV directly, but the official SDK includes camera/video clients in the same distribution. - -The installed `task=go2-walk-flat/hardware` recipe in `motrix_deploy_tasks` supplies hardware startup settings, -policy gains, robot-specific SDK fields, and command input settings. -The SDK is imported only when `open()` is called. Runtime behavior: - -- Subscribe to `rt/lowstate`, validate CRC, and map joint `q/dq` plus IMU data into canonical `RobotState`. -- Map canonical artifact joints to explicit Go2 motor indices and publish CRC-protected `rt/lowcmd` joint PD commands. -- Publish zero torque until Start, transition from measured pose to the artifact default pose, then hold until A. -- Latch Select as an emergency stop and report `exit_reason=emergency_stop`. -- Latch B as a lie-down request, interpolate to the configured shutdown pose, then enter damping and report - `exit_reason=lie_down`. Select retains priority and skips the lie-down trajectory. -- Publish damping commands before closing both DDS channels on all stop paths. - -## Direct read/write API - -`UnitreeGo2DirectInterface` opens the same production backend without creating a policy runtime. Reading and writing are -separate method calls: - -```python -import time - -from motrix_deploy_unitree import UnitreeGo2DirectInterface - -robot = UnitreeGo2DirectInterface.from_artifact( - "artifacts/go2-walk-rough.deploy", - network_interface="enp5s0", -) - -robot.open() -try: - state = robot.read_data() - print(state.joint_position, state.joint_velocity) - - # This still requires remote Start, default-pose transition, and A. - robot.enable_command_output() - - hold = robot.default_pose_command() - for _ in range(100): - robot.send_command(hold) - time.sleep(robot.control_period_s) -finally: - robot.close() -``` +Use a suspended robot in low-level/debug mode with an operator-ready emergency stop. +Opening a command interface may stand down the robot before the Start gate. Choose hardware-appropriate +startup gains: Start transitions to the default pose, A enables control, Select triggers emergency stop, +and B requests lie-down. Stop paths send damping commands. Even the single-joint helper publishes +commands for all 12 joints. The backend has fake-SDK test coverage; a real-robot smoke test remains pending. -`read_data()` returns canonical `RobotState`. `send_command()` accepts `RobotCommand`; alternatively, -`send_joint_command(position, joint_velocity=..., feedforward_torque=..., kp=..., kd=...)` constructs and sends one -command. Command output is rejected until `enable_command_output()` succeeds, and callers own periodic scheduling. +## Usage -Opening this direct interface creates a LowCmd publisher and `close()` sends damping. For strictly read-only diagnostics -with no publisher, use `motrix-deploy-unitree read-lowstate `. - -### Read LowState without commanding - -Use the read-only diagnostic before enabling any command path: - -```bash -motrix-deploy-unitree read-lowstate enp5s0 -``` - -It subscribes to `rt/lowstate` for 10 seconds by default and prints at most one sample every 0.5 seconds. It does not need -an artifact, and it never creates a `LowCmd` publisher. Each printed sample contains: - -| Output | Contents and units | -| ---------------- | ---------------------------------------------------------------------------------- | -| Frame statistics | Received, valid, CRC-error, and decode-error counts | -| Joint state | Canonical names for all 12 joints, position `q` in rad, and velocity `dq` in rad/s | -| IMU orientation | Quaternion in both SDK `wxyz` and canonical `xyzw` order | -| IMU motion | Gyroscope in rad/s and accelerometer in m/s² | -| Wireless remote | Pressed buttons and `lx`, `ly`, `rx`, `ry` axes | - -Use `--duration-s 0` to run until Ctrl+C and `--print-interval-s` to change output throttling. `--domain-id`, `--topic`, and -`--queue-depth` configure DDS. CRC validation is enabled by default; `--no-validate-crc` is intended only for focused -transport troubleshooting. The command returns `0` after receiving a valid frame, `1` when no valid frame arrives, and -`2` when the Unitree SDK cannot be imported. - -### Bounded single-joint motion - -The packaged helper requires a frozen deployment artifact as its robot, gain, timing, and limit contract: +Check incoming state without creating a command publisher: ```bash -motrix-deploy-unitree joint-control \ - enp5s0 \ - FL_thigh_joint \ - 0.9 \ - --artifact artifacts/go2-walk-rough.deploy +motrix-deploy-unitree read-lowstate ``` -The three positional arguments are the network interface, canonical joint name, and absolute target position in radians. -The helper validates the artifact contract before opening DDS. It does not import task environments or select an -environment profile. After the normal Start/default-pose/A enable sequence, it interpolates to the target, holds it, -returns to the default pose, and sends a damping stop while closing. Every control tick reads a fresh `LowState`, so -state timeout and the remote Select emergency stop remain active. - -Use `--move-duration`, `--hold-duration`, and `--return-duration` to change the default `2.0`, `1.0`, and `2.0` second -phases. Setting `--return-duration 0` skips the return trajectory, but closing still changes all joints to damping mode. -Run `motrix-deploy-unitree joint-control --help` for the complete command reference. Although the -helper changes one target, each Unitree `LowCmd` necessarily contains commands for all 12 joints. - -## Policy gain overrides - -`UnitreeGo2BackendConfig` defaults `runtime.kp` and `runtime.kd` to `null`, while -the installed Go2 hardware recipe sets them to `50` and `1`. Each accepts either one -non-negative scalar for all joints or 12 values in canonical `FL, FR, RL, RR` order. Overrides apply to the -default-pose transition and policy commands; damping stop still uses `kp=0` and `runtime.damping_kd`. - -From the repository root: +With `motrix-deploy-tasks` and a matching artifact installed: ```bash -motrix-deploy task=go2-walk-flat/hardware \ - artifact=artifacts/go2-walk-flat.deploy \ - runtime.network_interface=enp5s0 +motrix-deploy task=go2-walk-flat/hardware artifact=artifacts/go2-walk-flat.deploy \ + runtime.network_interface= ``` -All hardware settings live directly under `runtime`, with `runtime.backend=unitree_go2`. `runtime.backend` is a scalar -string selector, not a nested configuration mapping or Hydra group. The hardware runtime always runs -in real time internally and exposes no `runtime.viewer` or `runtime.realtime` fields. Before opening LowCmd, the backend uses MotionSwitcher to stand down and -release the active MCF mode, then calls `RobotStateClient.ServiceSwitch("sport_mode", False)`; failure aborts startup. Use a suspended -robot, low-level/debug mode, and an operator-ready emergency stop. The implementation is validated with an injected fake -SDK; the suspended real-robot smoke test remains pending. +Review configured startup gains and the +[deployment tutorial's safety procedures](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) +before running. Use `motrix-deploy-unitree --help` for diagnostic and joint-control tools. diff --git a/motrix_deploy_unitree/pyproject.toml b/motrix_deploy_unitree/pyproject.toml index 988fb45b..253c8870 100644 --- a/motrix_deploy_unitree/pyproject.toml +++ b/motrix_deploy_unitree/pyproject.toml @@ -11,7 +11,9 @@ requires-python = "==3.10.*" readme = "README.md" license = "Apache-2.0" dependencies = [ + "hydra-core>=1.3,<1.4", "motrix-deploy", + "motrix-env-core", "numpy>=1.26", "unitree-sdk2py", ] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/actuation.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/actuation.py new file mode 100644 index 00000000..d543f16f --- /dev/null +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/actuation.py @@ -0,0 +1,41 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot-intrinsic actuator wiring metadata.""" + +from dataclasses import dataclass, field + + +@dataclass(frozen=True) +class UnitreeMotorBinding: + """Canonical joint to Unitree motor address and physical mode.""" + + index: int + enabled_mode: int = 0x0A + + def __post_init__(self) -> None: + if not isinstance(self.index, int) or isinstance(self.index, bool) or self.index < 0: + raise ValueError("motor index must be a non-negative integer") + if ( + not isinstance(self.enabled_mode, int) + or isinstance(self.enabled_mode, bool) + or not 0 <= self.enabled_mode <= 0xFF + ): + raise ValueError("enabled motor mode must be an unsigned byte") + + +@dataclass(frozen=True) +class Actuation: + """Canonical joint-to-motor mappings.""" + + motors: dict[str, UnitreeMotorBinding] = field(default_factory=dict) + + def __post_init__(self) -> None: + indices = [binding.index for binding in self.motors.values()] + if len(indices) != len(set(indices)): + raise ValueError("motor indices must be unique") + if any(not name for name in self.motors): + raise ValueError("actuator joint names must not be empty") + + +__all__ = ["Actuation", "UnitreeMotorBinding"] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py index 537f0784..3f6aec9e 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py @@ -78,6 +78,8 @@ def _parser() -> argparse.ArgumentParser: joint.add_argument("joint_name", help="Canonical joint name, for example FL_thigh_joint") joint.add_argument("target_position", type=_finite_float, help="Absolute target position in radians") joint.add_argument("--artifact", type=Path, required=True, help="Frozen Go2 deployment artifact") + joint.add_argument("--kp", type=_non_negative_float, required=True, help="Explicit backend servo safety stiffness") + joint.add_argument("--kd", type=_non_negative_float, required=True, help="Explicit backend servo safety damping") joint.add_argument("--move-duration", type=_positive_float, default=2.0) joint.add_argument("--hold-duration", type=_non_negative_float, default=1.0) joint.add_argument("--return-duration", type=_non_negative_float, default=2.0) @@ -103,6 +105,8 @@ def main(argv: Sequence[str] | None = None) -> int: network_interface=args.network_interface, joint_name=args.joint_name, target_position=args.target_position, + kp=args.kp, + kd=args.kd, move_duration=args.move_duration, hold_duration=args.hold_duration, return_duration=args.return_duration, diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py index 3e5e43b1..3ecdb961 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py @@ -4,45 +4,21 @@ """Unitree Go2 hardware backend configuration.""" from collections.abc import Mapping -from dataclasses import MISSING, dataclass, field +from dataclasses import MISSING, dataclass from types import MappingProxyType from typing import Any import numpy as np +from hydra.utils import instantiate from motrix_deploy.errors import ValidationError +from motrix_deploy_unitree.hardware import HardwareRobotCfg, UnitreeGo2HardwareCfg from motrix_deploy_unitree.remote import BUTTON_NAMES GO2_MOTOR_COUNT = 12 +_DEFAULT_GO2_ROBOT = UnitreeGo2HardwareCfg() GO2_JOINT_NAME_TO_MOTOR_INDEX: Mapping[str, int] = MappingProxyType( - { - "FL_hip_joint": 3, - "FL_thigh_joint": 4, - "FL_calf_joint": 5, - "FR_hip_joint": 0, - "FR_thigh_joint": 1, - "FR_calf_joint": 2, - "RL_hip_joint": 9, - "RL_thigh_joint": 10, - "RL_calf_joint": 11, - "RR_hip_joint": 6, - "RR_thigh_joint": 7, - "RR_calf_joint": 8, - } -) -GO2_LIE_DOWN_JOINT_POSITION = ( - 0.05175, - 1.238835, - -2.74427, - -0.0608, - 1.24118, - -2.7375, - 0.31617, - 1.26637, - -2.79547, - -0.310495, - 1.266385, - -2.80177, + {name: binding.index for name, binding in _DEFAULT_GO2_ROBOT.actuation.motors.items()} ) @@ -51,7 +27,7 @@ class UnitreeGo2BackendConfig: """Strict DDS, motor mapping, enable-transition, and stop settings.""" network_interface: str - joint_name_to_motor_index: Mapping[str, int] = field(default_factory=lambda: GO2_JOINT_NAME_TO_MOTOR_INDEX) + joint_name_to_motor_index: Mapping[str, int] | None = None domain_id: int = 0 lowcmd_topic: str = "rt/lowcmd" lowstate_topic: str = "rt/lowstate" @@ -67,10 +43,11 @@ class UnitreeGo2BackendConfig: emergency_stop_button: str = "select" lie_down_button: str = "B" lie_down_duration_s: float = 2.0 - lie_down_joint_position: object = GO2_LIE_DOWN_JOINT_POSITION + lie_down_joint_position: object | None = None validate_crc: bool = True subscriber_queue_depth: int = 10 - motor_mode: int = 0x0A + motor_mode: int | None = None + robot: HardwareRobotCfg | None = None def __post_init__(self) -> None: if not isinstance(self.network_interface, str) or not self.network_interface: @@ -105,7 +82,8 @@ def __post_init__(self) -> None: raise ValidationError("runtime.damping", "non-negative duration and kd", "invalid value") if self.lie_down_duration_s <= 0: raise ValidationError("runtime.lie_down_duration_s", "a positive duration", self.lie_down_duration_s) - self.lie_down_position() + if self.lie_down_joint_position is not None: + self.lie_down_position() self._validate_gain_override("kp", self.kp) self._validate_gain_override("kd", self.kd) if not isinstance(self.domain_id, int) or isinstance(self.domain_id, bool) or self.domain_id < 0: @@ -120,7 +98,7 @@ def __post_init__(self) -> None: "a positive integer", self.subscriber_queue_depth, ) - if ( + if self.motor_mode is not None and ( not isinstance(self.motor_mode, int) or isinstance(self.motor_mode, bool) or not 0 <= self.motor_mode <= 0xFF @@ -133,12 +111,16 @@ def __post_init__(self) -> None: buttons = (self.start_button, self.enable_button, self.emergency_stop_button, self.lie_down_button) if len(set(buttons)) != len(buttons): raise ValidationError("runtime.remote_buttons", "four distinct buttons", "duplicate button") - if not isinstance(self.joint_name_to_motor_index, Mapping): + if self.joint_name_to_motor_index is not None and not isinstance(self.joint_name_to_motor_index, Mapping): raise ValidationError( "runtime.joint_name_to_motor_index", "a joint-name to motor-index mapping", type(self.joint_name_to_motor_index).__name__, ) + if self.robot is not None and not isinstance(self.robot, HardwareRobotCfg): + raise ValidationError("runtime.robot", "a HardwareRobotCfg config", type(self.robot).__name__) + if self.robot is None: + object.__setattr__(self, "robot", UnitreeGo2HardwareCfg()) @classmethod def from_mapping(cls, value: Mapping[str, Any]) -> "UnitreeGo2BackendConfig": @@ -159,6 +141,10 @@ def from_mapping(cls, value: Mapping[str, Any]) -> "UnitreeGo2BackendConfig": mapping = values.get("joint_name_to_motor_index") if isinstance(mapping, Mapping): values["joint_name_to_motor_index"] = dict(mapping) + robot = values.get("robot") + if isinstance(robot, Mapping): + # Deployment recipes are trusted local configuration, like other Hydra targets. + values["robot"] = instantiate(robot, _convert_="object") return cls(**values) @staticmethod @@ -183,22 +169,29 @@ def _validate_gain_override(name: str, value: object | None) -> None: ) def lie_down_position(self) -> np.ndarray: - """Return the backend-owned shutdown pose in canonical joint order.""" + """Return the named shutdown pose or an explicit canonical override.""" + if self.lie_down_joint_position is not None: + try: + position = np.asarray(self.lie_down_joint_position, dtype=np.float32) + except (TypeError, ValueError) as error: + raise ValidationError( + "runtime.lie_down_joint_position", + "12 finite canonical joint positions", + self.lie_down_joint_position, + ) from error + if position.shape != (GO2_MOTOR_COUNT,) or not np.all(np.isfinite(position)): + raise ValidationError( + "runtime.lie_down_joint_position", + "12 finite canonical joint positions", + self.lie_down_joint_position, + ) + return position.copy() + assert self.robot is not None try: - position = np.asarray(self.lie_down_joint_position, dtype=np.float32) - except (TypeError, ValueError) as error: - raise ValidationError( - "runtime.lie_down_joint_position", - "12 finite canonical joint positions", - self.lie_down_joint_position, - ) from error - if position.shape != (GO2_MOTOR_COUNT,) or not np.all(np.isfinite(position)): - raise ValidationError( - "runtime.lie_down_joint_position", - "12 finite canonical joint positions", - self.lie_down_joint_position, - ) - return np.array(position, dtype=np.float32, copy=True) + pose = self.robot.key_pose.poses["lie_down"] + except KeyError as error: + raise ValidationError("runtime.robot.key_pose", "a named lie_down pose", "missing") from error + return np.asarray(pose, dtype=np.float32).copy() def gain_override(self, name: str, joint_count: int) -> np.ndarray | None: """Expand an optional scalar or canonical per-joint gain override.""" @@ -214,15 +207,19 @@ def gain_override(self, name: str, joint_count: int) -> np.ndarray | None: def motor_indices(self, joint_names: tuple[str, ...]) -> np.ndarray: """Return motor indices in artifact canonical joint order.""" + mapping = self.joint_name_to_motor_index + if mapping is None: + assert self.robot is not None + mapping = {name: binding.index for name, binding in self.robot.actuation.motors.items()} expected = set(joint_names) - actual = set(self.joint_name_to_motor_index) + actual = set(mapping) if actual != expected: raise ValidationError( "runtime.joint_name_to_motor_index", f"exactly joints {sorted(expected)}", f"missing={sorted(expected - actual)}, unknown={sorted(actual - expected)}", ) - values = tuple(self.joint_name_to_motor_index[name] for name in joint_names) + values = tuple(mapping[name] for name in joint_names) if any(not isinstance(index, int) or isinstance(index, bool) for index in values): raise ValidationError("runtime.motor_indices", "integer motor indices", values) if len(set(values)) != len(values) or any(not 0 <= index < GO2_MOTOR_COUNT for index in values): @@ -236,7 +233,6 @@ def motor_indices(self, joint_names: tuple[str, ...]) -> np.ndarray: __all__ = [ "GO2_JOINT_NAME_TO_MOTOR_INDEX", - "GO2_LIE_DOWN_JOINT_POSITION", "GO2_MOTOR_COUNT", "UnitreeGo2BackendConfig", ] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py index 5a4ee0d3..f56ef610 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py @@ -8,21 +8,35 @@ from dataclasses import replace from pathlib import Path from types import TracebackType -from typing import Any +from typing import Any, Protocol, runtime_checkable import numpy as np from motrix_deploy.artifact import read_artifact -from motrix_deploy.contracts import HealthStatus, JointServoCommand, RobotSpec, RobotState +from motrix_deploy.contracts import HealthStatus, JointServoCommand, RobotCommand, RobotSpec, RobotState from motrix_deploy.errors import ValidationError from motrix_deploy.profile import DeploymentProfile -from motrix_deploy_unitree.config import ( - GO2_JOINT_NAME_TO_MOTOR_INDEX, - UnitreeGo2BackendConfig, -) +from motrix_deploy_unitree.config import UnitreeGo2BackendConfig from motrix_deploy_unitree.interface import UnitreeGo2RobotInterface, UnitreeSdkBindings +@runtime_checkable +class _JointGainSpec(Protocol): + """Structural task contract needed by direct joint control, not a task runtime.""" + + @property + def kp(self) -> list[float]: ... + + @property + def kd(self) -> list[float]: ... + + +def _joint_gains(task: object) -> _JointGainSpec: + if not isinstance(task, _JointGainSpec): + raise ValidationError("task", "a typed task spec with kp and kd joint vectors", type(task).__name__) + return task + + class UnitreeGo2DirectInterface: """Read state and send canonical commands without running a policy. @@ -35,8 +49,8 @@ class UnitreeGo2DirectInterface: def __init__( self, *, - robot: RobotSpec, - task_config: Mapping[str, Any], + kp: list[float], + kd: list[float], control_period_s: float, state_timeout_s: float, backend: UnitreeGo2RobotInterface, @@ -47,17 +61,20 @@ def __init__( ): if not isinstance(value, (int, float)) or isinstance(value, bool) or not np.isfinite(value) or value <= 0: raise ValidationError(path, "a positive finite number", value) - self.robot = robot - self.task_config = dict(task_config) + self.robot = backend.spec + self._kp = self._command_array(kp, "kp") + self._kd = self._command_array(kd, "kd") self.control_period_s = float(control_period_s) self.state_timeout_s = float(state_timeout_s) self.backend = backend self._opened = False self._closed = False + # Direct callers provide explicit typed gains; preserve the main direct API's + # startup fallback without changing policy runtime gain requirements. backend.config = replace( backend.config, - kp=self._task_gain("kp") if backend.config.kp is None else backend.config.kp, - kd=self._task_gain("kd") if backend.config.kd is None else backend.config.kd, + kp=self._kp.copy() if backend.config.kp is None else backend.config.kp, + kd=self._kd.copy() if backend.config.kd is None else backend.config.kd, ) @classmethod @@ -66,16 +83,18 @@ def from_profile( profile: DeploymentProfile, *, network_interface: str, - joint_name_to_motor_index: Mapping[str, int] = GO2_JOINT_NAME_TO_MOTOR_INDEX, + joint_name_to_motor_index: Mapping[str, int] | None = None, backend_options: Mapping[str, Any] | None = None, sdk: UnitreeSdkBindings | None = None, clock_ns: Callable[[], int] = time.monotonic_ns, sleep: Callable[[float], None] = time.sleep, ) -> "UnitreeGo2DirectInterface": """Build the direct API from an explicit robot, gain, and timing profile.""" + gains = _joint_gains(profile.task) return cls._from_contract( robot=profile.robot, - task_config=profile.task.model_dump(mode="python"), + kp=gains.kp, + kd=gains.kd, control_period_s=profile.control.period_s, state_timeout_s=profile.control.state_timeout_s, network_interface=network_interface, @@ -92,7 +111,7 @@ def from_artifact( artifact_path: str | Path, *, network_interface: str, - joint_name_to_motor_index: Mapping[str, int] = GO2_JOINT_NAME_TO_MOTOR_INDEX, + joint_name_to_motor_index: Mapping[str, int] | None = None, backend_options: Mapping[str, Any] | None = None, sdk: UnitreeSdkBindings | None = None, clock_ns: Callable[[], int] = time.monotonic_ns, @@ -101,9 +120,11 @@ def from_artifact( """Build the direct API from the same artifact and settings as deployment.""" artifact = read_artifact(artifact_path) manifest = artifact.manifest + gains = _joint_gains(manifest.task) return cls._from_contract( robot=manifest.robot, - task_config=manifest.task.model_dump(mode="python"), + kp=gains.kp, + kd=gains.kd, control_period_s=manifest.control.period_s, state_timeout_s=manifest.control.state_timeout_s, network_interface=network_interface, @@ -119,11 +140,12 @@ def _from_contract( cls, *, robot: RobotSpec, - task_config: Mapping[str, Any], + kp: list[float], + kd: list[float], control_period_s: float, state_timeout_s: float, network_interface: str, - joint_name_to_motor_index: Mapping[str, int], + joint_name_to_motor_index: Mapping[str, int] | None, backend_options: Mapping[str, Any] | None, sdk: UnitreeSdkBindings | None, clock_ns: Callable[[], int], @@ -131,10 +153,9 @@ def _from_contract( ) -> "UnitreeGo2DirectInterface": """Construct the production backend from validated control contracts.""" options = dict(backend_options or {}) - options.update( - network_interface=network_interface, - joint_name_to_motor_index=dict(joint_name_to_motor_index), - ) + options["network_interface"] = network_interface + if joint_name_to_motor_index is not None: + options["joint_name_to_motor_index"] = dict(joint_name_to_motor_index) config = UnitreeGo2BackendConfig.from_mapping(options) backend = UnitreeGo2RobotInterface( config, @@ -146,8 +167,8 @@ def _from_contract( sleep=sleep, ) return cls( - robot=robot, - task_config=task_config, + kp=kp, + kd=kd, control_period_s=control_period_s, state_timeout_s=state_timeout_s, backend=backend, @@ -181,8 +202,8 @@ def default_pose_command(self) -> JointServoCommand: joint_position=np.array(self.robot.default_joint_position, copy=True), joint_velocity=zeros, feedforward_torque=zeros, - kp=self._task_gain("kp"), - kd=self._task_gain("kd"), + kp=self._kp.copy(), + kd=self._kd.copy(), ) def enable_command_output(self) -> None: @@ -207,11 +228,11 @@ def make_joint_command( feedforward_torque=( zeros if feedforward_torque is None else self._command_array(feedforward_torque, "feedforward_torque") ), - kp=self._task_gain("kp") if kp is None else self._command_array(kp, "kp"), - kd=self._task_gain("kd") if kd is None else self._command_array(kd, "kd"), + kp=self._kp.copy() if kp is None else self._command_array(kp, "kp"), + kd=self._kd.copy() if kd is None else self._command_array(kd, "kd"), ) - def send_command(self, command: JointServoCommand) -> None: + def send_command(self, command: RobotCommand) -> None: """Send one canonical command through the production LowCmd writer.""" self._require_open() self.backend.write_command(command) @@ -269,15 +290,6 @@ def __exit__( del exception_type, exception, traceback self.close() - def _task_gain(self, name: str) -> np.ndarray: - if name not in self.task_config: - raise ValidationError( - f"task.config.{name}", - f"{self.robot.joint_count} joint gains", - "missing", - ) - return self._command_array(self.task_config[name], name) - def _command_array(self, value: object, name: str) -> np.ndarray: try: array = np.asarray(value, dtype=np.float32) diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/go2_joint_control.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/go2_joint_control.py index d25423ff..6ff4e639 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/go2_joint_control.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/go2_joint_control.py @@ -16,10 +16,14 @@ def _build_robot( artifact: str | Path, network_interface: str, + *, + kp: float, + kd: float, ) -> UnitreeGo2DirectInterface: return UnitreeGo2DirectInterface.from_artifact( artifact, network_interface=network_interface, + backend_options={"kp": kp, "kd": kd}, ) @@ -69,6 +73,8 @@ def run_joint_control( network_interface: str, joint_name: str, target_position: float, + kp: float, + kd: float, move_duration: float, hold_duration: float, return_duration: float, @@ -79,7 +85,7 @@ def run_joint_control( if not math.isfinite(target_position): raise ValueError(f"target_position must be finite, got {target_position!r}") - robot = _build_robot(artifact, network_interface) + robot = _build_robot(artifact, network_interface, kp=kp, kd=kd) joint_names: Sequence[str] = robot.robot.joint_names if joint_name not in joint_names: raise ValueError(f"unknown joint {joint_name!r}; expected one of {list(joint_names)}") diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py new file mode 100644 index 00000000..f93d1c83 --- /dev/null +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py @@ -0,0 +1,88 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Asset-free hardware configuration and Unitree Go2 defaults.""" + +from motrix_deploy_unitree.actuation import Actuation, UnitreeMotorBinding +from motrix_deploy_unitree.sensor import HardwareSensorBinding, HardwareSensorBindings +from motrix_env_core.config import configclass +from motrix_env_core.config.scene.base import KeyPoseCfg + + +@configclass(kw_only=True) +class HardwareRobotCfg: + """SDK-neutral hardware wiring with asset-free canonical key poses. + + Enable sequences and transport remain backend-owned. Sensor bindings name + hardware message fields, not model resources or synthesized measurements. + """ + + key_pose: KeyPoseCfg = KeyPoseCfg() + sensors: HardwareSensorBindings = HardwareSensorBindings() + actuation: Actuation = Actuation() + + def validate(self) -> None: + self.key_pose.validate() + unknown_joints = self.actuation.motors.keys() - set(self.key_pose.joint_names) + if unknown_joints: + raise ValueError(f"Actuation references unknown canonical joints: {sorted(unknown_joints)}") + + +@configclass(kw_only=True) +class UnitreeGo2HardwareCfg(HardwareRobotCfg): + """Unitree Go2 hardware-owned key poses and SDK-neutral bindings.""" + + key_pose: KeyPoseCfg = KeyPoseCfg( + joint_names=[ + "FL_hip_joint", + "FL_thigh_joint", + "FL_calf_joint", + "FR_hip_joint", + "FR_thigh_joint", + "FR_calf_joint", + "RL_hip_joint", + "RL_thigh_joint", + "RL_calf_joint", + "RR_hip_joint", + "RR_thigh_joint", + "RR_calf_joint", + ], + poses={ + "default": [0.0, 0.8, -1.5, 0.0, 0.8, -1.5, 0.0, 1.0, -1.5, 0.0, 1.0, -1.5], + "lie_down": [ + 0.05175, + 1.238835, + -2.74427, + -0.0608, + 1.24118, + -2.7375, + 0.31617, + 1.26637, + -2.79547, + -0.310495, + 1.266385, + -2.80177, + ], + }, + ) + sensors: HardwareSensorBindings = HardwareSensorBindings( + base_orientation_xyzw=HardwareSensorBinding(field="imu_state.quaternion", quaternion_order="wxyz"), + base_angular_velocity=HardwareSensorBinding(field="imu_state.gyroscope"), + base_linear_acceleration=HardwareSensorBinding(field="imu_state.accelerometer"), + ) + actuation: Actuation = Actuation( + motors={ + "FL_hip_joint": UnitreeMotorBinding(index=3), + "FL_thigh_joint": UnitreeMotorBinding(index=4), + "FL_calf_joint": UnitreeMotorBinding(index=5), + "FR_hip_joint": UnitreeMotorBinding(index=0), + "FR_thigh_joint": UnitreeMotorBinding(index=1), + "FR_calf_joint": UnitreeMotorBinding(index=2), + "RL_hip_joint": UnitreeMotorBinding(index=9), + "RL_thigh_joint": UnitreeMotorBinding(index=10), + "RL_calf_joint": UnitreeMotorBinding(index=11), + "RR_hip_joint": UnitreeMotorBinding(index=6), + "RR_thigh_joint": UnitreeMotorBinding(index=7), + "RR_calf_joint": UnitreeMotorBinding(index=8), + } + ) diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py index e24a2323..461c5c90 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py @@ -8,6 +8,7 @@ import time from collections.abc import Callable from dataclasses import dataclass +from operator import attrgetter from typing import Any import numpy as np @@ -16,6 +17,7 @@ HealthStatus, JointControlMode, JointServoCommand, + JointTorqueCommand, RobotCapabilities, RobotCommand, RobotSpec, @@ -31,6 +33,13 @@ _VELOCITY_STOP = 16000.0 +def _sdk_field_reader(field: str) -> Callable[[Any], Any]: + """Resolve public dotted SDK attributes, never expressions or indexing.""" + if any(not part.isidentifier() or part.startswith("_") for part in field.split(".")): + raise ValidationError("runtime.robot.sensors.hardware.field", "public dotted SDK attributes", field) + return attrgetter(field) + + @dataclass(frozen=True) class UnitreeSdkBindings: """Late-bound SDK symbols, injectable for adapter contract tests.""" @@ -101,6 +110,7 @@ def __init__( self._state_timeout_s = float(state_timeout_s) self._sdk = sdk self._clock_ns = clock_ns + self._spec = spec self._sleep = sleep self._condition = threading.Condition() self._write_lock = threading.Lock() @@ -109,8 +119,11 @@ def __init__( self._low_cmd: Any = None self._command_crc: Any = None self._state_crc: Any = None - self._spec = spec self._motor_indices = np.empty(0, dtype=np.int64) + self._motor_modes: tuple[int, ...] = () + self._sensor_readers: dict[str, Callable[[Any], Any]] = {} + self._quaternion_indices = [1, 2, 3, 0] + self._lie_down_position = np.empty(0, dtype=np.float32) self._kp_override: np.ndarray | None = None self._kd_override: np.ndarray | None = None self._capabilities: RobotCapabilities | None = None @@ -185,13 +198,11 @@ def _disable_high_level_motion_services(self, sdk: UnitreeSdkBindings) -> None: raise RuntimeError(f"RobotState ServiceSwitch(sport_mode, false) failed with code {service_status}") def open(self) -> None: - spec = self._spec if self._opened and not self._closed: raise RuntimeError("Unitree backend is already open") - self._motor_indices = self.config.motor_indices(spec.joint_names) - self._spec = spec - self._kp_override = self.config.gain_override("kp", spec.joint_count) - self._kd_override = self.config.gain_override("kd", spec.joint_count) + self._resolve_hardware_bindings(self._spec) + self._kp_override = self.config.gain_override("kp", self._spec.joint_count) + self._kd_override = self.config.gain_override("kd", self._spec.joint_count) sdk = self._sdk or _load_sdk_bindings() self._sdk = sdk sdk.channel_factory_initialize(self.config.domain_id, self.config.network_interface) @@ -220,7 +231,7 @@ def open(self) -> None: len(self._low_cmd.motor_cmd), ) self._capabilities = RobotCapabilities( - control_modes=(JointControlMode.SERVO,), + control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), state_fields=frozenset( { "joint_position", @@ -242,6 +253,35 @@ def open(self) -> None: self._health_reason = "" self._last_communication_ns = self._latest_receive_time_ns + def _resolve_hardware_bindings(self, spec: RobotSpec) -> None: + robot = self.config.robot + assert robot is not None + self._motor_indices = self.config.motor_indices(spec.joint_names) + bindings = robot.actuation.motors + if set(bindings) != set(spec.joint_names): + raise ValidationError("runtime.robot.actuation.motors", "exactly the canonical joints", set(bindings)) + self._motor_modes = tuple( + bindings[name].enabled_mode if self.config.motor_mode is None else self.config.motor_mode + for name in spec.joint_names + ) + for role in ("base_orientation_xyzw", "base_angular_velocity", "base_linear_acceleration"): + sensor = getattr(robot.sensors, role) + if sensor is None: + raise ValidationError(f"runtime.robot.sensors.{role}", "a hardware binding", "missing") + self._sensor_readers[role] = _sdk_field_reader(sensor.field) + order = robot.sensors.base_orientation_xyzw.quaternion_order + if order not in ("wxyz", "xyzw"): + raise ValidationError("runtime.robot.sensors.base_orientation_xyzw", "a declared quaternion order", order) + self._quaternion_indices = [1, 2, 3, 0] if order == "wxyz" else [0, 1, 2, 3] + robot.key_pose.validate() + target = self.config.lie_down_position() + if self.config.lie_down_joint_position is None: + pose_by_joint = dict(zip(robot.key_pose.joint_names, target)) + if set(pose_by_joint) != set(spec.joint_names): + raise ValidationError("runtime.robot.key_pose", "exactly the canonical joints", set(pose_by_joint)) + target = np.asarray([pose_by_joint[name] for name in spec.joint_names], dtype=np.float32) + self._lie_down_position = target + def enable(self) -> None: self._require_open() if self._enabled: @@ -261,7 +301,6 @@ def enable(self) -> None: kd=self._kd_override, ) self._validate_command(initial_command) - kp, kd = self._effective_gains(initial_command) self._last_position_kp = np.array(kp, dtype=np.float32, copy=True) self._last_position_kd = np.array(kd, dtype=np.float32, copy=True) @@ -445,21 +484,23 @@ def _latest_robot_state(self) -> RobotState: def _state_from_message(self, message: Any, receive_time_ns: int) -> RobotState: position = np.asarray([message.motor_state[index].q for index in self._motor_indices], dtype=np.float32) velocity = np.asarray([message.motor_state[index].dq for index in self._motor_indices], dtype=np.float32) - quaternion_wxyz = np.asarray(message.imu_state.quaternion, dtype=np.float32) - if quaternion_wxyz.shape != (4,) or not np.all(np.isfinite(quaternion_wxyz)): - raise ValidationError("state.imu.quaternion", "four finite wxyz values", quaternion_wxyz) - norm = float(np.linalg.norm(quaternion_wxyz)) + quaternion = np.asarray(self._sensor_readers["base_orientation_xyzw"](message), dtype=np.float32) + if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)): + raise ValidationError("state.base_orientation_xyzw", "four finite quaternion values", quaternion) + norm = float(np.linalg.norm(quaternion)) if norm <= 1e-8: - raise ValidationError("state.imu.quaternion", "a non-zero quaternion", norm) - quaternion_xyzw = (quaternion_wxyz[[1, 2, 3, 0]] / np.float32(norm)).astype(np.float32) + raise ValidationError("state.base_orientation_xyzw", "a non-zero quaternion", norm) + quaternion_xyzw = (quaternion[self._quaternion_indices] / np.float32(norm)).astype(np.float32) return RobotState( sample_time_ns=receive_time_ns, receive_time_ns=receive_time_ns, joint_position=position, joint_velocity=velocity, base_orientation_xyzw=quaternion_xyzw, - base_angular_velocity=np.asarray(message.imu_state.gyroscope, dtype=np.float32), - base_linear_acceleration=np.asarray(message.imu_state.accelerometer, dtype=np.float32), + base_angular_velocity=np.asarray(self._sensor_readers["base_angular_velocity"](message), dtype=np.float32), + base_linear_acceleration=np.asarray( + self._sensor_readers["base_linear_acceleration"](message), dtype=np.float32 + ), ) def _validate_message_fields(self, message: Any) -> None: @@ -469,20 +510,32 @@ def _validate_message_fields(self, message: Any) -> None: f"at least {GO2_MOTOR_COUNT} motor states", len(message.motor_state), ) - for name, expected in (("quaternion", 4), ("gyroscope", 3), ("accelerometer", 3)): - value = np.asarray(getattr(message.imu_state, name), dtype=np.float32) + for role, expected in ( + ("base_orientation_xyzw", 4), + ("base_angular_velocity", 3), + ("base_linear_acceleration", 3), + ): + value = np.asarray(self._sensor_readers[role](message), dtype=np.float32) if value.shape != (expected,) or not np.all(np.isfinite(value)): - raise ValidationError(f"state.imu.{name}", f"{expected} finite values", value) + raise ValidationError(f"state.{role}", f"{expected} finite values", value) def _validate_command(self, command: RobotCommand) -> None: - if not isinstance(command, JointServoCommand): - raise ValidationError("command.mode", "joint_servo", command.mode) - assert self._spec is not None expected = (self._spec.joint_count,) - for name in ("joint_position", "joint_velocity", "feedforward_torque", "kp", "kd"): + if not isinstance(command, (JointServoCommand, JointTorqueCommand)): + raise ValidationError("command.type", "JointServoCommand or JointTorqueCommand", type(command).__name__) + fields = ( + ("torque",) + if isinstance(command, JointTorqueCommand) + else ("joint_position", "joint_velocity", "feedforward_torque", "kp", "kd") + ) + for name in fields: value = getattr(command, name) if value.shape != expected or value.dtype != np.float32 or not np.all(np.isfinite(value)): raise ValidationError(f"command.{name}", f"finite float32 shape {expected}", value) + if isinstance(command, JointTorqueCommand): + if np.any(np.abs(command.torque) > self._spec.torque_limit): + raise ValidationError("command.torque", "inside RobotSpec torque limits", command.torque.tolist()) + return if np.any(command.joint_position < self._spec.position_lower) or np.any( command.joint_position > self._spec.position_upper ): @@ -504,14 +557,29 @@ def _initialize_low_cmd(self) -> None: self._low_cmd.level_flag = 0xFF self._low_cmd.gpio = 0 for motor in self._low_cmd.motor_cmd: - motor.mode = self.config.motor_mode + motor.mode = 0 motor.q = _POSITION_STOP motor.qd = _VELOCITY_STOP motor.kp = 0.0 motor.kd = 0.0 motor.tau = 0.0 + for motor_index, mode in zip(self._motor_indices, self._motor_modes): + self._low_cmd.motor_cmd[int(motor_index)].mode = mode def _publish_robot_command(self, command: RobotCommand) -> None: + if isinstance(command, JointTorqueCommand): + # Do not replace the saved servo safety gains with torque-mode zeros. + with self._write_lock: + for canonical_index, motor_index in enumerate(self._motor_indices): + motor = self._low_cmd.motor_cmd[int(motor_index)] + motor.mode = self._motor_modes[canonical_index] + motor.q = 0.0 + motor.qd = 0.0 + motor.kp = 0.0 + motor.kd = 0.0 + motor.tau = float(command.torque[canonical_index]) + self._write_low_cmd_locked() + return kp, kd = self._effective_gains(command) self._publish_joint_fields( command.joint_position, @@ -521,7 +589,7 @@ def _publish_robot_command(self, command: RobotCommand) -> None: kd, ) - def _effective_gains(self, command: RobotCommand) -> tuple[np.ndarray, np.ndarray]: + def _effective_gains(self, command: JointServoCommand) -> tuple[np.ndarray, np.ndarray]: kp = command.kp if self._kp_override is None else self._kp_override kd = command.kd if self._kd_override is None else self._kd_override return kp, kd @@ -539,7 +607,7 @@ def _publish_joint_fields( with self._write_lock: for canonical_index, motor_index in enumerate(self._motor_indices): motor = self._low_cmd.motor_cmd[int(motor_index)] - motor.mode = self.config.motor_mode + motor.mode = self._motor_modes[canonical_index] motor.q = float(position[canonical_index]) motor.qd = float(velocity[canonical_index]) motor.kp = float(kp[canonical_index]) @@ -549,9 +617,9 @@ def _publish_joint_fields( def _publish_zero_torque(self) -> None: with self._write_lock: - for motor_index in self._motor_indices: + for canonical_index, motor_index in enumerate(self._motor_indices): motor = self._low_cmd.motor_cmd[int(motor_index)] - motor.mode = self.config.motor_mode + motor.mode = self._motor_modes[canonical_index] motor.q = 0.0 motor.qd = 0.0 motor.kp = 0.0 @@ -563,7 +631,7 @@ def _publish_lie_down_trajectory(self) -> None: if self._last_position_kp.shape != (GO2_MOTOR_COUNT,) or self._last_position_kd.shape != (GO2_MOTOR_COUNT,): raise RuntimeError("position gains are unavailable for lie-down shutdown") start = self._latest_robot_state().joint_position - target = self.config.lie_down_position() + target = self._lie_down_position zeros = np.zeros(GO2_MOTOR_COUNT, dtype=np.float32) steps = max(1, math.ceil(self.config.lie_down_duration_s / self._control_period_s)) for step in range(1, steps + 1): @@ -572,20 +640,22 @@ def _publish_lie_down_trajectory(self) -> None: raise EmergencyStopError(f"remote {self.config.emergency_stop_button!r} requested damping stop") alpha = np.float32(step / steps) position = ((np.float32(1.0) - alpha) * start + alpha * target).astype(np.float32) - self._publish_joint_fields( - position, - zeros, - zeros, - self._last_position_kp, - self._last_position_kd, + self._publish_robot_command( + JointServoCommand( + joint_position=position, + joint_velocity=zeros, + feedforward_torque=zeros, + kp=self._last_position_kp, + kd=self._last_position_kd, + ) ) self._wait_control_period() def _publish_damping(self) -> None: with self._write_lock: - for motor_index in self._motor_indices: + for canonical_index, motor_index in enumerate(self._motor_indices): motor = self._low_cmd.motor_cmd[int(motor_index)] - motor.mode = self.config.motor_mode + motor.mode = self._motor_modes[canonical_index] motor.q = 0.0 motor.qd = 0.0 motor.kp = 0.0 diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/sensor.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/sensor.py new file mode 100644 index 00000000..5b16ab30 --- /dev/null +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/sensor.py @@ -0,0 +1,34 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""SDK-neutral hardware sensor field bindings.""" + +from dataclasses import dataclass + +from motrix_env_core.config import configclass + + +@dataclass(frozen=True) +class HardwareSensorBinding: + """A declarative message field understood by a concrete hardware backend.""" + + field: str + quaternion_order: str | None = None + + def __post_init__(self) -> None: + if not self.field: + raise ValueError("hardware sensor field must not be empty") + if self.quaternion_order not in (None, "wxyz", "xyzw"): + raise ValueError(f"unsupported quaternion order: {self.quaternion_order!r}") + + +@configclass +class HardwareSensorBindings: + """Optional hardware message bindings for canonical base measurements.""" + + base_orientation_xyzw: HardwareSensorBinding | None = None + base_angular_velocity: HardwareSensorBinding | None = None + base_linear_acceleration: HardwareSensorBinding | None = None + + +__all__ = ["HardwareSensorBinding", "HardwareSensorBindings"] diff --git a/motrix_deploy_unitree/tests/test_hardware_contracts.py b/motrix_deploy_unitree/tests/test_hardware_contracts.py new file mode 100644 index 00000000..154b1ca8 --- /dev/null +++ b/motrix_deploy_unitree/tests/test_hardware_contracts.py @@ -0,0 +1,249 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Hardware-owned configuration, wiring, and dependency-boundary contracts.""" + +import subprocess +import sys +import textwrap +from dataclasses import FrozenInstanceError + +import pytest +from hydra import compose, initialize +from hydra.core.config_store import ConfigStore +from hydra.utils import instantiate +from omegaconf import OmegaConf +from omegaconf.errors import ConfigAttributeError, ConfigKeyError, ReadonlyConfigError + +from motrix_deploy_unitree.actuation import Actuation, UnitreeMotorBinding +from motrix_deploy_unitree.hardware import HardwareRobotCfg, UnitreeGo2HardwareCfg +from motrix_deploy_unitree.sensor import HardwareSensorBinding, HardwareSensorBindings +from motrix_env_core.config.scene.base import KeyPoseCfg + + +def test_hardware_preserves_sensor_and_motor_mapping() -> None: + robot = UnitreeGo2HardwareCfg() + robot.validate() + assert type(robot.key_pose) is KeyPoseCfg + assert "default" in robot.key_pose.poses + assert "lie_down" in robot.key_pose.poses + assert robot.sensors == HardwareSensorBindings( + base_orientation_xyzw=HardwareSensorBinding("imu_state.quaternion", quaternion_order="wxyz"), + base_angular_velocity=HardwareSensorBinding("imu_state.gyroscope"), + base_linear_acceleration=HardwareSensorBinding("imu_state.accelerometer"), + ) + # SDK motor addresses, enabled modes, and wire quaternion order are hardware contracts. + assert [robot.actuation.motors[name].index for name in robot.key_pose.joint_names] == [ + 3, + 4, + 5, + 0, + 1, + 2, + 9, + 10, + 11, + 6, + 7, + 8, + ] + assert all(binding.enabled_mode == 0x0A for binding in robot.actuation.motors.values()) + assert set(robot.actuation.motors) == set(robot.key_pose.joint_names) + + +def test_hardware_defaults_are_instance_local() -> None: + first = UnitreeGo2HardwareCfg() + second = UnitreeGo2HardwareCfg() + assert first == second + assert first.sensors is not second.sensors + assert first.sensors.base_angular_velocity is not second.sensors.base_angular_velocity + assert first.key_pose.joint_names is not second.key_pose.joint_names + assert first.key_pose.poses is not second.key_pose.poses + for pose_name in first.key_pose.poses: + assert first.key_pose.poses[pose_name] is not second.key_pose.poses[pose_name] + first.key_pose.poses[pose_name][0] += 1.0 + first.sensors.base_angular_velocity = HardwareSensorBinding("custom.gyro") + first.key_pose.joint_names[0] = "custom_joint" + first.actuation.motors.clear() + assert second == UnitreeGo2HardwareCfg() + + first_generic = HardwareRobotCfg() + second_generic = HardwareRobotCfg() + first_generic.sensors.base_angular_velocity = HardwareSensorBinding("custom.gyro") + assert first_generic.sensors is not second_generic.sensors + assert second_generic.sensors == HardwareSensorBindings() + + +def test_sensor_binding_leaf_is_frozen_but_role_can_be_replaced() -> None: + bindings = HardwareSensorBindings(base_angular_velocity=HardwareSensorBinding("imu.gyro")) + with pytest.raises(FrozenInstanceError): + bindings.base_angular_velocity.field = "custom.gyro" + cfg = OmegaConf.structured(bindings) + with pytest.raises(ReadonlyConfigError): + cfg.base_angular_velocity.field = "custom.gyro" + cfg.base_angular_velocity = HardwareSensorBinding("custom.gyro") + restored = OmegaConf.to_object(cfg) + assert type(restored) is HardwareSensorBindings + assert type(restored.base_angular_velocity) is HardwareSensorBinding + assert restored.base_angular_velocity.field == "custom.gyro" + assert bindings.base_angular_velocity.field == "imu.gyro" + + +@pytest.mark.parametrize("source", [UnitreeGo2HardwareCfg, UnitreeGo2HardwareCfg()]) +def test_hardware_structured_configuration_overrides_round_trip(source) -> None: + robot = UnitreeGo2HardwareCfg() + cfg = OmegaConf.structured(source) + cfg.key_pose.poses.default[0] += 1.0 + cfg.sensors.base_angular_velocity = HardwareSensorBinding("custom.gyro") + cfg.sensors.base_linear_acceleration = None + restored = OmegaConf.to_object(cfg) + restored.validate() + assert type(restored) is UnitreeGo2HardwareCfg + assert type(restored.key_pose) is KeyPoseCfg + assert type(restored.sensors) is HardwareSensorBindings + assert restored.key_pose.poses["default"][0] == robot.key_pose.poses["default"][0] + 1.0 + assert restored.sensors == HardwareSensorBindings( + base_orientation_xyzw=robot.sensors.base_orientation_xyzw, + base_angular_velocity=HardwareSensorBinding("custom.gyro"), + ) + assert robot == UnitreeGo2HardwareCfg() + + +def test_hardware_hydra_composition_and_instantiation() -> None: + schema_name = "hardware_sensor_bindings_test_schema" + ConfigStore.instance().store(name=schema_name, node=UnitreeGo2HardwareCfg) + with initialize(version_base=None, config_path=None): + cfg = compose( + config_name=schema_name, + overrides=[ + "sensors.base_angular_velocity=null", + "sensors.base_linear_acceleration=null", + ], + ) + restored = OmegaConf.to_object(cfg) + restored.validate() + assert type(restored.sensors) is HardwareSensorBindings + assert restored.sensors == HardwareSensorBindings( + base_orientation_xyzw=UnitreeGo2HardwareCfg().sensors.base_orientation_xyzw, + ) + # Frozen leaf mappings are replaced as typed values rather than merged field-by-field. + cfg.sensors.base_angular_velocity = HardwareSensorBinding("custom.gyro") + restored = OmegaConf.to_object(cfg) + assert restored.sensors.base_angular_velocity == HardwareSensorBinding("custom.gyro") + composed = instantiate( + {"_target_": "motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg", "sensors": cfg.sensors}, + _convert_="object", + ) + composed.validate() + assert composed.sensors == restored.sensors + assert type(composed.sensors) is HardwareSensorBindings + + customized = instantiate( + { + "_target_": "motrix_deploy_unitree.hardware.HardwareRobotCfg", + "sensors": { + "_target_": "motrix_deploy_unitree.sensor.HardwareSensorBindings", + "base_angular_velocity": { + "_target_": "motrix_deploy_unitree.sensor.HardwareSensorBinding", + "field": "custom.gyro", + }, + }, + } + ) + customized.validate() + assert customized.sensors == HardwareSensorBindings( + base_angular_velocity=HardwareSensorBinding("custom.gyro"), + ) + assert type(customized.sensors.base_angular_velocity) is HardwareSensorBinding + + +def test_sensor_roles_are_checked_at_structured_configuration_boundary() -> None: + cfg = OmegaConf.structured(HardwareRobotCfg) + with pytest.raises(ConfigAttributeError, match="base_angluar_velocity"): + OmegaConf.update(cfg, "sensors.base_angluar_velocity", {"field": "imu.gyro"}) + with pytest.raises(ConfigKeyError, match="base_angluar_velocity"): + OmegaConf.merge(cfg, {"sensors": {"base_angluar_velocity": {"field": "imu.gyro"}}}) + + +def test_custom_asset_free_hardware_key_pose() -> None: + key_pose = KeyPoseCfg(joint_names=["hip"], poses={"default": [0.0]}) + sensors = HardwareSensorBindings(base_angular_velocity=HardwareSensorBinding("imu.gyro")) + robot = HardwareRobotCfg( + key_pose=key_pose, + sensors=sensors, + actuation=Actuation(motors={"hip": UnitreeMotorBinding(0)}), + ) + robot.validate() + assert robot.key_pose is key_pose + assert robot.sensors is sensors + assert robot.sensors.base_angular_velocity == HardwareSensorBinding("imu.gyro") + cfg = OmegaConf.structured(robot) + cfg.sensors.base_linear_acceleration = HardwareSensorBinding("imu.accelerometer") + restored = OmegaConf.to_object(cfg) + restored.validate() + assert restored.sensors == HardwareSensorBindings( + base_angular_velocity=HardwareSensorBinding("imu.gyro"), + base_linear_acceleration=HardwareSensorBinding("imu.accelerometer"), + ) + + +def test_key_pose_validates_canonical_order() -> None: + with pytest.raises(ValueError, match="unique"): + HardwareRobotCfg(key_pose=KeyPoseCfg(joint_names=["hip", "hip"])).validate() + with pytest.raises(ValueError, match="contain 1"): + KeyPoseCfg(joint_names=["hip"], poses={"default": [0.0, 1.0]}).validate() + + +def test_actuation_rejects_duplicate_motor_indices_and_unknown_joints() -> None: + with pytest.raises(ValueError, match="unique"): + Actuation(motors={"a": UnitreeMotorBinding(1), "b": UnitreeMotorBinding(1)}) + with pytest.raises(ValueError, match="unknown canonical joints"): + HardwareRobotCfg(actuation=Actuation(motors={"unknown": UnitreeMotorBinding(0)})).validate() + + +@pytest.mark.parametrize("module", ["config", "sensor"]) +def test_hardware_configuration_imports_without_models_assets_or_sdk(module: str) -> None: + script = textwrap.dedent( + f""" + import importlib + import importlib.abc + import sys + from pathlib import Path + + blocked = ('motrix_robots', 'motrix_envs', 'motrixsim', 'mujoco', + 'torch', 'onnx', 'onnxruntime', 'unitree_sdk2py') + + class BlockHardwareRuntimeDependencies(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if any(fullname == name or fullname.startswith(name + '.') for name in blocked): + raise ImportError('hardware configuration cannot import ' + fullname) + + sys.meta_path.insert(0, BlockHardwareRuntimeDependencies()) + importlib.import_module('motrix_deploy_unitree.{module}') + from motrix_deploy_unitree.config import UnitreeGo2BackendConfig + from motrix_deploy_unitree.hardware import UnitreeGo2HardwareCfg + from motrix_deploy_unitree.interface import _load_sdk_bindings + + def forbidden_exists(self): + raise AssertionError('hardware must not check asset path existence') + + Path.exists = forbidden_exists + hardware = UnitreeGo2HardwareCfg() + hardware.validate() + config = UnitreeGo2BackendConfig(network_interface='eth0', robot=hardware) + assert config.robot is hardware + assert config.lie_down_position().shape == (12,) + assert not any(name == root or name.startswith(root + '.') + for name in sys.modules for root in blocked) + # The SDK is requested only by the explicit transport loader, not imports/configuration. + try: + _load_sdk_bindings() + except RuntimeError as error: + assert 'Unitree Go2 support requires' in str(error) + assert isinstance(error.__cause__, ImportError) + else: + raise AssertionError('transport loader must request the blocked SDK') + """ + ) + result = subprocess.run([sys.executable, "-c", script], capture_output=True, text=True) + assert result.returncode == 0, result.stderr diff --git a/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py b/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py index beb4fd24..6ae32618 100644 --- a/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py +++ b/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py @@ -5,28 +5,30 @@ import copy import struct +from dataclasses import asdict, replace from types import SimpleNamespace +from typing import ClassVar import numpy as np import pytest +from omegaconf import OmegaConf from motrix_deploy.artifact import ControlSpec, TaskSpec -from motrix_deploy.contracts import JointServoCommand, JointTorqueCommand, RobotSpec +from motrix_deploy.contracts import JointControlMode, JointServoCommand, JointTorqueCommand, RobotSpec from motrix_deploy.errors import EmergencyStopError, LieDownRequestedError, ValidationError from motrix_deploy.profile import DeploymentProfile from motrix_deploy.robot.interface import GamePadDeviceProvider from motrix_deploy.runtime.factory import RuntimeCreateContext from motrix_deploy.runtime.hardware import HardwareRuntime -from motrix_deploy_unitree import ( - UnitreeGo2BackendConfig, - UnitreeGo2DirectInterface, - UnitreeGo2RobotInterface, - UnitreeRemoteGamePadDevice, - UnitreeSdkBindings, - decode_wireless_remote, -) +from motrix_deploy_unitree.actuation import Actuation, UnitreeMotorBinding from motrix_deploy_unitree.cli import _parser, main +from motrix_deploy_unitree.config import UnitreeGo2BackendConfig +from motrix_deploy_unitree.direct import UnitreeGo2DirectInterface +from motrix_deploy_unitree.hardware import UnitreeGo2HardwareCfg +from motrix_deploy_unitree.interface import UnitreeGo2RobotInterface, UnitreeSdkBindings from motrix_deploy_unitree.plugin import create_runtime +from motrix_deploy_unitree.remote import UnitreeRemoteGamePadDevice, decode_wireless_remote +from motrix_deploy_unitree.sensor import HardwareSensorBinding, HardwareSensorBindings JOINT_NAMES = ( "FL_hip_joint", @@ -268,9 +270,9 @@ def _backend( ) -> UnitreeGo2RobotInterface: return UnitreeGo2RobotInterface( config, + spec=_spec(), control_period_s=0.02, state_timeout_s=0.1, - spec=_spec(), sdk=bus.sdk(), sleep=lambda _: None, ) @@ -382,30 +384,126 @@ def test_lowstate_and_lowcmd_are_mapped_by_canonical_joint_name() -> None: assert backend.health().reason == "backend is closed" -def test_enable_requires_explicit_hardware_safety_gains_before_publishing() -> None: +@pytest.mark.parametrize("gains", [{"kp": None}, {"kd": None}, {"kp": None, "kd": None}]) +def test_enable_requires_explicit_servo_safety_gains_before_output(gains: dict) -> None: bus = _Bus() - backend = _backend(_config(kp=None, kd=None), bus) + backend = _backend(_config(**gains), bus) backend.open() - + torque = JointTorqueCommand(torque=np.zeros(12, dtype=np.float32)) + with pytest.raises(RuntimeError, match="not enabled"): + backend.write_command(torque) with pytest.raises(ValidationError, match="safety gains"): backend.enable() assert bus.commands == [] backend.close() -def test_servo_only_interface_rejects_torque_commands_before_publishing() -> None: +def test_torque_enable_uses_servo_transition_then_maps_pure_torque_and_preserves_shutdown_gains() -> None: bus = _Bus() - backend = _backend(_config(), bus) + backend = _backend(_config(kp=30.0, kd=0.7, lie_down_duration_s=0.02, damping_duration_s=0.02), bus) backend.open() + assert backend.capabilities.control_modes == (JointControlMode.SERVO, JointControlMode.TORQUE) + torque = JointTorqueCommand(torque=np.linspace(-12.0, 12.0, 12, dtype=np.float32)) backend.enable() - command_count = len(bus.commands) + transition = copy.deepcopy(bus.commands[-1]) + for index, name in enumerate(JOINT_NAMES): + motor = transition.motor_cmd[MOTOR_MAPPING[name]] + assert motor.q == pytest.approx(_spec().default_joint_position[index]) + assert motor.kp == pytest.approx(30.0) + assert motor.kd == pytest.approx(0.7) + assert motor.tau == 0.0 + backend.write_command(torque) + for index, name in enumerate(JOINT_NAMES): + motor = bus.commands[-1].motor_cmd[MOTOR_MAPPING[name]] + assert (motor.q, motor.qd, motor.kp, motor.kd) == (0.0, 0.0, 0.0, 0.0) + assert motor.tau == pytest.approx(torque.torque[index]) + assert motor.mode == transition.motor_cmd[MOTOR_MAPPING[name]].mode + count = len(bus.commands) + with pytest.raises(ValidationError, match="command.torque"): + backend.write_command(JointTorqueCommand(torque=_spec().torque_limit + 1.0)) + assert len(bus.commands) == count + bus.state.wireless_remote = _remote_payload(_button("B")) + bus.emit() + backend.stop() + for index in MOTOR_MAPPING.values(): + assert bus.commands[-2].motor_cmd[index].kp == pytest.approx(30.0) + assert bus.commands[-2].motor_cmd[index].kd == pytest.approx(0.7) + assert bus.commands[-2].motor_cmd[index].tau == 0.0 + backend.close() - with pytest.raises(ValidationError, match="joint_servo"): - backend.write_command(JointTorqueCommand(torque=np.zeros(12, dtype=np.float32))) - assert len(bus.commands) == command_count + +def test_custom_robot_config_drives_sensor_wiring_motor_addresses_modes_and_shutdown_pose() -> None: + robot = UnitreeGo2HardwareCfg() + robot.actuation = Actuation( + motors={ + name: UnitreeMotorBinding(index=11 - index, enabled_mode=20 + index) + for index, name in enumerate(JOINT_NAMES) + } + ) + robot.sensors = HardwareSensorBindings( + base_orientation_xyzw=HardwareSensorBinding(field="custom.orientation", quaternion_order="xyzw"), + base_angular_velocity=HardwareSensorBinding(field="custom.angular_velocity"), + base_linear_acceleration=HardwareSensorBinding(field="custom.acceleration"), + ) + target = np.linspace(-0.2, 0.2, 12, dtype=np.float32) + robot.key_pose.joint_names = list(reversed(JOINT_NAMES)) + robot.key_pose.poses["lie_down"] = target[::-1].tolist() + bus = _Bus() + bus.state.custom = SimpleNamespace( + orientation=[0.0, 0.0, 2.0, 0.0], angular_velocity=[4.0, 5.0, 6.0], acceleration=[7.0, 8.0, 9.0] + ) + backend = _backend(_config(robot=robot, lie_down_duration_s=0.02, damping_duration_s=0.02), bus) + backend.open() + # Bindings and pose are resolved at open, not re-read from mutable robot configs. + robot.actuation = Actuation() + robot.sensors = HardwareSensorBindings() + robot.key_pose.poses["lie_down"] = [0.0] * 12 + state = backend.read_state(0.1) + np.testing.assert_array_equal(state.joint_position, np.arange(11, -1, -1)) + np.testing.assert_array_equal(state.base_orientation_xyzw, [0.0, 0.0, 1.0, 0.0]) + np.testing.assert_array_equal(state.base_angular_velocity, [4.0, 5.0, 6.0]) + np.testing.assert_array_equal(state.base_linear_acceleration, [7.0, 8.0, 9.0]) + backend.enable() + backend.write_command(_command(_spec().default_joint_position)) + for index in range(12): + assert bus.commands[-1].motor_cmd[11 - index].mode == 20 + index + bus.state.wireless_remote = _remote_payload(_button("B")) + bus.emit() + backend.stop() + for index in range(12): + assert bus.commands[-2].motor_cmd[11 - index].q == pytest.approx(target[index]) + assert bus.commands[-1].motor_cmd[11 - index].mode == 20 + index backend.close() +def test_direct_api_explicit_mapping_and_mode_overrides_remain_supported() -> None: + bus = _Bus() + mapping = dict(zip(JOINT_NAMES, range(12))) + direct = UnitreeGo2DirectInterface.from_profile( + _profile(), + network_interface="enp3s0", + joint_name_to_motor_index=mapping, + backend_options={ + "motor_mode": 42, + "wait_for_remote_buttons": False, + "kp": 35.0, + "kd": 0.5, + "default_transition_duration_s": 0.02, + "damping_duration_s": 0.0, + }, + sdk=bus.sdk(), + sleep=lambda _: None, + ) + direct.open() + np.testing.assert_array_equal(direct.read_data().joint_position, np.arange(12)) + direct.enable_command_output() + direct.send_joint_command(direct.robot.default_joint_position) + for index, motor in enumerate(bus.commands[-1].motor_cmd): + assert motor.mode == 42 + assert motor.q == pytest.approx(direct.robot.default_joint_position[index]) + direct.close() + + def test_runtime_gain_overrides_apply_to_transition_and_policy_commands() -> None: bus = _Bus() spec = _spec() @@ -435,8 +533,8 @@ def test_direct_interface_reads_and_sends_through_production_backend() -> None: spec = _spec() backend = _backend(_config(), bus) direct = UnitreeGo2DirectInterface( - robot=spec, - task_config={"kp": [35.0] * 12, "kd": [0.5] * 12}, + kp=[35.0] * 12, + kd=[0.5] * 12, control_period_s=0.02, state_timeout_s=0.1, backend=backend, @@ -456,6 +554,10 @@ def test_direct_interface_reads_and_sends_through_production_backend() -> None: published = bus.commands[-1] np.testing.assert_array_equal(command.joint_position, target) + np.testing.assert_array_equal(command.kp, np.full(12, 35.0, dtype=np.float32)) + np.testing.assert_array_equal(command.kd, np.full(12, 0.5, dtype=np.float32)) + command.kp[:] = 0.0 + np.testing.assert_array_equal(direct.default_pose_command().kp, np.full(12, 35.0, dtype=np.float32)) assert published.motor_cmd[MOTOR_MAPPING["FL_hip_joint"]].q == pytest.approx(target[0]) assert published.motor_cmd[MOTOR_MAPPING["RR_calf_joint"]].q == pytest.approx(target[-1]) @@ -463,32 +565,29 @@ def test_direct_interface_reads_and_sends_through_production_backend() -> None: assert direct.health().reason == "backend is closed" -def test_enable_waits_for_start_transitions_then_waits_for_a() -> None: +def test_torque_enable_obeys_start_and_a_gates_without_sending_rollout_torque() -> None: bus = _Bus() bus.remote_keys_after_writes = { 1: _button("start"), 2: _button("start"), 3: _button("start") | _button("A"), } - backend = _backend( - UnitreeGo2BackendConfig( - network_interface="enp3s0", - default_transition_duration_s=0.02, - damping_duration_s=0.0, - kp=35.0, - kd=0.5, - ), - bus, - ) + backend = _backend(_config(wait_for_remote_buttons=True, kp=35.0, kd=0.5, damping_duration_s=0.0), bus) backend.open() backend.read_state(0.1) - + torque = JointTorqueCommand(torque=np.full(12, 1.0, dtype=np.float32)) backend.enable() - assert len(bus.commands) == 3 + assert all(motor.tau == 0.0 for command in bus.commands for motor in command.motor_cmd) assert all(motor.kp == 0.0 and motor.kd == 0.0 for motor in bus.commands[0].motor_cmd) assert any(motor.kp == 35.0 for motor in bus.commands[1].motor_cmd) assert any(motor.kp == 35.0 for motor in bus.commands[2].motor_cmd) + backend.write_command(torque) + assert all(bus.commands[-1].motor_cmd[index].tau == 1.0 for index in MOTOR_MAPPING.values()) + bus.state.wireless_remote = _remote_payload(_button("select")) + bus.emit() + with pytest.raises(EmergencyStopError): + backend.write_command(torque) backend.close() @@ -568,16 +667,154 @@ def test_select_has_priority_over_simultaneous_lie_down_request() -> None: backend.close() -def test_lie_down_config_rejects_invalid_pose() -> None: +def test_trusted_recipe_instantiates_robot_config() -> None: + backend = create_runtime( + { + "name": "unitree_go2", + "network_interface": "enp3s0", + "robot": {"_target_": "motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg"}, + }, + _context(), + ) + assert isinstance(backend, HardwareRuntime) + assert backend.realtime is True + assert isinstance(backend.robot.config.robot, UnitreeGo2HardwareCfg) + + +@pytest.mark.parametrize("structured", [False, True]) +def test_recipe_parser_preserves_typed_custom_sensor_bindings(structured: bool) -> None: + sensors = HardwareSensorBindings( + base_orientation_xyzw=HardwareSensorBinding("custom.orientation", "xyzw"), + base_angular_velocity=HardwareSensorBinding("custom.angular_velocity"), + base_linear_acceleration=HardwareSensorBinding("custom.acceleration"), + ) + if structured: + sensor_recipe = OmegaConf.structured(sensors) + else: + # Plain YAML declares the canonical nested Hydra targets explicitly. + sensor_recipe = { + "_target_": "motrix_deploy_unitree.sensor.HardwareSensorBindings", + **{ + role: {"_target_": "motrix_deploy_unitree.sensor.HardwareSensorBinding", **binding} + for role, binding in asdict(sensors).items() + }, + } + recipe = OmegaConf.create( + { + "network_interface": "enp3s0", + "robot": { + "_target_": "motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg", + "sensors": sensor_recipe, + }, + } + ) + if not structured: + recipe = OmegaConf.to_container(recipe, resolve=True) + config = UnitreeGo2BackendConfig.from_mapping(recipe) + assert isinstance(config.robot, UnitreeGo2HardwareCfg) + assert isinstance(config.robot.sensors, HardwareSensorBindings) + assert config.robot.sensors == sensors + assert isinstance(config.robot.sensors.base_orientation_xyzw, HardwareSensorBinding) + bus = _Bus() + bus.state.custom = SimpleNamespace( + orientation=[0.0, 2.0, 0.0, 0.0], angular_velocity=[4.0, 5.0, 6.0], acceleration=[7.0, 8.0, 9.0] + ) + backend = _backend(config, bus) + backend.open() + state = backend.read_state(0.1) + np.testing.assert_array_equal(state.base_orientation_xyzw, [0.0, 1.0, 0.0, 0.0]) + np.testing.assert_array_equal(state.base_angular_velocity, [4.0, 5.0, 6.0]) + np.testing.assert_array_equal(state.base_linear_acceleration, [7.0, 8.0, 9.0]) + backend.close() + + +@pytest.mark.parametrize("role", ["base_orientation_xyzw", "base_angular_velocity", "base_linear_acceleration"]) +def test_missing_sensor_binding_aborts_before_dds_or_command_output(role: str) -> None: + robot = UnitreeGo2HardwareCfg() + robot.sensors = replace(robot.sensors, **{role: None}) + bus = _Bus() + backend = _backend(_config(robot=robot), bus) + with pytest.raises(ValidationError, match=f"runtime.robot.sensors.{role}"): + backend.open() + assert bus.factory_calls == [] + assert bus.service_calls == [] + assert bus.callback is None + assert bus.commands == [] + + +def test_missing_quaternion_order_aborts_before_dds() -> None: + robot = UnitreeGo2HardwareCfg() + robot.sensors = replace( + robot.sensors, + base_orientation_xyzw=HardwareSensorBinding("imu_state.quaternion"), + ) + bus = _Bus() + with pytest.raises(ValidationError, match="declared quaternion order"): + _backend(_config(robot=robot), bus).open() + assert bus.factory_calls == [] + assert bus.commands == [] + + +def test_unsupported_quaternion_order_is_rejected_by_binding() -> None: + with pytest.raises(ValueError, match="unsupported quaternion order"): + HardwareSensorBinding("imu_state.quaternion", "zwxy") + + +@pytest.mark.parametrize("field", ["imu_state.quaternion[0]", "imu_state._quaternion", "imu_state.quaternion()"]) +def test_sensor_sdk_field_rejects_indexing_private_attributes_and_expressions(field: str) -> None: + robot = UnitreeGo2HardwareCfg() + robot.sensors = replace(robot.sensors, base_orientation_xyzw=HardwareSensorBinding(field, "wxyz")) + bus = _Bus() + with pytest.raises(ValidationError, match="public dotted SDK attributes"): + _backend(_config(robot=robot), bus).open() + assert bus.factory_calls == [] + + +@pytest.mark.parametrize( + ("field", "value"), + [ + ("quaternion", [1.0, 0.0, 0.0]), + ("quaternion", [float("nan"), 0.0, 0.0, 0.0]), + ("gyroscope", [0.0, 0.0]), + ("accelerometer", [0.0, 0.0, float("inf")]), + ], +) +def test_invalid_sdk_sensor_shape_or_finiteness_aborts_open(field: str, value: list[float]) -> None: + bus = _Bus() + setattr(bus.state.imu_state, field, value) + with pytest.raises(RuntimeError, match="invalid Unitree LowState"): + _backend(_config(), bus).open() + assert bus.commands == [] + + +def test_zero_quaternion_is_rejected_before_enable_output() -> None: + bus = _Bus() + bus.state.imu_state.quaternion = [0.0] * 4 + backend = _backend(_config(), bus) + backend.open() + with pytest.raises(ValidationError, match="non-zero quaternion"): + backend.enable() + assert bus.commands == [] + + +def test_explicit_lie_down_override_remains_canonical_and_validated() -> None: + pose = np.linspace(-0.2, 0.2, 12, dtype=np.float32) + np.testing.assert_array_equal(_config(lie_down_joint_position=pose).lie_down_position(), pose) with pytest.raises(ValidationError, match="runtime.lie_down_joint_position"): _config(lie_down_joint_position=[0.0] * 11) -def test_go2_hardware_defaults_define_motor_mapping_and_lie_down_pose() -> None: +def test_go2_hardware_defaults_are_resolved_from_robot_config() -> None: config = UnitreeGo2BackendConfig(network_interface="enp3s0") + assert config.robot is not None - assert config.joint_name_to_motor_index == MOTOR_MAPPING - assert config.lie_down_position().shape == (12,) + np.testing.assert_array_equal( + config.motor_indices(JOINT_NAMES), + [config.robot.actuation.motors[name].index for name in JOINT_NAMES], + ) + np.testing.assert_array_equal( + config.lie_down_position(), np.asarray(config.robot.key_pose.poses["lie_down"], dtype=np.float32) + ) def test_motor_mapping_must_exactly_cover_artifact_joint_names() -> None: @@ -590,10 +827,7 @@ def test_motor_mapping_must_exactly_cover_artifact_joint_names() -> None: def _context() -> RuntimeCreateContext: - return RuntimeCreateContext( - robot=_spec(), - control=ControlSpec(period_s=0.02, state_timeout_s=0.1), - ) + return RuntimeCreateContext(robot=_spec(), control=ControlSpec(period_s=0.02, state_timeout_s=0.1)) def test_plugin_factory_creates_realtime_hardware_runtime_and_strict_config() -> None: @@ -602,45 +836,15 @@ def test_plugin_factory_creates_realtime_hardware_runtime_and_strict_config() -> "network_interface": "enp3s0", } - runtime = create_runtime(config, _context()) - assert isinstance(runtime, HardwareRuntime) - assert runtime.realtime is True - assert isinstance(runtime.robot, UnitreeGo2RobotInterface) - assert runtime.robot.spec.joint_names == JOINT_NAMES + backend = create_runtime(config, _context()) + assert isinstance(backend, HardwareRuntime) + assert backend.realtime is True + assert isinstance(backend.robot, UnitreeGo2RobotInterface) with pytest.raises(ValidationError, match="unknown"): UnitreeGo2BackendConfig.from_mapping({**config, "unexpected": 1}) -def test_plugin_runtime_uses_production_interface_with_injected_sdk(monkeypatch) -> None: - bus = _Bus() - monkeypatch.setattr("motrix_deploy_unitree.interface._load_sdk_bindings", bus.sdk) - runtime = create_runtime( - { - "network_interface": "enp3s0", - "kp": 35.0, - "kd": 0.5, - "default_transition_duration_s": 0.02, - "damping_duration_s": 0.0, - "wait_for_remote_buttons": False, - }, - _context(), - ) - robot = runtime.robot - try: - robot.open() - robot.read_state(0.1) - robot.enable() - robot.write_command(_command(robot.spec.default_joint_position)) - assert robot.health().healthy - assert bus.factory_calls == [(0, "enp3s0")] - assert bus.commands[-1].crc == 123 - finally: - robot.close() - runtime.close() - assert robot.health().reason == "backend is closed" - - @pytest.mark.parametrize( ("name", "value"), [ @@ -655,16 +859,20 @@ def test_backend_config_rejects_invalid_gain_overrides(name: str, value: object) _config(**{name: value}) -class _JointControlTaskSpec(TaskSpec): - task_name = "unitree_joint_control/v1" +class _JointGainTaskSpec(TaskSpec): + task_name: ClassVar[str] = "go2_walk/v1" kp: list[float] kd: list[float] +class _NoGainTaskSpec(TaskSpec): + task_name: ClassVar[str] = "test/v1" + + def _profile() -> DeploymentProfile: return DeploymentProfile( robot=_spec(), - task=_JointControlTaskSpec(kp=[35.0] * 12, kd=[0.5] * 12), + task=_JointGainTaskSpec(kp=[35.0] * 12, kd=[0.5] * 12), control=ControlSpec(period_s=0.02, state_timeout_s=0.1), observation_size=1, action_size=12, @@ -680,6 +888,8 @@ def test_direct_interface_builds_from_profile_without_artifact() -> None: "default_transition_duration_s": 0.02, "damping_duration_s": 0.0, "wait_for_remote_buttons": False, + "kp": 35.0, + "kd": 0.5, }, sdk=bus.sdk(), sleep=lambda _: None, @@ -699,20 +909,49 @@ def test_direct_interface_builds_from_profile_without_artifact() -> None: assert bus.commands -def test_joint_control_cli_dispatches_artifact_and_motion_defaults(monkeypatch) -> None: - calls = [] - monkeypatch.setattr("motrix_deploy_unitree.cli.run_joint_control", lambda **kwargs: calls.append(kwargs)) +def test_direct_artifact_factory_reads_typed_joint_gains(monkeypatch: pytest.MonkeyPatch) -> None: + profile = _profile() + monkeypatch.setattr("motrix_deploy_unitree.direct.read_artifact", lambda path: SimpleNamespace(manifest=profile)) + direct = UnitreeGo2DirectInterface.from_artifact( + "example.deploy", + network_interface="enp5s0", + sdk=_Bus().sdk(), + ) + np.testing.assert_array_equal(direct.default_pose_command().kp, profile.task.kp) + np.testing.assert_array_equal(direct.default_pose_command().kd, profile.task.kd) + np.testing.assert_array_equal(direct.backend.config.kp, profile.task.kp) + np.testing.assert_array_equal(direct.backend.config.kd, profile.task.kd) - assert main(["joint-control", "enp5s0", "FL_thigh_joint", "0.9", "--artifact", "example.deploy"]) == 0 - assert len(calls) == 1 - assert str(calls[0]["artifact"]) == "example.deploy" - assert calls[0]["network_interface"] == "enp5s0" - assert calls[0]["joint_name"] == "FL_thigh_joint" - assert calls[0]["target_position"] == pytest.approx(0.9) - assert calls[0]["move_duration"] == pytest.approx(2.0) - assert calls[0]["hold_duration"] == pytest.approx(1.0) - assert calls[0]["return_duration"] == pytest.approx(2.0) +def test_direct_api_supplied_gains_enable_without_backend_overrides() -> None: + bus = _Bus() + direct = UnitreeGo2DirectInterface.from_profile( + _profile(), + network_interface="enp5s0", + backend_options={ + "wait_for_remote_buttons": False, + "default_transition_duration_s": 0.02, + "damping_duration_s": 0.0, + }, + sdk=bus.sdk(), + sleep=lambda _: None, + ) + with direct: + direct.enable_command_output() + direct.send_command(JointTorqueCommand(torque=np.ones(12, dtype=np.float32))) + assert all(bus.commands[-1].motor_cmd[index].tau == 1.0 for index in MOTOR_MAPPING.values()) + + +def test_direct_profile_rejects_task_without_typed_joint_gains_before_dds() -> None: + bus = _Bus() + profile = replace(_profile(), task=_NoGainTaskSpec()) + with pytest.raises(ValidationError, match="typed task spec with kp and kd"): + UnitreeGo2DirectInterface.from_profile( + profile, + network_interface="enp5s0", + sdk=bus.sdk(), + ) + assert bus.factory_calls == [] def test_joint_control_requires_artifact() -> None: @@ -720,6 +959,61 @@ def test_joint_control_requires_artifact() -> None: _parser().parse_args(["joint-control", "enp5s0", "FL_thigh_joint", "0.9"]) +def test_joint_control_accepts_explicit_backend_safety_gains(monkeypatch) -> None: + from motrix_deploy_unitree import cli as unitree_cli + + calls = [] + monkeypatch.setattr(unitree_cli, "run_joint_control", lambda **kwargs: calls.append(kwargs)) + assert ( + unitree_cli.main( + [ + "joint-control", + "enp5s0", + "FL_thigh_joint", + "0.9", + "--artifact", + "example.deploy", + "--kp", + "42", + "--kd", + "0.8", + ] + ) + == 0 + ) + assert calls[0]["kp"] == 42.0 + assert calls[0]["kd"] == 0.8 + + +@pytest.mark.parametrize("flag", ["--kp", "--kd"]) +def test_joint_control_requires_each_backend_safety_gain(flag: str) -> None: + with pytest.raises(SystemExit): + _parser().parse_args( + [ + "joint-control", + "enp5s0", + "FL_thigh_joint", + "0.9", + "--artifact", + "example.deploy", + flag, + "1", + ] + ) + + +def test_joint_control_builds_backend_with_explicit_safety_gains(monkeypatch) -> None: + from motrix_deploy_unitree.go2_joint_control import _build_robot + + calls = [] + robot = object() + monkeypatch.setattr( + UnitreeGo2DirectInterface, "from_artifact", lambda *args, **kwargs: calls.append((args, kwargs)) or robot + ) + assert _build_robot("example.deploy", "enp5s0", kp=42.0, kd=0.8) is robot + assert calls[0][1]["backend_options"] == {"kp": 42.0, "kd": 0.8} + + def test_read_lowstate_uses_unified_cli_defaults() -> None: args = _parser().parse_args(["read-lowstate", "enp5s0"]) @@ -729,9 +1023,12 @@ def test_read_lowstate_uses_unified_cli_defaults() -> None: assert args.duration_s == pytest.approx(10.0) -def test_joint_control_rejects_environment_option() -> None: - with pytest.raises(SystemExit): - _parser().parse_args( +def test_joint_control_cli_dispatches_artifact_and_motion_defaults(monkeypatch) -> None: + calls = [] + monkeypatch.setattr("motrix_deploy_unitree.cli.run_joint_control", lambda **kwargs: calls.append(kwargs)) + + assert ( + main( [ "joint-control", "enp5s0", @@ -739,7 +1036,49 @@ def test_joint_control_rejects_environment_option() -> None: "0.9", "--artifact", "example.deploy", - "--env", - "go2-walk-rough", + "--kp", + "35", + "--kd", + "0.5", ] ) + == 0 + ) + + assert len(calls) == 1 + assert str(calls[0]["artifact"]) == "example.deploy" + assert calls[0]["network_interface"] == "enp5s0" + assert calls[0]["joint_name"] == "FL_thigh_joint" + assert calls[0]["target_position"] == pytest.approx(0.9) + assert calls[0]["move_duration"] == pytest.approx(2.0) + assert calls[0]["hold_duration"] == pytest.approx(1.0) + assert calls[0]["return_duration"] == pytest.approx(2.0) + + +def test_plugin_runtime_uses_production_interface_with_injected_sdk(monkeypatch) -> None: + bus = _Bus() + monkeypatch.setattr("motrix_deploy_unitree.interface._load_sdk_bindings", bus.sdk) + runtime = create_runtime( + { + "network_interface": "enp3s0", + "kp": 35.0, + "kd": 0.5, + "default_transition_duration_s": 0.02, + "damping_duration_s": 0.0, + "wait_for_remote_buttons": False, + }, + _context(), + ) + robot = runtime.robot + try: + robot.open() + robot.read_state(0.1) + robot.enable() + robot.write_command(_command(robot.spec.default_joint_position)) + assert robot.health().healthy + assert bus.factory_calls == [(0, "enp3s0")] + assert bus.commands[-1].crc == 123 + finally: + robot.close() + runtime.close() + assert robot.health().reason == "backend is closed" diff --git a/motrix_envs/src/motrix_envs/deploy/go2_walk.py b/motrix_envs/src/motrix_envs/deploy/go2_walk.py index 8af0d3db..b8b8f0b8 100644 --- a/motrix_envs/src/motrix_envs/deploy/go2_walk.py +++ b/motrix_envs/src/motrix_envs/deploy/go2_walk.py @@ -61,8 +61,8 @@ def build_go2_walk_profile(env_name: str) -> DeploymentProfile: feet_phase_offsets=feet_offsets.tolist(), gait_frequency_hz=env_cfg.gait_frequency, standing_threshold=env_cfg.commands.velocity.standing_threshold, - # Match QuadrupedWalkEnv.compute_transition: orientation-only falls. - termination_min_up_z=0.5, + # Match training's orientation-only fall termination. + termination_min_up_z=env_cfg.termination_min_up_z, termination_min_base_height=None, kp=kp.tolist(), kd=kd.tolist(), diff --git a/motrix_envs/src/motrix_envs/locomotion/quadruped/cfg.py b/motrix_envs/src/motrix_envs/locomotion/quadruped/cfg.py index aff1ee44..ec2e825e 100644 --- a/motrix_envs/src/motrix_envs/locomotion/quadruped/cfg.py +++ b/motrix_envs/src/motrix_envs/locomotion/quadruped/cfg.py @@ -180,9 +180,9 @@ class Commands: @configclass class Sensor: - # General-purpose sensors exposed by the assembled scene. - local_linvel: str = "local_linvel" + # Scene-declared names are used directly; model-local names follow robot affixes. gyro: str = "gyro" + local_linvel: str = "local_linvel" upvector: str = "upvector" foot_positions: tuple[str, str, str, str] = ("FL_pos", "FR_pos", "RL_pos", "RR_pos") @@ -229,6 +229,8 @@ class QuadrupedWalkEnvCfg(DirectEnvCfg): spawn_xy_range: float = 0.0 trot_pairs: tuple[tuple[int, int], ...] = ((0, 3), (1, 2)) gait_frequency: float = 2.0 + # Shared by training and the compiled deployment task, not backend recipes. + termination_min_up_z: float = 0.5 sim: SimCfg = SimCfg(dt=0.01, solver_iterations=1) ctrl_dt: float = 0.02 @@ -236,6 +238,8 @@ def validate(self) -> None: super().validate() self.randomization.validate() self.commands.velocity.validate() + if not np.isfinite(self.termination_min_up_z) or not -1.0 <= self.termination_min_up_z <= 1.0: + raise ValueError("termination_min_up_z must be finite and inside [-1, 1]") if ( self.randomization.enabled and any(self.randomization.action_delay_steps) diff --git a/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py b/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py index 31c4dcbf..c5a0c0eb 100644 --- a/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py +++ b/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py @@ -44,24 +44,30 @@ def _sim_data_queries(cfg: QuadrupedWalkEnvCfg): - base_link_name = cfg.scene.objs.robot.resolved_base_link_name + robot = cfg.scene.objs.robot + base_link_name = robot.resolved_base_link_name + scene_sensor_names = {name for name, _ in cfg.scene.iter_sensors()} + + def resolve_sensor_name(name: str) -> str: + return name if name in scene_sensor_names else robot.resolve_name(name) + return { "joint_dof_pos": BodyJointPositionQuery(body=base_link_name), "joint_dof_vel": BodyJointVelocityQuery(body=base_link_name), "dof_vel": DofVelocityQuery(), "actuator_ctrls": ActuatorCtrlQuery(), - "local_linvel": SensorValuesQuery(sensors=(cfg.sensor.local_linvel,)), - "gyro": SensorValuesQuery(sensors=(cfg.sensor.gyro,)), - "upvector": SensorValuesQuery(sensors=(cfg.sensor.upvector,)), + "local_linvel": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.local_linvel),)), + "gyro": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.gyro),)), + "upvector": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.upvector),)), "base_pos": LinkPositionQuery(link=base_link_name), "front_left_contact": SensorValuesQuery(sensors=("front_left_contact",)), "front_right_contact": SensorValuesQuery(sensors=("front_right_contact",)), "rear_left_contact": SensorValuesQuery(sensors=("rear_left_contact",)), "rear_right_contact": SensorValuesQuery(sensors=("rear_right_contact",)), - "FL_pos": SensorValuesQuery(sensors=(cfg.sensor.foot_positions[0],)), - "FR_pos": SensorValuesQuery(sensors=(cfg.sensor.foot_positions[1],)), - "RL_pos": SensorValuesQuery(sensors=(cfg.sensor.foot_positions[2],)), - "RR_pos": SensorValuesQuery(sensors=(cfg.sensor.foot_positions[3],)), + "FL_pos": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.foot_positions[0]),)), + "FR_pos": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.foot_positions[1]),)), + "RL_pos": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.foot_positions[2]),)), + "RR_pos": SensorValuesQuery(sensors=(resolve_sensor_name(cfg.sensor.foot_positions[3]),)), } @@ -378,7 +384,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: self._advance_phase() self._update_feet_buffers() - terminated = self.get_gravity()[:, 2] <= 0.5 + terminated = self.get_gravity()[:, 2] <= self.cfg.termination_min_up_z reward = self._compute_reward(state, self.get_local_linvel(), self.get_gyro(), self.get_dof_pos()) return state.replace(reward=reward, terminated=terminated) diff --git a/motrix_envs/tests/test_quadruped_walk.py b/motrix_envs/tests/test_quadruped_walk.py index fd01b9e8..3daa0579 100644 --- a/motrix_envs/tests/test_quadruped_walk.py +++ b/motrix_envs/tests/test_quadruped_walk.py @@ -6,11 +6,22 @@ import motrix_envs # noqa: F401 registers built-in environments from motrix_env_core import registry -from motrix_env_core.config.scene import HFieldTerrainCfg, ProceduralHFieldAssetCfg +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import ( + HFieldTerrainCfg, + KeyPoseCfg, + MjcfFileCfg, + ProceduralHFieldAssetCfg, + SceneCfg, + SceneObjsCfg, +) +from motrix_env_core.config.scene.base import RobotCfg +from motrix_env_core.sim import BodyAngularVelocityWrite from motrix_env_core.sim.model import ActuatorType +from motrix_env_motrixsim.runtime import MotrixSimBackend from motrix_envs.locomotion.quadruped.cfg import RewardScales from motrix_envs.locomotion.quadruped.velocity_command import RandomPlanarVelocityBinding -from motrix_envs.locomotion.quadruped.walk_np import QuadrupedWalkTask +from motrix_envs.locomotion.quadruped.walk_np import QuadrupedWalkTask, _sim_data_queries def _read_param_overrides(env): @@ -395,6 +406,70 @@ def test_foot_position_sensor_names_belong_to_walk_task_config(): assert anymalc.sensor.foot_positions == go1.sensor.foot_positions +@pytest.mark.parametrize("robot_name", ["go1", "go2", "anymalc"]) +def test_walk_task_gyro_config_compiles_for_each_robot(robot_name): + cfg = registry.make_env_config(f"{robot_name}-walk-flat") + rough_cfg = registry.make_env_config(f"{robot_name}-walk-rough") + # Terrain variants use the same model-local gyro wiring; compile each robot only once. + assert _sim_data_queries(rough_cfg)["gyro"] == _sim_data_queries(cfg)["gyro"] + robot = cfg.scene.objs.robot + backend = MotrixSimBackend(cfg.scene, cfg.sim, 1) + reads = backend.compile_reads({"gyro": _sim_data_queries(cfg)["gyro"]}) + reset = backend.write_compiler.compile( + {"angular_velocity": BodyAngularVelocityWrite((robot.resolved_base_link_name,))}, reset=True + ) + velocity = np.asarray([0.4, -0.5, 0.6], dtype=np.float32) + reset.buffer("angular_velocity")[0, 0] = velocity + reset.execute(np.asarray([0], dtype=np.int64)) + reads.execute() + np.testing.assert_allclose(reads["gyro"][0], velocity, atol=1e-6) + + +def test_walk_task_gyro_config_is_instance_local(): + first = registry.make_env_config("anymalc-walk-flat") + second = registry.make_env_config("anymalc-walk-flat") + unitree = registry.make_env_config("go2-walk-flat") + second_sensor = second.sensor.gyro + unitree_sensor = unitree.sensor.gyro + first.sensor.gyro = "custom_gyro" + assert second.sensor.gyro == second_sensor + assert unitree.sensor.gyro == unitree_sensor + assert _sim_data_queries(first)["gyro"].sensors == (first.scene.objs.robot.resolve_name("custom_gyro"),) + + +@pytest.mark.parametrize(("prefix", "suffix"), [("robot_", None), (None, "_instance"), ("robot_", "_instance")]) +def test_walk_gyro_query_reads_renamed_model_sensor_with_scene_affixes(tmp_path, prefix, suffix): + # A real imported gyro with a custom name catches accidental hard-coded sensor lookup. + model_path = tmp_path / "robot.xml" + model_path.write_text( + '' + '' + '' + ) + robot = RobotCfg( + model=MjcfFileCfg(file=model_path), base_link_name="base", prefix=prefix, suffix=suffix, key_pose=KeyPoseCfg() + ) + + @configclass + class RobotObjsCfg(SceneObjsCfg): + robot: RobotCfg + + cfg = registry.make_env_config("go2-walk-flat") + cfg.scene = SceneCfg(objs=RobotObjsCfg(robot=robot)) + cfg.sensor.gyro = "custom_gyro" + queries = _sim_data_queries(cfg) + backend = MotrixSimBackend(cfg.scene, cfg.sim, 2) + reads = backend.compile_reads({"gyro": queries["gyro"]}) + reset = backend.write_compiler.compile( + {"angular_velocity": BodyAngularVelocityWrite((robot.resolved_base_link_name,))}, reset=True + ) + velocity = np.asarray([[0.4, -0.5, 0.6], [-0.7, 0.8, -0.9]], dtype=np.float32) + reset.buffer("angular_velocity")[:, 0] = velocity + reset.execute(np.arange(2, dtype=np.int64)) + reads.execute() + np.testing.assert_allclose(reads["gyro"], velocity, atol=1e-6) + + def test_walk_task_selects_named_robot_key_pose(): cfg = registry.make_env_config("go1-walk-flat") joint_count = len(cfg.scene.objs.robot.key_pose.joint_names) @@ -447,3 +522,33 @@ def test_swing_contact_penalty_detects_dragging_feet(quadruped_env): np.testing.assert_array_equal(no_drag, np.zeros((1,), dtype=np.float32)) np.testing.assert_array_equal(all_drag, np.ones((1,), dtype=np.float32)) + + +@pytest.mark.parametrize("robot_name", ["go1", "go2", "anymalc"]) +def test_walk_state_sensor_queries_resolve_model_and_scene_names(robot_name): + cfg = registry.make_env_config(f"{robot_name}-walk-flat") + robot = cfg.scene.objs.robot + robot.prefix = "robot_" + robot.suffix = "_instance" + # Scene sensor targets are already assembled names; update their references + # when changing instance affixes after scene construction. + for _, sensor in cfg.scene.iter_sensors(): + if hasattr(sensor, "geom2"): + sensor.geom2 = robot.resolve_name(sensor.geom2) + else: + sensor.object_name = robot.resolve_name(sensor.object_name) + if sensor.ref_object_name is not None: + sensor.ref_object_name = robot.resolve_name(sensor.ref_object_name) + queries = _sim_data_queries(cfg) + names = ("gyro", "local_linvel", "upvector", "FL_pos", "FR_pos", "RL_pos", "RR_pos") + scene_names = {name for name, _ in cfg.scene.iter_sensors()} + for name in names: + sensor_name = cfg.sensor.gyro if name == "gyro" else name + expected = sensor_name if sensor_name in scene_names else robot.resolve_name(sensor_name) + assert queries[name].sensors == (expected,) + backend = MotrixSimBackend(cfg.scene, cfg.sim, 1) + reads = backend.compile_reads({name: queries[name] for name in names}) + reads.execute() + for name in names: + assert reads[name].shape == (1, 3) + assert np.isfinite(reads[name]).all() diff --git a/motrix_robots/src/motrix_robots/assets/go1/go1.xml b/motrix_robots/src/motrix_robots/assets/go1/go1.xml index 7bb04c4b..f009832c 100644 --- a/motrix_robots/src/motrix_robots/assets/go1/go1.xml +++ b/motrix_robots/src/motrix_robots/assets/go1/go1.xml @@ -190,6 +190,7 @@ + diff --git a/motrix_robots/src/motrix_robots/unitree.py b/motrix_robots/src/motrix_robots/unitree.py index f095bf11..f944c9a2 100644 --- a/motrix_robots/src/motrix_robots/unitree.py +++ b/motrix_robots/src/motrix_robots/unitree.py @@ -166,5 +166,21 @@ class UnitreeGo2Robot(QuadrupedRobotCfg): "RR_thigh_joint", "RR_calf_joint", ], - poses={"default": [0.0, 0.8, -1.5, 0.0, 0.8, -1.5, 0.0, 1.0, -1.5, 0.0, 1.0, -1.5]}, + poses={ + "default": [0.0, 0.8, -1.5, 0.0, 0.8, -1.5, 0.0, 1.0, -1.5, 0.0, 1.0, -1.5], + "lie_down": [ + 0.05175, + 1.238835, + -2.74427, + -0.0608, + 1.24118, + -2.7375, + 0.31617, + 1.26637, + -2.79547, + -0.310495, + 1.266385, + -2.80177, + ], + }, ) diff --git a/uv.lock b/uv.lock index be301684..bfd2c129 100644 --- a/uv.lock +++ b/uv.lock @@ -996,14 +996,18 @@ name = "motrix-deploy-unitree" version = "0.4.0b0" source = { editable = "motrix_deploy_unitree" } dependencies = [ + { name = "hydra-core" }, { name = "motrix-deploy" }, + { name = "motrix-env-core" }, { name = "numpy" }, { name = "unitree-sdk2py" }, ] [package.metadata] requires-dist = [ + { name = "hydra-core", specifier = ">=1.3,<1.4" }, { name = "motrix-deploy", editable = "motrix_deploy" }, + { name = "motrix-env-core", editable = "motrix_env_core" }, { name = "numpy", specifier = ">=1.26" }, { name = "unitree-sdk2py", git = "https://github.com/motphys-developers/unitree_sdk2_python?rev=ff87e731174f7258e0d09980897c55116634939a" }, ] diff --git a/wiki/design/deploy-command-input.md b/wiki/design/deploy-command-input.md index cd238502..2baa60cc 100644 --- a/wiki/design/deploy-command-input.md +++ b/wiki/design/deploy-command-input.md @@ -342,15 +342,19 @@ class PolicyContext(Generic[CommandT]): `input_error` 结束 rollout；`KeyboardInterrupt` 映射为 `interrupted`。更细的 timeout、disconnect、stale input 错误等真实设备需要区分时再加入。 -realtime/fixed-step scheduler 由 runtime recipe 显式选择，不从 device capabilities 推断。 +仿真 realtime/fixed-step scheduler 由 `runtime.realtime` 选择，`null` 跟随 `runtime.viewer`；硬件内部始终实时调度，不从 device capabilities 推断。 ## 7. 配置与创建 -deploy application factory 从 backend 的可选 `KeyboardDeviceProvider` 取得 GLFW keyboard device 并创建 binding， -不建立 input registry，也不暴露 input type、scale 或 constant command 配置。Go2 task 从 artifact 读取 +`KeyboardDeviceProvider` 与 `GamePadDeviceProvider` 定义在 `motrix_deploy.robot.interface`，是可选输入设备 +capability protocol，不是 runtime 插件 factory；返回设备的生命周期由 provider 管理。 +deploy 应用在 runtime factory 返回 prepared host 后，从 runtime 的可选 `KeyboardDeviceProvider` 取得 GLFW keyboard device 并创建 binding， +再显式构造 `ControlSession(robot=runtime.robot, ...)`，通过 `runtime.bind_control_session(control)` 绑定； +`with runtime` 打开资源后调用 `runtime.run(steps=...)`。Runtime factory 不承担控制器构造。应用 +不建立 input registry；headless CLI 使用 `command.velocity` 配置 constant command。Go2 task 从 artifact 读取 `command_lower`、`command_upper` 和与 `[vx, vy, yaw_rate]` 对齐的三维 `command_scale`，向 factory 暴露逐元素 `range * scale` 后的映射端点；scale 属于 artifact 的 task contract，不在 runtime recipe 重复配置。内置交互式 -keyboard path 要求 backend 提供具有焦点的 viewer window；headless 调用须由程序传入其他 binding。 +keyboard path 要求 backend 提供具有焦点的 viewer window；headless CLI 使用 constant binding，程序化调用也可传入其他 binding。 training command sampling 使用： @@ -362,8 +366,8 @@ command_binding: standing_probability: 0.1 ``` -deploy application factory 只创建 core 的 keyboard binding；GLFW device 和 window 的生命周期由 MuJoCo backend -持有，binding 不负责关闭 device。当前 keyboard 是唯一的 deployment device，不另设 device type。 +deploy application 使用 core 的 keyboard、constant 或 bounded gamepad binding；viewer device 和 window 的生命周期由仿真 runtime +持有，Unitree gamepad 由硬件端口提供，binding 不负责关闭 device。 training task 直接创建自己的 `RandomPlanarVelocityBinding`，RNG 由训练入口按现有 seed 机制注入。等第二个外部 package 确实需要独立注册 device/binding 时，再引入 entry point 或 registry。 @@ -381,7 +385,8 @@ motrix_deploy/src/motrix_deploy/ └── runtime/ # PolicyContext；调用 CommandBinding.read_command() motrix_deploy_tasks/src/motrix_deploy_tasks/ -└── go2_walk.py # 直接消费 PlanarVelocityCommand +└── tasks/ + └── go2_walk.py # Go2WalkTaskSpec 与 task 同置；直接消费 PlanarVelocityCommand motrix_envs/src/motrix_envs/locomotion/quadruped/ └── velocity_command.py # task-specific RandomPlanarVelocityBinding、standing sampling @@ -395,7 +400,10 @@ motrix_env_core <- motrix_envs ``` `motrix_env_core.input` 不 import deploy runtime 或具体 task。`motrix_deploy` 复用 input contract； -`motrix_deploy_tasks.go2_walk` 直接消费公共 command，不重复实现 device 或 core binding。`motrix_envs` 复用公共 +`motrix_deploy_tasks.tasks.go2_walk` 中的 task 直接消费公共 command，不重复实现 device 或 core binding。 +其 `Go2WalkTaskSpec` 持有 typed command range/scale，由 task 类的 `spec_type` 关联；manifest 保持 +`{name, config}`，具体 spec 解码与 task class discovery 的边界见 +[Manifest 内容](./motrix-deploy.md#82-manifest-内容)。`motrix_envs` 复用公共 contract，并在四足训练模块中实现带训练分布语义的 random binding。 ## 9. 第一版测试边界 @@ -449,7 +457,7 @@ contract，并在四足训练模块中实现带训练分布语义的 random bind - device/binding plugin registry； - 公共 command ABC、独立 command schema 与 schema version； - 通用 declarative mapping DSL； -- ROS 2、vendor remote 和多设备组合。 +- ROS 2 和多设备组合；Unitree remote 已通过 gamepad provider 接入。 新增这些能力前必须先有当前最小接口无法表达的真实 consumer，并保持现有 `poll()`、边沿/持续状态查询与 `read_command(batch_size=...)` 语义可兼容演进。 diff --git a/wiki/design/index.md b/wiki/design/index.md index 33cb8fb5..aebd762b 100644 --- a/wiki/design/index.md +++ b/wiki/design/index.md @@ -14,7 +14,7 @@ - [ONNX 模型导出设计](./onnx-export.md) 基于 `RlFramework`/`AgentProvider` capability 的统一 ONNX policy exporter：从 metadata-backed run 恢复不同训练后端的 deterministic policy，烘焙 observation normalization，并执行 ONNX Runtime parity validation。 - [Motrix Deploy 框架设计](./motrix-deploy.md) - 独立策略部署包 `motrix_deploy` 的统一 ONNX export、deployment artifact、RobotState/RobotCommand/RobotInterface 契约、inference control loop、组件扩展边界与 MuJoCo sim2sim vertical slice。 + 独立策略部署包 `motrix_deploy` 的统一 ONNX export、deployment artifact、RobotState/RobotCommand/RobotInterface 契约、inference control loop、组件扩展边界、显式 runtime/control-session 组装，以及 MuJoCo 与原生 MotrixSim 仿真部署。 - [Deploy Runtime Command Input 分层设计](./deploy-command-input.md) 面向 Go2 的最小 command input 抽象：batch-first `PlanarVelocityCommand` 统一表达 training 与 deploy；core 内置 keyboard/gamepad/constant bindings，带 standing probability 的 `RandomPlanarVelocityBinding` 作为 task-specific 训练策略留在四足环境模块。 - [RL 多算法架构设计](./rl-multi-algorithm-architecture.md) diff --git a/wiki/design/motrix-deploy.md b/wiki/design/motrix-deploy.md index bf368498..eb2dcc83 100644 --- a/wiki/design/motrix-deploy.md +++ b/wiki/design/motrix-deploy.md @@ -11,7 +11,7 @@ sim2real vertical slice。`motrix_deploy` 是独立于训练框架的策略部 [Deploy Runtime Command Input 分层设计](./deploy-command-input.md) 定义。sim2sim 与 sim2real 只替换 `RobotInterface`，不得分别维护 policy tensor 拼接、command mapping 或 action 后处理逻辑。 -当前实现包含公共框架、MuJoCo plugin 和 Unitree SDK2 DDS plugin；Unitree 链路已通过注入式假 SDK 验证， +当前实现包含公共框架、MuJoCo 与原生 MotrixSim plugin，以及 Unitree SDK2 DDS plugin；Unitree 链路已通过注入式假 SDK 验证， 吊架真机 smoke test 仍待执行。当前 `motrix-deploy/v1` 固定 Go2 locomotion vertical slice 所需的单输入、单输出和 单关节控制组契约；面向多型号机器人与多任务的 general manifest 演进方向见 8.4 节。 @@ -65,8 +65,8 @@ checkpoint，但职责不同：play 验证训练实现，deploy 验证可移植 目录结构或 tensor 语义。 4. sim2sim 与 sim2real 使用同一个控制循环、artifact 和 `DeployTask` 实现。 5. backend 只负责设备生命周期以及 `RobotState` / `RobotCommand` 与底层 SDK 或仿真器之间的转换。 -6. 所有静态不匹配在控制循环开始前 fail fast；运行期超时、非有限数值和 backend 故障产生明确的停止原因。 -7. backend 和 policy runtime 保持通用；`motrix_deploy` 提供 task registry，具体任务包在 import 时注册，不修改主循环。 +6. artifact、模型元数据与 backend 配置的静态不匹配在对应边界 fail fast；实际 observation/action 数组在 policy/task 边界校验，运行期超时、非有限数值和 backend 故障产生明确的停止原因。 +7. backend 和 policy runtime 保持通用；`motrix_deploy` 通过 `motrix_deploy.tasks` entry-point group 发现具体任务插件，按需加载，不修改主循环。 8. MuJoCo sim2sim 支持确定性初始化、无界面运行、基础指标和可判定的进程退出状态。 9. deploy 固定使用 keyboard binding，训练 task 使用自己的 random binding；二者都产生公共 `PlanarVelocityCommand`，Go2 task 不感知 command 的来源。 @@ -99,7 +99,10 @@ motrix_deploy <- motrix_envs deploy integration `motrix_envs` 或 `motrix_rl`。`motrix_deploy_tasks` 依赖 `motrix_deploy`，Go2 task 直接消费公共 `PlanarVelocityCommand`。训练侧 integration 可以同时导入 `RobotCfg` 和 `motrix_deploy.artifact`，将训练时的机器人配置解析为自包含 `RobotSpec`；部署运行时只读取 -artifact，不再查询当前 robot registry。`motrix_deploy_tasks` 直接读取 artifact、选择具体 task 并装配 runtime。 +artifact 中的 RobotSpec，而非重新推导控制契约。仿真应用单独通过现有 robot registry 选择模型并组装场景。 +`motrix_deploy.cli` 读取 artifact、通过 task 插件发现选择具体实现并装配 runtime； +`motrix_deploy_tasks` 提供具体 task 类与已安装 Hydra recipe/search-path plugin，不反向承担 core CLI bootstrap。 +Hydra 配置搜索发现与版本化 Python task 实现发现互相独立。 deployment export integration 应调用 `motrix_deploy.artifact` 的写入 API；policy checkpoint 只由 `motrix_rl.deploy` 的 framework-owned adapter 解析，不能交给部署包或 profile compiler 处理。 @@ -119,8 +122,14 @@ motrix_rl framework adapter -> motrix_rl.deploy -> motrix_deploy.artifact 只按稳定的 task/profile/schema 选择运行时语义，不按 `rllib`、train backend 或 algorithm 再建立一套 exporter registry。manifest 中的 framework 来源只用于追踪，不能成为部署 runtime 或 profile 选择的分派键。 -未来若把 `RobotCfg`、内置机器人配置和公共机器人资产抽成独立 package，`motrix_deploy` 可以再选择依赖该中立 -package 或增加可选的一致性校验；这不是第一阶段的前置条件。 +`RobotCfg` 与 `KeyPoseCfg` 定义在 core；`motrix_robots` 独立提供仿真模型配置、模型自有关键姿态及完整 +MJCF/URDF、mesh 和纹理资产，仅依赖 core。Unitree G1、Go1、Go2 与其他内置模型均在所属模型类中内联声明 +`KeyPoseCfg`，不使用跨仿真与硬件的共享姿态 factory。 +`motrix_deploy_unitree.hardware` 定义 `HardwareRobotCfg` 与直接声明 Go2 自有 `KeyPoseCfg` 的 +`UnitreeGo2HardwareCfg`；`.sensor` 定义 `@configclass` 的 `HardwareSensorBindings` 与 frozen dataclass +`HardwareSensorBinding`，`.actuation` 定义 `Actuation` 与 `UnitreeMotorBinding`。硬件配置导入独立于模型和资产， +不会提前加载 simulator 或 Unitree SDK。模型与硬件默认姿态不要求相同；artifact 的关节顺序、默认姿态和限幅仍由 +policy 契约拥有，硬件接线和 shutdown pose 不替代该契约。 ### 4.2 关节顺序只有一个权威来源 @@ -179,22 +188,33 @@ motrix_deploy/ │ ├── artifact/ # schema、reader/writer、checksum、静态校验 │ ├── contracts.py # RobotSpec、RobotState、RobotCommand、capability │ ├── profile.py # DeploymentProfile、compiler registry 和统一 build 入口 -│ ├── task.py # DeployTask 接口、registry 和 factory lookup +│ ├── task.py # DeployTask 接口与版本化 task entry-point discovery +│ ├── env.py # robot-free 世界 discovery 与 assemble_deploy_scene(env ID, robot ID) │ ├── policy/ # PolicyRuntime 与 runtime registry │ ├── runtime/ # PolicyContext、lifecycle、control loop、scheduler、result/metrics -│ ├── backend/ # RobotInterface、entry-point backend discovery -│ └── cli.py # artifact 检查、task 选择和 runtime 装配 +│ │ ├── base.py # DeploymentRuntime / SimulationRuntime、bind_control_session +│ │ ├── control.py # ControlSession：共享 start/tick/stop +│ │ ├── hardware.py # HardwareRuntime：墙钟 scheduler +│ │ └── factory.py # motrix_deploy.backends discovery；返回 prepared runtime +│ ├── robot/ # robot I/O 与可选 input-device capability；精简 initializer +│ │ ├── interface.py # RobotInterface、KeyboardDeviceProvider、GamePadDeviceProvider +│ ├── config/ # deploy.yaml：task: null（通用帮助）；inspect.yaml：artifact 检查 +│ └── cli.py # 单一 Hydra 部署入口、inspect 子命令与 runtime 装配 └── tests/ motrix_deploy_mujoco/ ├── src/motrix_deploy_mujoco/ -│ ├── interface.py # MuJoCo RobotInterface lifecycle +│ ├── runtime.py # MujocoRuntime：世界资源、模型编译与 physics loop +│ ├── interface.py # MuJoCo RobotInterface：读取状态、缓存控制目标 │ ├── transform.py # position actuator -> torque motor MjSpec transform │ └── plugin.py # motrix_deploy.backends entry point └── tests/ motrix_deploy_unitree/ ├── src/motrix_deploy_unitree/ +│ ├── hardware.py # HardwareRobotCfg / UnitreeGo2HardwareCfg；硬件自有 KeyPoseCfg +│ ├── sensor.py # HardwareSensorBindings configclass / frozen HardwareSensorBinding +│ ├── actuation.py # Actuation / UnitreeMotorBinding │ ├── interface.py # Unitree SDK2 LowState/LowCmd DDS adapter │ ├── remote.py # 遥控器按键与摇杆解码 │ └── plugin.py # unitree_go2 backend entry point @@ -202,8 +222,13 @@ motrix_deploy_unitree/ motrix_deploy_tasks/ ├── src/motrix_deploy_tasks/ -│ ├── go2_walk.py # go2_walk/v1 的 observation/action 实现 -│ └── __init__.py # 注册具体实现并暴露 core CLI bootstrap +│ ├── tasks/ +│ │ └── go2_walk.py # Go2WalkTaskSpec 与 Go2WalkDeployTaskV1 同置 +│ ├── envs/ # flat.py / rough.py：独立、robot-free 部署世界 +│ ├── config/task/ # go2-walk-flat/{sim,hardware}.yaml、go2-walk-rough/sim.yaml +│ └── __init__.py # 精简 package initializer；task 类由 entry point 声明 +├── src/hydra_plugins/ +│ └── motrix_deploy_tasks_searchpath.py # 将 pkg://motrix_deploy_tasks.config 加入 Hydra 搜索路径 └── tests/ motrix_envs/src/motrix_envs/deploy/ @@ -213,7 +238,7 @@ motrix_envs/src/motrix_envs/deploy/ 依赖按能力分组： -- core：`motrix_env_core`、`numpy` 和 artifact 解析所需的标准库； +- core：`motrix_env_core`、`numpy`、typed task spec 校验与 codec 所需的 Pydantic、应用 YAML 加载/合并/解析所需的 OmegaConf，以及 artifact 解析所需的标准库； - `onnx` extra：`onnxruntime`，只在创建 ONNX policy runtime 时导入； - `motrix-deploy-mujoco`：独立安装的 backend plugin，依赖 SceneCfg compiler 与 MuJoCo Python package； - `motrix-deploy-tasks`：直接依赖并复用 `motrix_env_core.input`，Go2 不实现专用 binding； @@ -223,7 +248,7 @@ motrix_envs/src/motrix_envs/deploy/ ## 6. 公共运行时契约 -公共数据对象使用普通 `@dataclass`，不使用面向声明式环境配置的 `@configclass`。 +运行时数据对象使用普通 `@dataclass`；typed task 配置使用 Pydantic `TaskSpec`，不使用面向声明式环境配置的 `@configclass`。 ### 6.1 RobotSpec @@ -305,13 +330,26 @@ backend 可以把该命令传给原生 hybrid controller，也可以在本地计 ### 6.5 RobotInterface 与 capability +`motrix_deploy.robot.interface` 定义机器人 I/O 边界及可选输入设备 capability；它不是 runtime 插件 discovery 模块。 +`RobotInterface` 绑定 `RobotSpec`，负责 state read、command write、health 与机器人生命周期。 +`KeyboardDeviceProvider` 和 `GamePadDeviceProvider` 是可选 protocol，提供由 provider 管理生命周期的输入设备， +不承担 runtime factory 职责。测试目录中的 `FakeRobotInterface` 提供确定性内存实现，不作为运行时 adapter 发布。 +`motrix_deploy.robot` 的 initializer 保持精简，使用方从定义模块导入： + +```python +from motrix_deploy.robot.interface import GamePadDeviceProvider, KeyboardDeviceProvider, RobotInterface +``` + ```python class RobotInterface(ABC): @property def capabilities(self) -> RobotCapabilities: ... - def open(self, spec: RobotSpec) -> None: ... - def enable(self, initial_command: RobotCommand) -> None: ... + @property + def spec(self) -> RobotSpec: ... + + def open(self) -> None: ... + def enable(self) -> None: ... def read_state(self, timeout_s: float) -> RobotState: ... def write_command(self, command: RobotCommand) -> None: ... def health(self) -> HealthStatus: ... @@ -328,6 +366,10 @@ class RobotInterface(ABC): - 是否需要显式 enable，以及 stop 的安全语义。 仿真 backend 在 `open()` 中完成自身的确定性初始化；真机 backend 建立通信后由 `read_state()` 返回当前状态。 +`enable()` 不接收 initial command；Unitree backend 自行构造到 `RobotSpec.default_joint_position` 的姿态过渡， +目标速度与 feedforward torque 为零，使用解析后的 `runtime.kp` / `runtime.kd`，`null` 时保留 artifact 增益。 +不通过 task action 推导启动命令。`ControlSession.start()` 不调用 task `process_action()` 生成零动作启动命令； +初始状态的 safety/termination 检查仍先于 enable，Start/A 门控与 enable 后新状态检查保持有效。 `stop()` 和 `close()` 必须幂等，任何启动后异常都按 `stop -> close` 收尾。`HealthStatus` 包含状态、原因和最后 成功通信时间，不能只返回无语义的 bool。 @@ -335,13 +377,18 @@ class RobotInterface(ABC): ```python class DeployTask(ABC, Generic[CommandT]): + spec_type: ClassVar[type[TaskSpec]] + def reset(self, state: RobotState, context: PolicyContext[CommandT]) -> None: ... def build_observation(self, state: RobotState, context: PolicyContext[CommandT]) -> np.ndarray: ... def process_action(self, action: np.ndarray) -> RobotCommand: ... def validate_command(self, command: CommandT) -> None: ... ``` -artifact 只记录稳定的 task type、version、policy tensor size 和该次训练可变化的 config。具体 observation 顺序、 +artifact 的 task wire format 只记录版本化 task 标识和 config；内存中的 `TaskSpec` 是具体任务的 Pydantic model， +其 codec 与 class discovery 见 [Manifest 内容](#82-manifest-内容)。observation/action 维度只记录在 `policy.input/output.shape`。 +`DeployTask` 不声明 `observation_size` 或 `action_size`；实际 observation 数组由 policy 输入边界校验， +实际 action 数组由 task 的 `process_action()` 边界校验，CLI 不提前比较 task 维度。具体 observation 顺序、 previous action、phase 与 action 后处理算法由 `motrix_deploy_tasks` 中一个带版本的 task 类直接实现，不在 manifest 中构造通用计算 DSL。修改已有语义必须新增 task version，不能静默改变旧实现。部署时关闭训练 observation noise； 训练框架的 observation normalizer 必须烘焙进 ONNX。 @@ -350,10 +397,6 @@ previous action、phase 与 action 后处理算法由 `motrix_deploy_tasks` 中 ```python class PolicyRuntime(ABC): - @property - def input_spec(self) -> TensorSpec: ... - @property - def output_spec(self) -> TensorSpec: ... def reset(self) -> None: ... def infer(self, observation: np.ndarray) -> np.ndarray: ... ``` @@ -465,11 +508,10 @@ manifest 至少包含以下部分。下面是结构节选，为便于阅读省 }, "task": { "name": "go2_walk/v1", - "observation_size": 49, - "action_size": 12, "config": { - "action_scale": 0.25, - "raw_clip": [], + "action_scale": [], + "action_lower": [], + "action_upper": [], "kp": [], "kd": [], "gait_frequency_hz": 2.0, @@ -492,8 +534,25 @@ manifest 至少包含以下部分。下面是结构节选，为便于阅读省 示例中的长数组被省略，只表达 schema 形状；真实 manifest 不允许使用空数组代替必要值。task config 只保存 该次训练可能变化且部署必须复现的数值；observation/action 算法固定在对应版本化 task 标识的直接实现中。 -`schema_version` 管理 manifest 结构；版本化 task 标识管理具体任务语义。未知标识一律拒绝加载，控制循环 -不直接读取原始 dict。 +`schema_version` 管理 manifest 结构；版本化 task 标识管理具体任务语义。wire format 保持 +`task: {name, config}`，内存中则使用具体 typed spec，而非 dict config： + +- `motrix_deploy.artifact.schema.TaskSpec` 基于 Pydantic `BaseModel`，使用严格类型、`extra="forbid"`、 + frozen 字段赋值和有限数值约束；提供公共 `to_dict()` / `from_dict()` codec。具体 spec 声明 + `task_name: ClassVar[str]`，通过 `Annotated` / `Field` 约束字段，通过 `model_validator` 约束跨字段关系。 + 构造与 `model_validate()` 自动校验，不另设手写 `validate()` 或 dataclass `__post_init__`。 +- `Go2WalkTaskSpec` 与 `Go2WalkDeployTaskV1` 同置于 `motrix_deploy_tasks.tasks.go2_walk`；task 类通过 + `spec_type = Go2WalkTaskSpec` 关联配置类型。`action_scale` 是按 canonical joint order 排列的 + `list[float]` 向量；训练侧 compiler 在导出时把 scalar scale 展开为逐关节向量，codec 不接受 scalar fallback。 +- manifest 读取按 `task.name` 加载唯一 `motrix_deploy.tasks` entry point 指向的 task **类**，不构造 runtime task， + 再按其 `spec_type.model_validate()` 解码、校验具体 model。只使用这一组 task entry points，不增加独立 spec entry point， + 也不保留原始 dict fallback。未知或重复插件、未知字段、缺少必需字段和非法类型在读取边界拒绝。 + +Frozen 只阻止字段赋值，不使嵌套 list 不可变；`to_dict()` 在序列化边界重新校验当前字段内容。 +Artifact codec 不解析插值。应用侧继续通过 OmegaConf 加载、合并 YAML，并使用 +`OmegaConf.to_container(cfg, resolve=True)` 得到普通字典，再传给具体 `Spec.model_validate()`。 + +控制循环只消费已构造的 task；具体 task 从 typed spec 字段读取参数，不直接读取原始 dict。 ### 8.3 Artifact 生成边界 @@ -504,7 +563,8 @@ artifact 生成分为三个职责独立的阶段： checkpoint 转换为经过 ONNX Runtime parity 验证的自包含 ONNX，并返回实际 input/output tensor contract。 2. **Deployment profile compilation**：`motrix_envs.deploy` 根据稳定的环境 compiler 解析当次训练的 task snapshot、`RobotCfg` 和编译模型，生成 `RobotSpec`、`TaskSpec` 和 `ControlSpec`；`motrix_rl.deploy` - 通过注入的 profile builder 将 policy tensor contract 与 task observation/action size 做交叉校验。 + 通过注入的 profile builder 将 policy tensor contract 与编译环境的 observation/action size 做交叉校验。 + 这些维度仅作为 `DeploymentProfile.observation_size` / `action_size` 的导出期运行时数据，不序列化到 `TaskSpec`。 3. **Artifact writing**：`motrix_deploy.artifact` 校验完整 manifest，复制 policy payload、计算 checksum，并原子 写入最终 artifact 目录。 @@ -512,7 +572,10 @@ artifact 生成分为三个职责独立的阶段： deployment export 不得再按 `(rllib, train_backend, algo)` 实现 `export_rslrl_*`、`export_skrl_*` 等平行编排。 新增训练 backend 只增加对应的 ONNX adapter，不修改 profile compiler、artifact reader 或部署控制循环；新增 task/robot/action 语义时，在 `motrix_envs.deploy` 增加 profile compiler，在 `motrix_deploy_tasks` 增加 task 实现， -并分别通过 import-time 注册声明接入 core registry。 +compiler 通过 import-time 注册接入 profile registry，并直接构造该 task 的具体 spec；task 类通过唯一的 +`motrix_deploy.tasks` entry point 接入，如 +`"go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTaskV1"`，由类上的 `spec_type` 同时支持 +manifest 解码和运行时构造，不另设 spec 插件。 组合导出接口直接消费经过验证的 ONNX model bytes 和 `OnnxExportReport`。独立导出 `.onnx` 的用户入口继续 原子落盘，但 deployment artifact 生成不依靠临时文件在两个阶段之间传递 policy。ONNX exporter 的动态 @@ -592,7 +655,7 @@ DeploymentManifest 每个 group 内的名称顺序仍是唯一 canonical order。Go2 等单控制模式机器人可自然表示为一个 `legs` group； 多部件机器人可以让每个 action pipeline 绑定不同 group 和 command mode。 -未来即使把 `RobotCfg` 和机器人资产拆入独立 package，artifact 也不能只保存一个可变的 registry name。应同时 +仿真配置与资产由独立的 `motrix_robots` package 提供；general manifest 仍不能只保存可变的 registry name。应同时 保存 package/name/version 引用和 resolved RobotSpec snapshot，并对 snapshot 计算 checksum：引用用于发现和 一致性检查，snapshot 用于离线部署和历史复现。 @@ -758,7 +821,11 @@ groups。多频率调度和图像等大数据 channel 在真实任务需要时 ## 9. 启动前验证 -验证分为五层，并且全部在发送第一条 robot command 前完成： +CLI 读取 artifact 并实例化具体 `DeployTask`，不在创建 backend runtime 前比较 task observation/action +维度。policy 输入边界校验 task 实际构造的 observation 数组，task action 转换边界校验实际 policy 输出。 +`TaskSpec` 不序列化维度；manifest tensor specs 保持模型元数据，`DeploymentProfile` 维度仅用于导出期交叉校验。 + +静态验证分为以下五层；实际 observation/action 兼容性在首个控制 tick 校验，不属于 enable 前验证： 1. **Artifact validation**：schema、相对路径、checksum、枚举、数组长度、范围、频率和版本。 2. **Policy validation**：ONNX 可加载，输入输出名称、dtype、shape 与 manifest 一致，零输入 smoke inference @@ -779,12 +846,16 @@ groups。多频率调度和图像等大数据 channel 在真实任务需要时 只为确实需要按 artifact、环境或 runtime recipe 选择实现的组件保留 registry。`motrix_deploy.profile` 提供 profile compiler registry；`motrix_envs.deploy` import 具体 compiler 时通过 -`register_profile_compiler()` 装饰器按环境名自动注册。`motrix_deploy.task` 提供 task runtime registry； -`motrix_deploy_tasks` import 具体实现时通过 `register_task()` 装饰器按版本化 task 标识自动注册；该包发布的 -console script 完成 import 后委托 `motrix_deploy.cli` 加载 artifact 并调用 core registry 创建 task。这里不使用 -entry-point 插件发现，`motrix_deploy` core 也不反向 import 具体环境、task 或 SDK。 - -backend 通过 `motrix_deploy.backends` Python entry-point group 发现，core 只持有 `RobotInterface`、factory context +`register_profile_compiler()` 装饰器按环境名自动注册。task runtime 通过 `motrix_deploy.tasks` Python entry-point +group 按版本化 task 标识发现。`available_tasks()` 只读取已安装插件元数据；`load_task_type()` 懒加载唯一 +`DeployTask` 类，manifest codec 读取其 `spec_type` 而不实例化 task。`create_task()` 再将已解码的具体 +`TaskSpec` 和 `RobotSpec` 传给该类构造运行时 task。未知名称或重复插件报错，不按 import 顺序选择实现。 +`motrix_deploy` core 发布指向 `motrix_deploy.cli:main` 的 console script，不反向 import 具体环境、task 或 SDK； +具体部署 recipe 通过已安装 Hydra search-path plugin 提供的 `task` group 选择；外部应用也可显式指定 Hydra config path/name。 + +runtime 插件由 `motrix_deploy.runtime.factory` 通过 `motrix_deploy.backends` Python entry-point group 发现； +recipe 用 `runtime.backend` 选择插件，具体包为 `motrix_deploy_mujoco`、`motrix_deploy_motrixsim` 与 `motrix_deploy_unitree`。 +core 只持有 `motrix_deploy.robot.interface.RobotInterface`、factory context 和严格的重复/未知插件校验，不 import 具体仿真器。command input 第一版由 application factory 直接创建，暂不 建立 device/binding plugin registry，详见 [Command Input 配置与创建](./deploy-command-input.md#7-配置与创建)。 policy runtime 由 CLI 装配；observation 与 action 语义由具体 `DeployTask` 直接实现，也不拆分成 @@ -799,8 +870,8 @@ term/processor registry。 第一阶段以 `go2-walk` 为目标任务、`go2` 为 robot、MuJoCo 为 cross-simulator backend。Go2 walk 的 actor observation 由 IMU、joint state、previous action、速度指令和足端 phase 组成，适合先验证通用部署闭环。首个 -rollout 使用平地场景；rough terrain 只作为已有训练 checkpoint 的来源，待基础 pipeline 稳定后再接入 MuJoCo -height field。 +rollout 支持平地与确定性 procedural height-field 世界，分别通过通用部署世界 ID `flat`、`rough` 选择。 +世界模块独立于 Go2 robot namespace 和训练任务。 首个闭环必须完整经过： @@ -816,40 +887,136 @@ task compiler 应把 run-varying 参数写入 artifact，并通过数值一致 ### 11.2 Backend 配置 -仿真场景配置独立于 artifact。MuJoCo backend config 至少包含： +仿真场景配置独立于 artifact。MuJoCo backend 接收单一完整 `SceneCfg`，机器人只在 `scene.objs.robot` +声明；不持有 world/robot 分离配置、composition 模式、环境 ID 或训练 registry。CLI 的应用层配置使用 +`runtime.deploy_env_id` 与现有 robot registry ID `runtime.robot_id`，无需场景 `_target_`： + +```yaml +runtime: + kind: simulation + backend: mujoco + deploy_env_id: flat + robot_id: go2 + robot_translation: [0.0, 0.0, -0.114] + robot_rotation: [0.0, 0.0, 0.0, 1.0] +``` -- 已注册的环境 SceneCfg provider 和配置模式； -- backend 专用 physics timestep 与 solver iterations； -- base free joint、joint 和 actuator 的名称映射； -- 初始 root pose 和可选 fall condition； -- actuator 模式以及 command 到 torque/control 的映射； -- viewer 默认值；Hydra runtime config 中的 `viewer` 是最终运行模式。 +Placement 唯一来源是 `scene.objs.robot.translation`（Vec3）与 `.rotation`（xyzw），它们是 attachment transform， +与模型固有 base pose 组合一次，并非绝对世界 root 位姿。CLI 的可选 `runtime.robot_translation` / +`runtime.robot_rotation` 由应用层消费并写入该 robot，不进入 runtime backend config。 +Go2 固有 base 高度为 0.445 m；identity rotation 下，flat 的 -0.114 m 偏移得到 0.331 m，rough 的 +-0.025 m 偏移得到 0.42 m。runtime 保留复制场景中的 translation / rotation；`mj_resetData` 恢复编译的 +`model.qpos0`（含组合后的 root placement），只用绑定 `RobotSpec` 的关键姿态覆写 articulation 关节。 +`SimulationRuntimeConfig` 负责完整场景、渲染、实时节奏与 state-source 设置；其 `physics` 使用 +`motrix_env_core.config.sim.SimCfg`，统一负责 `dt`、`solver_iterations`、`solver_tolerance` 与 `gravity`。 +Runtime 默认 physics 为 `SimCfg(dt=0.002, solver_iterations=100)`，不定义 backend 专用 physics schema。 +YAML 仍使用 `runtime.physics`；`runtime.sensor_bindings` 保留角色到模型本地 sensor 名称的映射， +在配置边界通过 `SensorBindings(**mapping)` 转换。程序化 `SimulationRuntimeConfig.sensor_bindings` +使用 `motrix_deploy.runtime.config.SensorBindings`，而非 Python 字典。 +该 frozen dataclass 只接受 `base_angular_velocity`、`base_linear_acceleration`、`base_linear_velocity` +三个角色，各字段类型为 `str | None`，默认为 `None`；字符串必须非空且至少包含一个非空白字符， +未知角色与非字符串值均被拒绝。字段缺省或显式 `None` 表示没有绑定；MuJoCo 在打开机器人时拒绝 +这三个必需角色的未绑定状态。Backend 继续校验 sensor 是否存在、类型、维度与坐标系，不为其他角色 +合成状态，也不把所有 `RobotState` 字段视为 sensor-bound。 +程序化 `SimulationRuntimeConfig.scene` 接收同样的完整场景，physics 与 sensor bindings 保持独立。 +可选 solver/gravity 字段为 `None` 时保留模型来源值，显式值覆盖来源设置。 +物理配置仅在启动前设置：runtime 读取编译使用的共享 `SimCfg`，应在 runtime 构造前完成配置， +不支持编译后修改 `dt` 来动态调参。 +fall termination 属于 artifact task，而非 backend。 + +`motrix_deploy.env` 通过 `motrix_deploy.envs` entry-point group 发现部署世界。 +`available_deploy_envs()` 只读取 metadata；`create_deploy_env(id)` 按需加载唯一零参数 factory，要求返回 +`SceneCfg` 且 `scene.objs.robot` 为空。未知 ID、重复 provider、返回类型不符或 robot slot 已占用均报错。 +内置 `flat`、`rough` 分别位于 `motrix_deploy_tasks.envs.flat` 与 `.rough`，只使用 core floor/hfield scene +配置，不 import 训练任务、机器人模型或训练 contact 配置。 + +`motrix_deploy.env.assemble_deploy_scene(deploy_env_id, robot_id)` 创建无机器人世界，以 +`motrix_env_core.registry.make_robot_config(robot_id)` 填入 `scene.objs.robot`。core registry 惰性发现已安装的 +`motrix_env_core.robots` entry points，训练与部署复用同一机制，不重复注册模型。 +不创建通用 core scene-factory registry。世界扩展仍使用上述零参数 factory entry point。 + +CLI 仿真应用从 backend 配置中取出 `deploy_env_id`、`robot_id` 与可选 robot attachment overrides， +调用组装 helper 并将 overrides 写入 scene robot，再把完整场景传给 +simulation plugin；backend 不通过 Hydra instantiate 解释场景 YAML。用户仍可选择完整的 +`task=go2-walk-flat/sim` 或 `task=go2-walk-rough/sim` recipe。覆写 `runtime.deploy_env_id=rough` 仅替换世界， +保留 artifact 与机器人模型；attachment placement 不随世界单字段 override 改变， +需要不同净空时应显式调整 `runtime.robot_translation`。 + +程序化调用沿用 `SimulationRuntimeConfig`： -`motrix_deploy` core 不查询环境 registry，也不依赖任何仿真器。独立的 `motrix-deploy-mujoco` plugin 根据 -backend recipe 取得已注册环境的 `SceneCfg`，并根据 recipe 的 physics timestep 与 solver iterations 构造 -deployment 专用 `SimCfg`，再通过 `MuJoCoSceneCompiler.create_spec()` 组装 robot、terrain、friction、light、 -sensor 和 physics options，最后按部署控制语义改写 actuator。它不读取环境的 `SimCfg`；真机 backend 不加载 -或改写仿真模型。场景仍须保持 canonical joint names，并通过 RobotSpec、DoF、range 和 actuator 结构校验， -不能用位置索引掩盖模型差异。 +```python +from motrix_deploy.env import assemble_deploy_scene +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_env_core.config.sim import SimCfg + +scene = assemble_deploy_scene("flat", "go2") +scene.objs.robot.translation = (0.0, 0.0, -0.114) +scene.objs.robot.rotation = (0.0, 0.0, 0.0, 1.0) +config = SimulationRuntimeConfig( + scene=scene, + physics=SimCfg(dt=0.002, solver_iterations=100), + sensor_bindings=SensorBindings( + base_angular_velocity="gyro", + base_linear_acceleration="accelerometer", + base_linear_velocity="global_linvel", + ), +) +``` + +Runtime factory 只选择并准备宿主，不通过回调构造控制器。创建返回后，`runtime.robot` 及其只读 +`spec` 在 `open()` 之前已可用；应用显式构造自己的 task、policy、binding 和 `ControlSession`： + +```python +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.factory import create_simulation_runtime + +runtime = create_simulation_runtime("mujoco", config) +robot = runtime.robot +control = ControlSession( + robot=robot, + task=task, + policy=policy, + command_binding=command_binding, + period_s=0.02, +) +runtime.bind_control_session(control) +with runtime: + result = runtime.run(steps=50) +``` + +这里的 task、policy 与 command_binding 由应用持有；依赖机器人契约的 task 可在 runtime 创建后从 +`robot.spec` 构造。硬件由 `create_hardware_runtime(name, config, context)` 返回 prepared host 后，使用同样的 +显式组装和绑定流程。`run(control, steps=...)` 仍可直接接收 session；绑定和传入的 session 必须属于该 +runtime 的 robot，不能覆盖另一个已绑定的 session。`ControlSession.period_s` 是控制周期的唯一来源； +`SimulationRuntimeConfig` 不重复声明控制周期，仿真宿主在绑定或解析 session 时校验它与 physics timestep 的整数倍关系。 +Task 只由 control session 持有，不重复传给 runtime。 + +`motrix_deploy` core 不查询训练环境 registry，也不依赖仿真器。MuJoCo plugin 将完整场景与 +`config.physics` 直接传给 `MuJoCoSceneCompiler.create_spec()`，编译 terrain、robot、sensor 与视觉配置， +再按部署控制语义改写 actuator；MotrixSim compiler 同样直接消费共享 `SimCfg`，不重建或丢弃可选字段。 +Backend 不读取训练 `EnvCfg`。场景仍须保持 canonical joint names， +并通过 RobotSpec、DoF、range 与 actuator 结构校验，不能用位置索引掩盖模型差异。 ### 11.3 状态与命令转换 -`MuJoCoInterface.open()` 完成一次性模型构造和映射： +`MujocoRuntime` 准备机器人端口，`open()` 持有最终模型构造和绑定： -- 通过 `MuJoCoSceneCompiler.create_spec()` 组装 SceneCfg，并校验原始 position actuator； -- 把 canonical joint actuator 转换为 torque motor，按 `RobotSpec.torque_limit` 设置 control/force range； +- 程序化创建时通过 `MuJoCoSceneCompiler.create_spec()` 组装 SceneCfg 并派生物理 `RobotSpec`； +- 打开时校验 canonical source motor 或 position actuator，把 position actuator 转换为 torque motor， + 按 `RobotSpec.torque_limit` 设置 control/force range； - 编译经过 deployment transform 的最终 model； - 通过 joint name 解析 qpos/qvel address； -- 从 free joint 或显式 IMU sensor 得到 base orientation、angular velocity 和 linear acceleration； -- 解析 torque actuator 或声明的 native hybrid actuator； -- 校验 joint range、actuator force range 和 artifact limit 的包含关系。 +- 从 base body 得到 orientation，并按显式 `sensor_bindings` 解析 IMU 与 world-frame linear velocity； +- 校验 joint range、actuator force range 和外部 RobotSpec limit 的包含关系。 + +`RobotInterface.open()` 只激活该 runtime 已准备的机器人绑定，不构造或持有世界。 每个 physics substep 都读取最新 joint position/velocity，按 `tau = kp * (q_des - q) + kd * (dq_des - dq) + tau_ff` 计算 torque，再按 `RobotSpec.torque_limit` 硬限幅并写入 motor control。当前 MuJoCo deployment physics timestep 为 2 ms、control period 为 20 ms，因此每个 control tick 固定执行十个 `mj_step`，每步使用 100 次 solver iterations；backend 必须显式覆盖并验证 XML 中的原始 -timestep。初始化时调用 `mj_resetData`、写入确定的初始 root pose 和 joint pose、清空 velocity，然后执行 -`mj_forward`。 +timestep。初始化时调用 `mj_resetData` 恢复组合后的 `model.qpos0`，只覆写 articulation joint pose、 +清空 velocity，然后执行 `mj_forward`。 ### 11.4 运行终止与指标 @@ -870,6 +1037,45 @@ realtime scheduler，可通过 `runtime.realtime=false` 关闭实时节奏控制 `runtime.realtime=null` 时跟随 `runtime.viewer`，也可显式配置实时节奏。`ControlLoop.run(steps=...)` 保留为 程序化确定步数运行能力，不进入 CLI 配置。 +### 11.5 原生 MotrixSim runtime + +`motrix_deploy_motrixsim` 通过同一个 `motrix_deploy.backends` entry-point group 注册 `motrixsim`。 +已安装仿真 recipe 默认选择 MuJoCo；使用同一 task、artifact 和完整场景切换原生 backend： + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.backend=motrixsim runtime.viewer=false runtime.realtime=false duration_s=1.0 +``` + +粗糙地形同样使用 `task=go2-walk-rough/sim`，不复制 backend-specific recipe。程序化 factory 使用 +`create_simulation_runtime("motrixsim", config)`，返回 prepared `runtime.robot` 后显式构造并绑定 session。 +`ControlSession.period_s` 是唯一控制周期来源，绑定或解析 session 时验证 physics timestep 的整数倍关系。 + +`MotrixSimRuntime` 持有 `MotrixSimSceneCompiler` 产生的 MSD world、原生 model/data、reset、physics loop 与 +SDK `RenderApp` viewer，不借用 MuJoCo runtime，不实例化训练环境。Native robot port 只读状态、缓存控制目标， +不推进或关闭世界。Position actuator 转换为 unit-gear torque motor，native motor 同样支持；每个 physics substep +使用当前关节状态重算 PD + feedforward 并限幅。Direct torque 绕过 PD，但保留契约限幅。 + +Go2 使用相同的显式 model-local sensor 名称 `gyro`、`accelerometer`、`global_linvel`，分别校验 local +`FrameAngVel`、local `FrameLinAcc` 和 world `FrameLinVel`。IMU site 必须属于 base body 并与其坐标轴对齐； +world velocity 必须指向 base link 或对齐的 base-body site。名称通过 scene robot `resolve_name()` 处理 affixes， +缺少或不兼容的传感器报错，不伪造状态。Base position 与 linear velocity 标记为 privileged simulation state。 + +Reset 重新分配 native data，恢复编译的初始 placement，再覆写 articulation key pose、清空 velocity 与缓存控制； +不重复施加 attachment transform。公开状态是 query buffer 的独立快照。Sample timestamp 使用 simulation time， +receive timestamp 与 realtime pacing 使用 monotonic clock。Native viewer 提供 keyboard capability，关闭窗口中断控制， +离开 runtime context 释放 viewer/model/data；headless 不需要图形窗口。 + +模块职责如下： + +```text +motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/ +├── plugin.py # motrixsim entry point；共享 SimulationRuntimeConfig / artifact context +├── runtime.py # native world/model/data、physics、reset 与 ControlSession 调度 +├── interface.py # semantic sensor binding、query/write、非 owning RobotInterface +└── viewer.py # SDK RenderApp 与 viewer-owned KeyboardDevice +``` + ## 12. CLI 设计 package 暴露统一入口： @@ -933,14 +1139,15 @@ sim backend 打开时负责校验 profile 参数与其模型一致，真机 back artifact 记录规范化 `RobotCfg` 来源 fingerprint，便于追踪生成时使用的配置。第一阶段部署运行时不要求安装 该配置的 provider，也不重新计算 fingerprint；实际兼容性通过 artifact `RobotSpec` 与 backend model/device -直接校验。未来独立 robot package 建立后，可以把按 registry name 重新解析和 fingerprint 对比作为额外校验。 +直接校验。独立 `motrix_robots` package 提供仿真模型，但不要求部署 runtime 按 registry name 重新解析或重算 fingerprint。 ### 13.3 训练 obs/action 一致性 deployment-enabled task 必须在 `motrix_deploy_tasks` 提供一个带版本的 `DeployTask`，并在拥有源环境配置的 `motrix_envs.deploy` 提供对应 compiler。compiler 统一返回由 `robot / task / control` 组成的 `DeploymentProfile`；其中 `RobotSpec` 由通用 robot builder 从 `RobotCfg` 构建，`TaskSpec` 只保存版本化 task -标识、policy tensor size 和 run-varying config。compiler 不恢复 policy checkpoint，也不执行 ONNX 导出。 +标识和 config。profile 单独保留编译环境的 observation/action 维度供导出校验，不写入 task serialization； +artifact 的维度以 policy tensor shapes 为唯一来源。compiler 不恢复 policy checkpoint，也不执行 ONNX 导出。 每个 task version 至少有一组 golden state/context/action probe， 同时在训练环境和具体 `DeployTask` 上运行，断言 observation 和 command 数值一致。 @@ -953,7 +1160,7 @@ deployment-enabled task 必须在 `motrix_deploy_tasks` 提供一个带版本的 统一入口根据环境选择最新的 run 并生成默认 artifact 路径，不暴露训练框架专属子命令： ```bash -uv run scripts/export_deploy.py env=go2-walk-rough +python scripts/export_deploy.py env=go2-walk-rough ``` 入口先调用统一 policy export，再依据 task metadata 选择 deployment profile，最后交给 artifact writer。CLI 和 @@ -1009,7 +1216,7 @@ Go2 vertical slice 的验收基线为：确定性初始化的 1000-tick headless ### 第一阶段：框架与 sim2sim -1. 建立包含通用 CLI 的 `motrix_deploy` library，以及负责具体任务和可执行入口 bootstrap 的 +1. 建立包含通用 CLI 的 `motrix_deploy` library，以及通过 `motrix_deploy.tasks` entry points 提供具体任务类的 `motrix_deploy_tasks` workspace package。 2. 实现公共 contracts、artifact schema/reader/validator 和 component registry。 3. 实现 `DeployTask` 接口、ONNX `PolicyRuntime` 和直接编码 obs/action 的 `go2_walk/v1`。