diff --git a/AGENTS.md b/AGENTS.md index a9e246d5..9c0b21bc 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -11,10 +11,10 @@ MotrixLab 是构建在 MotrixSim 仿真后端之上的强化学习框架，提 项目使用 UV workspace，包含以下 package： -- `motrix_env_core`：backend 无关的环境框架，拥有 robot registry 与 `motrix_env_core.robots` entry-point discovery（不依赖任何 simulator 或 `motrix_robots`） +- `motrix_env_core`：backend 无关的环境框架、配置与机器人 registry，不依赖具体 simulator - `motrix_env_motrixsim`：MotrixSim 仿真后端（SimBackend、scene compiler、renderer、torch frontend） - `motrix_env_mujoco`：仅用于编译的 MuJoCo scene 后端 -- `motrix_robots`：仿真 `RobotCfg`、模型自有关键姿态及模型 MJCF/URDF、mesh 与模型纹理资产，仅依赖 core；模型定义模块与 `assets/` 位于 package 根目录，任务场景、地面与 terrain 资产属于 `motrix_envs`。根 package 以显式 `__all__` 提供模型类和类型化 humanoid/quadruped helper 的公共 API；框架内部从定义模块导入。注册回调由首次 robot registry 查询惰性调用；导入 package 或模型类不触发 discovery、注册、资产扫描或读取，也不加载 simulator 或训练环境 +- `motrix_robots`：可复用机器人配置、默认姿态与模型资产；仅依赖 core。框架代码从定义模块导入模型类，例如 `motrix_robots.unitree` - `motrix_envs`：内置环境与任务资产 - `motrix_rl`：RL 控制平面、训练编排与稳定 contracts（runner、run/checkpoint、backend 选择、`motrix_rl.frameworks` discovery、provider/trainer 接口） - `motrix_rl_builtin`：内置 Motrix FastSAC provider 插件 diff --git a/README.md b/README.md index 209dced4..a17a1b63 100644 --- a/README.md +++ b/README.md @@ -178,14 +178,14 @@ MotrixLab is a [uv](https://docs.astral.sh/uv/) workspace with the following pac | Package | PyPI name | Description | | --- | --- | --- | -| **motrix_deploy** | `motrix-deploy` | Framework-independent artifacts, runtime contracts, deployment registry, and CLI | +| **motrix_deploy** | `motrix-deploy` | Artifacts, control sessions, runtime contracts, and deployment CLI | | **motrix_deploy_mujoco** | `motrix-deploy-mujoco` | MuJoCo deployment backend plugin | | **motrix_deploy_unitree** | `motrix-deploy-unitree` | Unitree SDK2 DDS hardware backend plugin | -| **motrix_deploy_tasks** | `motrix-deploy-tasks` | Versioned deployment tasks and executable bootstrap | -| **motrix_env_core** | `motrix-env-core` | Backend-agnostic environment framework, configuration, registry, robot entry-point discovery, lifecycle, and rendering | +| **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 | | **motrix_env_mujoco** | `motrix-env-mujoco` | Compile-only MuJoCo scene backend | -| **motrix_robots** | `motrix-robots` | Simulation robot configs, model-owned poses, and model MJCF/URDF, meshes, and model textures; core-only dependency | +| **motrix_robots** | `motrix-robots` | Reusable robot configurations, default poses, and model assets | | **motrix_envs** | `motrix-envs` | Built-in environments, task assets, data, and deployment-profile compilers | | **motrix_rl** | `motrix-rl` | RL control plane, provider/trainer contracts, run/checkpoint handling, and discovery | | **motrix_rl_builtin** | `motrix-rl-builtin` | Built-in Motrix FastSAC provider | diff --git a/README.zh-CN.md b/README.zh-CN.md index 2136856b..a0e5c718 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -178,14 +178,14 @@ MotrixLab 是一个 [uv](https://docs.astral.sh/uv/) workspace，包含以下 pa | Package | PyPI 名称 | 说明 | | --- | --- | --- | -| **motrix_deploy** | `motrix-deploy` | 框架无关 artifact、运行时契约、部署 registry 与 CLI | +| **motrix_deploy** | `motrix-deploy` | artifact、控制会话、运行时契约与部署 CLI | | **motrix_deploy_mujoco** | `motrix-deploy-mujoco` | MuJoCo 部署 backend plugin | | **motrix_deploy_unitree** | `motrix-deploy-unitree` | Unitree SDK2 DDS 硬件 backend plugin | -| **motrix_deploy_tasks** | `motrix-deploy-tasks` | 带版本的部署任务与可执行入口 bootstrap | -| **motrix_env_core** | `motrix-env-core` | backend 无关的环境框架、配置、registry、robot entry-point discovery、生命周期与渲染 | +| **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 | | **motrix_env_mujoco** | `motrix-env-mujoco` | 仅负责编译场景的 MuJoCo backend | -| **motrix_robots** | `motrix-robots` | 仿真机器人配置、模型自有姿态及 模型 MJCF/URDF、mesh 与模型纹理资产；仅依赖 core | +| **motrix_robots** | `motrix-robots` | 可复用机器人配置、默认姿态与模型资产 | | **motrix_envs** | `motrix-envs` | 内置环境、任务资产、数据及部署 profile 编译实现 | | **motrix_rl** | `motrix-rl` | RL 控制平面、provider/trainer contract、run/checkpoint 管理与 discovery | | **motrix_rl_builtin** | `motrix-rl-builtin` | 内置 Motrix FastSAC provider | diff --git a/configs/deploy/sim2real/base.yaml b/configs/deploy/sim2real/base.yaml deleted file mode 100644 index 439b4a71..00000000 --- a/configs/deploy/sim2real/base.yaml +++ /dev/null @@ -1,30 +0,0 @@ -defaults: - - _self_ - - override hydra/job_logging: stdout - -hydra: - output_subdir: null - run: - dir: . - sweep: - dir: . - subdir: ${hydra.job.num} - job: - chdir: false - -artifact: ??? - -rollout: - steps: null - - duration_s: 300.0 -# Shared policy-control gains for physical deployment recipes. -backend: - kp: 50 - kd: 1 - -seed: 1 -hardware: - confirm: false -realtime: true -viewer: false diff --git a/configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml b/configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml deleted file mode 100644 index 4c2abefd..00000000 --- a/configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml +++ /dev/null @@ -1,34 +0,0 @@ -defaults: - - _self_ - - override hydra/job_logging: stdout - -hydra: - output_subdir: null - run: - dir: . - sweep: - dir: . - subdir: ${hydra.job.num} - job: - chdir: false - -artifact: ??? - -backend: - name: mujoco - scene: go2-walk-flat - scene_mode: play - sim_dt: 0.002 - solver_iterations: 100 - # Keep the deterministic default pose clear of the flat ground at reset. - base_position: [0.0, 0.0, 0.331] - base_orientation_xyzw: [0.0, 0.0, 0.0, 1.0] - base_body_name: base - imu_site_name: imu - gyro_sensor_name: gyro - accelerometer_sensor_name: accelerometer - global_linear_velocity_sensor_name: global_linvel - fall_height_m: 0.15 - fall_up_z: 0.5 - -viewer: true diff --git a/configs/deploy/sim2sim/go2_walk_sim2sim.yaml b/configs/deploy/sim2sim/go2_walk_sim2sim.yaml deleted file mode 100644 index 0c93c4e6..00000000 --- a/configs/deploy/sim2sim/go2_walk_sim2sim.yaml +++ /dev/null @@ -1,48 +0,0 @@ -defaults: - - _self_ - - override hydra/job_logging: stdout - -hydra: - output_subdir: null - run: - dir: . - sweep: - dir: . - subdir: ${hydra.job.num} - job: - chdir: false - -artifact: ??? - -backend: - name: mujoco - scene: go2-walk-rough - scene_mode: play - sim_dt: 0.002 - solver_iterations: 100 - # Absolute world position chosen to clear the deterministic rough terrain at the spawn point. - base_position: [0.0, 0.0, 0.42] - base_orientation_xyzw: [0.0, 0.0, 0.0, 1.0] - base_body_name: base - imu_site_name: imu - gyro_sensor_name: gyro - accelerometer_sensor_name: accelerometer - global_linear_velocity_sensor_name: global_linvel - fall_height_m: 0.15 - fall_up_z: 0.5 - -viewer: true - -# Used for headless or non-keyboard backends. Viewer mode uses the focused keyboard instead. -command: - velocity: [0.0, 0.0, 0.0] - -# Leaving both values null runs until the viewer closes or the process is interrupted. -rollout: - steps: null - duration_s: null - -seed: null -realtime: null -hardware: - confirm: false 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 50977e6d..2e6a55b8 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 @@ -29,10 +29,9 @@ input_devices_and_bindings/extending Backend-neutral InputDevice, binding, and task-command pipeline ``` -The control path from `CommandBinding` through `RobotInterface` is backend-neutral. The same binding, task command, -`PolicyContext`, task/policy contract, and `RobotCommand` semantics can control simulation and physical-robot targets. Moving -between the two requires replacing only the concrete device provider and `RobotInterface` implementation. A physical backend -that implements `RobotInterface` therefore reuses the same command pipeline, task, and policy. +`ControlSession` combines a binding, task, policy, and `RobotInterface`. It reads the command, builds the task observation, +runs the policy, and writes a `RobotCommand`. A simulation runtime advances physics and manages the viewer; a hardware +runtime schedules control against live robot state. Both use the same task-command semantics. A simulation and a physical robot can select different concrete devices. For example, simulation can use a GLFW keyboard while a physical robot uses a remote controller or network input; both reuse the same binding and task-command semantics. @@ -58,6 +57,8 @@ A high-level task command also has the opposite direction from `RobotCommand`: The runtime obtains a typed task command through `CommandBinding`: ```python +from motrix_deploy.runtime.context import PolicyContext + command = command_binding.read_command(batch_size=1) context = PolicyContext( step=step, @@ -68,7 +69,7 @@ task.validate_command(context.command) observation = task.build_observation(state, context) ``` -For example, MuJoCo deployment obtains a `MujocoKeyboardDevice` from the backend's `KeyboardDeviceProvider` and constructs a +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 d6e0e20e..9e1c7aa1 100644 --- a/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md +++ b/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md @@ -1,119 +1,109 @@ # Go2 Flat-Terrain Walking: From Training to Physical Deployment -This tutorial follows the full Go2 flat-terrain workflow: train a policy, export an artifact, check it in MuJoCo, and run -it on a real robot. If you already have a training run, start at “Export the artifact”. - -## 1. Install the environment - -Run this command from the repository root: +Run an exported Go2 policy in MuJoCo before attempting hardware deployment. With the task and MuJoCo plugins installed, +a bounded headless check is: ```bash -sh install.sh --all +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 ``` -This installs the training, MuJoCo, ONNX Runtime, and Unitree SDK2 dependencies. +The following steps create that artifact, explain interactive control, and cover the existing Unitree hardware workflow. -## 2. Train the policy +## 1. Install and train -If you do not have a run yet, train the flat-terrain Go2 policy: +From the repository root: ```bash +sh install.sh --all +source .venv/bin/activate python scripts/train.py task=go2-walk-flat/rslrl.ppo ``` -Training results are saved under `runs/go2-walk-flat/`. - -## 3. Export the artifact +Training results are saved under `runs/go2-walk-flat/`. Skip training if you already have a metadata-backed run. -Export the latest run: +## 2. Export and inspect the artifact ```bash python scripts/export_deploy.py env=go2-walk-flat +motrix-deploy inspect artifact=artifacts/go2-walk-flat.deploy ``` -The output is written to `artifacts/go2-walk-flat.deploy/`. This artifact is the only policy bundle needed for deployment; -it contains the model and its runtime configuration. +Export selects the latest run and creates `artifacts/go2-walk-flat.deploy/`. To select a specific run, use +`run=` instead of `env`; `output=` changes the destination. Existing artifact directories are +not overwritten. Confirm that inspection reports `valid: true`. -## 4. Inspect the artifact +The artifact contains the ONNX policy, robot/control settings, and task specification. Deployment also needs the installed +`motrix_deploy_tasks` plugin, which supplies the walking implementation, deployment scenes, and Hydra recipes, plus the +runtime plugin for the target. The recipes work outside the repository without a workspace config path. +Task defaults select the Hydra runtime group (`sim` or `hardware`); you do not need a separate runtime override. +Common top-level settings are `artifact`, `duration_s`, and `command`. All target-specific settings live directly under +`runtime`. `runtime.kind` selects the simulation or hardware path; `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. -```bash -motrix-deploy inspect \ - artifact=artifacts/go2-walk-flat.deploy -``` +## 3. Check in MuJoCo -Make sure the output reports `valid: true`. - -## 5. Run Sim2Sim first +Use the headless command above for a bounded run, or open the viewer: ```bash -motrix-deploy sim2sim \ - --config-name go2_walk_flat_sim2sim \ - artifact=artifacts/go2-walk-flat.deploy +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy ``` -This opens the MuJoCo viewer. Use `W/S` to move forward and backward, `A/D` to move sideways, and `Q/E` to turn. Close -the window, press Esc, or press Ctrl-C to stop. +Hold `W/S` to move forward/backward, `A/D` sideways, and `Q/E` to turn. Close the window, press Esc, or press Ctrl-C to stop. +The recipe selects the flat scene and the `go2` robot. Headless mode uses constant zero velocity; for a moving check, add +`'command.velocity=[0.5,0.0,0.0]'`. Set `duration_s` for a bounded run: it allows +`ceil(duration_s / control_period_s)` control ticks, using the artifact's control period, not a wall-clock timeout. +Startup and shutdown may take additional wall time. -## 6. Run on the real robot +Simulation-only settings are `runtime.viewer` and `runtime.realtime`. +`runtime.realtime=null` follows `runtime.viewer`; use `runtime.realtime=true` to pace a headless run in real time, +or `runtime.realtime=false` to run without real-time pacing. -Put the robot in low-level/debug mode, connect Ethernet, and keep an emergency stop ready. Replace `enp5s0` with the -actual network interface: +### Assemble a runtime in Python -```bash -motrix-deploy inspect \ - artifact=artifacts/go2-walk-flat.deploy - -motrix-deploy sim2real \ - --config-name go2_walk_flat_sim2real \ - artifact=artifacts/go2-walk-flat.deploy \ - backend.network_interface=enp5s0 \ - hardware.confirm=true -``` - -After startup, press Start on the remote. When the default-pose transition finishes, press A. Hold L1 and move the sticks -to send motion commands. Press B to enter the lie-down sequence; Select triggers the emergency stop. - -You can also inspect the robot state before sending policy commands: +The complete `examples/deploy_to_sim.py` example composes a scene, walking task, sample ONNX policy, and input binding: ```bash -python -m motrix_deploy_unitree.read_lowstate enp5s0 +python examples/deploy_to_sim.py --headless --steps 100 ``` -## Advanced usage +It creates a MuJoCo runtime with `create_simulation_runtime`, attaches a `ControlSession` using +`runtime.bind_control_session(control)`, and runs inside `with runtime:`. The control session handles input, observations, +inference, and robot commands; the runtime handles physics, timing, and the viewer. A raw ONNX file needs explicit task +preprocessing and action settings, whereas the artifact carries those settings for the CLI. -### Override policy PD gains +## 4. Run on the real robot -The physical-runtime base configuration, `configs/deploy/sim2real/base.yaml`, currently sets `backend.kp=50` and -`backend.kd=1`, overriding the gains stored in the artifact `TaskSpec.config`. Set both fields to `null` to keep the artifact -gains, or pass 12 non-negative values on the command line: +Suspend the robot, put it in low-level/debug mode, connect Ethernet, and keep an independent emergency stop and operator +ready. Startup can physically stand the robot down before the remote Start gate. Inspect the artifact first, then replace +`enp5s0` with the actual interface: ```bash -motrix-deploy sim2real \ +motrix-deploy task=go2-walk-flat/hardware \ artifact=artifacts/go2-walk-flat.deploy \ - backend.network_interface=enp5s0 \ - 'backend.kp=[20,25,30,20,25,30,22,27,32,22,27,32]' \ - 'backend.kd=[0.3,0.4,0.5,0.3,0.4,0.5,0.35,0.45,0.55,0.35,0.45,0.55]' \ - hardware.confirm=true + runtime.network_interface=enp5s0 ``` -### Inspect LowState without sending commands +Hardware runs are always real-time internally; the hardware runtime has no `runtime.viewer` or `runtime.realtime` fields. +Press Start, wait for the default-pose transition, then +press A. Hold L1 and move the sticks to command motion. B requests lie-down; Select triggers emergency stop. Stop paths +send damping commands before closing DDS. Software tests do not replace a suspended real-robot check. + +The installed hardware recipe sets `runtime.kp=50` and `runtime.kd=1`. Set both to `null` to retain artifact gains, or +supply non-negative scalars or per-joint values in canonical joint order. -Run the read-only diagnostic before sending any motion command: +For read-only diagnostics without a command publisher: ```bash -python -m motrix_deploy_unitree.read_lowstate enp5s0 +motrix-deploy-unitree read-lowstate enp5s0 ``` -### Send a single-joint motion command - -To bypass the policy and debug a single joint-position motion, use the bounded helper installed with -`motrix_deploy_unitree`. By default, it builds the control contract from the current `go2-walk-flat` deployment profile and -does not require a training run, checkpoint, policy, or deployment artifact: +For a bounded single-joint check, with the same hardware precautions: ```bash -python -m motrix_deploy_unitree.go2_joint_control \ - enp5s0 \ - FL_thigh_joint \ - 0.9 \ - --hardware-confirm +motrix-deploy-unitree joint-control enp5s0 FL_thigh_joint 0.9 \ + --artifact artifacts/go2-walk-flat.deploy ``` + +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. diff --git a/docs/source/en/user_guide/tutorial/basic_frame.md b/docs/source/en/user_guide/tutorial/basic_frame.md index bd2f3f35..407a44f0 100644 --- a/docs/source/en/user_guide/tutorial/basic_frame.md +++ b/docs/source/en/user_guide/tutorial/basic_frame.md @@ -51,7 +51,7 @@ MotrixLab is a UV workspace. The packages are grouped by ownership rather than t | `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 | Versioned deployment tasks and executable bootstrap | +| `motrix_deploy_tasks` | Policy deployment | Walking tasks, deployment scenes, and installed Hydra 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 @@ -76,6 +76,17 @@ python scripts/train.py task=cartpole/skrl.ppo python scripts/train.py task=cartpole/rslrl.ppo ``` +## Run an exported policy + +Deployment uses an artifact rather than a training environment: + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 +``` + +This runs the exported policy headlessly in MuJoCo. See [Go2 deployment](advanced/motrix_deploy.md) for export, +interactive control, and hardware operation. + ## What the layering buys you 1. **Environment reuse**: one registered environment can be trained by multiple RL frameworks. 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 29926fa6..df91c476 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 @@ -27,10 +27,9 @@ input_devices_and_bindings/extending InputDevice、Binding 与 backend-neutral task command 控制链路 ``` -从 `CommandBinding` 到 `RobotInterface` 的控制链路不依赖具体 backend。同一个 binding、任务指令、 -`PolicyContext`、task/policy contract 和 `RobotCommand` 语义可以同时用于仿真与真机；切换控制目标时，只需 -替换具体 device provider 和 `RobotInterface` 实现。真机 backend 实现 `RobotInterface` 后，即可复用相同的 -指令链路、task 和 policy。 +`ControlSession` 组合 binding、任务、策略和 `RobotInterface`，读取指令、构造任务观察、运行策略，并写入 +`RobotCommand`。仿真运行时推进物理并管理 viewer；硬件运行时根据实时机器人状态调度控制。 +两者使用相同的任务指令语义。 仿真和真机可以选择各自合适的具体 device。例如，仿真使用 GLFW keyboard，真机使用遥控器或网络输入；两端 复用同一个 binding 和任务指令语义。 @@ -55,6 +54,8 @@ InputDevice、Binding 与 backend-neutral task command 控制链路 运行时通过 `CommandBinding` 获取强类型任务指令： ```python +from motrix_deploy.runtime.context import PolicyContext + command = command_binding.read_command(batch_size=1) context = PolicyContext( step=step, @@ -65,7 +66,7 @@ task.validate_command(context.command) observation = task.build_observation(state, context) ``` -例如 MuJoCo deployment 从 backend 的 `KeyboardDeviceProvider` 取得 `MujocoKeyboardDevice`，再创建 +例如 MuJoCo deployment 从运行时的 `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 98683b00..96258a1b 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 @@ -1,115 +1,102 @@ # Go2 平地行走：从训练到真机部署 -这篇教程用 Go2 平地行走任务演示完整流程：训练策略、导出 artifact、先在 MuJoCo 中检查，最后连接真机运行。 -如果已经有训练好的 run，可以直接从“导出 artifact”开始。 - -## 1. 安装环境 - -在仓库根目录执行： +将导出的 Go2 策略先在 MuJoCo 中检查，再尝试真机部署。安装任务和 MuJoCo 插件后，可用以下命令进行有界无窗口检查： ```bash -sh install.sh --all +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 ``` -这个命令会安装训练、MuJoCo、ONNX Runtime 和 Unitree SDK2 依赖。 +下面介绍如何生成 artifact、交互控制，以及现有 Unitree 真机工作流。 -## 2. 训练策略 +## 1. 安装与训练 -没有现成 run 时，训练 flat-terrain Go2 策略： +在仓库根目录执行： ```bash +sh install.sh --all +source .venv/bin/activate python scripts/train.py task=go2-walk-flat/rslrl.ppo ``` -训练结果保存在 `runs/go2-walk-flat/`。 - -## 3. 导出 artifact +训练结果保存在 `runs/go2-walk-flat/`。已有包含 metadata 的 run 时可跳过训练。 -导出最近一次 run： +## 2. 导出并检查 artifact ```bash python scripts/export_deploy.py env=go2-walk-flat +motrix-deploy inspect artifact=artifacts/go2-walk-flat.deploy ``` -输出目录为 `artifacts/go2-walk-flat.deploy/`。artifact 是部署时唯一需要带走的策略文件，包含模型和运行所需的配置。 - -## 4. 检查 artifact +导出会选择最近一次 run，并创建 `artifacts/go2-walk-flat.deploy/`。用 `run=` 替代 `env` 可选择指定 run， +`output=` 可更改输出位置。已有 artifact 目录不会被覆盖。确认检查结果为 `valid: true`。 -```bash -motrix-deploy inspect \ - artifact=artifacts/go2-walk-flat.deploy -``` +artifact 包含 ONNX 策略、机器人与控制设置，以及任务规格。部署还需要安装 `motrix_deploy_tasks` 插件，提供行走任务实现、 +部署场景和 Hydra 配方，以及目标对应的运行时插件。配方安装在插件中，在仓库外运行也不需要 workspace 配置路径。 +任务 defaults 选择 Hydra 运行时组（`sim` 或 `hardware`），无需另行覆盖运行时组。 +顶层公共设置为 `artifact`、`duration_s` 和 `command`。所有目标专用设置直接位于 `runtime` 下； +`runtime.kind` 选择仿真或硬件执行路径；`runtime.backend` 选择插件（仿真为 `mujoco`，硬件为 `unitree_go2`）。 +`runtime.backend` 是标量字符串选择器，不是嵌套配置映射或 Hydra 配置组。 -确认输出中的 `valid` 为 `true`。 +## 3. 在 MuJoCo 中检查 -## 5. 先跑 Sim2Sim +使用开头的无窗口命令进行有界运行，或打开 viewer： ```bash -motrix-deploy sim2sim \ - --config-name go2_walk_flat_sim2sim \ - artifact=artifacts/go2-walk-flat.deploy +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy ``` -会打开 MuJoCo viewer。按 `W/S` 前后移动，`A/D` 横移，`Q/E` 转向；关闭窗口、按 Esc 或 Ctrl-C 退出。 +按住 `W/S` 前后移动，`A/D` 横移，`Q/E` 转向。关闭窗口、按 Esc 或 Ctrl-C 退出。 +配方选择平地场景和 `go2` 机器人。无窗口模式使用固定零速度；添加 `'command.velocity=[0.5,0.0,0.0]'` 可检查移动。 +通过 `duration_s` 限制运行长度：按 artifact 的控制周期换算为 +`ceil(duration_s / control_period_s)` 个控制 tick，而不是墙钟超时计时器。 +启动和关闭可能额外消耗墙钟时间。 -## 6. 运行真机 +仿真专用设置为 `runtime.viewer` 和 `runtime.realtime`。 +`runtime.realtime=null` 跟随 `runtime.viewer`；用 `runtime.realtime=true` 让无窗口运行按实时节奏执行， +用 `runtime.realtime=false` 则不进行实时节奏控制。 -先确认机器人处于低层/调试模式，网线已连接，并准备好急停。 -将 `enp5s0` 换成实际网卡名称： +### 在 Python 中组装运行时 -```bash -motrix-deploy inspect \ - artifact=artifacts/go2-walk-flat.deploy - -motrix-deploy sim2real \ - --config-name go2_walk_flat_sim2real \ - artifact=artifacts/go2-walk-flat.deploy \ - backend.network_interface=enp5s0 \ - hardware.confirm=true -``` - -启动后按遥控器 Start，完成默认姿态过渡后按 A；按住 L1 并移动左右摇杆，可发送移动命令。按 B 进入趴下流程，Select 触发急停。 - -发送策略前，也可以先查看机器人状态： +完整示例 `examples/deploy_to_sim.py` 组合场景、行走任务、示例 ONNX 策略和输入 binding： ```bash -python -m motrix_deploy_unitree.read_lowstate enp5s0 +python examples/deploy_to_sim.py --headless --steps 100 ``` -## 进阶说明 +它通过 `create_simulation_runtime` 创建 MuJoCo 运行时，用 `runtime.bind_control_session(control)` 连接 +`ControlSession`，并在 `with runtime:` 中运行。控制会话负责输入、观察、推理和机器人指令；运行时负责物理推进、 +调度和 viewer。直接使用 ONNX 文件时需要显式指定任务预处理和动作设置；artifact 则为 CLI 携带这些设置。 -### 显式覆盖策略 PD 增益 +## 4. 运行真机 -真机父配置 `configs/deploy/sim2real/base.yaml` 当前设置 `backend.kp=50`、`backend.kd=1`，会覆盖 artifact -`TaskSpec.config` 中保存的增益。把二者显式设为 `null` 才会保留 artifact 增益；也可以在命令行传入非负标量统一 -覆盖 12 个关节： +将机器人悬空，切换到低层/调试模式，连接网线，并准备好独立急停和操作人员。启动过程可能在遥控器 Start 门控前 +让机器人执行站下动作。先检查 artifact，再将 `enp5s0` 换成实际网卡名称： ```bash -motrix-deploy sim2real \ +motrix-deploy task=go2-walk-flat/hardware \ artifact=artifacts/go2-walk-flat.deploy \ - backend.network_interface=enp5s0 \ - 'backend.kp=[20,25,30,20,25,30,22,27,32,22,27,32]' \ - 'backend.kd=[0.3,0.4,0.5,0.3,0.4,0.5,0.35,0.45,0.55,0.35,0.45,0.55]' \ - hardware.confirm=true + runtime.network_interface=enp5s0 ``` -### 只读检查 LowState 数据 +真机运行在内部强制按实时节奏执行；真机运行时不提供 `runtime.viewer` 或 `runtime.realtime` 字段。 +按 Start，等待默认姿态过渡完成后按 A；按住 L1 并移动摇杆发送运动指令。 +B 请求趴下，Select 触发急停。停止时先发送阻尼指令，再关闭 DDS。软件测试不能替代悬空真机检查。 + +安装的真机配方设置 `runtime.kp=50`、`runtime.kd=1`。把二者设为 `null` 可保留 artifact 增益，也可传入非负标量 +或按规范关节顺序排列的逐关节值。 -发送任何运动指令前，可先运行只读诊断： +只读诊断不创建指令 publisher： ```bash -python -m motrix_deploy_unitree.read_lowstate enp5s0 +motrix-deploy-unitree read-lowstate enp5s0 ``` -### 发送单关节运动指令 - -如需绕过策略、调试一个关节的位置运动，可使用随 `motrix_deploy_unitree` 安装的有界运动脚本。默认从当前 -`go2-walk-flat` deployment profile 构建控制契约，不需要训练 run、checkpoint、策略或 deployment artifact： +在同样的硬件安全准备下，可进行有界单关节检查： ```bash -python -m motrix_deploy_unitree.go2_joint_control \ - enp5s0 \ - FL_thigh_joint \ - 0.9 \ - --hardware-confirm +motrix-deploy-unitree joint-control enp5s0 FL_thigh_joint 0.9 \ + --artifact artifacts/go2-walk-flat.deploy ``` + +该工具使用 artifact 中的机器人、增益、时序和限位设置。等待 Start 和 A 后，移动关节、保持、返回默认姿态， +最后以阻尼模式关闭。 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 55f94f04..43b1431e 100644 --- a/docs/source/zh_CN/user_guide/tutorial/basic_frame.md +++ b/docs/source/zh_CN/user_guide/tutorial/basic_frame.md @@ -46,7 +46,7 @@ MotrixLab 是一个 UV workspace。下面按所有权而不是按单一整体运 | `motrix_deploy` | 策略的部署 | 框架无关 artifact、运行时契约与部署 CLI | | `motrix_deploy_mujoco` | 策略的部署 | MuJoCo 部署 backend plugin | | `motrix_deploy_unitree` | 策略的部署 | Unitree SDK2 DDS 硬件 backend plugin | -| `motrix_deploy_tasks` | 策略的部署 | 带版本的部署任务与可执行入口 bootstrap | +| `motrix_deploy_tasks` | 策略的部署 | 行走任务、部署场景与安装的 Hydra 配方 | 仿真后端（如 `motrix_env_motrixsim`）通过 `SimBackend` 接口隔离在环境框架之下，使用环境时通常 无需关心它；需要选择或接入仿真后端时，见[SimBackend：与仿真器解耦](building_envs/sim_backend.md)。 @@ -70,6 +70,16 @@ python scripts/train.py task=cartpole/skrl.ppo python scripts/train.py task=cartpole/rslrl.ppo ``` +## 运行导出的策略 + +部署使用 artifact，而不是训练环境： + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 +``` + +上例在 MuJoCo 中无窗口运行导出的策略。导出、交互控制及真机操作见 [Go2 部署](advanced/motrix_deploy.md)。 + ## 分层带来的优势 1. **环境复用**：一个注册环境可以由多个 RL 框架训练。 diff --git a/examples/assets/go2-walk-flat.onnx b/examples/assets/go2-walk-flat.onnx new file mode 100644 index 00000000..a00280e3 Binary files /dev/null and b/examples/assets/go2-walk-flat.onnx differ diff --git a/examples/deploy_to_sim.py b/examples/deploy_to_sim.py new file mode 100644 index 00000000..d35ec993 --- /dev/null +++ b/examples/deploy_to_sim.py @@ -0,0 +1,129 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Programmatically assemble a Go2 scene, walking task, ONNX policy and control session.""" + +import argparse +import json +from pathlib import Path + +from motrix_robots.unitree import UnitreeGo2Robot + +from motrix_deploy.contracts import TensorSpec +from motrix_deploy.policy import OnnxPolicyRuntime +from motrix_deploy.robot.interface import KeyboardDeviceProvider +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.factory import create_simulation_runtime +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkDeployTaskV1, Go2WalkTaskSpec +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg, SceneVisualCfg, SystemCameraCfg +from motrix_env_core.config.scene.light import LightCfg +from motrix_env_core.config.sim import SimCfg +from motrix_env_core.input import ConstantPlanarVelocityBinding, KeyboardPlanarVelocityBinding + +POLICY_PATH = Path(__file__).resolve().parent / "assets/go2-walk-flat.onnx" + + +@configclass +class ExampleSceneObjsCfg(SceneObjsCfg): + """Objects used by this example's flat scene.""" + + floor: FlatTerrainCfg | None = None + sun: LightCfg = LightCfg(color=(0.7, 0.7, 0.7), illuminance=10_000.0) + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--backend", choices=("mujoco",), default="mujoco", help="Simulation runtime plugin") + parser.add_argument("--headless", action="store_true", help="No window; run fixed-step without wall-clock pacing") + parser.add_argument("--steps", type=int, help="Headless control ticks; GUI mode runs until the window closes") + args = parser.parse_args() + if args.headless and (args.steps is None or args.steps <= 0): + parser.error("--steps must be a positive integer in --headless mode") + if not args.headless and args.steps is not None: + parser.error("--steps is only valid in --headless mode; GUI mode runs until the window closes") + + policy = OnnxPolicyRuntime( + POLICY_PATH, + input_spec=TensorSpec(name="obs", shape=(1, 49)), + output_spec=TensorSpec(name="actions", shape=(1, 12)), + ) + + scene = SceneCfg( + objs=ExampleSceneObjsCfg( + # Placement is an attach transform; the model root is already at z=0.445. + robot=UnitreeGo2Robot(translation=(0.0, 0.0, -0.114), rotation=(0.0, 0.0, 0.0, 1.0)), + floor=FlatTerrainCfg(friction=(0.6, 0.005, 0.0001)), + ), + # Match StandardSceneCfg lighting without importing the training environment package. + visual=SceneVisualCfg( + ambient_light_color=(0.3, 0.3, 0.3), + ambient_light_brightness=1_000.0, + head_light_color=(0.6, 0.6, 0.6), + head_light_luminous_power=1_000.0, + haze=(0.1, 0.1, 0.1, 1.0), + tone_mapping="none", + ), + system_camera=SystemCameraCfg(distance=3.0, elevation=-20.0, azimuth=90.0), + ) + config = SimulationRuntimeConfig( + scene=scene, + physics=SimCfg(dt=0.002, solver_iterations=100), + render=not args.headless, + sensor_bindings=SensorBindings( + base_angular_velocity="gyro", + base_linear_acceleration="accelerometer", + base_linear_velocity="global_linvel", + ), + ) + + runtime = create_simulation_runtime(args.backend, config) + robot = runtime.robot + # Explicit preprocessing/action semantics for this walking policy. These must + # match its training configuration; a raw ONNX file does not describe them. + action_scale = 0.25 + task = Go2WalkDeployTaskV1( + Go2WalkTaskSpec( + action_scale=[action_scale] * robot.spec.joint_count, + kp=[35.0] * robot.spec.joint_count, + kd=[0.5] * robot.spec.joint_count, + action_lower=((robot.spec.position_lower - robot.spec.default_joint_position) / action_scale).tolist(), + action_upper=((robot.spec.position_upper - robot.spec.default_joint_position) / action_scale).tolist(), + command_lower=[-1.0, -0.5, -0.5], + command_upper=[1.0, 0.5, 0.5], + command_scale=[1.0, 1.0, 1.0], + feet_phase_offsets=[0.0, 0.5, 0.5, 0.0], + gait_frequency_hz=2.0, + standing_threshold=0.05, + termination_min_up_z=0.5, + termination_min_base_height=None, + ), + robot.spec, + ) + if not args.headless and isinstance(runtime, KeyboardDeviceProvider): + command_binding = KeyboardPlanarVelocityBinding( + runtime.get_keyboard_device(), + command_lower=task.command_lower, + command_upper=task.command_upper, + ) + else: + command_binding = ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)) + task.validate_command(command_binding.read_command()) + control = ControlSession( + robot=robot, + task=task, + policy=policy, + command_binding=command_binding, + period_s=0.02, + state_timeout_s=0.1, + ) + runtime.bind_control_session(control) + with runtime: + result = runtime.run(steps=args.steps) + print(json.dumps(result.to_dict(), indent=2, sort_keys=True)) + return 0 if result.success else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/motrix_deploy/README.md b/motrix_deploy/README.md index e71bde1a..cf1d59f3 100644 --- a/motrix_deploy/README.md +++ b/motrix_deploy/README.md @@ -1,111 +1,50 @@ # Motrix Deploy -`motrix_deploy` is MotrixLab's framework-independent policy deployment library. It defines artifact, backend, policy, -control-loop, deployment-profile registry, task registry, backend plugin discovery, and CLI without importing concrete -tasks, simulator implementations, training environments, or RL frameworks. The simulator and hardware backends live in the -sibling `motrix_deploy_mujoco` and -`motrix_deploy_unitree` plugin packages. Concrete task semantics live in `motrix_deploy_tasks`, which publishes the -`motrix-deploy` executable bootstrap so tasks are registered before the core CLI runs. Built-in environment profile -compilers live under `motrix_envs.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`. -The supported vertical slice exports a metadata-backed Go2 RSL-RL walk policy and runs the same artifact in MuJoCo or -on a physical Go2. -From the repository root, install the required development extras: +From the repository root, install and activate the environment, then export a trained Go2 flat-terrain run: ```bash sh install.sh --all -``` - -Export a new artifact directory, validate it without opening a backend, and run an interactive deployment: - -```bash -python scripts/export_deploy.py env=go2-walk-rough - -motrix-deploy inspect \ - artifact=artifacts/go2-walk-rough.deploy - -motrix-deploy sim2sim \ - artifact=artifacts/go2-walk-rough.deploy -``` - -For the flat-terrain environment, export its own artifact and select the matching runtime recipe: - -```bash +source .venv/bin/activate python scripts/export_deploy.py env=go2-walk-flat - -motrix-deploy inspect \ - artifact=artifacts/go2-walk-flat.deploy - -motrix-deploy sim2sim \ - --config-name go2_walk_flat_sim2sim \ - artifact=artifacts/go2-walk-flat.deploy +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 ``` -Export defaults are defined in `configs/deploy/export.yaml`. The default rough-terrain workspace runtime is defined in -`configs/deploy/sim2sim/go2_walk_sim2sim.yaml`; the explicit flat-terrain recipe is -`configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml`. Both can be changed with Hydra overrides. The packaged -`motrix_deploy/config/deploy.yaml` is only a task-agnostic mandatory-field template. The deployment runs until the viewer -closes, the user presses Esc/Ctrl-C, or a runtime failure occurs. It then prints a JSON `RolloutResult`; artifact or -configuration failures exit with code 2. +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 minimal MuJoCo GLFW viewer opens by default and uses realtime pacing. Physics stepping remains owned by the backend -control path, and closing the window interrupts deployment safely. +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`). -The artifact writer is create-only. Choose a new output path or move an existing artifact before exporting again. Joint, -actuator range, servo gain, tensor shape, checksum, and command-range mismatches fail before the first command. -Headless and physical runs may set either `rollout.steps` or `rollout.duration_s`; viewer runs may leave both unset. -`realtime` defaults to the viewer mode when omitted. +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. -Optional dependencies remain isolated behind extras for smaller runtime environments (ONNX -inference is a core dependency; `mujoco` and `unitree` select the deployment backends): +## Programmatic control -```bash -uv sync --package motrix-deploy-tasks --extra mujoco --extra unitree -``` - -Interactive deployment reads keyboard events directly from the focused GLFW viewer window: +[`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 -motrix-deploy sim2sim \ - artifact=artifacts/go2-walk-rough.deploy +python examples/deploy_to_sim.py --headless --steps 100 ``` -Viewer mode uses realtime pacing. Hold `W/S`, `A/D`, and `Q/E` to set the signed forward, lateral, and yaw-rate -axes to the lower or upper command bound stored in the artifact; releasing a key removes its contribution, and Esc -interrupts the rollout. Losing viewer focus releases every held key, so typing in another window cannot command the robot. -The deployment viewer does not register MuJoCo's built-in visualization shortcuts, so `W/A/S/D` do not have a second effect. -Drag with the left mouse button to rotate the camera, right-drag to move it, and use the middle button or wheel to zoom. -The built-in interactive keyboard path requires `viewer=true`. - -The standing-probability random binding is task-specific training behavior and is not exposed by the deployment runtime. - -## Physical Go2 Sim2Real - -The hardware plugin imports the Unitree SDK2 Python package only when a physical backend is opened. The normal -workspace sync installs the pinned Motphys-maintained fork into the same environment: - -```bash -sh install.sh --all -``` - -Inspect the artifact first. With the robot suspended, low-level/debug mode enabled, Ethernet connected, and an emergency -stop operator ready, explicitly select the interface and confirm the hardware checklist: - -```bash -motrix-deploy inspect \ - artifact=artifacts/go2-walk-rough.deploy - -motrix-deploy sim2real \ - artifact=artifacts/go2-walk-rough.deploy \ - backend.network_interface=enp3s0 \ - hardware.confirm=true -``` +`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. -`hardware.confirm` defaults to `false`; `viewer=false` and `realtime=true` are mandatory for this backend. Before creating the `LowCmd` publisher, the backend stands down and releases the active MotionSwitcher mode (MCF), then disables the `sport_mode` service through RobotStateClient; any failure aborts startup. `StandDown` is a physical motion that occurs before the Start-button gate, so the area must be clear and an independent emergency stop must be ready. The Unitree remote is the default command source: left-stick Y controls forward velocity, left-stick X lateral velocity, and right-stick X yaw. The default mapping preserves the forward axis and inverts the lateral and yaw axes to match the policy convention. A 0.12 deadzone is applied and L1 is a deadman switch; releasing L1 immediately commands zero, while artifact bounds remain authoritative. The plugin -publishes zero torque until Start, interpolates from the measured joints to the artifact default pose over two seconds, -holds that pose until A, then enables policy commands. B interpolates to the configured backend-owned lie-down pose, then -enters damping and exits with `exit_reason=lie_down`; this does not widen artifact policy limits. Select remains the -higher-priority emergency stop. Select, Ctrl+C, stale state, or a runtime failure enters damping before both DDS channels -are closed. Artifact order `FL, FR, RL, RR` is mapped explicitly -to SDK motor order `FR, FL, RR, RL`. Software and injected-fake-SDK tests cover this path; a suspended real-robot smoke -test is still required before real-world operation. +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). diff --git a/motrix_deploy/pyproject.toml b/motrix_deploy/pyproject.toml index e2dc4711..0eadbb75 100644 --- a/motrix_deploy/pyproject.toml +++ b/motrix_deploy/pyproject.toml @@ -17,4 +17,8 @@ dependencies = [ "motrix-env-core", "numpy>=1.26", "onnxruntime==1.23.2", + "pydantic===2.12.5", ] + +[project.scripts] +motrix-deploy = "motrix_deploy.cli:main" diff --git a/motrix_deploy/src/motrix_deploy/artifact/io.py b/motrix_deploy/src/motrix_deploy/artifact/io.py index db59d713..d06500bf 100644 --- a/motrix_deploy/src/motrix_deploy/artifact/io.py +++ b/motrix_deploy/src/motrix_deploy/artifact/io.py @@ -149,8 +149,8 @@ def inspect_artifact(root: str | Path) -> dict[str, Any]: "joint_names": list(manifest.robot.joint_names), }, "task": manifest.task.to_dict(), - "observation_size": manifest.task.observation_size, - "action_size": manifest.task.action_size, + "observation_size": manifest.policy.input.shape[1], + "action_size": manifest.policy.output.shape[1], "control_period_s": manifest.control.period_s, } diff --git a/motrix_deploy/src/motrix_deploy/artifact/schema.py b/motrix_deploy/src/motrix_deploy/artifact/schema.py index 341046b5..3f64c6ef 100644 --- a/motrix_deploy/src/motrix_deploy/artifact/schema.py +++ b/motrix_deploy/src/motrix_deploy/artifact/schema.py @@ -4,13 +4,16 @@ """Schema v1 for the deployment manifest.""" import re +from abc import ABC from collections.abc import Mapping from dataclasses import dataclass -from typing import Any +from typing import Any, ClassVar import numpy as np +from pydantic import BaseModel, ConfigDict +from pydantic import ValidationError as PydanticValidationError -from motrix_deploy.contracts import RobotSpec, TensorSpec +from motrix_deploy.contracts import JointControlMode, RobotSpec, TensorSpec from motrix_deploy.errors import ValidationError SCHEMA_VERSION = "motrix-deploy/v1" @@ -157,49 +160,73 @@ def _robot_from_dict(value: object) -> RobotSpec: position_upper=np.asarray(data["position_upper"], dtype=np.float32), torque_limit=np.asarray(data["torque_limit"], dtype=np.float32), ) + except ValidationError: + raise except (TypeError, ValueError) as error: - if isinstance(error, ValidationError): - raise raise ValidationError("robot", "numeric arrays matching joint_names", str(error)) from error -@dataclass(frozen=True) -class TaskSpec: - """Versioned task runtime selection and its JSON-compatible configuration.""" +def _task_validation_error(error: PydanticValidationError) -> ValidationError: + """Translate model diagnostics into the deployment artifact field namespace.""" + detail = error.errors(include_url=False)[0] + location = ".".join(str(part) for part in detail["loc"]) + path = f"task.config.{location}" if location else "task.config" + if detail["type"] == "missing": + return ValidationError( + path, "a required task field (re-export artifacts using an older task schema)", "missing" + ) + return ValidationError(path, detail["msg"], detail.get("input")) - name: str - observation_size: int - action_size: int - config: Mapping[str, Any] - def __post_init__(self) -> None: - if not isinstance(self.name, str) or _TASK_NAME_PATTERN.fullmatch(self.name) is None: - raise ValidationError("task.name", "a versioned task identifier such as go2_walk/v1", self.name) - for field_name in ("observation_size", "action_size"): - value = getattr(self, field_name) - if not isinstance(value, int) or isinstance(value, bool) or value <= 0: - raise ValidationError(f"task.{field_name}", "a positive integer", value) - _mapping(self.config, "task.config") +class TaskSpec(BaseModel, ABC): + """Typed task configuration, resolved through an installed versioned task class. + + Concrete implementations declare ``task_name`` as a ClassVar and use + Pydantic fields and validators for configuration semantics. YAML composition + and interpolation resolution belong to the caller, not the artifact codec. + """ + + model_config = ConfigDict(strict=True, extra="forbid", frozen=True, allow_inf_nan=False, validate_default=True) + task_name: ClassVar[str] + + @property + def name(self) -> str: + return self.task_name def to_dict(self) -> dict[str, Any]: - return { - "name": self.name, - "observation_size": self.observation_size, - "action_size": self.action_size, - "config": dict(self.config), - } + # Frozen models may still contain mutable lists. Revalidate a fresh + # mapping so mutations cannot bypass the artifact serialization boundary. + try: + spec = type(self).model_validate(self.model_dump()) + except PydanticValidationError as error: + raise _task_validation_error(error) from error + return {"name": self.name, "config": spec.model_dump(mode="json")} @classmethod def from_dict(cls, value: object) -> "TaskSpec": data = _mapping(value, "task") - required = {"name", "observation_size", "action_size", "config"} - _keys(data, path="task", required=required) - return cls( - name=data["name"], - observation_size=data["observation_size"], - action_size=data["action_size"], - config=data["config"], - ) + _keys(data, path="task", required={"name", "config"}) + name = data["name"] + if not isinstance(name, str) or _TASK_NAME_PATTERN.fullmatch(name) is None: + raise ValidationError("task.name", "a versioned task identifier such as go2_walk/v1", name) + config = _mapping(data["config"], "task.config") + # Lazy import: the runtime contract itself depends on TaskSpec. + from motrix_deploy.task import load_task_type + + try: + spec_type = getattr(load_task_type(name), "spec_type", None) + except Exception as error: + raise ValidationError("task.name", "an available installed task class plugin", str(error)) from error + if ( + not isinstance(spec_type, type) + or not issubclass(spec_type, TaskSpec) + or getattr(spec_type, "task_name", None) != name + ): + raise ValidationError("task.name", "a TaskSpec BaseModel with matching task_name", name) + try: + return spec_type.model_validate(config) + except PydanticValidationError as error: + raise _task_validation_error(error) from error @dataclass(frozen=True) @@ -211,8 +238,13 @@ class ControlSpec: quaternion_order: str = "xyzw" base_orientation: str = "body_to_world" angular_velocity_frame: str = "body" + mode: JointControlMode = JointControlMode.SERVO def __post_init__(self) -> None: + try: + object.__setattr__(self, "mode", JointControlMode(self.mode)) + except (TypeError, ValueError) as error: + raise ValidationError("control.mode", "joint_servo or joint_torque", self.mode) from error _validate_positive_number(self.period_s, "control.period_s") _validate_positive_number(self.state_timeout_s, "control.state_timeout_s") if self.quaternion_order != "xyzw": @@ -224,6 +256,7 @@ def __post_init__(self) -> None: def to_dict(self) -> dict[str, Any]: return { + "mode": self.mode.value, "period_s": self.period_s, "state_timeout_s": self.state_timeout_s, "quaternion_order": self.quaternion_order, @@ -235,6 +268,7 @@ def to_dict(self) -> dict[str, Any]: def from_dict(cls, value: object) -> "ControlSpec": data = _mapping(value, "control") required = { + "mode", "period_s", "state_timeout_s", "quaternion_order", @@ -259,14 +293,10 @@ class DeploymentManifest: def __post_init__(self) -> None: if self.schema_version != SCHEMA_VERSION: raise ValidationError("schema_version", SCHEMA_VERSION, self.schema_version) - if self.policy.input.shape != (1, self.task.observation_size): - raise ValidationError( - "policy.input.shape", - str((1, self.task.observation_size)), - self.policy.input.shape, - ) - if self.policy.output.shape != (1, self.task.action_size): - raise ValidationError("policy.output.shape", str((1, self.task.action_size)), self.policy.output.shape) + for field_name in ("input", "output"): + shape = getattr(self.policy, field_name).shape + if len(shape) != 2 or shape[0] != 1: + raise ValidationError(f"policy.{field_name}.shape", "(1, size): a single batch-1 vector", shape) def to_dict(self) -> dict[str, Any]: return { diff --git a/motrix_deploy/src/motrix_deploy/backend/__init__.py b/motrix_deploy/src/motrix_deploy/backend/__init__.py deleted file mode 100644 index ec378928..00000000 --- a/motrix_deploy/src/motrix_deploy/backend/__init__.py +++ /dev/null @@ -1,128 +0,0 @@ -# Copyright Motphys Technology Co., Ltd. 2025, 2026 -# SPDX-License-Identifier: Apache-2.0 - -"""Robot backend lifecycle contract.""" - -from abc import ABC, abstractmethod -from collections.abc import Callable, Mapping -from dataclasses import dataclass -from importlib import metadata -from typing import Any, Protocol, runtime_checkable - -from motrix_deploy.artifact import ControlSpec -from motrix_deploy.contracts import HealthStatus, RobotCapabilities, RobotCommand, RobotSpec, RobotState -from motrix_env_core.input import GamePadDevice, KeyboardDevice - - -class RobotInterface(ABC): - """Backend boundary shared by simulators and physical robots.""" - - @property - @abstractmethod - def capabilities(self) -> RobotCapabilities: - """Return immutable capabilities used during startup negotiation.""" - - @abstractmethod - def open(self, spec: RobotSpec) -> None: - """Open resources and bind backend joints to the canonical spec.""" - - def enable(self, initial_command: RobotCommand) -> None: - """Explicitly enable command output when required by the backend.""" - del initial_command - if self.capabilities.requires_enable: - raise NotImplementedError("backend declares requires_enable=True but does not implement enable()") - - @abstractmethod - def read_state(self, timeout_s: float) -> RobotState: - """Read one state sample or raise on timeout.""" - - @abstractmethod - def write_command(self, command: RobotCommand) -> None: - """Apply one canonical command.""" - - @abstractmethod - def health(self) -> HealthStatus: - """Return current backend health.""" - - @abstractmethod - def stop(self) -> None: - """Enter the backend's declared safe stopped state; must be idempotent.""" - - @abstractmethod - def close(self) -> None: - """Release resources; must be idempotent.""" - - -@runtime_checkable -class KeyboardDeviceProvider(Protocol): - """Optional backend capability that supplies a focused keyboard device.""" - - def get_keyboard_device(self) -> KeyboardDevice: - """Return a device whose lifecycle is owned by the backend.""" - - -@runtime_checkable -class GamePadDeviceProvider(Protocol): - """Optional backend capability that supplies a gamepad-like device.""" - - def get_gamepad_device(self) -> GamePadDevice: - """Return a device whose lifecycle is owned by the backend.""" - - -@dataclass(frozen=True) -class BackendCreateContext: - """Runtime values supplied by the deployment core when constructing a backend.""" - - control: ControlSpec - viewer: bool - realtime: bool = False - hardware_confirmed: bool = False - - -BackendFactory = Callable[[Mapping[str, Any], BackendCreateContext], RobotInterface] -BACKEND_ENTRY_POINT_GROUP = "motrix_deploy.backends" - - -def _backend_entry_points() -> tuple[metadata.EntryPoint, ...]: - return tuple(metadata.entry_points(group=BACKEND_ENTRY_POINT_GROUP)) - - -def registered_backends() -> tuple[str, ...]: - """Return backend names advertised by installed plugin distributions.""" - return tuple(sorted(entry_point.name for entry_point in _backend_entry_points())) - - -def create_backend( - name: str, - config: Mapping[str, Any], - context: BackendCreateContext, -) -> RobotInterface: - """Discover and construct one installed backend plugin by name.""" - matches = [entry_point for entry_point in _backend_entry_points() if entry_point.name == name] - if not matches: - available = ", ".join(registered_backends()) or "none" - raise ValueError(f"Unsupported deployment backend {name!r}; installed backends: {available}") - if len(matches) != 1: - values = sorted(entry_point.value for entry_point in matches) - raise ValueError(f"Multiple deployment backend plugins are registered as {name!r}: {values}") - factory = matches[0].load() - if not callable(factory): - raise TypeError(f"Deployment backend plugin {name!r} must load a callable factory") - backend = factory(config, context) - if not isinstance(backend, RobotInterface): - raise TypeError( - f"Deployment backend plugin {name!r} returned {type(backend).__name__}, expected RobotInterface" - ) - return backend - - -__all__ = [ - "BACKEND_ENTRY_POINT_GROUP", - "BackendCreateContext", - "BackendFactory", - "GamePadDeviceProvider", - "KeyboardDeviceProvider", - "RobotInterface", - "create_backend", - "registered_backends", -] diff --git a/motrix_deploy/src/motrix_deploy/cli.py b/motrix_deploy/src/motrix_deploy/cli.py index 07fc4d92..f1e1bd39 100644 --- a/motrix_deploy/src/motrix_deploy/cli.py +++ b/motrix_deploy/src/motrix_deploy/cli.py @@ -15,26 +15,19 @@ from omegaconf.errors import OmegaConfBaseException from motrix_deploy import __version__ -from motrix_deploy.artifact import inspect_artifact, read_artifact -from motrix_deploy.backend import ( - BackendCreateContext, - GamePadDeviceProvider, - KeyboardDeviceProvider, - create_backend, -) +from motrix_deploy.artifact.io import inspect_artifact, read_artifact from motrix_deploy.config import DeployRunConfig from motrix_deploy.errors import ArtifactError, ValidationError +from motrix_deploy.robot.interface import GamePadDeviceProvider, KeyboardDeviceProvider +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_deploy.runtime.factory import RuntimeCreateContext, create_hardware_runtime, create_simulation_runtime from motrix_deploy.task import create_task def run(cfg: DeployRunConfig) -> int: """Run one deployment rollout from typed application configuration.""" from motrix_deploy.policy import OnnxPolicyRuntime - from motrix_deploy.runtime import ( - ControlLoop, - FixedStepScheduler, - RealtimeScheduler, - ) + from motrix_deploy.runtime.control import ControlSession from motrix_env_core.input import ( BoundedGamePadPlanarVelocityBinding, ConstantPlanarVelocityBinding, @@ -43,29 +36,55 @@ def run(cfg: DeployRunConfig) -> int: artifact = read_artifact(Path(cfg.artifact)) manifest = artifact.manifest + steps = _duration_steps(cfg.duration_s, manifest.control.period_s) task = create_task(manifest.task, manifest.robot) - viewer = cfg.viewer - realtime = viewer if cfg.realtime is None else cfg.realtime - hardware_confirmed = cfg.hardware.get("confirm", False) - backend = create_backend( - cfg.backend_name, - cfg.backend_options, - BackendCreateContext( - control=manifest.control, - viewer=viewer, - realtime=realtime, - hardware_confirmed=hardware_confirmed, - ), - ) + simulation = cfg.runtime["kind"] == "simulation" + viewer = cfg.runtime["viewer"] if simulation else False + realtime = cfg.runtime["realtime"] if simulation else True + if realtime is None: + realtime = viewer + runtime_options = cfg.runtime_options + if simulation: + from motrix_deploy.env import assemble_deploy_scene + + for field in ("deploy_env_id", "robot_id"): + if not isinstance(runtime_options.get(field), str) or not runtime_options[field]: + raise ValidationError(f"runtime.{field}", "a non-empty registered ID", runtime_options.get(field)) + deploy_env_id = runtime_options.pop("deploy_env_id") + robot_id = runtime_options.pop("robot_id") + scene = assemble_deploy_scene(deploy_env_id, robot_id) + for field in ("translation", "rotation"): + option = f"robot_{field}" + if option in runtime_options: + value = runtime_options.pop(option) + setattr(scene.objs.robot, field, None if value is None else tuple(value)) + runtime_options["scene"] = scene + runtime_config = SimulationRuntimeConfig.from_mapping(runtime_options, render=viewer, realtime=realtime) + runtime = create_simulation_runtime(cfg.backend_name, runtime_config) + else: + runtime = create_hardware_runtime( + cfg.backend_name, + runtime_options, + RuntimeCreateContext( + robot=manifest.robot, + control=manifest.control, + ), + ) + backend = runtime.robot + try: + manifest.robot.validate_compatible(backend.spec) + except ValidationError: + runtime.close() + raise command_lower = getattr(task, "command_lower", None) command_upper = getattr(task, "command_upper", None) if command_lower is None or command_upper is None: raise ValueError(f"Task {manifest.task.name!r} does not provide planar velocity command bounds") command_config = cfg.command or {} command_source = command_config.get("source", "constant") - if viewer and isinstance(backend, KeyboardDeviceProvider): + if viewer and isinstance(runtime, KeyboardDeviceProvider): command_binding = KeyboardPlanarVelocityBinding( - backend.get_keyboard_device(), + runtime.get_keyboard_device(), command_lower=command_lower, command_upper=command_upper, ) @@ -94,44 +113,33 @@ def run(cfg: DeployRunConfig) -> int: task.validate_command(command_binding.read_command()) else: raise ValueError(f"Unsupported command source {command_source!r}; use gamepad or configure command.velocity") - loop = ControlLoop( - robot=manifest.robot, - backend=backend, + control = ControlSession( + robot=backend, task=task, policy=OnnxPolicyRuntime(artifact.policy_path, manifest.policy.input, manifest.policy.output), command_binding=command_binding, - scheduler=( - RealtimeScheduler(manifest.control.period_s) if realtime else FixedStepScheduler(manifest.control.period_s) - ), + period_s=manifest.control.period_s, state_timeout_s=manifest.control.state_timeout_s, ) - result = loop.run(steps=_rollout_steps(cfg.rollout, manifest.control.period_s)) + runtime.bind_control_session(control) + with runtime: + result = runtime.run(steps=steps) print(json.dumps(result.to_dict(), indent=2, sort_keys=True)) return 0 if result.success else 1 -def _rollout_steps(cfg: dict[str, object] | None, control_period_s: float) -> int | None: - if cfg is None: +def _duration_steps(duration_s: float | None, control_period_s: float) -> int | None: + """Convert a control-time duration to a tick budget, not a wall-clock timeout.""" + if duration_s is None: return None - steps = cfg.get("steps") - duration_s = cfg.get("duration_s") - if steps is None and duration_s is None: - return None - if steps is not None and duration_s is not None: - raise ValueError("Exactly one of rollout.steps and rollout.duration_s must be configured") - if steps is not None: - if isinstance(steps, bool) or not isinstance(steps, int) or steps <= 0: - raise ValueError(f"rollout.steps must be a positive integer, got {steps!r}") - return steps if isinstance(duration_s, bool) or not isinstance(duration_s, (int, float)): - raise ValueError(f"rollout.duration_s must be a positive finite number, got {duration_s!r}") + raise ValueError(f"duration_s must be a positive finite number, got {duration_s!r}") if not np.isfinite(duration_s) or duration_s <= 0: - raise ValueError(f"rollout.duration_s must be a positive finite number, got {duration_s!r}") + raise ValueError(f"duration_s must be a positive finite number, got {duration_s!r}") return math.ceil(duration_s / control_period_s) -@hydra.main(version_base=None, config_path="config", config_name="deploy") -def _hydra_main(cfg: DictConfig) -> None: +def _run_config(cfg: DictConfig) -> None: try: typed_cfg = OmegaConf.to_object(OmegaConf.merge(OmegaConf.structured(DeployRunConfig), cfg)) if not isinstance(typed_cfg, DeployRunConfig): @@ -144,12 +152,15 @@ def _hydra_main(cfg: DictConfig) -> None: raise SystemExit(exit_code) +_hydra_main = hydra.main(version_base=None, config_path="pkg://motrix_deploy.config", config_name="deploy")(_run_config) + + def inspect(cfg: DictConfig) -> None: """Validate one deployment artifact without opening a backend.""" print(json.dumps(inspect_artifact(Path(cfg.artifact)), indent=2, sort_keys=True)) -@hydra.main(version_base=None, config_path="config", config_name="inspect") +@hydra.main(version_base=None, config_path="pkg://motrix_deploy.config", config_name="inspect") def _hydra_inspect_main(cfg: DictConfig) -> None: try: inspect(cfg) @@ -178,8 +189,6 @@ def main(argv: Sequence[str] | None = None) -> int | None: return 0 if not args: sys.argv.append("--help") - elif args[:1] in (["sim2sim"], ["sim2real"]): - del sys.argv[1] _hydra_main() return None diff --git a/motrix_deploy/src/motrix_deploy/config/__init__.py b/motrix_deploy/src/motrix_deploy/config/__init__.py index bea3496e..311eef32 100644 --- a/motrix_deploy/src/motrix_deploy/config/__init__.py +++ b/motrix_deploy/src/motrix_deploy/config/__init__.py @@ -3,44 +3,45 @@ """Typed application configuration for the deployment CLI.""" -from dataclasses import dataclass, field +from dataclasses import dataclass from typing import Any @dataclass class DeployRunConfig: - """Resolved, task-agnostic configuration consumed by one deployment run.""" + """Resolved execution settings with one selected runtime configuration.""" artifact: str - backend: dict[str, Any] - viewer: bool + runtime: dict[str, Any] + duration_s: float | None = None command: dict[str, Any] | None = None - rollout: dict[str, Any] | None = None - seed: int | None = None - realtime: bool | None = None - hardware: dict[str, Any] = field(default_factory=dict) def __post_init__(self) -> None: if not self.artifact: raise ValueError("artifact must be a non-empty path") - if not isinstance(self.viewer, bool): - raise ValueError(f"viewer must be bool, got {self.viewer!r}") - if self.realtime is not None and not isinstance(self.realtime, bool): - raise ValueError(f"realtime must be bool or null, got {self.realtime!r}") - confirmed = self.hardware.get("confirm", False) - if not isinstance(confirmed, bool): - raise ValueError(f"hardware.confirm must be bool, got {confirmed!r}") - backend_name = self.backend.get("name") + kind = self.runtime.get("kind") + if kind not in {"simulation", "hardware"}: + raise ValueError(f"runtime.kind must be simulation or hardware, got {kind!r}") + if kind == "simulation": + if not isinstance(self.runtime.get("viewer"), bool): + raise ValueError("runtime.viewer must be bool") + realtime = self.runtime.get("realtime") + if realtime is not None and not isinstance(realtime, bool): + raise ValueError("runtime.realtime must be bool or null") + backend_name = self.runtime.get("backend") if not isinstance(backend_name, str) or not backend_name: - raise ValueError(f"backend.name must be a non-empty string, got {backend_name!r}") + raise ValueError(f"runtime.backend must be a non-empty string, got {backend_name!r}") @property def backend_name(self) -> str: - return self.backend["name"] + return self.runtime["backend"] @property - def backend_options(self) -> dict[str, Any]: - return {name: value for name, value in self.backend.items() if name != "name"} + def runtime_options(self) -> dict[str, Any]: + execution_fields = {"kind", "backend"} + if self.runtime["kind"] == "simulation": + execution_fields |= {"viewer", "realtime"} + return {name: value for name, value in self.runtime.items() if name not in execution_fields} __all__ = ["DeployRunConfig"] diff --git a/motrix_deploy/src/motrix_deploy/config/deploy.yaml b/motrix_deploy/src/motrix_deploy/config/deploy.yaml index da9b118c..fa7154df 100644 --- a/motrix_deploy/src/motrix_deploy/config/deploy.yaml +++ b/motrix_deploy/src/motrix_deploy/config/deploy.yaml @@ -1,5 +1,6 @@ defaults: - _self_ + - task: null - override hydra/job_logging: stdout hydra: @@ -13,5 +14,6 @@ hydra: chdir: false artifact: ??? -backend: ??? -viewer: ??? +runtime: ??? +duration_s: null +command: null diff --git a/motrix_deploy/src/motrix_deploy/config/runtime/hardware.yaml b/motrix_deploy/src/motrix_deploy/config/runtime/hardware.yaml new file mode 100644 index 00000000..4684c816 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/config/runtime/hardware.yaml @@ -0,0 +1,2 @@ +kind: hardware +backend: ??? diff --git a/motrix_deploy/src/motrix_deploy/config/runtime/sim.yaml b/motrix_deploy/src/motrix_deploy/config/runtime/sim.yaml new file mode 100644 index 00000000..a0ea0f6d --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/config/runtime/sim.yaml @@ -0,0 +1,4 @@ +kind: simulation +backend: ??? +viewer: true +realtime: null diff --git a/motrix_deploy/src/motrix_deploy/contracts.py b/motrix_deploy/src/motrix_deploy/contracts.py index 9b02f25b..6f4519f0 100644 --- a/motrix_deploy/src/motrix_deploy/contracts.py +++ b/motrix_deploy/src/motrix_deploy/contracts.py @@ -5,6 +5,8 @@ from collections.abc import Mapping from dataclasses import dataclass, field +from enum import Enum +from typing import TypeAlias import numpy as np from numpy.typing import NDArray @@ -99,6 +101,19 @@ def __post_init__(self) -> None: if np.any(self.torque_limit <= 0): raise ValidationError("robot.torque_limit", "positive values", self.torque_limit.tolist()) + def validate_compatible(self, actual: "RobotSpec") -> None: + """Require a runtime robot to match this policy/task contract in canonical order.""" + for name in ("base_link_name", "joint_names"): + expected_value = getattr(self, name) + actual_value = getattr(actual, name) + if expected_value != actual_value: + raise ValidationError(f"robot.{name}", expected_value, actual_value) + for name in ("default_joint_position", "position_lower", "position_upper", "torque_limit"): + expected_value = getattr(self, name) + actual_value = getattr(actual, name) + if not np.allclose(expected_value, actual_value, atol=1e-6, rtol=0.0): + raise ValidationError(f"robot.{name}", expected_value.tolist(), actual_value.tolist()) + @property def joint_count(self) -> int: return len(self.joint_names) @@ -170,9 +185,16 @@ def __post_init__(self) -> None: _validate_float32_array(value, path=f"state.extras.{name}", shape=value.shape) +class JointControlMode(str, Enum): + """Canonical joint command modes shared by artifacts and backends.""" + + SERVO = "joint_servo" + TORQUE = "joint_torque" + + @dataclass -class RobotCommand: - """Canonical joint PD plus feed-forward command in SI units.""" +class JointServoCommand: + """Canonical joint servo plus feed-forward command in SI units.""" joint_position: FloatArray joint_velocity: FloatArray @@ -203,14 +225,40 @@ def __post_init__(self) -> None: if np.any(self.kp < 0) or np.any(self.kd < 0): raise ValidationError("command.gains", "non-negative values", "negative gain") + @property + def mode(self) -> JointControlMode: + return JointControlMode.SERVO + + +@dataclass +class JointTorqueCommand: + """Canonical direct joint torque command in SI units.""" + + torque: FloatArray + + def __post_init__(self) -> None: + if not isinstance(self.torque, np.ndarray) or self.torque.ndim != 1 or not self.torque.size: + actual = self.torque.shape if isinstance(self.torque, np.ndarray) else type(self.torque) + raise ValidationError("command.torque.shape", "(joint_count,)", actual) + _validate_float32_array(self.torque, path="command.torque", shape=(self.torque.size,)) + + @property + def mode(self) -> JointControlMode: + return JointControlMode.TORQUE + + +RobotCommand: TypeAlias = JointServoCommand | JointTorqueCommand + @dataclass(frozen=True) class RobotCapabilities: """Static backend capabilities negotiated before control starts. Attributes: - control_modes: Canonical command modes accepted by the backend, such as ``joint_pd``. + control_modes: Canonical joint command modes accepted by the backend. state_fields: ``RobotState`` fields populated by every state sample. + privileged_state_fields: Subset of state_fields supplied as privileged simulator ground truth, + not measurements guaranteed to be available on hardware. extra_sensors: Additional entries available in ``RobotState.extras``, mapped to their fixed shapes. supports_rendering: Whether the backend can present a live viewer. max_command_rate_hz: Highest command update rate accepted by the backend, or ``None`` when unconstrained. @@ -218,20 +266,37 @@ class RobotCapabilities: stop_semantics: Backend behavior after ``RobotInterface.stop()``, for example ``hold_position`` or ``damping``. """ - control_modes: tuple[str, ...] + control_modes: tuple[JointControlMode, ...] state_fields: frozenset[str] extra_sensors: Mapping[str, tuple[int, ...]] = field(default_factory=dict) supports_rendering: bool = False max_command_rate_hz: float | None = None requires_enable: bool = False stop_semantics: str = "hold_position" + privileged_state_fields: frozenset[str] = frozenset() def __post_init__(self) -> None: _validate_names(self.control_modes, path="capabilities.control_modes") + try: + modes = tuple(JointControlMode(mode) for mode in self.control_modes) + except ValueError as error: + raise ValidationError( + "capabilities.control_modes", "joint_servo or joint_torque", self.control_modes + ) from error + object.__setattr__(self, "control_modes", modes) if not isinstance(self.state_fields, frozenset) or any( not isinstance(name, str) or not name for name in self.state_fields ): raise ValidationError("capabilities.state_fields", "a frozenset of non-empty names", self.state_fields) + if ( + not isinstance(self.privileged_state_fields, frozenset) + or not self.privileged_state_fields <= self.state_fields + ): + raise ValidationError( + "capabilities.privileged_state_fields", + "a frozenset subset of state_fields", + self.privileged_state_fields, + ) if self.max_command_rate_hz is not None and self.max_command_rate_hz <= 0: raise ValidationError("capabilities.max_command_rate_hz", "a positive value", self.max_command_rate_hz) if not self.stop_semantics: @@ -245,7 +310,10 @@ def __post_init__(self) -> None: @dataclass(frozen=True) class HealthStatus: - """Backend health with a reason and last successful communication time.""" + """Backend resource/communication health, not task success or fall detection. + + Task-owned termination is evaluated separately by DeployTask.check_termination. + """ healthy: bool reason: str diff --git a/motrix_deploy/src/motrix_deploy/env.py b/motrix_deploy/src/motrix_deploy/env.py new file mode 100644 index 00000000..2c4f78e7 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/env.py @@ -0,0 +1,40 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Installed robot-independent deployment world factories.""" + +from importlib import metadata + +from motrix_env_core import registry +from motrix_env_core.config.scene import SceneCfg + +DEPLOY_ENV_ENTRY_POINT_GROUP = "motrix_deploy.envs" + + +def available_deploy_envs() -> tuple[str, ...]: + """List installed deployment environment IDs without loading factories.""" + return tuple(sorted({entry.name for entry in metadata.entry_points(group=DEPLOY_ENV_ENTRY_POINT_GROUP)})) + + +def create_deploy_env(deploy_env_id: str) -> SceneCfg: + """Create a fresh robot-free scene from one installed environment factory.""" + entries = tuple(metadata.entry_points(group=DEPLOY_ENV_ENTRY_POINT_GROUP)) + matches = [entry for entry in entries if entry.name == deploy_env_id] + if not matches: + available = ", ".join(sorted({entry.name for entry in entries})) or "none" + raise ValueError(f"Unknown deployment environment {deploy_env_id!r}; available environments: {available}") + if len(matches) != 1: + raise ValueError(f"Multiple deployment environment plugins provide {deploy_env_id!r}") + scene = matches[0].load()() + if not isinstance(scene, SceneCfg): + raise TypeError(f"Deployment environment {deploy_env_id!r} must return SceneCfg, got {type(scene).__name__}") + if scene.objs.robot is not None: + raise ValueError(f"Deployment environment {deploy_env_id!r} must leave scene.objs.robot empty") + return scene + + +def assemble_deploy_scene(deploy_env_id: str, robot_id: str) -> SceneCfg: + """Assemble an independently selected world and registered robot for deployment.""" + scene = create_deploy_env(deploy_env_id) + scene.objs.robot = registry.make_robot_config(robot_id) + return scene diff --git a/motrix_deploy/src/motrix_deploy/policy/__init__.py b/motrix_deploy/src/motrix_deploy/policy/__init__.py index fdb858d7..7d91d05a 100644 --- a/motrix_deploy/src/motrix_deploy/policy/__init__.py +++ b/motrix_deploy/src/motrix_deploy/policy/__init__.py @@ -14,26 +14,37 @@ class PolicyRuntime(ABC): - """Framework-independent deterministic policy inference.""" + """Decision runtime for a task's vector observation/action interface. - @property - @abstractmethod - def input_spec(self) -> TensorSpec: - """Return the policy input contract.""" - - @property - @abstractmethod - def output_spec(self) -> TensorSpec: - """Return the policy output contract.""" + Implementations may use networks, planners, or fixed decisions. Tensor names + and dimensions are implementation details, not requirements of this contract. + """ def reset(self) -> None: - """Reset recurrent state; v1 policies are stateless.""" + """Reset decision state before a run; stateless implementations do nothing.""" @abstractmethod def infer(self, observation: FloatArray) -> FloatArray: """Infer one unbatched action.""" +class NoOpPolicyRuntime(PolicyRuntime): + """Ignore observations and return zero actions; the task defines their meaning. + + A zero action does not imply zero torque or skipping a command. For example, + a pose-offset task interprets it as holding its reference pose. + """ + + def __init__(self, action_size: int) -> None: + if isinstance(action_size, bool) or not isinstance(action_size, int) or action_size < 0: + raise ValidationError("policy.noop.action_size", "a non-negative integer", action_size) + self._action = np.zeros(action_size, dtype=np.float32) + + def infer(self, observation: FloatArray) -> FloatArray: + del observation + return self._action.copy() + + class OnnxPolicyRuntime(PolicyRuntime): """Single-input, single-output ONNX Runtime policy.""" @@ -100,4 +111,4 @@ def infer(self, observation: FloatArray) -> FloatArray: return float32_array(output[0], path="policy.output", shape=(self.output_spec.shape[1],)) -__all__ = ["OnnxPolicyRuntime", "PolicyRuntime"] +__all__ = ["NoOpPolicyRuntime", "OnnxPolicyRuntime", "PolicyRuntime"] diff --git a/motrix_deploy/src/motrix_deploy/profile.py b/motrix_deploy/src/motrix_deploy/profile.py index e3c60867..df68d774 100644 --- a/motrix_deploy/src/motrix_deploy/profile.py +++ b/motrix_deploy/src/motrix_deploy/profile.py @@ -6,7 +6,7 @@ from collections.abc import Callable from dataclasses import dataclass -from motrix_deploy.artifact import ControlSpec, TaskSpec +from motrix_deploy.artifact.schema import ControlSpec, TaskSpec from motrix_deploy.contracts import RobotSpec @@ -17,6 +17,9 @@ class DeploymentProfile: robot: RobotSpec task: TaskSpec control: ControlSpec + # Training-space dimensions used only to validate ONNX export, not serialized in TaskSpec. + observation_size: int + action_size: int ProfileCompiler = Callable[[str], DeploymentProfile] diff --git a/motrix_deploy/src/motrix_deploy/robot/__init__.py b/motrix_deploy/src/motrix_deploy/robot/__init__.py new file mode 100644 index 00000000..7981cefe --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/robot/__init__.py @@ -0,0 +1,4 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot I/O contracts and adapters.""" diff --git a/motrix_deploy/src/motrix_deploy/robot/interface.py b/motrix_deploy/src/motrix_deploy/robot/interface.py new file mode 100644 index 00000000..725e0737 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/robot/interface.py @@ -0,0 +1,72 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Public robot I/O and device capability contracts.""" + +from abc import ABC, abstractmethod +from typing import Protocol, runtime_checkable + +from motrix_deploy.contracts import HealthStatus, RobotCapabilities, RobotCommand, RobotSpec, RobotState +from motrix_env_core.input import GamePadDevice, KeyboardDevice + + +class RobotInterface(ABC): + """Robot boundary shared by simulation and hardware runtimes.""" + + @property + @abstractmethod + def spec(self) -> RobotSpec: + """Return the canonical robot specification bound to this interface.""" + + @property + @abstractmethod + def capabilities(self) -> RobotCapabilities: + """Return immutable capabilities used during startup negotiation.""" + + @abstractmethod + def open(self) -> None: + """Open resources for the robot specification supplied at construction.""" + + def enable(self) -> None: + """Enable command output using backend-owned startup safety behavior.""" + if self.capabilities.requires_enable: + raise NotImplementedError("robot declares requires_enable=True but does not implement enable()") + + @abstractmethod + def read_state(self, timeout_s: float) -> RobotState: + """Read one state sample or raise on timeout.""" + + @abstractmethod + def write_command(self, command: RobotCommand) -> None: + """Apply one canonical command.""" + + @abstractmethod + def health(self) -> HealthStatus: + """Return current robot I/O health.""" + + @abstractmethod + def stop(self) -> None: + """Enter the declared safe stopped state; must be idempotent.""" + + @abstractmethod + def close(self) -> None: + """Release robot I/O resources; must be idempotent.""" + + +@runtime_checkable +class KeyboardDeviceProvider(Protocol): + """Optional runtime capability supplying keyboard input.""" + + def get_keyboard_device(self) -> KeyboardDevice: + """Return a device whose lifecycle is owned by its provider.""" + + +@runtime_checkable +class GamePadDeviceProvider(Protocol): + """Optional robot capability supplying gamepad input.""" + + def get_gamepad_device(self) -> GamePadDevice: + """Return a device whose lifecycle is owned by its provider.""" + + +__all__ = ["GamePadDeviceProvider", "KeyboardDeviceProvider", "RobotInterface"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/__init__.py b/motrix_deploy/src/motrix_deploy/runtime/__init__.py index 401144a3..560dddbf 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/__init__.py +++ b/motrix_deploy/src/motrix_deploy/runtime/__init__.py @@ -1,26 +1,36 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Unified policy deployment runtime.""" +"""Unified policy deployment runtime public API.""" +from motrix_deploy.runtime.base import DeploymentRuntime, SimulationRuntime +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig from motrix_deploy.runtime.context import PolicyContext +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.factory import ( + RuntimeCreateContext, + create_hardware_runtime, + create_simulation_runtime, + registered_runtimes, +) +from motrix_deploy.runtime.hardware import HardwareRuntime from motrix_deploy.runtime.result import RolloutResult from motrix_deploy.runtime.scheduler import FixedStepScheduler, LoopScheduler, RealtimeScheduler - -def __getattr__(name: str): - if name == "ControlLoop": - from motrix_deploy.runtime.loop import ControlLoop - - return ControlLoop - raise AttributeError(name) - - __all__ = [ - "ControlLoop", + "ControlSession", + "DeploymentRuntime", "FixedStepScheduler", + "HardwareRuntime", "LoopScheduler", "PolicyContext", "RealtimeScheduler", "RolloutResult", + "SensorBindings", + "SimulationRuntimeConfig", + "SimulationRuntime", + "RuntimeCreateContext", + "create_simulation_runtime", + "create_hardware_runtime", + "registered_runtimes", ] diff --git a/motrix_deploy/src/motrix_deploy/runtime/base.py b/motrix_deploy/src/motrix_deploy/runtime/base.py new file mode 100644 index 00000000..3fafaef4 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/base.py @@ -0,0 +1,83 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Deployment runtime lifecycle contracts.""" + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING + +from motrix_deploy.errors import ValidationError + +if TYPE_CHECKING: + from motrix_deploy.robot.interface import RobotInterface + from motrix_deploy.runtime.control import ControlSession + from motrix_deploy.runtime.result import RolloutResult + + +class DeploymentRuntime(ABC): + """Own runtime resources while a control session owns the robot backend.""" + + def __init__(self) -> None: + self._opened = False + self._control: ControlSession | None = None + + @property + def control(self) -> "ControlSession | None": + """Control session bound to this runtime, if any.""" + return self._control + + def bind_control_session(self, control: "ControlSession") -> None: + """Bind one control session, rejecting conflicting robot ownership.""" + if control.robot is not self.robot: + raise ValidationError("runtime.control.robot", self.robot, control.robot) + if self._control is not None and self._control is not control: + raise ValidationError("runtime.control", "the already bound control session", control) + self._control = control + + def _resolve_control(self, control: "ControlSession | None") -> "ControlSession": + if control is not None and self.control is not None and control is not self.control: + raise ValidationError("runtime.control", "the already bound control session", control) + resolved = control if control is not None else self.control + if resolved is None: + raise ValidationError("runtime.control", "a supplied or bound control session", None) + if resolved.robot is not self.robot: + raise ValidationError("runtime.robot", "the control session robot", resolved.robot) + return resolved + + @property + @abstractmethod + def robot(self) -> "RobotInterface": + """Robot interface associated with this runtime.""" + raise NotImplementedError + + @property + def opened(self) -> bool: + """Whether this runtime lifecycle is open.""" + return self._opened + + def open(self) -> None: + """Open runtime resources; safe to call repeatedly.""" + self._opened = True + + def close(self) -> None: + """Close runtime resources; safe to call repeatedly.""" + self._opened = False + + def __enter__(self) -> "DeploymentRuntime": + self.open() + return self + + def __exit__(self, exc_type, exc_value, traceback) -> None: + del exc_type, exc_value, traceback + self.close() + + @abstractmethod + def run(self, control: "ControlSession | None" = None, *, steps: int | None = None) -> "RolloutResult": + """Run a control session and return its structured result.""" + + +class SimulationRuntime(DeploymentRuntime, ABC): + """Base for runtimes that own simulation advancement and robot creation.""" + + +__all__ = ["DeploymentRuntime", "SimulationRuntime"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/config.py b/motrix_deploy/src/motrix_deploy/runtime/config.py new file mode 100644 index 00000000..7ec3f0f9 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/config.py @@ -0,0 +1,76 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Simulator-independent deployment scene and state-source configuration.""" + +from collections.abc import Mapping +from dataclasses import dataclass, field +from typing import Any + +from motrix_deploy.errors import ValidationError +from motrix_env_core.config.scene import SceneCfg +from motrix_env_core.config.sim import SimCfg + + +@dataclass(frozen=True) +class SensorBindings: + """Semantic robot state fields bound to exact model-local sensor names.""" + + base_angular_velocity: str | None = None + base_linear_acceleration: str | None = None + base_linear_velocity: str | None = None + + def __post_init__(self) -> None: + for field_name in ( + "base_angular_velocity", + "base_linear_acceleration", + "base_linear_velocity", + ): + name = getattr(self, field_name) + if name is None: + continue + if not isinstance(name, str): + raise TypeError(f"{field_name} must be a string or None") + if not name.strip(): + raise ValueError(f"{field_name} must be a non-empty sensor name") + + +@dataclass(frozen=True) +class SimulationRuntimeConfig: + scene: SceneCfg + physics: SimCfg = field(default_factory=lambda: SimCfg(dt=0.002, solver_iterations=100)) + render: bool = False + realtime: bool | None = None + # Deployment recipe: semantic RobotState fields mapped to model-local sensor names. + sensor_bindings: SensorBindings = field(default_factory=SensorBindings) + + def __post_init__(self) -> None: + self.physics.validate() + + @classmethod + def from_mapping( + cls, value: Mapping[str, Any], *, render: bool = False, realtime: bool | None = None + ) -> "SimulationRuntimeConfig": + """Decode an assembled simulation recipe at the application boundary.""" + values = dict(value) + values.pop("name", None) + values.setdefault("sensor_bindings", {}) + expected = {"scene", "physics", "sensor_bindings"} + if set(values) != expected: + raise ValidationError( + "runtime", + f"fields {sorted(expected)}", + f"missing={sorted(expected - set(values))}, unknown={sorted(set(values) - expected)}", + ) + if not isinstance(values["scene"], SceneCfg): + raise ValidationError("runtime.scene", "a complete SceneCfg", values["scene"]) + missing = {"dt", "solver_iterations"} - set(values["physics"]) + if missing: + raise ValidationError("runtime.physics", "dt and solver_iterations", f"missing={sorted(missing)}") + return cls( + scene=values["scene"], + physics=SimCfg(**values["physics"]), + sensor_bindings=SensorBindings(**values["sensor_bindings"]), + render=render, + realtime=realtime, + ) diff --git a/motrix_deploy/src/motrix_deploy/runtime/control.py b/motrix_deploy/src/motrix_deploy/runtime/control.py new file mode 100644 index 00000000..0b113e92 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/control.py @@ -0,0 +1,294 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Backend-neutral control executor, independent of scheduling and physics.""" + +import hashlib +import time +from typing import TYPE_CHECKING, Generic, TypeVar + +from motrix_deploy.contracts import JointServoCommand, RobotCommand, RobotState +from motrix_deploy.errors import EmergencyStopError, LieDownRequestedError, ValidationError +from motrix_deploy.policy import PolicyRuntime +from motrix_deploy.robot.interface import RobotInterface +from motrix_deploy.runtime.context import PolicyContext +from motrix_deploy.runtime.result import LatencyRecorder, RolloutResult +from motrix_env_core.input import CommandBinding + +if TYPE_CHECKING: + from motrix_deploy.task import DeployTask + +CommandT = TypeVar("CommandT") + + +class _LoopFailure(RuntimeError): + def __init__(self, reason: str, message: str) -> None: + self.reason = reason + super().__init__(message) + + +class ControlSession(Generic[CommandT]): + """Own one control lifecycle; callers own scheduling and simulation advancement. + + A completed tick means a healthy command write, not a completed physics step. + Errors are recorded by start/tick/fail; only stop performs backend cleanup. + Sessions are single-use, and repeated start/stop calls are idempotent. + Each command selects its mode, which must be supported by the robot backend. + """ + + def __init__( + self, + *, + robot: RobotInterface, + task: "DeployTask[CommandT]", + policy: PolicyRuntime, + command_binding: CommandBinding[CommandT], + period_s: float, + state_timeout_s: float, + ) -> None: + if period_s <= 0 or state_timeout_s <= 0: + raise ValidationError("control.timing", "positive period and timeout", (period_s, state_timeout_s)) + self.robot = robot + self.task = task + self.policy = policy + self.command_binding = command_binding + self.period_s = period_s + self.state_timeout_s = state_timeout_s + self._started = False + self._stopped = False + self._completed_steps = 0 + self._initial_state: RobotState | None = None + self._previous_sample_time: int | None = None + self._recorder = LatencyRecorder() + self._trace = hashlib.sha256() + self._start_ns: int | None = None + self._stop_ns: int | None = None + self._exit_reason = "completed" + self._error_message: str | None = None + + @property + def completed_steps(self) -> int: + return self._completed_steps + + @property + def active(self) -> bool: + return self._started and not self._stopped and self._exit_reason == "completed" + + def start(self) -> bool: + """Open, validate and optionally enable the backend without scheduling.""" + if self._started or self._stopped or self._exit_reason != "completed": + return self.active + self._start_ns = time.monotonic_ns() + try: + self.robot.open() + self._validate_capabilities() + initial_state = self._read_and_validate_state(None) + self._previous_sample_time = initial_state.sample_time_ns + self._check_task_termination(initial_state) + self.policy.reset() + if self.robot.capabilities.requires_enable: + initial_context = self._read_context(0, 0.0) + self.task.reset(initial_state, initial_context) + try: + self.robot.enable() + except (EmergencyStopError, LieDownRequestedError, ValidationError): + raise + except Exception as error: + raise _LoopFailure("backend_enable_error", str(error)) from error + initial_state = self._read_and_validate_state(self._previous_sample_time) + self._previous_sample_time = initial_state.sample_time_ns + self._check_task_termination(initial_state) + self.task.reset(initial_state, initial_context) + self.policy.reset() + self._initial_state = initial_state + self._started = True + except (Exception, KeyboardInterrupt) as error: + self.fail(error) + return self.active + + def tick(self, *, elapsed_time_s: float) -> bool: + """Execute one control tick using caller-supplied relative elapsed time.""" + if not self.active: + return False + step = self.completed_steps + loop_start = time.monotonic_ns() + try: + phase_start = time.monotonic_ns() + context = self._read_context(step, elapsed_time_s) + self._recorder.add("input", time.monotonic_ns() - phase_start) + if step == 0: + self.task.reset(self._initial_state, context) + phase_start = time.monotonic_ns() + state = self._read_and_validate_state(self._previous_sample_time) + self._recorder.add("read", time.monotonic_ns() - phase_start) + self._previous_sample_time = state.sample_time_ns + self._check_task_termination(state) + + phase_start = time.monotonic_ns() + try: + observation = self.task.build_observation(state, context) + except Exception as error: + raise _LoopFailure("invalid_observation", str(error)) from error + self._recorder.add("observation", time.monotonic_ns() - phase_start) + + phase_start = time.monotonic_ns() + try: + raw_action = self.policy.infer(observation) + except Exception as error: + raise _LoopFailure("policy_error", str(error)) from error + self._recorder.add("inference", time.monotonic_ns() - phase_start) + + phase_start = time.monotonic_ns() + try: + command = self.task.process_action(raw_action) + self._validate_command_mode(command) + except Exception as error: + raise _LoopFailure("invalid_action", str(error)) from error + self._recorder.add("action", time.monotonic_ns() - phase_start) + + phase_start = time.monotonic_ns() + try: + self.robot.write_command(command) + except (EmergencyStopError, LieDownRequestedError): + raise + except Exception as error: + raise _LoopFailure("backend_write_error", str(error)) from error + self._recorder.add("write", time.monotonic_ns() - phase_start) + health = self.robot.health() + if not health.healthy: + raise _LoopFailure("backend_unhealthy", health.reason) + self._trace.update(step.to_bytes(8, byteorder="little", signed=False)) + for value in ( + state.joint_position, + state.joint_velocity, + state.base_orientation_xyzw, + observation, + raw_action, + command.joint_position if isinstance(command, JointServoCommand) else command.torque, + ): + self._trace.update(value.tobytes(order="C")) + self._completed_steps += 1 + self._recorder.add("loop", time.monotonic_ns() - loop_start) + except (Exception, KeyboardInterrupt) as error: + self.fail(error) + return False + return True + + def fail(self, error: BaseException) -> None: + """Record the first failure, including errors from an external driver.""" + if not isinstance(error, (Exception, KeyboardInterrupt)): + raise error + if self._exit_reason != "completed": + return + if isinstance(error, KeyboardInterrupt): + self._exit_reason = "interrupted" + self._error_message = "rollout interrupted by user" + return + if isinstance(error, EmergencyStopError): + self._exit_reason = "emergency_stop" + elif isinstance(error, LieDownRequestedError): + self._exit_reason = "lie_down" + elif isinstance(error, _LoopFailure): + self._exit_reason = error.reason + elif isinstance(error, ValidationError): + self._exit_reason = "validation_error" + else: + self._exit_reason = "backend_error" + self._error_message = str(error) + + def stop(self) -> None: + """Stop and close once, preserving any earlier failure over cleanup errors.""" + if self._stopped: + return + self._stopped = True + cleanup_error: BaseException | None = None + for cleanup in (self.robot.stop, self.robot.close): + try: + cleanup() + except (Exception, KeyboardInterrupt) as error: + cleanup_error = cleanup_error or error + if cleanup_error is not None and self._exit_reason == "completed": + self._exit_reason = "cleanup_error" + self._error_message = str(cleanup_error) + self._stop_ns = time.monotonic_ns() + + def result(self, *, steps: int | None, overrun_count: int = 0) -> RolloutResult: + """Summarize completed control ticks; finite success requires the target count.""" + end_ns = self._stop_ns if self._stop_ns is not None else time.monotonic_ns() + wall_time_s = (end_ns - self._start_ns) / 1e9 if self._start_ns is not None else 0.0 + simulation_time_s = self.completed_steps * self.period_s + return RolloutResult( + success=self._exit_reason == "completed" and steps is not None and self.completed_steps == steps, + exit_reason=self._exit_reason, + completed_steps=self.completed_steps, + simulation_time_s=simulation_time_s, + wall_time_s=wall_time_s, + real_time_factor=simulation_time_s / wall_time_s if wall_time_s > 0 else 0.0, + overrun_count=overrun_count, + trace_sha256=self._trace.hexdigest(), + error=self._error_message, + latency=self._recorder.summarize(), + ) + + def _check_task_termination(self, state: RobotState) -> None: + try: + reason = self.task.check_termination(state) + except Exception as error: + raise _LoopFailure("task_termination_error", str(error)) from error + if reason is not None: + raise _LoopFailure("task_terminated", reason) + + def _read_context(self, step: int, elapsed_time_s: float) -> PolicyContext[CommandT]: + try: + command = self.command_binding.read_command(batch_size=1) + self.task.validate_command(command) + return PolicyContext(step=step, elapsed_time_s=elapsed_time_s, command=command) + except Exception as error: + raise _LoopFailure("input_error", str(error)) from error + + def _validate_command_mode(self, command: RobotCommand) -> None: + modes = self.robot.capabilities.control_modes + if command.mode not in modes: + raise ValidationError("backend.control_modes", command.mode.value, modes) + + def _validate_capabilities(self) -> None: + capabilities = self.robot.capabilities + if capabilities.max_command_rate_hz is not None: + requested_rate = 1.0 / self.period_s + if requested_rate > capabilities.max_command_rate_hz + 1e-9: + raise ValidationError( + "control.command_rate_hz", f"<= {capabilities.max_command_rate_hz}", requested_rate + ) + + def _read_and_validate_state(self, previous_sample_time: int | None) -> RobotState: + try: + state = self.robot.read_state(self.state_timeout_s) + except TimeoutError as error: + raise _LoopFailure("state_timeout", str(error)) from error + except (EmergencyStopError, LieDownRequestedError): + raise + except Exception as error: + raise _LoopFailure("backend_read_error", str(error)) from error + try: + self._validate_state(state, previous_sample_time) + except _LoopFailure: + raise + except Exception as error: + raise _LoopFailure("invalid_state", str(error)) from error + health = self.robot.health() + if not health.healthy: + raise _LoopFailure("backend_unhealthy", health.reason) + return state + + def _validate_state(self, state: RobotState, previous_sample_time: int | None) -> None: + expected = (self.robot.spec.joint_count,) + if state.joint_position.shape != expected or state.joint_velocity.shape != expected: + raise _LoopFailure("invalid_state", f"joint shape must be {expected}") + if previous_sample_time is not None and state.sample_time_ns < previous_sample_time: + raise _LoopFailure( + "invalid_state", + f"state sample timestamp moved backwards: {state.sample_time_ns} < {previous_sample_time}", + ) + age_s = (time.monotonic_ns() - state.receive_time_ns) / 1e9 + if age_s > self.state_timeout_s: + raise _LoopFailure("state_timeout", f"state age {age_s:.6f}s exceeds {self.state_timeout_s:.6f}s") diff --git a/motrix_deploy/src/motrix_deploy/runtime/factory.py b/motrix_deploy/src/motrix_deploy/runtime/factory.py new file mode 100644 index 00000000..b973593d --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/factory.py @@ -0,0 +1,82 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Discover deployment runtimes through installed plugins.""" + +from collections.abc import Callable, Mapping +from dataclasses import dataclass +from importlib import metadata +from typing import Any + +from motrix_deploy.artifact.schema import ControlSpec +from motrix_deploy.contracts import RobotSpec +from motrix_deploy.runtime.base import DeploymentRuntime, SimulationRuntime +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_deploy.runtime.hardware import HardwareRuntime + + +@dataclass(frozen=True) +class RuntimeCreateContext: + """Artifact-owned robot and control contracts for hardware creation.""" + + robot: RobotSpec + control: ControlSpec + + +RUNTIME_ENTRY_POINT_GROUP = "motrix_deploy.backends" + + +def _runtime_entry_points() -> tuple[metadata.EntryPoint, ...]: + return tuple(metadata.entry_points(group=RUNTIME_ENTRY_POINT_GROUP)) + + +def registered_runtimes() -> tuple[str, ...]: + return tuple(sorted(entry.name for entry in _runtime_entry_points())) + + +def create_simulation_runtime( + name: str, + config: SimulationRuntimeConfig, +) -> SimulationRuntime: + """Create a simulation host with its robot ready for control-session assembly.""" + runtime = _validate_runtime(name, _load_factory(name)(config)) + if not isinstance(runtime, SimulationRuntime): + runtime.close() + raise TypeError(f"Deployment runtime plugin {name!r} does not provide a SimulationRuntime") + return runtime + + +def create_hardware_runtime( + name: str, + config: Mapping[str, Any], + context: RuntimeCreateContext, +) -> HardwareRuntime: + """Create a hardware host, preserving plugin operator safety checks.""" + runtime = _validate_runtime(name, _load_factory(name)(config, context)) + if not isinstance(runtime, HardwareRuntime): + runtime.close() + raise TypeError(f"Deployment runtime plugin {name!r} does not provide a HardwareRuntime") + return runtime + + +def _load_factory(name: str) -> Callable[..., DeploymentRuntime]: + """Resolve the selected plugin; simulation and hardware have distinct contracts.""" + matches = [entry for entry in _runtime_entry_points() if entry.name == name] + if not matches: + available = ", ".join(registered_runtimes()) or "none" + raise ValueError(f"Unsupported deployment runtime {name!r}; installed runtimes: {available}") + if len(matches) != 1: + values = sorted(entry.value for entry in matches) + raise ValueError(f"Multiple deployment runtime plugins are registered as {name!r}: {values}") + factory = matches[0].load() + if not callable(factory): + raise TypeError(f"Deployment runtime plugin {name!r} must load a callable factory") + return factory + + +def _validate_runtime(name: str, runtime: DeploymentRuntime) -> DeploymentRuntime: + if not isinstance(runtime, DeploymentRuntime): + raise TypeError( + f"Deployment runtime plugin {name!r} returned {type(runtime).__name__}, expected DeploymentRuntime" + ) + return runtime diff --git a/motrix_deploy/src/motrix_deploy/runtime/hardware.py b/motrix_deploy/src/motrix_deploy/runtime/hardware.py new file mode 100644 index 00000000..ff2d0a82 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/hardware.py @@ -0,0 +1,68 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Standalone hardware runtime driver.""" + +from motrix_deploy.errors import ValidationError +from motrix_deploy.robot.interface import RobotInterface +from motrix_deploy.runtime.base import DeploymentRuntime +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.result import RolloutResult +from motrix_deploy.runtime.scheduler import FixedStepScheduler, LoopScheduler, RealtimeScheduler + + +class HardwareRuntime(DeploymentRuntime): + """Drive a control session without owning or advancing robot physics. + + The control session owns robot open/stop/close. This runtime only owns the + scheduler and ensures an active session is stopped when its lifecycle closes. + """ + + def __init__( + self, + robot: RobotInterface, + *, + scheduler: LoopScheduler | None = None, + realtime: bool = True, + ) -> None: + super().__init__() + self._robot = robot + self.scheduler = scheduler + self.realtime = realtime + + @property + def robot(self) -> RobotInterface: + return self._robot + + def close(self) -> None: + control = self.control + if control is not None and control.active: + control.stop() + super().close() + + def run(self, control: ControlSession | None = None, *, steps: int | None = None) -> RolloutResult: + control = self._resolve_control(control) + if steps is not None and (not isinstance(steps, int) or isinstance(steps, bool) or steps <= 0): + raise ValidationError("steps", "a positive integer", steps) + scheduler = self.scheduler + if scheduler is None: + scheduler = RealtimeScheduler(control.period_s) if self.realtime else FixedStepScheduler(control.period_s) + elif scheduler.period_s != control.period_s: + raise ValidationError("runtime.scheduler.period_s", control.period_s, scheduler.period_s) + self.open() + try: + if control.start(): + scheduler.reset() + while control.active and (steps is None or control.completed_steps < steps): + step = control.completed_steps + scheduler.wait(step) + control.tick(elapsed_time_s=scheduler.elapsed_time_s(step)) + except (Exception, KeyboardInterrupt) as error: + control.fail(error) + finally: + control.stop() + self.close() + return control.result(steps=steps, overrun_count=scheduler.overrun_count) + + +__all__ = ["HardwareRuntime"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/loop.py b/motrix_deploy/src/motrix_deploy/runtime/loop.py deleted file mode 100644 index fcbbf0d8..00000000 --- a/motrix_deploy/src/motrix_deploy/runtime/loop.py +++ /dev/null @@ -1,271 +0,0 @@ -# Copyright Motphys Technology Co., Ltd. 2025, 2026 -# SPDX-License-Identifier: Apache-2.0 - -"""Backend-neutral policy control loop.""" - -import hashlib -import time -from typing import Generic, TypeVar - -import numpy as np - -from motrix_deploy.backend import RobotInterface -from motrix_deploy.contracts import RobotSpec, RobotState -from motrix_deploy.errors import EmergencyStopError, LieDownRequestedError, ValidationError -from motrix_deploy.policy import PolicyRuntime -from motrix_deploy.runtime.context import PolicyContext -from motrix_deploy.runtime.result import LatencyRecorder, RolloutResult -from motrix_deploy.runtime.scheduler import LoopScheduler -from motrix_deploy.task import DeployTask -from motrix_env_core.input import CommandBinding - -CommandT = TypeVar("CommandT") - - -class _LoopFailure(RuntimeError): - def __init__(self, reason: str, message: str) -> None: - self.reason = reason - super().__init__(message) - - -class ControlLoop(Generic[CommandT]): - """Execute state -> observation -> policy -> action -> command.""" - - def __init__( - self, - *, - robot: RobotSpec, - backend: RobotInterface, - task: DeployTask[CommandT], - policy: PolicyRuntime, - command_binding: CommandBinding[CommandT], - scheduler: LoopScheduler, - state_timeout_s: float, - ) -> None: - self.robot = robot - self.backend = backend - self.task = task - self.policy = policy - self.command_binding = command_binding - self.scheduler = scheduler - self.state_timeout_s = state_timeout_s - if scheduler.period_s <= 0 or state_timeout_s <= 0: - raise ValidationError( - "control.timing", - "positive period and timeout", - (scheduler.period_s, state_timeout_s), - ) - if policy.input_spec.shape != (1, task.observation_size): - raise ValidationError("policy.input.shape", str((1, task.observation_size)), policy.input_spec.shape) - if policy.output_spec.shape != (1, task.action_size): - raise ValidationError("policy.output.shape", str((1, task.action_size)), policy.output_spec.shape) - - def run(self, *, steps: int | None = None) -> RolloutResult: - if steps is not None and (not isinstance(steps, int) or isinstance(steps, bool) or steps <= 0): - raise ValidationError("steps", "a positive integer", steps) - completed = 0 - recorder = LatencyRecorder() - trace = hashlib.sha256() - start_ns = time.monotonic_ns() - exit_reason = "completed" - error_message: str | None = None - previous_sample_time: int | None = None - cleanup_error: Exception | None = None - try: - self.backend.open(self.robot) - self._validate_capabilities() - initial_state = self._read_and_validate_state(None) - previous_sample_time = initial_state.sample_time_ns - self.policy.reset() - if self.backend.capabilities.requires_enable: - initial_context = self._read_context(0) - self.task.reset(initial_state, initial_context) - try: - initial_command = self.task.process_action(np.zeros(self.task.action_size, dtype=np.float32)) - self.backend.enable(initial_command) - except (EmergencyStopError, LieDownRequestedError): - raise - except Exception as error: - raise _LoopFailure("backend_enable_error", str(error)) from error - initial_state = self._read_and_validate_state(previous_sample_time) - previous_sample_time = initial_state.sample_time_ns - self.task.reset(initial_state, initial_context) - self.policy.reset() - self.scheduler.reset() - while steps is None or completed < steps: - step = completed - loop_start = time.monotonic_ns() - self.scheduler.wait(step) - phase_start = time.monotonic_ns() - context = self._read_context(step) - recorder.add("input", time.monotonic_ns() - phase_start) - if step == 0: - self.task.reset(initial_state, context) - phase_start = time.monotonic_ns() - state = self._read_and_validate_state(previous_sample_time) - recorder.add("read", time.monotonic_ns() - phase_start) - previous_sample_time = state.sample_time_ns - - phase_start = time.monotonic_ns() - try: - observation = self.task.build_observation(state, context) - except Exception as error: - raise _LoopFailure("invalid_observation", str(error)) from error - recorder.add("observation", time.monotonic_ns() - phase_start) - - phase_start = time.monotonic_ns() - try: - raw_action = self.policy.infer(observation) - except Exception as error: - raise _LoopFailure("policy_error", str(error)) from error - recorder.add("inference", time.monotonic_ns() - phase_start) - - phase_start = time.monotonic_ns() - try: - command = self.task.process_action(raw_action) - except Exception as error: - raise _LoopFailure("invalid_action", str(error)) from error - recorder.add("action", time.monotonic_ns() - phase_start) - - phase_start = time.monotonic_ns() - try: - self.backend.write_command(command) - except (EmergencyStopError, LieDownRequestedError): - raise - except Exception as error: - raise _LoopFailure("backend_write_error", str(error)) from error - recorder.add("write", time.monotonic_ns() - phase_start) - health = self.backend.health() - if not health.healthy: - raise _LoopFailure("backend_unhealthy", health.reason) - trace.update(step.to_bytes(8, byteorder="little", signed=False)) - for value in ( - state.joint_position, - state.joint_velocity, - state.base_orientation_xyzw, - observation, - raw_action, - command.joint_position, - ): - trace.update(value.tobytes(order="C")) - completed += 1 - recorder.add("loop", time.monotonic_ns() - loop_start) - except KeyboardInterrupt: - exit_reason = "interrupted" - error_message = "rollout interrupted by user" - except EmergencyStopError as error: - exit_reason = "emergency_stop" - error_message = str(error) - except LieDownRequestedError as error: - exit_reason = "lie_down" - error_message = str(error) - except _LoopFailure as error: - exit_reason = error.reason - error_message = str(error) - except ValidationError as error: - exit_reason = "validation_error" - error_message = str(error) - except Exception as error: - exit_reason = "backend_error" - error_message = str(error) - finally: - try: - self.backend.stop() - except Exception as error: - cleanup_error = cleanup_error or error - try: - self.backend.close() - except Exception as error: - cleanup_error = cleanup_error or error - if cleanup_error is not None and exit_reason == "completed": - exit_reason = "cleanup_error" - error_message = str(cleanup_error) - wall_time_s = (time.monotonic_ns() - start_ns) / 1e9 - simulation_time_s = completed * self.scheduler.period_s - real_time_factor = simulation_time_s / wall_time_s if wall_time_s > 0 else 0.0 - return RolloutResult( - success=exit_reason == "completed" and steps is not None and completed == steps, - exit_reason=exit_reason, - completed_steps=completed, - simulation_time_s=simulation_time_s, - wall_time_s=wall_time_s, - real_time_factor=real_time_factor, - overrun_count=self.scheduler.overrun_count, - trace_sha256=trace.hexdigest(), - error=error_message, - latency=recorder.summarize(), - ) - - def _read_context(self, step: int) -> PolicyContext[CommandT]: - try: - command = self.command_binding.read_command(batch_size=1) - self.task.validate_command(command) - return PolicyContext( - step=step, - elapsed_time_s=self.scheduler.elapsed_time_s(step), - command=command, - ) - except KeyboardInterrupt: - raise - except Exception as error: - raise _LoopFailure("input_error", str(error)) from error - - def _validate_capabilities(self) -> None: - capabilities = self.backend.capabilities - if "joint_pd" not in capabilities.control_modes: - raise ValidationError("backend.control_modes", "joint_pd", capabilities.control_modes) - required_fields = { - "joint_position", - "joint_velocity", - "base_orientation_xyzw", - "base_angular_velocity", - "base_linear_acceleration", - } - missing = required_fields - capabilities.state_fields - if missing: - raise ValidationError( - "backend.state_fields", - f"contains {sorted(required_fields)}", - f"missing {sorted(missing)}", - ) - if capabilities.max_command_rate_hz is not None: - requested_rate = 1.0 / self.scheduler.period_s - if requested_rate > capabilities.max_command_rate_hz + 1e-9: - raise ValidationError( - "control.command_rate_hz", - f"<= {capabilities.max_command_rate_hz}", - requested_rate, - ) - - def _read_and_validate_state(self, previous_sample_time: int | None) -> RobotState: - try: - state = self.backend.read_state(self.state_timeout_s) - except TimeoutError as error: - raise _LoopFailure("state_timeout", str(error)) from error - except (EmergencyStopError, LieDownRequestedError): - raise - except Exception as error: - raise _LoopFailure("backend_read_error", str(error)) from error - try: - self._validate_state(state, previous_sample_time) - except _LoopFailure: - raise - except Exception as error: - raise _LoopFailure("invalid_state", str(error)) from error - health = self.backend.health() - if not health.healthy: - raise _LoopFailure("backend_unhealthy", health.reason) - return state - - def _validate_state(self, state: RobotState, previous_sample_time: int | None) -> None: - expected = (self.robot.joint_count,) - if state.joint_position.shape != expected or state.joint_velocity.shape != expected: - raise _LoopFailure("invalid_state", f"joint shape must be {expected}") - if previous_sample_time is not None and state.sample_time_ns < previous_sample_time: - raise _LoopFailure( - "invalid_state", - f"state sample timestamp moved backwards: {state.sample_time_ns} < {previous_sample_time}", - ) - age_s = (time.monotonic_ns() - state.receive_time_ns) / 1e9 - if age_s > self.state_timeout_s: - raise _LoopFailure("state_timeout", f"state age {age_s:.6f}s exceeds {self.state_timeout_s:.6f}s") diff --git a/motrix_deploy/src/motrix_deploy/task.py b/motrix_deploy/src/motrix_deploy/task.py index 932e5cab..a6576402 100644 --- a/motrix_deploy/src/motrix_deploy/task.py +++ b/motrix_deploy/src/motrix_deploy/task.py @@ -3,12 +3,11 @@ """Runtime contract implemented by concrete deployment-task packages.""" -import re from abc import ABC, abstractmethod -from collections.abc import Callable -from typing import Any, Generic, TypeVar +from importlib import metadata +from typing import Any, ClassVar, Generic, TypeVar -from motrix_deploy.artifact import TaskSpec +from motrix_deploy.artifact.schema import TaskSpec from motrix_deploy.contracts import FloatArray, RobotCommand, RobotSpec, RobotState from motrix_deploy.runtime.context import PolicyContext @@ -16,17 +15,19 @@ class DeployTask(ABC, Generic[CommandT]): - """Own task-specific observation, action, and command semantics.""" + """Own task-specific observation, action, command, and termination semantics. - @property - @abstractmethod - def observation_size(self) -> int: - """Return the flat policy observation size.""" + ControlSession checks backend state and calls validate_command before passing + inputs to reset and build_observation. Tasks check optional state fields at + their usage sites when a feature needs them. Backend capabilities describe + which fields are privileged ground truth. + """ - @property - @abstractmethod - def action_size(self) -> int: - """Return the flat policy action size.""" + spec_type: ClassVar[type[TaskSpec]] + + def check_termination(self, state: RobotState) -> str | None: + """Return a task termination reason, or None to continue, without side effects.""" + return None @abstractmethod def reset(self, state: RobotState, context: PolicyContext[CommandT]) -> None: @@ -38,46 +39,39 @@ def build_observation(self, state: RobotState, context: PolicyContext[CommandT]) @abstractmethod def process_action(self, action: FloatArray) -> RobotCommand: - """Convert one raw policy action into a canonical robot command.""" + """Convert a policy action into a servo or torque command supported by the backend.""" @abstractmethod def validate_command(self, command: CommandT) -> None: """Validate one external high-level command before using it.""" -TaskFactory = Callable[[TaskSpec, RobotSpec], DeployTask[Any]] +TASK_ENTRY_POINT_GROUP = "motrix_deploy.tasks" -_TASK_FACTORIES: dict[str, TaskFactory] = {} -_TASK_NAME_PATTERN = re.compile(r"[^/\s]+/v[1-9][0-9]*") +def available_tasks() -> tuple[str, ...]: + """List installed task plugins without importing their implementations.""" + return tuple(sorted({entry.name for entry in metadata.entry_points(group=TASK_ENTRY_POINT_GROUP)})) -def register_task(name: str) -> Callable[[TaskFactory], TaskFactory]: - """Register one concrete task factory when its implementation module is imported.""" - if not isinstance(name, str) or _TASK_NAME_PATTERN.fullmatch(name) is None: - raise ValueError(f"task name must be a versioned identifier such as go2_walk/v1, got {name!r}") - def register(factory: TaskFactory) -> TaskFactory: - if name in _TASK_FACTORIES: - raise ValueError(f"Deployment task {name} is already registered") - _TASK_FACTORIES[name] = factory - return factory - - return register +def load_task_type(name: str) -> type[DeployTask[Any]]: + """Load only the selected installed task class, shared by codec and runtime.""" + entries = tuple(metadata.entry_points(group=TASK_ENTRY_POINT_GROUP)) + matches = [entry for entry in entries if entry.name == name] + if not matches: + supported = ", ".join(sorted({entry.name for entry in entries})) or "none" + raise ValueError(f"Unsupported deployment task {name}; available tasks: {supported}") + if len(matches) != 1: + raise ValueError(f"Multiple deployment task plugins provide {name}") + task_type = matches[0].load() + if not isinstance(task_type, type) or not issubclass(task_type, DeployTask): + raise TypeError(f"Deployment task plugin {name} must provide a DeployTask class") + return task_type def create_task(spec: TaskSpec, robot: RobotSpec) -> DeployTask[Any]: - """Create the task implementation selected by an artifact.""" - try: - factory = _TASK_FACTORIES[spec.name] - except KeyError as error: - supported = ", ".join(sorted(_TASK_FACTORIES)) or "none" - raise ValueError(f"Unsupported deployment task {spec.name}; supported tasks: {supported}") from error - return factory(spec, robot) - - -def registered_tasks() -> tuple[str, ...]: - """Return versioned task identifiers registered in the current process.""" - return tuple(sorted(_TASK_FACTORIES)) + """Instantiate the installed task class selected by an artifact.""" + return load_task_type(spec.name)(spec, robot) -__all__ = ["DeployTask", "create_task", "register_task", "registered_tasks"] +__all__ = ["DeployTask", "create_task", "available_tasks"] diff --git a/motrix_deploy/tests/conftest.py b/motrix_deploy/tests/conftest.py index 538b9db8..e430f66e 100644 --- a/motrix_deploy/tests/conftest.py +++ b/motrix_deploy/tests/conftest.py @@ -4,16 +4,17 @@ """Shared artifact fixtures.""" from collections.abc import Callable +from types import SimpleNamespace import numpy as np import pytest +from task_specs import TestDeployTask, TestTaskSpec from motrix_deploy.artifact import ( ControlSpec, DeploymentManifest, PolicySpec, SourceSpec, - TaskSpec, sha256_bytes, ) from motrix_deploy.contracts import RobotSpec, TensorSpec @@ -22,7 +23,18 @@ @pytest.fixture -def manifest_factory() -> Callable[[], DeploymentManifest]: +def manifest_factory(monkeypatch) -> Callable[[], DeploymentManifest]: + from motrix_deploy import task + + original = task.metadata.entry_points + + def entry_points(*, group): + if group == task.TASK_ENTRY_POINT_GROUP: + return [SimpleNamespace(name="test/v1", load=lambda: TestDeployTask)] + return original(group=group) + + monkeypatch.setattr(task.metadata, "entry_points", entry_points) + def create() -> DeploymentManifest: return DeploymentManifest( schema_version="motrix-deploy/v1", @@ -42,12 +54,7 @@ def create() -> DeploymentManifest: position_upper=np.array([1.0, 1.0], dtype=np.float32), torque_limit=np.array([3.0, 3.0], dtype=np.float32), ), - task=TaskSpec( - name="test/v1", - observation_size=4, - action_size=2, - config={}, - ), + task=TestTaskSpec(), control=ControlSpec(period_s=0.02, state_timeout_s=0.1), ) diff --git a/motrix_deploy/src/motrix_deploy/backend/fake.py b/motrix_deploy/tests/fake_robot.py similarity index 71% rename from motrix_deploy/src/motrix_deploy/backend/fake.py rename to motrix_deploy/tests/fake_robot.py index 6bf30057..d9ea05b0 100644 --- a/motrix_deploy/src/motrix_deploy/backend/fake.py +++ b/motrix_deploy/tests/fake_robot.py @@ -7,17 +7,25 @@ import numpy as np -from motrix_deploy.backend import RobotInterface -from motrix_deploy.contracts import HealthStatus, RobotCapabilities, RobotCommand, RobotSpec, RobotState +from motrix_deploy.contracts import ( + HealthStatus, + JointControlMode, + JointServoCommand, + RobotCapabilities, + RobotCommand, + RobotSpec, + RobotState, +) from motrix_deploy.errors import ValidationError +from motrix_deploy.robot.interface import RobotInterface class FakeRobotInterface(RobotInterface): - """Echo position commands into state without a physics dependency.""" + """Echo servo positions; record torque commands without inventing physics.""" def __init__( self, - joint_names: tuple[str, ...], + spec: RobotSpec, *, sample_period_s: float = 0.02, response: float = 1.0, @@ -26,7 +34,7 @@ def __init__( unhealthy_at: int | None = None, ) -> None: self._capabilities = RobotCapabilities( - control_modes=("joint_pd",), + control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), state_fields=frozenset( { "joint_position", @@ -38,16 +46,15 @@ def __init__( ), max_command_rate_hz=1.0 / sample_period_s, ) - self._joint_names = joint_names + self._spec = spec self._period_s = sample_period_s self._period_ns = round(sample_period_s * 1e9) self._response = response self._fail_read_at = fail_read_at self._fail_write_at = fail_write_at self._unhealthy_at = unhealthy_at - self._spec: RobotSpec | None = None - self._position = np.zeros(len(joint_names), dtype=np.float32) - self._velocity = np.zeros(len(joint_names), dtype=np.float32) + self._position = np.zeros(len(spec.joint_names), dtype=np.float32) + self._velocity = np.zeros(len(spec.joint_names), dtype=np.float32) self._sample_time_ns = 0 self._read_count = 0 self._write_count = 0 @@ -56,18 +63,19 @@ def __init__( self.events: list[str] = [] self.commands: list[RobotCommand] = [] + @property + def spec(self) -> RobotSpec: + return self._spec + @property def capabilities(self) -> RobotCapabilities: return self._capabilities - def open(self, spec: RobotSpec) -> None: + def open(self) -> None: if self._opened and not self._closed: raise RuntimeError("fake backend is already open") self.events.append("open") - if len(self._joint_names) != len(spec.joint_names) or set(self._joint_names) != set(spec.joint_names): - raise ValidationError("backend.joint_names", str(spec.joint_names), self._joint_names) - self._spec = spec - self._position = np.array(spec.default_joint_position, copy=True) + self._position = np.array(self._spec.default_joint_position, copy=True) self._velocity.fill(0) self._sample_time_ns = 0 self._read_count = 0 @@ -90,9 +98,19 @@ def write_command(self, command: RobotCommand) -> None: self._require_open() if self._fail_write_at == self._write_count: raise RuntimeError(f"injected write failure at command {self._write_count}") - previous = self._position.copy() - self._position += np.float32(self._response) * (command.joint_position - self._position) - self._velocity = ((self._position - previous) / np.float32(self._period_s)).astype(np.float32) + expected = (self.spec.joint_count,) + if isinstance(command, JointServoCommand): + if command.joint_position.shape != expected: + raise ValidationError("command.joint_position.shape", str(expected), command.joint_position.shape) + previous = self._position.copy() + self._position += np.float32(self._response) * (command.joint_position - self._position) + self._velocity = ((self._position - previous) / np.float32(self._period_s)).astype(np.float32) + else: + if command.torque.shape != expected: + raise ValidationError("command.torque.shape", str(expected), command.torque.shape) + if np.any(np.abs(command.torque) > self.spec.torque_limit): + raise ValidationError("command.torque", "inside robot torque limits", command.torque.tolist()) + self._velocity.fill(0) self._sample_time_ns += self._period_ns self.commands.append(command) self.events.append("write") @@ -133,10 +151,3 @@ def _state(self) -> RobotState: base_position=np.array([0.0, 0.0, 0.3], dtype=np.float32), base_linear_velocity=zeros, ) - - -class LaggedFakeRobotInterface(FakeRobotInterface): - """A second adapter whose state moves halfway to each command.""" - - def __init__(self, joint_names: tuple[str, ...], *, sample_period_s: float = 0.02) -> None: - super().__init__(joint_names, sample_period_s=sample_period_s, response=0.5) diff --git a/motrix_deploy/tests/task_specs.py b/motrix_deploy/tests/task_specs.py new file mode 100644 index 00000000..222aca15 --- /dev/null +++ b/motrix_deploy/tests/task_specs.py @@ -0,0 +1,33 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Lightweight test-owned task spec plugin; never installed in production.""" + +from typing import ClassVar + +from motrix_deploy.artifact.schema import TaskSpec +from motrix_deploy.task import DeployTask + + +class TestTaskSpec(TaskSpec): + task_name: ClassVar[str] = "test/v1" + + +class TestDeployTask(DeployTask[object]): + spec_type = TestTaskSpec + + def __init__(self, spec, robot): + self.spec = spec + self.robot = robot + + def reset(self, state, context): + pass + + def build_observation(self, state, context): + raise NotImplementedError + + def process_action(self, action): + raise NotImplementedError + + def validate_command(self, command): + pass diff --git a/motrix_deploy/tests/test_artifact.py b/motrix_deploy/tests/test_artifact.py index 97825180..b892eb88 100644 --- a/motrix_deploy/tests/test_artifact.py +++ b/motrix_deploy/tests/test_artifact.py @@ -4,6 +4,7 @@ """Artifact schema, safety and checksum tests.""" import json +import os import subprocess import sys from collections.abc import Callable @@ -11,7 +12,8 @@ import pytest -from motrix_deploy.artifact import DeploymentManifest, inspect_artifact, read_artifact, write_artifact +from motrix_deploy.artifact import ControlSpec, DeploymentManifest, inspect_artifact, read_artifact, write_artifact +from motrix_deploy.contracts import JointControlMode from motrix_deploy.errors import ArtifactError, ValidationError POLICY_BYTES = b"deterministic ONNX placeholder" @@ -27,7 +29,9 @@ def test_artifact_round_trip_and_inspect( assert artifact.manifest.to_dict() == read_artifact(output).manifest.to_dict() assert summary["valid"] is True - assert summary["task"]["name"] == "test/v1" + assert summary["task"] == {"name": "test/v1", "config": {}} + assert summary["observation_size"] == artifact.manifest.policy.input.shape[1] + assert summary["action_size"] == artifact.manifest.policy.output.shape[1] assert summary["robot"]["joint_names"] == ["left_joint", "right_joint"] assert summary["policy"]["input"] == {"name": "observation", "shape": [1, 4], "dtype": "float32"} @@ -36,8 +40,19 @@ def test_cli_inspect_outputs_json(tmp_path: Path, manifest_factory: Callable[[], output = tmp_path / "fixture.deploy" write_artifact(output, manifest_factory(), {"policy/model.onnx": POLICY_BYTES}) + # A subprocess cannot inherit monkeypatched discovery. Install only test + # metadata on its temporary import path, not a production fixture plugin. + dist_info = tmp_path / "test_task_spec-1.0.dist-info" + dist_info.mkdir() + (dist_info / "METADATA").write_text("Name: test-task-spec\nVersion: 1.0\n", encoding="utf-8") + (dist_info / "entry_points.txt").write_text( + "[motrix_deploy.tasks]\ntest/v1 = task_specs:TestDeployTask\n", encoding="utf-8" + ) + env = dict(os.environ) + env["PYTHONPATH"] = os.pathsep.join([str(tmp_path), str(Path(__file__).parent), env.get("PYTHONPATH", "")]) result = subprocess.run( [sys.executable, "-m", "motrix_deploy.cli", "inspect", f"artifact={output}"], + env=env, check=False, capture_output=True, text=True, @@ -92,6 +107,55 @@ def test_invalid_task_name_is_rejected( DeploymentManifest.from_dict(manifest) +@pytest.mark.parametrize("field_name", ["input", "output"]) +@pytest.mark.parametrize("shape", [[4], [2, 4], [1, 2, 3], [1, 0], [1, -1], [1, 1.5]]) +def test_policy_requires_positive_batch_one_vector( + manifest_factory: Callable[[], DeploymentManifest], + field_name: str, + shape: list[int | float], +) -> None: + manifest = manifest_factory().to_dict() + manifest["policy"][field_name]["shape"] = shape + + with pytest.raises(ValidationError, match=rf"policy\.{field_name}\.shape"): + DeploymentManifest.from_dict(manifest) + + +@pytest.mark.parametrize("field_name", ["input", "output"]) +def test_policy_vector_size_is_independent_of_robot_joint_count( + manifest_factory: Callable[[], DeploymentManifest], + field_name: str, +) -> None: + manifest = manifest_factory().to_dict() + manifest["policy"][field_name]["shape"] = [1, 7] + + parsed = DeploymentManifest.from_dict(manifest) + + assert getattr(parsed.policy, field_name).shape == (1, 7) + assert parsed.task.to_dict() == {"name": "test/v1", "config": {}} + + +@pytest.mark.parametrize("mode", list(JointControlMode)) +def test_control_mode_serializes_explicitly(mode: JointControlMode) -> None: + control = ControlSpec(period_s=0.02, state_timeout_s=0.1, mode=mode) + assert control.to_dict()["mode"] == mode.value + assert ControlSpec.from_dict(control.to_dict()).mode is mode + assert ControlSpec(period_s=0.02, state_timeout_s=0.1).mode is JointControlMode.SERVO + + +def test_control_mode_is_required_in_manifest(manifest_factory: Callable[[], DeploymentManifest]) -> None: + manifest = manifest_factory().to_dict() + del manifest["control"]["mode"] + with pytest.raises(ValidationError, match="control.*mode"): + DeploymentManifest.from_dict(manifest) + + +@pytest.mark.parametrize("mode", ["invalid", None, 1]) +def test_invalid_control_mode_is_rejected(mode: object) -> None: + with pytest.raises(ValidationError, match="control.mode"): + ControlSpec(period_s=0.02, state_timeout_s=0.1, mode=mode) + + def test_joint_count_mismatch_is_rejected(manifest_factory: Callable[[], DeploymentManifest]) -> None: manifest = manifest_factory().to_dict() manifest["robot"]["joint_names"] = ["left_joint"] diff --git a/motrix_deploy/tests/test_backend_plugins.py b/motrix_deploy/tests/test_backend_plugins.py index 81221f0a..1620beff 100644 --- a/motrix_deploy/tests/test_backend_plugins.py +++ b/motrix_deploy/tests/test_backend_plugins.py @@ -1,17 +1,27 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Deployment backend plugin discovery contract tests.""" +"""Deployment runtime plugin discovery contract tests.""" from dataclasses import dataclass from typing import Any +import numpy as np import pytest +from fake_robot import FakeRobotInterface -import motrix_deploy.backend as backend_registry +import motrix_deploy.runtime.factory as runtime_factory from motrix_deploy.artifact import ControlSpec -from motrix_deploy.backend import BackendCreateContext, create_backend, registered_backends -from motrix_deploy.backend.fake import FakeRobotInterface +from motrix_deploy.contracts import RobotSpec +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_deploy.runtime.factory import ( + RuntimeCreateContext, + create_hardware_runtime, + create_simulation_runtime, + registered_runtimes, +) +from motrix_deploy.runtime.hardware import HardwareRuntime +from motrix_env_core.config.scene import SceneCfg @dataclass(frozen=True) @@ -24,41 +34,67 @@ def load(self) -> Any: return self.target -def test_backend_plugin_is_discovered_and_constructed(monkeypatch: pytest.MonkeyPatch) -> None: +def test_runtime_plugin_is_discovered_and_constructed(monkeypatch: pytest.MonkeyPatch) -> None: calls = [] + spec = RobotSpec( + base_link_name="base", + joint_names=("joint",), + default_joint_position=np.zeros(1, dtype=np.float32), + position_lower=np.full(1, -1.0, dtype=np.float32), + position_upper=np.full(1, 1.0, dtype=np.float32), + torque_limit=np.ones(1, dtype=np.float32), + ) def factory(config, context): calls.append((config, context)) - return FakeRobotInterface(("joint",)) + robot = FakeRobotInterface(context.robot, response=config["response"]) + return HardwareRuntime(robot=robot, realtime=True) entry_point = _EntryPoint("fake", "test:factory", factory) - monkeypatch.setattr(backend_registry, "_backend_entry_points", lambda: (entry_point,)) - context = BackendCreateContext(control=ControlSpec(period_s=0.02, state_timeout_s=0.1), viewer=False) + monkeypatch.setattr(runtime_factory, "_runtime_entry_points", lambda: (entry_point,)) + context = RuntimeCreateContext( + robot=spec, + control=ControlSpec(period_s=0.02, state_timeout_s=0.1), + ) - backend = create_backend("fake", {"name": "fake", "response": 0.5}, context) + runtime = create_hardware_runtime("fake", {"name": "fake", "response": 0.5}, context) - assert isinstance(backend, FakeRobotInterface) - assert registered_backends() == ("fake",) + assert isinstance(runtime, HardwareRuntime) + assert isinstance(runtime.robot, FakeRobotInterface) + assert runtime.robot.spec is context.robot + assert registered_runtimes() == ("fake",) assert calls == [({"name": "fake", "response": 0.5}, context)] + with pytest.raises(TypeError, match="does not provide a SimulationRuntime"): + monkeypatch.setattr( + runtime_factory, + "_runtime_entry_points", + lambda: (_EntryPoint("fake", "test:factory", lambda config: runtime),), + ) + create_simulation_runtime("fake", SimulationRuntimeConfig(scene=SceneCfg())) -def test_backend_plugin_discovery_rejects_missing_duplicate_and_invalid_factories( +def test_runtime_plugin_discovery_rejects_missing_duplicate_and_invalid_factories( monkeypatch: pytest.MonkeyPatch, + manifest_factory, ) -> None: - context = BackendCreateContext(control=ControlSpec(period_s=0.02, state_timeout_s=0.1), viewer=False) - monkeypatch.setattr(backend_registry, "_backend_entry_points", lambda: ()) - with pytest.raises(ValueError, match="installed backends: none"): - create_backend("missing", {}, context) + spec = manifest_factory().robot + context = RuntimeCreateContext( + robot=spec, + control=ControlSpec(period_s=0.02, state_timeout_s=0.1), + ) + monkeypatch.setattr(runtime_factory, "_runtime_entry_points", lambda: ()) + with pytest.raises(ValueError, match="installed runtimes: none"): + create_hardware_runtime("missing", {}, context) duplicates = ( _EntryPoint("duplicate", "first:factory", lambda config, context: object()), _EntryPoint("duplicate", "second:factory", lambda config, context: object()), ) - monkeypatch.setattr(backend_registry, "_backend_entry_points", lambda: duplicates) - with pytest.raises(ValueError, match="Multiple deployment backend plugins"): - create_backend("duplicate", {}, context) + monkeypatch.setattr(runtime_factory, "_runtime_entry_points", lambda: duplicates) + with pytest.raises(ValueError, match="Multiple deployment runtime plugins"): + create_hardware_runtime("duplicate", {}, context) invalid = _EntryPoint("invalid", "invalid:factory", lambda config, context: object()) - monkeypatch.setattr(backend_registry, "_backend_entry_points", lambda: (invalid,)) - with pytest.raises(TypeError, match="expected RobotInterface"): - create_backend("invalid", {}, context) + monkeypatch.setattr(runtime_factory, "_runtime_entry_points", lambda: (invalid,)) + with pytest.raises(TypeError, match="expected DeploymentRuntime"): + create_hardware_runtime("invalid", {}, context) diff --git a/motrix_deploy/tests/test_config.py b/motrix_deploy/tests/test_config.py index 5274dcf7..77ac8382 100644 --- a/motrix_deploy/tests/test_config.py +++ b/motrix_deploy/tests/test_config.py @@ -13,8 +13,14 @@ def _run_mapping() -> dict[str, object]: return { "artifact": "artifact.deploy", - "backend": {"name": "test", "option": 1}, - "viewer": False, + "runtime": { + "kind": "simulation", + "backend": "test", + "option": 1, + "viewer": False, + "realtime": None, + }, + "duration_s": 0.1, } @@ -28,9 +34,32 @@ def test_deploy_run_config_parses_resolved_application_tree() -> None: config = _typed_config(_run_mapping()) assert config.artifact == "artifact.deploy" + assert config.runtime == _run_mapping()["runtime"] assert config.backend_name == "test" - assert config.backend_options == {"option": 1} - assert config.viewer is False + assert config.runtime_options == {"option": 1} + assert config.duration_s == pytest.approx(0.1) + assert config.command is None + + +def test_deploy_run_config_defaults_to_unbounded_duration() -> None: + values = _run_mapping() + values.pop("duration_s") + + config = _typed_config(values) + + assert config.duration_s is None + + +def test_deploy_run_config_parses_hardware_runtime() -> None: + runtime = {"kind": "hardware", "backend": "test", "network_interface": "enp3s0"} + + config = _typed_config({"artifact": "artifact.deploy", "runtime": runtime}) + + assert config.runtime == runtime + assert config.backend_name == "test" + assert config.runtime_options == {"network_interface": "enp3s0"} + assert config.duration_s is None + assert config.command is None def test_deploy_run_config_rejects_unknown_top_level_fields() -> None: diff --git a/motrix_deploy/tests/test_contracts.py b/motrix_deploy/tests/test_contracts.py index 6bc5360b..6bc99a87 100644 --- a/motrix_deploy/tests/test_contracts.py +++ b/motrix_deploy/tests/test_contracts.py @@ -8,7 +8,14 @@ import numpy as np import pytest -from motrix_deploy.contracts import RobotCommand, RobotSpec, TensorSpec +from motrix_deploy.contracts import ( + JointControlMode, + JointServoCommand, + JointTorqueCommand, + RobotCapabilities, + RobotSpec, + TensorSpec, +) from motrix_deploy.errors import ValidationError @@ -29,7 +36,7 @@ def test_contracts_preserve_valid_array_references() -> None: source = np.array([0.0, 0.0], dtype=np.float32) spec = robot_spec(default_joint_position=source) zero = np.zeros(2, dtype=np.float32) - command = RobotCommand( + command = JointServoCommand( joint_position=source, joint_velocity=zero, feedforward_torque=zero, @@ -41,6 +48,68 @@ def test_contracts_preserve_valid_array_references() -> None: assert command.joint_position is source +def test_joint_command_modes_are_readonly() -> None: + zero = np.zeros(2, dtype=np.float32) + servo = JointServoCommand(zero, zero, zero, zero, zero) + torque = JointTorqueCommand(zero) + assert servo.mode is JointControlMode.SERVO + assert torque.mode is JointControlMode.TORQUE + assert torque.torque is zero + for command in (servo, torque): + with pytest.raises(AttributeError): + command.mode = JointControlMode.SERVO + + +@pytest.mark.parametrize("modes", [("joint_servo",), ("joint_torque",), ("joint_servo", "joint_torque")]) +def test_capabilities_convert_mode_strings_to_enums(modes: tuple[str, ...]) -> None: + capabilities = RobotCapabilities(control_modes=modes, state_fields=frozenset()) + assert all(isinstance(mode, JointControlMode) for mode in capabilities.control_modes) + assert tuple(mode.value for mode in capabilities.control_modes) == modes + + +@pytest.mark.parametrize("modes", [("unknown",), ("joint_servo", "joint_servo"), (), ["joint_servo"]]) +def test_capabilities_reject_invalid_modes(modes: object) -> None: + with pytest.raises(ValidationError, match="capabilities.control_modes"): + RobotCapabilities(control_modes=modes, state_fields=frozenset()) + + +@pytest.mark.parametrize( + "value", + [ + np.zeros(2, dtype=np.float64), + np.zeros((1, 2), dtype=np.float32), + np.array([np.inf], dtype=np.float32), + np.array([np.nan], dtype=np.float32), + ], +) +def test_torque_command_rejects_invalid_arrays(value: np.ndarray) -> None: + with pytest.raises(ValidationError, match="command.torque"): + JointTorqueCommand(value) + + +def test_robot_spec_compatibility_accepts_matching_contract_with_float_roundoff() -> None: + expected = robot_spec() + actual = robot_spec(torque_limit=expected.torque_limit + np.float32(2e-7)) + expected.validate_compatible(actual) + + +@pytest.mark.parametrize( + ("field", "value"), + [ + ("base_link_name", "other"), + ("joint_names", ("b", "a")), + ("joint_names", ("a", "other")), + ("default_joint_position", np.array([0.1, 0.0], dtype=np.float32)), + ("position_lower", np.array([-0.9, -1.0], dtype=np.float32)), + ("position_upper", np.array([0.9, 1.0], dtype=np.float32)), + ("torque_limit", np.array([2.0, 3.0], dtype=np.float32)), + ], +) +def test_robot_spec_compatibility_rejects_semantic_mismatch(field, value) -> None: + with pytest.raises(ValidationError, match=f"robot.{field}"): + robot_spec().validate_compatible(robot_spec(**{field: value})) + + def test_robot_spec_fields_are_frozen() -> None: spec = robot_spec() @@ -62,6 +131,31 @@ def test_robot_spec_rejects_invalid_arrays(field: str, value: np.ndarray, error_ robot_spec(**{field: value}) +def test_privileged_capabilities_default_empty_and_require_available_fields() -> None: + capabilities = RobotCapabilities(control_modes=("joint_servo",), state_fields=frozenset({"base_position"})) + assert capabilities.privileged_state_fields == frozenset() + privileged = RobotCapabilities( + control_modes=("joint_servo",), + state_fields=frozenset({"base_position"}), + privileged_state_fields=frozenset({"base_position"}), + ) + assert privileged.privileged_state_fields <= privileged.state_fields + with pytest.raises(ValidationError, match="capabilities.privileged_state_fields"): + RobotCapabilities( + control_modes=("joint_servo",), + state_fields=frozenset(), + privileged_state_fields=frozenset({"base_position"}), + ) + + +@pytest.mark.parametrize("value", [{"base_position"}, frozenset({"unavailable"})]) +def test_invalid_privileged_state_fields_are_rejected(value: object) -> None: + with pytest.raises(ValidationError, match="capabilities.privileged_state_fields"): + RobotCapabilities( + control_modes=("joint_servo",), state_fields=frozenset({"base_position"}), privileged_state_fields=value + ) + + def test_tensor_spec_requires_float32_fixed_shape() -> None: with pytest.raises(ValidationError, match="tensor.dtype"): TensorSpec(name="input", shape=(1, 4), dtype="float64") @@ -70,7 +164,7 @@ def test_tensor_spec_requires_float32_fixed_shape() -> None: def test_robot_command_rejects_negative_gains() -> None: zero = np.zeros(2, dtype=np.float32) with pytest.raises(ValidationError, match="command.gains"): - RobotCommand( + JointServoCommand( joint_position=zero, joint_velocity=zero, feedforward_torque=zero, diff --git a/motrix_deploy/tests/test_deploy_env_plugins.py b/motrix_deploy/tests/test_deploy_env_plugins.py new file mode 100644 index 00000000..5b7e2730 --- /dev/null +++ b/motrix_deploy/tests/test_deploy_env_plugins.py @@ -0,0 +1,81 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot-independent deployment environment discovery contracts.""" + +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest + +from motrix_deploy import env as env_module +from motrix_deploy.env import ( + DEPLOY_ENV_ENTRY_POINT_GROUP, + assemble_deploy_scene, + available_deploy_envs, + create_deploy_env, +) +from motrix_env_core.config.scene import MjcfFileCfg, RobotCfg, SceneCfg + + +def _install(monkeypatch, entries): + def entry_points(*, group): + assert group == DEPLOY_ENV_ENTRY_POINT_GROUP + return entries + + monkeypatch.setattr(env_module.metadata, "entry_points", entry_points) + + +def test_world_discovery_is_lazy(monkeypatch): + entries = [SimpleNamespace(name=name, load=Mock()) for name in ("rough", "flat")] + _install(monkeypatch, entries) + assert available_deploy_envs() == ("flat", "rough") + for entry in entries: + entry.load.assert_not_called() + + +def test_factory_creates_instance_local_worlds(monkeypatch): + factory = Mock(side_effect=SceneCfg) + entry = SimpleNamespace(name="flat", load=Mock(return_value=factory)) + other = SimpleNamespace(name="rough", load=Mock()) + _install(monkeypatch, [entry, other]) + first = create_deploy_env("flat") + second = create_deploy_env("flat") + assert first is not second + assert first.objs is not second.objs + assert first.objs.robot is None + other.load.assert_not_called() + + +def test_assembly_uses_independently_registered_robot(monkeypatch): + _install(monkeypatch, [SimpleNamespace(name="world", load=Mock(return_value=SceneCfg))]) + robot = RobotCfg(model=MjcfFileCfg(file="synthetic.xml"), base_link_name="base") + factory = Mock(return_value=robot) + monkeypatch.setattr(env_module.registry, "make_robot_config", factory) + scene = assemble_deploy_scene("world", "custom-robot") + assert scene.objs.robot is robot + factory.assert_called_once_with("custom-robot") + + +def test_unknown_environment_reports_choices(monkeypatch): + _install(monkeypatch, [SimpleNamespace(name="flat", load=Mock())]) + with pytest.raises(ValueError, match="Unknown deployment environment.*available environments: flat"): + create_deploy_env("unknown") + + +def test_duplicate_environment_rejected_before_loading(monkeypatch): + entries = [SimpleNamespace(name="flat", load=Mock()) for _ in range(2)] + _install(monkeypatch, entries) + with pytest.raises(ValueError, match="Multiple deployment environment plugins"): + create_deploy_env("flat") + for entry in entries: + entry.load.assert_not_called() + + +@pytest.mark.parametrize("scene", [object(), SceneCfg()]) +def test_environment_factory_contract(monkeypatch, scene): + if isinstance(scene, SceneCfg): + scene.objs.robot = RobotCfg(model=MjcfFileCfg(file="synthetic.xml"), base_link_name="base") + _install(monkeypatch, [SimpleNamespace(name="bad", load=Mock(return_value=lambda: scene))]) + with pytest.raises((TypeError, ValueError), match="Deployment environment 'bad'"): + create_deploy_env("bad") diff --git a/motrix_deploy/tests/test_policy.py b/motrix_deploy/tests/test_policy.py index 7a8b131b..d39846ba 100644 --- a/motrix_deploy/tests/test_policy.py +++ b/motrix_deploy/tests/test_policy.py @@ -12,7 +12,33 @@ from motrix_deploy.contracts import TensorSpec from motrix_deploy.errors import ValidationError -from motrix_deploy.policy import OnnxPolicyRuntime +from motrix_deploy.policy import NoOpPolicyRuntime, OnnxPolicyRuntime, PolicyRuntime + + +def test_decision_runtime_only_requires_infer() -> None: + class Planner(PolicyRuntime): + def infer(self, observation): + return observation[::-1].copy() + + runtime = Planner() + runtime.reset() + np.testing.assert_array_equal(runtime.infer(np.array([1, 2], dtype=np.float32)), [2, 1]) + + +def test_noop_returns_independent_zero_actions_and_ignores_observation() -> None: + runtime = NoOpPolicyRuntime(3) + first = runtime.infer(np.array([1.0], dtype=np.float32)) + assert first.shape == (3,) and first.dtype == np.float32 + np.testing.assert_array_equal(first, np.zeros(3)) + first[:] = 10 + runtime.reset() + np.testing.assert_array_equal(runtime.infer(np.arange(10, dtype=np.float32)), np.zeros(3)) + + +@pytest.mark.parametrize("size", [-1, True, 1.5]) +def test_noop_rejects_invalid_action_size(size) -> None: + with pytest.raises(ValidationError, match="policy.noop.action_size"): + NoOpPolicyRuntime(size) def _write_linear_model( diff --git a/motrix_deploy/tests/test_runtime.py b/motrix_deploy/tests/test_runtime.py index 7fd3eadb..1ce8d22d 100644 --- a/motrix_deploy/tests/test_runtime.py +++ b/motrix_deploy/tests/test_runtime.py @@ -7,15 +7,26 @@ import numpy as np import pytest +from fake_robot import FakeRobotInterface from motrix_deploy.artifact import DeploymentManifest -from motrix_deploy.backend.fake import FakeRobotInterface, LaggedFakeRobotInterface -from motrix_deploy.contracts import FloatArray, RobotCapabilities, RobotCommand, RobotState, TensorSpec, float32_array -from motrix_deploy.errors import LieDownRequestedError +from motrix_deploy.contracts import ( + FloatArray, + JointControlMode, + JointServoCommand, + JointTorqueCommand, + RobotCapabilities, + RobotCommand, + RobotSpec, + RobotState, + float32_array, +) +from motrix_deploy.errors import LieDownRequestedError, ValidationError from motrix_deploy.policy import PolicyRuntime from motrix_deploy.runtime import ( - ControlLoop, + ControlSession, FixedStepScheduler, + HardwareRuntime, PolicyContext, RealtimeScheduler, ) @@ -25,21 +36,12 @@ class ConstantPolicy(PolicyRuntime): def __init__(self, input_size: int, action: FloatArray) -> None: - self._input_spec = TensorSpec(name="observation", shape=(1, input_size)) - self._output_spec = TensorSpec(name="action", shape=(1, action.size)) + self._input_size = input_size self._action = float32_array(action, path="test.action", shape=(action.size,)) self.observations: list[FloatArray] = [] - @property - def input_spec(self) -> TensorSpec: - return self._input_spec - - @property - def output_spec(self) -> TensorSpec: - return self._output_spec - def infer(self, observation: FloatArray) -> FloatArray: - value = float32_array(observation, path="test.observation", shape=(self.input_spec.shape[1],)) + value = float32_array(observation, path="test.observation", shape=(self._input_size,)) self.observations.append(value) return self._action @@ -47,15 +49,7 @@ def infer(self, observation: FloatArray) -> FloatArray: class _TestDeployTask(DeployTask[PlanarVelocityCommand]): def __init__(self, manifest: DeploymentManifest) -> None: self.robot = manifest.robot - self._previous_action = np.zeros(manifest.task.action_size, dtype=np.float32) - - @property - def observation_size(self) -> int: - return 4 - - @property - def action_size(self) -> int: - return 2 + self._previous_action = np.zeros(self.robot.joint_count, dtype=np.float32) def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: del state, context @@ -71,7 +65,7 @@ def process_action(self, action: FloatArray) -> RobotCommand: executed = np.clip(action, -1.0, 1.0).astype(np.float32) self._previous_action = executed zeros = np.zeros(2, dtype=np.float32) - return RobotCommand( + return JointServoCommand( joint_position=self.robot.default_joint_position + np.float32(0.25) * executed, joint_velocity=zeros, feedforward_torque=zeros, @@ -84,27 +78,308 @@ def validate_command(self, command: PlanarVelocityCommand) -> None: raise ValueError("expected one planar velocity command") -def _loop(manifest: DeploymentManifest, backend: FakeRobotInterface) -> ControlLoop: - return ControlLoop( - robot=manifest.robot, - backend=backend, +class _TorqueDeployTask(_TestDeployTask): + def process_action(self, action: FloatArray) -> JointTorqueCommand: + return JointTorqueCommand(np.array(action, dtype=np.float32, copy=True)) + + +@pytest.mark.parametrize("requires_enable", [False, True]) +def test_torque_session_success_is_deterministic_without_fake_physics( + manifest_factory: Callable[[], DeploymentManifest], + requires_enable: bool, +) -> None: + manifest = manifest_factory() + traces = [] + for _ in range(2): + backend = EnablingBackend(manifest.robot) if requires_enable else FakeRobotInterface(manifest.robot) + if requires_enable: + backend._capabilities = RobotCapabilities( + control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), + state_fields=backend.capabilities.state_fields, + requires_enable=True, + ) + session = _session(manifest, backend) + session.task = _TorqueDeployTask(manifest) + assert session.start() + for step in range(3): + assert session.tick(elapsed_time_s=step * manifest.control.period_s) + session.stop() + result = session.result(steps=3) + assert result.success + if requires_enable: + assert backend.events.index("enable") < backend.events.index("write") + assert all(command.mode is JointControlMode.TORQUE for command in backend.commands) + np.testing.assert_array_equal(backend._position, manifest.robot.default_joint_position) + np.testing.assert_array_equal(backend._velocity, np.zeros(2, dtype=np.float32)) + assert backend._sample_time_ns == 3 * backend._period_ns + traces.append(result.trace_sha256) + assert traces[0] == traces[1] + + +@pytest.mark.parametrize("requires_enable", [False, True]) +def test_unsupported_command_mode_prevents_write_after_enable( + manifest_factory: Callable[[], DeploymentManifest], + requires_enable: bool, +) -> None: + manifest = manifest_factory() + backend = EnablingBackend(manifest.robot) if requires_enable else FakeRobotInterface(manifest.robot) + backend._capabilities = RobotCapabilities( + control_modes=(JointControlMode.SERVO,), + state_fields=backend.capabilities.state_fields, + requires_enable=requires_enable, + ) + session = _session(manifest, backend) + session.task = _TorqueDeployTask(manifest) + assert session.start() + assert not session.tick(elapsed_time_s=0.0) + session.stop() + result = session.result(steps=1) + assert result.exit_reason == "invalid_action" + assert "backend.control_modes" in result.error + assert ("enable" in backend.events) is requires_enable + assert "write" not in backend.events + assert backend.commands == [] + assert backend.events[-2:] == ["stop", "close"] + + +class _AlternatingDeployTask(_TestDeployTask): + def __init__(self, manifest: DeploymentManifest) -> None: + super().__init__(manifest) + self._next_mode = JointControlMode.SERVO + + def process_action(self, action: FloatArray) -> RobotCommand: + if self._next_mode is JointControlMode.SERVO: + self._next_mode = JointControlMode.TORQUE + return super().process_action(action) + self._next_mode = JointControlMode.SERVO + return JointTorqueCommand(np.array(action, dtype=np.float32, copy=True)) + + +@pytest.mark.parametrize("requires_enable", [False, True]) +def test_one_session_alternates_servo_and_torque_commands( + manifest_factory: Callable[[], DeploymentManifest], + requires_enable: bool, +) -> None: + manifest = manifest_factory() + backend = EnablingBackend(manifest.robot) if requires_enable else FakeRobotInterface(manifest.robot) + backend._capabilities = RobotCapabilities( + control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), + state_fields=backend.capabilities.state_fields, + requires_enable=requires_enable, + ) + session = _session(manifest, backend) + session.task = _AlternatingDeployTask(manifest) + assert session.start() + for step in range(4): + assert session.tick(elapsed_time_s=step * session.period_s) + session.stop() + assert session.result(steps=4).success + expected_modes = [JointControlMode.SERVO, JointControlMode.TORQUE] * 2 + if requires_enable: + assert backend.events.count("enable") == 1 + assert [command.mode for command in backend.commands] == expected_modes + assert backend.events.count("open") == 1 + assert backend.events.count("write") == 4 + + +def test_switch_to_unsupported_mode_preserves_successful_tick( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = FakeRobotInterface(manifest.robot) + backend._capabilities = RobotCapabilities( + control_modes=(JointControlMode.SERVO,), + state_fields=backend.capabilities.state_fields, + ) + session = _session(manifest, backend) + session.task = _AlternatingDeployTask(manifest) + assert session.start() + assert session.tick(elapsed_time_s=0.0) + trace = session.result(steps=1).trace_sha256 + assert not session.tick(elapsed_time_s=session.period_s) + session.stop() + result = session.result(steps=2) + assert result.exit_reason == "invalid_action" + assert "backend.control_modes" in result.error + assert result.completed_steps == 1 + assert result.trace_sha256 == trace + assert len(backend.commands) == 1 + assert backend.commands[0].mode is JointControlMode.SERVO + assert backend.events.count("write") == 1 + + +@pytest.mark.parametrize("torque", [[1.0], [100.0, 0.0]]) +def test_fake_validates_torque_shape_and_limits( + manifest_factory: Callable[[], DeploymentManifest], + torque: list[float], +) -> None: + from motrix_deploy.errors import ValidationError + + backend = FakeRobotInterface(manifest_factory().robot) + backend.open() + try: + with pytest.raises(ValidationError, match="command.torque"): + backend.write_command(JointTorqueCommand(np.array(torque, dtype=np.float32))) + assert backend.commands == [] + assert "write" not in backend.events + finally: + backend.close() + + +def _loop( + manifest: DeploymentManifest, + backend: FakeRobotInterface, + *, + command_binding: CommandBinding[PlanarVelocityCommand] | None = None, +) -> tuple[HardwareRuntime, ControlSession]: + runtime = HardwareRuntime(backend, scheduler=FixedStepScheduler(manifest.control.period_s)) + return runtime, _session(manifest, backend, command_binding=command_binding) + + +def test_fake_interface_binds_readonly_spec_at_construction( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + spec = manifest_factory().robot + backend = FakeRobotInterface(spec) + + assert backend.spec is spec + with pytest.raises(AttributeError): + backend.spec = spec + backend.open() + np.testing.assert_array_equal(backend.read_state(0.1).joint_position, spec.default_joint_position) + backend.close() + + +def _session( + manifest: DeploymentManifest, + backend: FakeRobotInterface, + *, + command_binding: CommandBinding[PlanarVelocityCommand] | None = None, +) -> ControlSession: + return ControlSession( + robot=backend, task=_TestDeployTask(manifest), - policy=ConstantPolicy(manifest.task.observation_size, np.array([0.4, -0.4], dtype=np.float32)), - command_binding=ConstantPlanarVelocityBinding([0.5, 0.0, 0.0]), - scheduler=FixedStepScheduler(manifest.control.period_s), + policy=ConstantPolicy(manifest.policy.input.shape[1], np.array([0.4, -0.4], dtype=np.float32)), + command_binding=command_binding or ConstantPlanarVelocityBinding([0.5, 0.0, 0.0]), + period_s=manifest.control.period_s, state_timeout_s=manifest.control.state_timeout_s, ) -@pytest.mark.parametrize("backend_type", [FakeRobotInterface, LaggedFakeRobotInterface]) -def test_backend_contract_runs_without_control_loop_changes( +class _SimulationOwnedFakeRobotInterface(FakeRobotInterface): + """Cache commands while a separate simulation callback advances state.""" + + def write_command(self, command: RobotCommand) -> None: + self._require_open() + if self._fail_write_at == self._write_count: + raise RuntimeError("injected write failure") + assert command.joint_position.shape == self._position.shape + self.commands.append(command) + self.events.append("write") + self._write_count += 1 + + def advance(self) -> None: + self._require_open() + self.events.append("advance") + previous = self._position.copy() + self._position = self.commands[-1].joint_position.copy() + self._velocity = ((self._position - previous) / np.float32(self._period_s)).astype(np.float32) + self._sample_time_ns += self._period_ns + + +class _TrackingPolicy(ConstantPolicy): + def __init__(self, input_size: int, action: FloatArray, events: list[str]) -> None: + super().__init__(input_size, action) + self.events = events + + def infer(self, observation: FloatArray) -> FloatArray: + self.events.append("infer") + return super().infer(observation) + + +def test_control_session_allows_external_simulation_advance_after_each_tick( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = _SimulationOwnedFakeRobotInterface(manifest.robot) + loop = _session(manifest, backend) + policy = _TrackingPolicy(manifest.policy.input.shape[1], np.array([0.4, -0.4], dtype=np.float32), backend.events) + loop.policy = policy + + assert loop.start() + try: + for step in range(3): + assert loop.tick(elapsed_time_s=step * manifest.control.period_s) + backend.advance() + finally: + loop.stop() + result = loop.result(steps=3) + + assert result.success + assert result.completed_steps == 3 + assert result.simulation_time_s == pytest.approx(3 * manifest.control.period_s) + assert backend.events == ["open", "read", *(["read", "infer", "write", "advance"] * 3), "stop", "close"] + assert backend._sample_time_ns == 3 * backend._period_ns + np.testing.assert_array_equal(policy.observations[0], np.zeros(4, dtype=np.float32)) + np.testing.assert_allclose( + policy.observations[1][:2], backend.commands[0].joint_position - manifest.robot.default_joint_position + ) + np.testing.assert_array_equal(backend._position, backend.commands[-1].joint_position) + + +def test_standalone_driver_does_not_advance_simulation( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = _SimulationOwnedFakeRobotInterface(manifest.robot) + runtime, loop = _loop(manifest, backend) + + result = runtime.run(loop, steps=3) + + assert result.success + assert backend._sample_time_ns == 0 + assert "advance" not in backend.events + np.testing.assert_array_equal(backend._position, manifest.robot.default_joint_position) + for observation in loop.policy.observations: + np.testing.assert_array_equal(observation, np.zeros(4, dtype=np.float32)) + + +def test_external_simulation_failure_preserves_completed_control_tick( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = _SimulationOwnedFakeRobotInterface(manifest.robot) + session = _session(manifest, backend) + assert session.start() + assert session.tick(elapsed_time_s=0.0) + backend.events.append("advance") + session.fail(RuntimeError("simulation advance failed")) + assert not session.active + assert not session.tick(elapsed_time_s=manifest.control.period_s) + session.stop() + result = session.result(steps=3) + + assert not result.success + assert result.exit_reason == "backend_error" + assert result.error == "simulation advance failed" + assert result.completed_steps == 1 + assert result.simulation_time_s == manifest.control.period_s + assert "write" in result.latency + assert "loop" in result.latency + assert backend.events == ["open", "read", "read", "write", "advance", "stop", "close"] + assert len(backend.commands) == 1 + + +@pytest.mark.parametrize("response", [1.0, 0.5]) +def test_control_loop_runs_with_different_robot_responses( manifest_factory: Callable[[], DeploymentManifest], - backend_type: type[FakeRobotInterface], + response: float, ) -> None: manifest = manifest_factory() - backend = backend_type(manifest.robot.joint_names) + backend = FakeRobotInterface(manifest.robot, response=response) - result = _loop(manifest, backend).run(steps=5) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=5) assert result.success assert result.exit_reason == "completed" @@ -117,13 +392,13 @@ def test_backend_contract_runs_without_control_loop_changes( def test_fixed_step_rollout_is_deterministic(manifest_factory: Callable[[], DeploymentManifest]) -> None: manifest = manifest_factory() - first_backend = FakeRobotInterface(manifest.robot.joint_names) - second_backend = FakeRobotInterface(manifest.robot.joint_names) + first_backend = FakeRobotInterface(manifest.robot) + second_backend = FakeRobotInterface(manifest.robot) - first_loop = _loop(manifest, first_backend) - second_loop = _loop(manifest, second_backend) - first = first_loop.run(steps=4) - second = second_loop.run(steps=4) + first_runtime, first_loop = _loop(manifest, first_backend) + second_runtime, second_loop = _loop(manifest, second_backend) + first = first_runtime.run(first_loop, steps=4) + second = second_runtime.run(second_loop, steps=4) assert (first.success, first.exit_reason, first.completed_steps) == ( second.success, @@ -174,17 +449,10 @@ def test_control_loop_reads_one_singleton_command_batch_per_tick( ) -> None: manifest = manifest_factory() binding = _TrackingBinding() - loop = ControlLoop( - robot=manifest.robot, - backend=FakeRobotInterface(manifest.robot.joint_names), - task=_TestDeployTask(manifest), - policy=ConstantPolicy(manifest.task.observation_size, np.zeros(2, dtype=np.float32)), - command_binding=binding, - scheduler=FixedStepScheduler(manifest.control.period_s), - state_timeout_s=manifest.control.state_timeout_s, - ) + runtime, loop = _loop(manifest, FakeRobotInterface(manifest.robot), command_binding=binding) + loop.policy = ConstantPolicy(manifest.policy.input.shape[1], np.zeros(2, dtype=np.float32)) - result = loop.run(steps=3) + result = runtime.run(loop, steps=3) assert result.success assert binding.batch_sizes == [1, 1, 1] @@ -195,17 +463,10 @@ def test_control_loop_runs_without_a_step_bound_until_interrupted( ) -> None: manifest = manifest_factory() binding = _InterruptingBinding(interrupt_at=3) - loop = ControlLoop( - robot=manifest.robot, - backend=FakeRobotInterface(manifest.robot.joint_names), - task=_TestDeployTask(manifest), - policy=ConstantPolicy(manifest.task.observation_size, np.zeros(2, dtype=np.float32)), - command_binding=binding, - scheduler=FixedStepScheduler(manifest.control.period_s), - state_timeout_s=manifest.control.state_timeout_s, - ) + runtime, loop = _loop(manifest, FakeRobotInterface(manifest.robot), command_binding=binding) + loop.policy = ConstantPolicy(manifest.policy.input.shape[1], np.zeros(2, dtype=np.float32)) - result = loop.run() + result = runtime.run(loop) assert not result.success assert result.exit_reason == "interrupted" @@ -214,18 +475,11 @@ def test_control_loop_runs_without_a_step_bound_until_interrupted( def test_command_binding_failure_returns_input_error(manifest_factory: Callable[[], DeploymentManifest]) -> None: manifest = manifest_factory() - backend = FakeRobotInterface(manifest.robot.joint_names) - loop = ControlLoop( - robot=manifest.robot, - backend=backend, - task=_TestDeployTask(manifest), - policy=ConstantPolicy(manifest.task.observation_size, np.zeros(2, dtype=np.float32)), - command_binding=_FailingBinding(), - scheduler=FixedStepScheduler(manifest.control.period_s), - state_timeout_s=manifest.control.state_timeout_s, - ) + backend = FakeRobotInterface(manifest.robot) + runtime, loop = _loop(manifest, backend, command_binding=_FailingBinding()) + loop.policy = ConstantPolicy(manifest.policy.input.shape[1], np.zeros(2, dtype=np.float32)) - result = loop.run(steps=1) + result = runtime.run(loop, steps=1) assert not result.success assert result.exit_reason == "input_error" @@ -248,9 +502,10 @@ def test_backend_failures_return_structured_results_and_cleanup( completed_steps: int, ) -> None: manifest = manifest_factory() - backend = FakeRobotInterface(manifest.robot.joint_names, **backend_kwargs) + backend = FakeRobotInterface(manifest.robot, **backend_kwargs) - result = _loop(manifest, backend).run(steps=5) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=5) assert not result.success assert result.exit_reason == reason @@ -258,24 +513,19 @@ def test_backend_failures_return_structured_results_and_cleanup( assert backend.events[-2:] == ["stop", "close"] -def test_backend_native_joint_order_can_differ_from_canonical_order( +def test_backend_open_validation_failure_cleans_up_before_first_command( manifest_factory: Callable[[], DeploymentManifest], ) -> None: - manifest = manifest_factory() - backend = FakeRobotInterface(tuple(reversed(manifest.robot.joint_names))) - - result = _loop(manifest, backend).run(steps=2) - - assert result.success + class InvalidRobot(FakeRobotInterface): + def open(self) -> None: + self.events.append("open") + raise ValidationError("backend.joint_names", "matching robot joints", "missing_joint") - -def test_missing_backend_joint_fails_before_first_command( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: manifest = manifest_factory() - backend = FakeRobotInterface((*manifest.robot.joint_names[:-1], "missing_joint")) + backend = InvalidRobot(manifest.robot) - result = _loop(manifest, backend).run(steps=2) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=2) assert result.exit_reason == "validation_error" assert result.completed_steps == 0 @@ -291,9 +541,10 @@ def read_state(self, timeout_s: float): def test_user_interrupt_returns_result_and_cleans_up(manifest_factory: Callable[[], DeploymentManifest]) -> None: manifest = manifest_factory() - backend = InterruptingBackend(manifest.robot.joint_names) + backend = InterruptingBackend(manifest.robot) - result = _loop(manifest, backend).run(steps=2) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=2) assert result.exit_reason == "interrupted" assert backend.events[-2:] == ["stop", "close"] @@ -309,9 +560,10 @@ def test_invalid_state_fails_and_lifecycle_cleanup_is_idempotent( manifest_factory: Callable[[], DeploymentManifest], ) -> None: manifest = manifest_factory() - backend = InvalidStateBackend(manifest.robot.joint_names) + backend = InvalidStateBackend(manifest.robot) - result = _loop(manifest, backend).run(steps=2) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=2) events_after_run = list(backend.events) backend.stop() backend.close() @@ -343,20 +595,18 @@ def sleep(duration: float) -> None: class EnablingBackend(FakeRobotInterface): - def __init__(self, joint_names: tuple[str, ...], *, sample_period_s: float = 0.02) -> None: - super().__init__(joint_names, sample_period_s=sample_period_s) + def __init__(self, spec: RobotSpec, *, sample_period_s: float = 0.02) -> None: + super().__init__(spec, sample_period_s=sample_period_s) self._capabilities = RobotCapabilities( - control_modes=("joint_pd",), + control_modes=("joint_servo",), state_fields=self._capabilities.state_fields, max_command_rate_hz=1.0 / sample_period_s, requires_enable=True, ) self.enabled = False - self.initial_command: RobotCommand | None = None - def enable(self, initial_command: RobotCommand) -> None: + def enable(self) -> None: assert "write" not in self.events - self.initial_command = initial_command self.events.append("enable") self.enabled = True @@ -375,9 +625,10 @@ def test_lie_down_request_returns_structured_result_and_cleans_up( manifest_factory: Callable[[], DeploymentManifest], ) -> None: manifest = manifest_factory() - backend = LieDownBackend(manifest.robot.joint_names) + backend = LieDownBackend(manifest.robot) - result = _loop(manifest, backend).run(steps=1) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=1) assert not result.success assert result.exit_reason == "lie_down" @@ -385,13 +636,290 @@ def test_lie_down_request_returns_structured_result_and_cleans_up( assert backend.events[-2:] == ["stop", "close"] +@pytest.mark.parametrize("terminate_at", [0, 1, 3]) +def test_task_termination_prevents_inference_and_commands( + manifest_factory: Callable[[], DeploymentManifest], terminate_at: int +) -> None: + manifest = manifest_factory() + backend = EnablingBackend(manifest.robot) if terminate_at == 0 else FakeRobotInterface(manifest.robot) + runtime, loop = _loop(manifest, backend) + checks = [] + + def check(state: RobotState) -> str | None: + checks.append(state) + return "task fell" if len(checks) - 1 == terminate_at else None + + loop.task.check_termination = check + result = runtime.run(loop, steps=5) + + assert result.exit_reason == "task_terminated" + assert result.error == "task fell" + assert not result.success + assert result.completed_steps == max(0, terminate_at - 1) + assert len(loop.policy.observations) == result.completed_steps + assert len(backend.commands) == result.completed_steps + if terminate_at == 0: + assert "enable" not in backend.events + assert backend.events[-2:] == ["stop", "close"] + + +def test_task_termination_checks_state_after_enable_before_inference( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = EnablingBackend(manifest.robot) + runtime, loop = _loop(manifest, backend) + loop.task.check_termination = lambda state: "fell after enable" if backend.enabled else None + result = runtime.run(loop, steps=1) + assert result.exit_reason == "task_terminated" + assert result.completed_steps == 0 + assert backend.commands == [] + assert loop.policy.observations == [] + assert backend.events == ["open", "read", "enable", "read", "stop", "close"] + + +def test_task_termination_hook_failure_has_task_error_and_cleanup( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = FakeRobotInterface(manifest.robot) + runtime, loop = _loop(manifest, backend) + + def fail(state: RobotState) -> str | None: + raise RuntimeError("termination failed") + + loop.task.check_termination = fail + result = runtime.run(loop, steps=1) + assert result.exit_reason == "task_termination_error" + assert result.error == "termination failed" + assert backend.commands == [] + assert backend.events[-2:] == ["stop", "close"] + + +@pytest.mark.parametrize("privileged", [False, True]) +def test_backend_state_metadata_does_not_gate_task_execution( + manifest_factory: Callable[[], DeploymentManifest], privileged: bool +) -> None: + manifest = manifest_factory() + backend = EnablingBackend(manifest.robot) + backend._capabilities = RobotCapabilities( + control_modes=("joint_servo",), + state_fields=frozenset({"joint_position", "joint_velocity", "base_linear_velocity"}), + privileged_state_fields=frozenset({"base_linear_velocity"}) if privileged else frozenset(), + requires_enable=True, + ) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=1) + assert result.success + assert len(backend.commands) == 1 + assert backend.events.index("enable") < backend.events.index("write") + assert backend.events[-2:] == ["stop", "close"] + + def test_required_enable_completes_before_first_policy_write( manifest_factory: Callable[[], DeploymentManifest], ) -> None: manifest = manifest_factory() - backend = EnablingBackend(manifest.robot.joint_names) + backend = EnablingBackend(manifest.robot) - result = _loop(manifest, backend).run(steps=1) + runtime, control = _loop(manifest, backend) + result = runtime.run(control, steps=1) assert result.success assert backend.events[:6] == ["open", "read", "enable", "read", "read", "write"] + + +def test_enable_does_not_process_a_synthetic_policy_action(manifest_factory) -> None: + manifest = manifest_factory() + backend = EnablingBackend(manifest.robot) + session = _session(manifest, backend) + actions = [] + process_action = session.task.process_action + + def record_action(action): + actions.append(np.array(action, copy=True)) + return process_action(action) + + session.task.process_action = record_action + assert session.start() + assert actions == [] + assert session.tick(elapsed_time_s=0.0) + assert len(actions) == 1 + session.stop() + assert session.result(steps=1).success + + +def test_direct_session_lifecycle_is_single_use_and_cleanup_is_explicit( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = FakeRobotInterface(manifest.robot) + session = _session(manifest, backend) + assert not session.active + assert not session.tick(elapsed_time_s=0.0) + assert session.start() + assert session.start() + assert backend.events == ["open", "read"] + assert session.tick(elapsed_time_s=7.25) + assert session.completed_steps == 1 + assert session.active + session.stop() + events = list(backend.events) + session.stop() + assert not session.start() + assert not session.tick(elapsed_time_s=8.0) + assert not session.active + assert backend.events == events + result = session.result(steps=1, overrun_count=2) + assert result.success + assert result.overrun_count == 2 + assert result.simulation_time_s == manifest.control.period_s + assert not session.result(steps=None).success + assert not session.result(steps=2).success + assert session.result(steps=1).wall_time_s == result.wall_time_s + + +def test_failed_start_records_error_without_implicit_cleanup( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + backend = FakeRobotInterface(manifest.robot, fail_read_at=0) + session = _session(manifest, backend) + assert not session.start() + assert not session.start() + assert not session.active + assert not session.tick(elapsed_time_s=0.0) + assert "stop" not in backend.events + session.stop() + assert session.result(steps=1).exit_reason == "state_timeout" + assert backend.events[-2:] == ["stop", "close"] + + +class _StatefulTask(_TestDeployTask): + def __init__(self, manifest: DeploymentManifest) -> None: + super().__init__(manifest) + self.contexts: list[PolicyContext[PlanarVelocityCommand]] = [] + self.reset_contexts: list[PolicyContext[PlanarVelocityCommand]] = [] + + def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: + super().reset(state, context) + self.reset_contexts.append(context) + + def build_observation(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: + self.contexts.append(context) + observation = super().build_observation(state, context) + observation[:2] += self._previous_action + observation[2:] += np.float32(context.elapsed_time_s) + return observation + + +class _StatefulPolicy(ConstantPolicy): + def __init__(self) -> None: + super().__init__(4, np.zeros(2, dtype=np.float32)) + self.reset_count = 0 + self.counter = 99 + + def reset(self) -> None: + self.reset_count += 1 + self.counter = 0 + + def infer(self, observation: FloatArray) -> FloatArray: + super().infer(observation) + self.counter += 1 + return np.tanh(observation[:2] + np.float32(self.counter * 0.1)).astype(np.float32) + + +@pytest.mark.parametrize("enabling", [False, True]) +def test_stateful_session_and_standalone_driver_have_identical_trace_and_reset_semantics( + manifest_factory: Callable[[], DeploymentManifest], + enabling: bool, +) -> None: + manifest = manifest_factory() + backend_type = EnablingBackend if enabling else FakeRobotInterface + direct = _session(manifest, backend_type(manifest.robot)) + runtime, driver = _loop(manifest, backend_type(manifest.robot)) + for executor in (direct, driver): + executor.task = _StatefulTask(manifest) + executor.policy = _StatefulPolicy() + assert direct.start() + for step in range(4): + assert direct.tick(elapsed_time_s=step * direct.period_s) + direct.stop() + direct_result = direct.result(steps=4) + driver_result = runtime.run(driver, steps=4) + assert direct_result.success and driver_result.success + assert direct_result.trace_sha256 == driver_result.trace_sha256 + assert direct.robot.events == driver.robot.events + for executor in (direct, driver): + assert executor.policy.reset_count == (2 if enabling else 1) + assert len(executor.task.reset_contexts) == (3 if enabling else 1) + assert [context.step for context in executor.task.contexts] == list(range(4)) + assert [context.elapsed_time_s for context in executor.task.contexts] == pytest.approx( + [step * executor.period_s for step in range(4)] + ) + for first, second in zip(direct.policy.observations, driver.policy.observations, strict=True): + np.testing.assert_array_equal(first, second) + for first, second in zip(direct.robot.commands, driver.robot.commands, strict=True): + np.testing.assert_array_equal(first.joint_position, second.joint_position) + + +def test_direct_session_uses_supplied_simulation_relative_context_time( + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + manifest = manifest_factory() + session = _session(manifest, FakeRobotInterface(manifest.robot)) + task = _StatefulTask(manifest) + session.task = task + assert session.start() + assert session.tick(elapsed_time_s=0.125) + assert session.tick(elapsed_time_s=0.75) + session.stop() + assert task.reset_contexts[0].elapsed_time_s == 0.125 + assert [context.elapsed_time_s for context in task.contexts] == [0.125, 0.75] + assert session.result(steps=2).simulation_time_s == 2 * session.period_s + + +@pytest.mark.parametrize("phase", ["reset", "wait"]) +def test_driver_scheduler_failure_is_recorded_and_cleans_up( + manifest_factory: Callable[[], DeploymentManifest], + phase: str, +) -> None: + manifest = manifest_factory() + backend = FakeRobotInterface(manifest.robot) + runtime, loop = _loop(manifest, backend) + + def fail(*args) -> None: + raise RuntimeError("scheduler failed") + + setattr(runtime.scheduler, phase, fail) + result = runtime.run(loop, steps=1) + assert result.exit_reason == "backend_error" + assert result.error == "scheduler failed" + assert result.completed_steps == 0 + assert backend.events[-2:] == ["stop", "close"] + + +@pytest.mark.parametrize("prior_failure", [False, True]) +def test_cleanup_errors_do_not_replace_existing_failure_and_close_is_attempted( + manifest_factory: Callable[[], DeploymentManifest], + prior_failure: bool, +) -> None: + manifest = manifest_factory() + backend = FakeRobotInterface(manifest.robot) + session = _session(manifest, backend) + assert session.start() + assert session.tick(elapsed_time_s=0.0) + + def fail_stop() -> None: + backend.events.append("stop-failed") + raise RuntimeError("cleanup failed") + + backend.stop = fail_stop + if prior_failure: + session.fail(LieDownRequestedError("lie down")) + session.stop() + session.stop() + result = session.result(steps=1) + assert result.exit_reason == ("lie_down" if prior_failure else "cleanup_error") + assert result.error == ("lie down" if prior_failure else "cleanup failed") + assert backend.events[-2:] == ["stop-failed", "close"] diff --git a/motrix_deploy/tests/test_runtime_binding.py b/motrix_deploy/tests/test_runtime_binding.py new file mode 100644 index 00000000..f64b75a6 --- /dev/null +++ b/motrix_deploy/tests/test_runtime_binding.py @@ -0,0 +1,92 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Runtime/control ownership binding contracts.""" + +from types import SimpleNamespace + +import pytest +from fake_robot import FakeRobotInterface +from test_runtime import _session + +import motrix_deploy.runtime.factory as runtime_factory +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.factory import create_hardware_runtime +from motrix_deploy.runtime.hardware import HardwareRuntime +from motrix_deploy.runtime.scheduler import FixedStepScheduler + + +def test_hardware_runtime_runs_bound_control_without_duplicate_session(manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + control = _session(manifest, robot) + runtime = HardwareRuntime(robot, scheduler=FixedStepScheduler(manifest.control.period_s)) + + runtime.bind_control_session(control) + assert runtime.control is control + result = runtime.run(steps=2) + + assert result.success + assert runtime.control is control + assert robot.events[-2:] == ["stop", "close"] + + +def test_binding_rejects_conflicting_robot_and_duplicate_control(manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + other = FakeRobotInterface(manifest.robot) + runtime = HardwareRuntime(robot) + first = _session(manifest, robot) + second = _session(manifest, robot) + foreign = _session(manifest, other) + + runtime.bind_control_session(first) + runtime.bind_control_session(first) + with pytest.raises(ValidationError, match="already bound"): + runtime.bind_control_session(second) + with pytest.raises(ValidationError, match="runtime.control.robot"): + runtime.bind_control_session(foreign) + with pytest.raises(ValidationError, match="already bound"): + runtime.run(second, steps=1) + + +def test_unbound_hardware_runtime_requires_control(manifest_factory): + manifest = manifest_factory() + runtime = HardwareRuntime(FakeRobotInterface(manifest.robot)) + + with pytest.raises(ValidationError, match="runtime.control"): + runtime.run(steps=1) + + +def test_hardware_factory_prepares_robot_for_explicit_control_binding(monkeypatch, manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + calls = [] + + def factory(config, context): + calls.append((config, context)) + return HardwareRuntime(robot, realtime=False) + + entry = SimpleNamespace(name="fake", value="fake:factory", load=lambda: factory) + monkeypatch.setattr(runtime_factory, "_runtime_entry_points", lambda: (entry,)) + + context = runtime_factory.RuntimeCreateContext(robot=manifest.robot, control=manifest.control) + config = {} + runtime = create_hardware_runtime("fake", config, context) + assert calls == [(config, context)] + assert runtime.robot is robot + assert not runtime.opened + assert robot.events == [] + + control = _session(manifest, runtime.robot) + runtime.bind_control_session(control) + assert runtime.control is control + assert control.robot is runtime.robot + assert robot.events == [] + + with runtime: + assert runtime.opened + result = runtime.run(steps=1) + assert result.success + assert robot.events[-2:] == ["stop", "close"] + assert not runtime.opened diff --git a/motrix_deploy/tests/test_simulation_runtime.py b/motrix_deploy/tests/test_simulation_runtime.py new file mode 100644 index 00000000..3736ec16 --- /dev/null +++ b/motrix_deploy/tests/test_simulation_runtime.py @@ -0,0 +1,166 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Public simulation runtime configuration and plugin selection contracts.""" + +from dataclasses import FrozenInstanceError, asdict +from types import SimpleNamespace + +import numpy as np +import pytest +from fake_robot import FakeRobotInterface + +import motrix_deploy.runtime.factory as runtime_factory +from motrix_deploy.runtime.base import SimulationRuntime +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_deploy.runtime.factory import create_simulation_runtime +from motrix_env_core.config.scene import SceneCfg +from motrix_env_core.config.sim import SimCfg + + +def test_simulation_factory_uses_installed_plugin_and_public_config(monkeypatch, manifest_factory): + spec = manifest_factory().robot + + class Runtime(SimulationRuntime): + def __init__(self): + super().__init__() + self._robot = FakeRobotInterface(spec) + + @property + def robot(self): + return self._robot + + def run(self, control, *, steps=None): + raise NotImplementedError + + calls = [] + + def factory(config): + calls.append(config) + assert config.scene.objs.robot is robot_cfg + return Runtime() + + entry = SimpleNamespace(name="test", value="test:factory", load=lambda: factory) + monkeypatch.setattr(runtime_factory, "_runtime_entry_points", lambda: (entry,)) + robot_cfg = object() + config = SimulationRuntimeConfig( + scene=SceneCfg(objs=SimpleNamespace(robot=robot_cfg)), + ) + + runtime = create_simulation_runtime("test", config) + + assert isinstance(runtime, SimulationRuntime) + assert calls == [config] + assert isinstance(runtime.robot, FakeRobotInterface) + assert runtime.robot.spec is spec + with runtime: + assert runtime.opened + assert not runtime.opened + + +def test_simulation_config_uses_independent_typed_sensor_defaults(): + first = SimulationRuntimeConfig(scene=SceneCfg()) + second = SimulationRuntimeConfig(scene=SceneCfg()) + + assert isinstance(first.sensor_bindings, SensorBindings) + assert first.sensor_bindings is not second.sensor_bindings + assert asdict(first.sensor_bindings) == { + "base_angular_velocity": None, + "base_linear_acceleration": None, + "base_linear_velocity": None, + } + with pytest.raises(FrozenInstanceError): + first.sensor_bindings.base_angular_velocity = "gyro" + + +@pytest.mark.parametrize( + "field_name", + ["base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"], +) +@pytest.mark.parametrize("invalid_name", ["", " ", "\t\n", 1, True, b"sensor", ["sensor"]]) +def test_sensor_bindings_reject_invalid_sensor_names(field_name, invalid_name): + expected_error = ValueError if isinstance(invalid_name, str) else TypeError + with pytest.raises(expected_error, match=field_name): + SensorBindings(**{field_name: invalid_name}) + + +def test_sensor_bindings_reject_unknown_semantics(): + with pytest.raises(TypeError, match="base_orientation"): + SensorBindings(**{"base_orientation": "orientation"}) + + +def test_sensor_bindings_validate_plain_dict_without_changing_sensor_names(): + # YAML parsing produces a plain dictionary; validation belongs at that boundary. + bindings = SensorBindings( + **{ + "base_angular_velocity": " gyro-local ", + "base_linear_acceleration": "robot/imu.accel", + "base_linear_velocity": "robot-velocity", + } + ) + config = SimulationRuntimeConfig(scene=SceneCfg(), sensor_bindings=bindings) + + assert config.sensor_bindings is bindings + assert bindings.base_angular_velocity == " gyro-local " + assert bindings.base_linear_acceleration == "robot/imu.accel" + assert bindings.base_linear_velocity == "robot-velocity" + + +@pytest.mark.parametrize( + "values", + [{}, {"base_linear_velocity": None}, {"base_linear_velocity": "velocity"}], +) +def test_sensor_bindings_allow_partial_and_none_bindings(values): + bindings = SensorBindings(**values) + + assert bindings.base_angular_velocity is None + assert bindings.base_linear_acceleration is None + assert bindings.base_linear_velocity == values.get("base_linear_velocity") + + +def test_sensor_bindings_are_available_on_public_runtime_facade(): + from motrix_deploy.runtime import SensorBindings as PublicSensorBindings + + assert PublicSensorBindings is SensorBindings + + +def test_simulation_config_uses_independent_canonical_physics_defaults(): + first = SimulationRuntimeConfig(scene=SceneCfg()) + second = SimulationRuntimeConfig(scene=SceneCfg()) + + assert isinstance(first.physics, SimCfg) + # Deployment's timestep and solver defaults are a preserved runtime contract. + assert first.physics.dt == 0.002 + assert first.physics.solver_iterations == 100 + assert first.physics is not second.physics + first.physics.dt = 0.01 + assert second.physics.dt == 0.002 + + +@pytest.mark.parametrize("dt", [0.0, -0.1, np.nan, np.inf, -np.inf, True, False]) +def test_simulation_config_rejects_invalid_timestep_at_boundary(dt): + physics = SimCfg(dt=dt) + with pytest.raises(ValueError, match=r"sim\.dt must be positive and finite"): + SimulationRuntimeConfig(scene=SceneCfg(), physics=physics) + + +@pytest.mark.parametrize("iterations", [0, -1, 1.5, 1.0, True, False, "100"]) +def test_simulation_config_rejects_invalid_solver_iterations_at_boundary(iterations): + with pytest.raises(ValueError, match=r"sim\.solver_iterations must be a positive integer"): + SimulationRuntimeConfig(scene=SceneCfg(), physics=SimCfg(solver_iterations=iterations)) + + +@pytest.mark.parametrize("tolerance", [0.0, -0.1, np.nan, np.inf]) +def test_simulation_config_rejects_invalid_solver_tolerance_at_boundary(tolerance): + with pytest.raises(ValueError, match=r"sim\.solver_tolerance must be positive and finite"): + SimulationRuntimeConfig(scene=SceneCfg(), physics=SimCfg(solver_tolerance=tolerance)) + + +def test_simulation_config_preserves_optional_backend_physics_settings(): + physics = SimCfg(solver_iterations=None, solver_tolerance=None, gravity=None) + config = SimulationRuntimeConfig(scene=SceneCfg(), physics=physics) + + assert config.physics is physics + assert config.physics.solver_iterations is None + assert config.physics.solver_tolerance is None + assert config.physics.gravity is None diff --git a/motrix_deploy/tests/test_task_plugins.py b/motrix_deploy/tests/test_task_plugins.py new file mode 100644 index 00000000..42cffe63 --- /dev/null +++ b/motrix_deploy/tests/test_task_plugins.py @@ -0,0 +1,67 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Deployment task plugin discovery and loading boundary.""" + +from types import SimpleNamespace +from unittest.mock import Mock + +import pytest +from task_specs import TestDeployTask as FixtureDeployTask + +from motrix_deploy import task as task_module +from motrix_deploy.task import TASK_ENTRY_POINT_GROUP, available_tasks, create_task + + +def _install(monkeypatch, entries): + def entry_points(*, group): + assert group == TASK_ENTRY_POINT_GROUP + return entries + + monkeypatch.setattr(task_module.metadata, "entry_points", entry_points) + + +def test_listing_is_sorted_and_lazy(monkeypatch): + entries = [SimpleNamespace(name=name, load=Mock()) for name in ("other/v1", "test/v1")] + _install(monkeypatch, entries) + assert available_tasks() == ("other/v1", "test/v1") + for entry in entries: + entry.load.assert_not_called() + + +def test_selected_class_receives_artifact_contract(monkeypatch, manifest_factory): + manifest = manifest_factory() + selected = SimpleNamespace(name=manifest.task.name, load=Mock(return_value=FixtureDeployTask)) + other = SimpleNamespace(name="other/v1", load=Mock()) + _install(monkeypatch, [other, selected]) + task = create_task(manifest.task, manifest.robot) + assert isinstance(task, FixtureDeployTask) + assert task.spec is manifest.task + assert task.robot is manifest.robot + selected.load.assert_called_once_with() + other.load.assert_not_called() + + +def test_missing_plugin_reports_available_names(monkeypatch, manifest_factory): + manifest = manifest_factory() + _install(monkeypatch, [SimpleNamespace(name="other/v1", load=Mock())]) + with pytest.raises(ValueError, match="test/v1.*available tasks: other/v1"): + create_task(manifest.task, manifest.robot) + + +def test_duplicate_plugins_fail_before_loading(monkeypatch, manifest_factory): + manifest = manifest_factory() + entries = [SimpleNamespace(name=manifest.task.name, load=Mock()) for _ in range(2)] + _install(monkeypatch, entries) + with pytest.raises(ValueError, match="Multiple deployment task plugins"): + create_task(manifest.task, manifest.robot) + for entry in entries: + entry.load.assert_not_called() + + +@pytest.mark.parametrize("factory", [object(), dict, lambda spec, robot: object()]) +def test_invalid_plugin_class_contract(monkeypatch, manifest_factory, factory): + manifest = manifest_factory() + _install(monkeypatch, [SimpleNamespace(name=manifest.task.name, load=Mock(return_value=factory))]) + with pytest.raises(TypeError, match="Deployment task plugin test/v1"): + create_task(manifest.task, manifest.robot) diff --git a/motrix_deploy/tests/test_task_spec_codec.py b/motrix_deploy/tests/test_task_spec_codec.py new file mode 100644 index 00000000..49d6c0b3 --- /dev/null +++ b/motrix_deploy/tests/test_task_spec_codec.py @@ -0,0 +1,238 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Typed task configuration codec and installed-plugin boundary.""" + +import json +from types import SimpleNamespace +from typing import ClassVar +from unittest.mock import Mock + +import pytest +from omegaconf import OmegaConf +from pydantic import BaseModel, Field, field_validator +from pydantic import ValidationError as PydanticValidationError +from task_specs import TestDeployTask as FixtureDeployTask + +from motrix_deploy import task as task_module +from motrix_deploy.artifact.schema import TaskSpec +from motrix_deploy.errors import ValidationError + + +class NumericSpec(TaskSpec): + task_name: ClassVar[str] = "numeric/v1" + gain: float + values: list[float] + bounds: list[list[float]] + threshold: float | None = None + enabled: bool = False + count: int = 1 + label: str = "literal" + + @field_validator("gain") + @classmethod + def positive_gain(cls, value: float) -> float: + if value <= 0: + raise ValueError("gain must be positive") + return value + + +class NumericTask(FixtureDeployTask): + spec_type = NumericSpec + + +@pytest.fixture +def codec_entries(monkeypatch): + selected = SimpleNamespace(name="numeric/v1", load=Mock(return_value=NumericTask)) + other = SimpleNamespace(name="other/v1", load=Mock()) + entries = [selected, other] + + def entry_points(*, group): + assert group == task_module.TASK_ENTRY_POINT_GROUP + return entries + + monkeypatch.setattr(task_module.metadata, "entry_points", entry_points) + return entries + + +@pytest.fixture +def encoded(): + return NumericSpec(gain=2.0, values=[1.0, 2.0], bounds=[[0.0, 1.0]]).to_dict() + + +def test_roundtrip_frozen_concrete_spec(codec_entries, encoded): + parsed = TaskSpec.from_dict(encoded) + assert type(parsed) is NumericSpec + assert parsed.to_dict() == encoded + codec_entries[0].load.assert_called_once_with() + codec_entries[1].load.assert_not_called() + + +@pytest.mark.parametrize( + ("field", "value", "path"), + [ + ("gain", True, "gain"), + ("gain", "2.0", "gain"), + ("gain", "${oc.env:HOME}", "gain"), + ("values", [True], "values.0"), + ("values", ["1.0"], "values.0"), + ("values", (1.0, 2.0), "values"), + ("values", "1,2", "values"), + ("bounds", [[False]], "bounds.0.0"), + ("bounds", [["${gain}"]], "bounds.0.0"), + ("threshold", True, "threshold"), + ("enabled", 1, "enabled"), + ("count", True, "count"), + ("count", "1", "count"), + ("enabled", "true", "enabled"), + ("label", 1, "label"), + ("gain", float("nan"), "gain"), + ("gain", float("inf"), "gain"), + ("values", [float("-inf")], "values.0"), + ], +) +def test_invalid_types_are_rejected_before_coercion(codec_entries, encoded, field, value, path): + encoded["config"][field] = value + with pytest.raises(ValidationError) as exc: + TaskSpec.from_dict(encoded) + assert exc.value.path == f"task.config.{path}" + + +def test_numeric_integers_and_optional_float(codec_entries, encoded): + encoded["config"].update(gain=2, threshold=3) + parsed = TaskSpec.from_dict(encoded) + assert parsed.gain == 2.0 + assert parsed.threshold == 3.0 + + +@pytest.mark.parametrize("kind", ["missing", "unknown", "semantic"]) +def test_config_validation_paths(codec_entries, encoded, kind): + if kind == "missing": + del encoded["config"]["gain"] + elif kind == "unknown": + encoded["config"]["extra"] = 1 + else: + encoded["config"]["gain"] = -1 + with pytest.raises(ValidationError) as exc: + TaskSpec.from_dict(encoded) + assert exc.value.path == ("task.config.extra" if kind == "unknown" else "task.config.gain") + if kind == "missing": + assert "required task field" in str(exc.value) + assert "re-export" in str(exc.value) + + +@pytest.mark.parametrize("config", [None, [], "not an object"]) +def test_config_must_be_mapping(codec_entries, encoded, config): + encoded["config"] = config + with pytest.raises(ValidationError) as exc: + TaskSpec.from_dict(encoded) + assert exc.value.path == "task.config" + + +@pytest.mark.parametrize( + "kind", ["missing", "duplicate", "unavailable", "invalid", "missing_spec", "invalid_spec", "wrong_name"] +) +def test_spec_plugin_failures(codec_entries, encoded, kind): + selected = codec_entries[0] + if kind == "missing": + codec_entries.remove(selected) + elif kind == "duplicate": + codec_entries.append(SimpleNamespace(name="numeric/v1", load=Mock())) + elif kind == "unavailable": + selected.load.side_effect = ImportError("plugin dependency unavailable") + elif kind == "invalid": + selected.load.return_value = dict + elif kind == "missing_spec": + selected.load.return_value = type("MissingSpecTask", (NumericTask,), {"spec_type": None}) + elif kind == "invalid_spec": + selected.load.return_value = type("InvalidSpecTask", (NumericTask,), {"spec_type": dict}) + else: + wrong_spec = type("WrongName", (NumericSpec,), {"task_name": "wrong/v1"}) + selected.load.return_value = type("WrongTask", (NumericTask,), {"spec_type": wrong_spec}) + with pytest.raises(ValidationError) as exc: + TaskSpec.from_dict(encoded) + assert exc.value.path == "task.name" + if kind == "missing": + codec_entries[0].load.assert_not_called() + if kind == "duplicate": + selected.load.assert_not_called() + + +def test_artifact_cannot_select_arbitrary_import(codec_entries, encoded): + encoded["name"] = "os:system" + with pytest.raises(ValidationError) as exc: + TaskSpec.from_dict(encoded) + assert exc.value.path == "task.name" + for entry in codec_entries: + entry.load.assert_not_called() + + +def test_taskspec_model_contract(): + spec = NumericSpec(gain=2.0, values=[1.0], bounds=[[0.0, 1.0]]) + assert isinstance(spec, BaseModel) + assert spec.name == "numeric/v1" + assert "name" not in spec.model_dump() + assert "task_name" not in spec.model_dump() + with pytest.raises(PydanticValidationError) as exc: + spec.gain = 3.0 + assert exc.value.errors()[0]["type"] == "frozen_instance" + + +@pytest.mark.parametrize("mutation", ["nested_type", "nested_nonfinite", "semantic"]) +def test_serialization_revalidates_mutable_values(mutation): + spec = NumericSpec(gain=2.0, values=[1.0], bounds=[[0.0, 1.0]]) + if mutation == "nested_type": + spec.bounds[0][0] = True + path = "task.config.bounds.0.0" + elif mutation == "nested_nonfinite": + spec.values.append(float("nan")) + path = "task.config.values.1" + else: + # model_construct is an explicit escape hatch; serialization still + # establishes the public artifact contract for such instances. + spec = NumericSpec.model_construct(gain=-1.0, values=[], bounds=[]) + path = "task.config.gain" + with pytest.raises(ValidationError) as exc: + spec.to_dict() + assert exc.value.path == path + + +def test_invalid_plugin_defaults_are_validated(codec_entries, encoded): + class InvalidDefaultSpec(NumericSpec): + tolerance: float = Field(default=-1.0, gt=0) + + codec_entries[0].load.return_value = type("InvalidDefaultTask", (NumericTask,), {"spec_type": InvalidDefaultSpec}) + with pytest.raises(ValidationError) as exc: + TaskSpec.from_dict(encoded) + assert exc.value.path == "task.config.tolerance" + + +def test_serialization_uses_json_primitives(codec_entries): + spec = NumericSpec(gain=2, values=[1, 2], bounds=[[0, 1]], enabled=True) + wire = spec.to_dict() + json_wire = json.loads(json.dumps(wire, allow_nan=False)) + assert json_wire == wire + assert json_wire["config"]["bounds"] == [[0.0, 1.0]] + assert TaskSpec.from_dict(json_wire) == spec + + +def test_literal_interpolation_string_is_not_resolved(codec_entries, encoded): + encoded["config"]["label"] = "${oc.env:HOME}" + parsed = TaskSpec.from_dict(encoded) + assert parsed.label == "${oc.env:HOME}" + assert parsed.to_dict() == encoded + + +def test_yaml_composition_resolves_before_model_validation(tmp_path): + source = tmp_path / "task.yaml" + source.write_text( + "gain: 2.0\nvalues: [1.0, 2.0]\nbounds: [[0.0, 1.0]]\nthreshold: ${gain}\n", + encoding="utf-8", + ) + composed = OmegaConf.merge(OmegaConf.load(source), OmegaConf.create({"gain": 3.0, "enabled": True})) + config = OmegaConf.to_container(composed, resolve=True, throw_on_missing=True) + spec = NumericSpec.model_validate(config) + assert spec.gain == 3.0 + assert spec.threshold == spec.gain + assert spec.enabled is True + assert spec.to_dict()["config"] == spec.model_dump() diff --git a/motrix_deploy_mujoco/README.md b/motrix_deploy_mujoco/README.md index f7953ca8..7b9e2040 100644 --- a/motrix_deploy_mujoco/README.md +++ b/motrix_deploy_mujoco/README.md @@ -1,17 +1,22 @@ # motrix-deploy-mujoco -`motrix-deploy-mujoco` is the MuJoCo backend plugin for `motrix-deploy`. It discovers a registered MotrixLab -environment configuration, builds its `SceneCfg` through `MuJoCoSceneCompiler` with deployment-owned MuJoCo timestep and -solver settings, converts the policy-training position actuators to deployment torque motors, and implements the -`RobotInterface` lifecycle. +`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. -The plugin is advertised through the `motrix_deploy.backends` Python entry-point group and is loaded only when a -deployment recipe selects `backend.name=mujoco`. +With `motrix_deploy_tasks` installed and a matching exported artifact: -The workspace provides Go2 recipes for both registered scenes: +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy runtime.viewer=false duration_s=2.0 +``` -- `configs/deploy/sim2sim/go2_walk_sim2sim.yaml` selects `go2-walk-rough` and remains the CLI default. -- `configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml` selects `go2-walk-flat` explicitly. +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. -Both use a `0.002` second MuJoCo timestep and `100` solver iterations. The flat recipe resets the base at `z=0.331` to keep -the default feet clear of the plane. +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. diff --git a/motrix_deploy_mujoco/pyproject.toml b/motrix_deploy_mujoco/pyproject.toml index c39d8927..28e79efa 100644 --- a/motrix_deploy_mujoco/pyproject.toml +++ b/motrix_deploy_mujoco/pyproject.toml @@ -13,9 +13,9 @@ license = "Apache-2.0" dependencies = [ "motrix-deploy", "motrix-env-mujoco", - "motrix-envs", + "hydra-core>=1.3", "numpy>=1.26", ] [project.entry-points."motrix_deploy.backends"] -mujoco = "motrix_deploy_mujoco.plugin:create_backend" +mujoco = "motrix_deploy_mujoco.plugin:create_runtime" diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/__init__.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/__init__.py index 9b34e2d3..674b4492 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/__init__.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/__init__.py @@ -3,17 +3,16 @@ """SceneCfg-backed MuJoCo deployment backend plugin.""" -from motrix_deploy_mujoco.config import MujocoBackendConfig -from motrix_deploy_mujoco.interface import MujocoRobotInterface, wxyz_to_xyzw, xyzw_to_wxyz -from motrix_deploy_mujoco.plugin import create_backend +from motrix_deploy_mujoco.plugin import create_runtime +from motrix_deploy_mujoco.robot import wxyz_to_xyzw, xyzw_to_wxyz +from motrix_deploy_mujoco.runtime import MujocoRuntime from motrix_deploy_mujoco.viewer import MujocoGlfwViewer, MujocoKeyboardDevice __all__ = [ - "MujocoBackendConfig", "MujocoGlfwViewer", "MujocoKeyboardDevice", - "MujocoRobotInterface", - "create_backend", + "MujocoRuntime", + "create_runtime", "wxyz_to_xyzw", "xyzw_to_wxyz", ] diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/config.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/config.py deleted file mode 100644 index 103c2496..00000000 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/config.py +++ /dev/null @@ -1,79 +0,0 @@ -# Copyright Motphys Technology Co., Ltd. 2025, 2026 -# SPDX-License-Identifier: Apache-2.0 - -"""MuJoCo deployment backend configuration schema.""" - -from collections.abc import Mapping -from dataclasses import dataclass -from typing import Any - -import numpy as np - -from motrix_deploy.errors import ValidationError - - -@dataclass -class MujocoBackendConfig: - """Application-level scene selection and MuJoCo state mapping.""" - - scene: str - scene_mode: str - sim_dt: float - solver_iterations: int - base_position: tuple[float, float, float] - base_orientation_xyzw: tuple[float, float, float, float] - base_body_name: str - imu_site_name: str - gyro_sensor_name: str - accelerometer_sensor_name: str - global_linear_velocity_sensor_name: str - fall_height_m: float - fall_up_z: float - - def __post_init__(self) -> None: - if not self.scene: - raise ValidationError("backend.scene", "a registered environment name", self.scene) - if self.scene_mode not in ("train", "play"): - raise ValidationError("backend.scene_mode", "'train' or 'play'", self.scene_mode) - if not np.isfinite(self.sim_dt) or self.sim_dt <= 0: - raise ValidationError("backend.sim_dt", "a positive finite value", self.sim_dt) - if not isinstance(self.solver_iterations, int) or isinstance(self.solver_iterations, bool): - raise ValidationError("backend.solver_iterations", "a positive integer", self.solver_iterations) - if self.solver_iterations <= 0: - raise ValidationError("backend.solver_iterations", "a positive integer", self.solver_iterations) - for name in ( - "base_body_name", - "imu_site_name", - "gyro_sensor_name", - "accelerometer_sensor_name", - "global_linear_velocity_sensor_name", - ): - if not getattr(self, name): - raise ValidationError(f"backend.{name}", "a non-empty string", getattr(self, name)) - if not np.isfinite(self.fall_height_m) or self.fall_height_m <= 0: - raise ValidationError("backend.fall_height_m", "a positive finite value", self.fall_height_m) - if not np.isfinite(self.fall_up_z) or not -1.0 <= self.fall_up_z <= 1.0: - raise ValidationError("backend.fall_up_z", "a finite value in [-1, 1]", self.fall_up_z) - if len(self.base_position) != 3 or not np.isfinite(self.base_position).all(): - raise ValidationError("backend.base_position", "three finite values", self.base_position) - if len(self.base_orientation_xyzw) != 4 or not np.isfinite(self.base_orientation_xyzw).all(): - raise ValidationError("backend.base_orientation_xyzw", "four finite values", self.base_orientation_xyzw) - quaternion_norm = np.linalg.norm(self.base_orientation_xyzw) - if not np.isclose(quaternion_norm, 1.0, atol=1e-6): - raise ValidationError("backend.base_orientation_xyzw", "a unit quaternion", quaternion_norm) - - @classmethod - def from_mapping(cls, value: Mapping[str, Any]) -> "MujocoBackendConfig": - """Parse the backend section of a resolved deployment configuration.""" - values: dict[str, Any] = dict(value) - values.pop("name", None) - expected_fields = set(cls.__dataclass_fields__) - if set(values) != expected_fields: - raise ValidationError( - "backend", - f"fields {sorted(expected_fields)}", - f"missing={sorted(expected_fields - set(values))}, unknown={sorted(set(values) - expected_fields)}", - ) - values["base_position"] = tuple(values["base_position"]) - values["base_orientation_xyzw"] = tuple(values["base_orientation_xyzw"]) - return cls(**values) diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/interface.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/interface.py deleted file mode 100644 index c720e19d..00000000 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/interface.py +++ /dev/null @@ -1,374 +0,0 @@ -# Copyright Motphys Technology Co., Ltd. 2025, 2026 -# SPDX-License-Identifier: Apache-2.0 - -"""MuJoCo state/command adapter with no policy tensor logic.""" - -import logging -import time -from collections.abc import Callable -from typing import Any - -import numpy as np - -import motrix_envs # noqa: F401 registers built-in environment configurations -from motrix_deploy.backend import RobotInterface -from motrix_deploy.contracts import HealthStatus, RobotCapabilities, RobotCommand, RobotSpec, RobotState, float32_array -from motrix_deploy.errors import ValidationError -from motrix_deploy_mujoco.config import MujocoBackendConfig -from motrix_deploy_mujoco.transform import convert_position_actuators_to_motors -from motrix_deploy_mujoco.viewer import MujocoGlfwViewer -from motrix_env_core import SimCfg, registry -from motrix_env_core.config.scene import SystemCameraCfg -from motrix_env_core.input import KeyboardDevice -from motrix_env_mujoco.compiler import MuJoCoSceneCompiler - -logger = logging.getLogger(__name__) - - -def wxyz_to_xyzw(quaternion: object) -> np.ndarray: - """Convert MuJoCo's explicit ``wxyz`` order to the public ``xyzw`` order.""" - return np.asarray(quaternion, dtype=np.float32)[[1, 2, 3, 0]] - - -def xyzw_to_wxyz(quaternion: object) -> np.ndarray: - """Convert the public ``xyzw`` order to MuJoCo's explicit ``wxyz`` order.""" - return np.asarray(quaternion)[[3, 0, 1, 2]] - - -class MujocoRobotInterface(RobotInterface): - """Build a MuJoCo scene and apply canonical hybrid-PD commands as torque.""" - - def __init__( - self, - config: MujocoBackendConfig, - *, - control_period_s: float, - render: bool = False, - viewer_factory: Callable[[Any, SystemCameraCfg], Any] = MujocoGlfwViewer, - ) -> None: - try: - import mujoco - except ImportError as error: - raise RuntimeError("MuJoCo backend support requires the 'motrix-deploy-mujoco' package") from error - env_cfg = registry.make_env_config(config.scene, mode=config.scene_mode) - if not np.isclose(control_period_s, env_cfg.ctrl_dt, atol=1e-12, rtol=0.0): - raise ValidationError("backend.scene.ctrl_dt", str(control_period_s), env_cfg.ctrl_dt) - ratio = control_period_s / config.sim_dt - if not np.isclose(ratio, round(ratio), atol=1e-9) or round(ratio) < 1: - raise ValidationError( - "backend.control_period_s", - "an integer multiple of scene SimCfg.dt", - control_period_s, - ) - self.config = config - self._mj = mujoco - self._scene = env_cfg.scene - self._sim = SimCfg(dt=config.sim_dt, solver_iterations=config.solver_iterations) - self._control_period_s = control_period_s - self._physics_substeps = round(ratio) - self._render = render - self._model: Any = None - self._data: Any = None - self._viewer = viewer_factory(mujoco, self._scene.system_camera) if render else None - self._spec: RobotSpec | None = None - self._capabilities: RobotCapabilities | None = None - self._joint_qpos_indices = np.empty(0, dtype=np.int64) - self._joint_qvel_indices = np.empty(0, dtype=np.int64) - self._actuator_indices = np.empty(0, dtype=np.int64) - self._base_body_id = -1 - self._base_qpos_address = -1 - self._last_communication_ns = 0 - self._health_reason = "backend is not open" - self._opened = False - self._closed = False - - @property - def capabilities(self) -> RobotCapabilities: - if self._capabilities is None: - raise RuntimeError("MuJoCo capabilities are available after open()") - return self._capabilities - - def open(self, spec: RobotSpec) -> None: - if self._opened and not self._closed: - raise RuntimeError("MuJoCo backend is already open") - model = self._build_model(spec) - data = self._mj.MjData(model) - self._model = model - self._data = data - self._spec = spec - self._bind_base() - self._bind_joints_and_actuators(spec) - self._validate_torque_actuator_contract(spec) - self._validate_sensors() - self._mj.mj_resetData(model, data) - qpos = data.qpos - qpos[self._base_qpos_address : self._base_qpos_address + 3] = self.config.base_position - qpos[self._base_qpos_address + 3 : self._base_qpos_address + 7] = xyzw_to_wxyz( - self.config.base_orientation_xyzw - ) - qpos[self._joint_qpos_indices] = spec.default_joint_position - data.qvel.fill(0.0) - data.ctrl[self._actuator_indices] = 0.0 - self._mj.mj_forward(model, data) - self._capabilities = RobotCapabilities( - control_modes=("joint_pd",), - state_fields=frozenset( - { - "joint_position", - "joint_velocity", - "base_orientation_xyzw", - "base_angular_velocity", - "base_linear_acceleration", - "base_position", - "base_linear_velocity", - } - ), - supports_rendering=True, - max_command_rate_hz=1.0 / self._control_period_s, - stop_semantics="zero_torque", - ) - self._opened = True - self._closed = False - self._last_communication_ns = time.monotonic_ns() - self._health_reason = "" - if self._render: - assert self._viewer is not None - self._viewer.open(model, data) - self._sync_viewer() - - def get_keyboard_device(self) -> KeyboardDevice: - """Return keyboard events from the deployment viewer window.""" - if self._viewer is None: - raise RuntimeError("MuJoCo keyboard input requires viewer=true") - return self._viewer.keyboard_device - - def read_state(self, timeout_s: float) -> RobotState: - self._require_open() - if timeout_s <= 0: - raise TimeoutError(f"MuJoCo state timeout must be positive, got {timeout_s}") - self._check_viewer_running() - self._last_communication_ns = time.monotonic_ns() - return self._state() - - def write_command(self, command: RobotCommand) -> None: - self._require_open() - assert self._model is not None and self._data is not None and self._spec is not None - joint_count = self._spec.joint_count - for field_name in ("joint_position", "joint_velocity", "feedforward_torque", "kp", "kd"): - value = getattr(command, field_name) - if value.shape != (joint_count,) or not np.isfinite(value).all(): - raise ValidationError(f"command.{field_name}", f"finite shape ({joint_count},)", value) - lower = self._spec.position_lower - upper = self._spec.position_upper - if np.any(command.joint_position < lower) or np.any(command.joint_position > upper): - raise ValidationError("command.joint_position", "inside RobotSpec position range", command.joint_position) - for _ in range(self._physics_substeps): - position = self._data.qpos[self._joint_qpos_indices] - velocity = self._data.qvel[self._joint_qvel_indices] - torque = ( - command.kp * (command.joint_position - position) - + command.kd * (command.joint_velocity - velocity) - + command.feedforward_torque - ) - self._data.ctrl[self._actuator_indices] = np.clip( - torque, - -self._spec.torque_limit, - self._spec.torque_limit, - ) - self._mj.mj_step(self._model, self._data) - self._last_communication_ns = time.monotonic_ns() - self._update_fall_health() - self._sync_viewer() - - def health(self) -> HealthStatus: - return HealthStatus( - healthy=self._opened and not self._closed and not self._health_reason, - reason=self._health_reason, - last_successful_communication_ns=self._last_communication_ns, - ) - - def stop(self) -> None: - if not self._opened or self._closed or self._data is None: - return - if self._joint_qpos_indices.size: - self._data.ctrl[self._actuator_indices] = 0.0 - - def close(self) -> None: - if self._closed: - return - viewer = self._viewer - try: - if viewer is not None: - viewer.close() - deadline = time.monotonic() + 2.0 - while viewer.is_running() and time.monotonic() < deadline: - time.sleep(0.001) - if viewer.is_running(): - raise RuntimeError("MuJoCo viewer did not stop within 2 seconds") - finally: - self._data = None - self._model = None - self._closed = True - - def _sync_viewer(self) -> None: - if self._viewer is None: - return - self._check_viewer_running() - self._viewer.sync() - - def _check_viewer_running(self) -> None: - if self._viewer is None: - return - if not self._viewer.is_running(): - raise KeyboardInterrupt("MuJoCo viewer was closed") - - def _bind_base(self) -> None: - assert self._model is not None - body_id = self._name_id(self._mj.mjtObj.mjOBJ_BODY, self.config.base_body_name, "base body") - joint_address = int(self._model.body_jntadr[body_id]) - if joint_address < 0 or self._model.jnt_type[joint_address] != self._mj.mjtJoint.mjJNT_FREE: - raise ValidationError("backend.base_body_name", "a body with a free joint", self.config.base_body_name) - self._base_body_id = body_id - self._base_qpos_address = int(self._model.jnt_qposadr[joint_address]) - - def _build_model(self, robot: RobotSpec) -> Any: - model_spec = MuJoCoSceneCompiler().create_spec(self._scene, self._sim) - source_model = model_spec.compile() - self._model = source_model - self._bind_joints_and_actuators(robot) - self._validate_position_actuator_contract(robot) - - actuator_names = convert_position_actuators_to_motors( - self._mj, - model_spec, - source_model, - self._actuator_indices, - robot, - ) - logger.info( - "Converting %d actuators from position servos to torque motors with MjSpec: %s", - len(actuator_names), - ", ".join(actuator_names), - ) - model = model_spec.compile() - logger.info("Built MuJoCo deployment model from SceneCfg %s", self.config.scene) - return model - - def _bind_joints_and_actuators(self, spec: RobotSpec) -> None: - assert self._model is not None - qpos_indices: list[int] = [] - qvel_indices: list[int] = [] - actuator_indices: list[int] = [] - for name in spec.joint_names: - joint_id = self._name_id(self._mj.mjtObj.mjOBJ_JOINT, name, "joint") - if self._model.jnt_type[joint_id] != self._mj.mjtJoint.mjJNT_HINGE: - raise ValidationError(f"backend.joints.{name}", "a one-DoF hinge joint", self._model.jnt_type[joint_id]) - matches = np.flatnonzero(self._model.actuator_trnid[:, 0] == joint_id) - if matches.size != 1: - raise ValidationError(f"backend.actuators.{name}", "exactly one actuator", matches.tolist()) - qpos_indices.append(int(self._model.jnt_qposadr[joint_id])) - qvel_indices.append(int(self._model.jnt_dofadr[joint_id])) - actuator_indices.append(int(matches[0])) - self._joint_qpos_indices = np.asarray(qpos_indices, dtype=np.int64) - self._joint_qvel_indices = np.asarray(qvel_indices, dtype=np.int64) - self._actuator_indices = np.asarray(actuator_indices, dtype=np.int64) - - def _validate_position_actuator_contract(self, spec: RobotSpec) -> None: - assert self._model is not None - indices = self._actuator_indices - ctrl_range = self._model.actuator_ctrlrange[indices] - force_range = self._model.actuator_forcerange[indices] - kp = self._model.actuator_gainprm[indices, 0] - stiffness = -self._model.actuator_biasprm[indices, 1] - kd = -self._model.actuator_biasprm[indices, 2] - checks = ( - ("ctrl_lower", ctrl_range[:, 0], spec.position_lower), - ("ctrl_upper", ctrl_range[:, 1], spec.position_upper), - ("force_lower", force_range[:, 0], -spec.torque_limit), - ("force_upper", force_range[:, 1], spec.torque_limit), - ) - for name, actual, expected in checks: - if not np.allclose(actual, expected, atol=1e-6, rtol=0.0): - raise ValidationError(f"backend.actuators.{name}", expected.tolist(), actual.tolist()) - if not np.allclose(kp, stiffness, atol=1e-7, rtol=0.0): - raise ValidationError("backend.actuators.kp", "gain equals stiffness", (kp.tolist(), stiffness.tolist())) - if np.any(kp < 0) or np.any(kd < 0): - raise ValidationError("backend.actuators.gains", "non-negative kp/kd", (kp.tolist(), kd.tolist())) - - def _validate_torque_actuator_contract(self, spec: RobotSpec) -> None: - assert self._model is not None - indices = self._actuator_indices - checks = ( - ("ctrl_lower", self._model.actuator_ctrlrange[indices, 0], -spec.torque_limit), - ("ctrl_upper", self._model.actuator_ctrlrange[indices, 1], spec.torque_limit), - ("force_lower", self._model.actuator_forcerange[indices, 0], -spec.torque_limit), - ("force_upper", self._model.actuator_forcerange[indices, 1], spec.torque_limit), - ("gain", self._model.actuator_gainprm[indices, 0], np.ones(spec.joint_count)), - ("bias", self._model.actuator_biastype[indices], np.zeros(spec.joint_count)), - ) - for name, actual, expected in checks: - if not np.allclose(actual, expected, atol=1e-6, rtol=0.0): - raise ValidationError(f"backend.torque_actuators.{name}", expected.tolist(), actual.tolist()) - - def _validate_sensors(self) -> None: - assert self._model is not None - self._name_id(self._mj.mjtObj.mjOBJ_SITE, self.config.imu_site_name, "IMU site") - for field_name, expected_dimension in ( - ("gyro_sensor_name", 3), - ("accelerometer_sensor_name", 3), - ("global_linear_velocity_sensor_name", 3), - ): - sensor_name = getattr(self.config, field_name) - sensor_id = self._name_id(self._mj.mjtObj.mjOBJ_SENSOR, sensor_name, "sensor") - actual_dimension = int(self._model.sensor_dim[sensor_id]) - if actual_dimension != expected_dimension: - raise ValidationError(f"backend.{field_name}", f"a {expected_dimension}D sensor", actual_dimension) - - def _state(self) -> RobotState: - assert self._model is not None and self._data is not None and self._spec is not None - orientation_xyzw = wxyz_to_xyzw(self._data.xquat[self._base_body_id]) - return RobotState( - sample_time_ns=round(self._data.time * 1e9), - receive_time_ns=self._last_communication_ns, - joint_position=np.asarray(self._data.qpos[self._joint_qpos_indices], dtype=np.float32), - joint_velocity=np.asarray(self._data.qvel[self._joint_qvel_indices], dtype=np.float32), - base_orientation_xyzw=orientation_xyzw, - base_angular_velocity=self._sensor(self.config.gyro_sensor_name), - base_linear_acceleration=self._sensor(self.config.accelerometer_sensor_name), - base_position=np.asarray(self._data.xpos[self._base_body_id], dtype=np.float32), - base_linear_velocity=self._sensor(self.config.global_linear_velocity_sensor_name), - ) - - def _sensor(self, name: str) -> np.ndarray: - assert self._model is not None and self._data is not None - sensor_id = self._name_id(self._mj.mjtObj.mjOBJ_SENSOR, name, "sensor") - address = int(self._model.sensor_adr[sensor_id]) - dimension = int(self._model.sensor_dim[sensor_id]) - return float32_array( - np.asarray(self._data.sensordata[address : address + dimension], dtype=np.float32), - path=f"backend.sensors.{name}", - shape=(dimension,), - ) - - def _update_fall_health(self) -> None: - assert self._data is not None - height = self._data.xpos[self._base_body_id, 2] - rotation = self._data.xmat[self._base_body_id].reshape(3, 3) - up_z = rotation[2, 2] - if not np.isfinite(height) or not np.isfinite(up_z): - self._health_reason = f"non-finite base pose: height={height}, up_z={up_z}" - elif height < self.config.fall_height_m: - self._health_reason = f"base height {height:.6f}m is below {self.config.fall_height_m:.6f}m" - elif up_z < self.config.fall_up_z: - self._health_reason = f"base up_z {up_z:.6f} is below {self.config.fall_up_z:.6f}" - - def _name_id(self, object_type: Any, name: str, description: str) -> int: - assert self._model is not None - object_id = int(self._mj.mj_name2id(self._model, object_type, name)) - if object_id < 0: - raise ValidationError(f"backend.{description}", f"existing name {name!r}", "missing") - return object_id - - def _require_open(self) -> None: - if not self._opened or self._closed or self._model is None or self._data is None: - raise RuntimeError("MuJoCo backend is not open") diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/plugin.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/plugin.py index e45bdbb1..2e24fa34 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/plugin.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/plugin.py @@ -1,23 +1,15 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Entry-point factory for the MuJoCo deployment backend.""" +"""Typed entry-point factory for the MuJoCo simulation runtime.""" -from collections.abc import Mapping -from typing import Any +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_deploy_mujoco.runtime import MujocoRuntime -from motrix_deploy.backend import BackendCreateContext, RobotInterface -from motrix_deploy_mujoco.config import MujocoBackendConfig -from motrix_deploy_mujoco.interface import MujocoRobotInterface +def create_runtime(config: SimulationRuntimeConfig) -> MujocoRuntime: + """Create a simulation whose robot contract is derived from its scene.""" + return MujocoRuntime(config) -def create_backend(config: Mapping[str, Any], context: BackendCreateContext) -> RobotInterface: - """Construct the MuJoCo backend selected by a deployment recipe.""" - return MujocoRobotInterface( - MujocoBackendConfig.from_mapping(config), - control_period_s=context.control.period_s, - render=context.viewer, - ) - -__all__ = ["create_backend"] +__all__ = ["create_runtime"] diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py new file mode 100644 index 00000000..5ea4ef33 --- /dev/null +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py @@ -0,0 +1,400 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot I/O adapter bound to an externally owned MuJoCo simulation.""" + +from __future__ import annotations + +import logging +import time +from typing import TYPE_CHECKING, cast + +import numpy as np +from numpy.typing import ArrayLike, NDArray + +from motrix_deploy.contracts import ( + HealthStatus, + JointControlMode, + JointServoCommand, + JointTorqueCommand, + RobotCapabilities, + RobotCommand, + RobotSpec, + RobotState, +) +from motrix_deploy.errors import ValidationError +from motrix_deploy.robot.interface import RobotInterface +from motrix_deploy_mujoco.transform import convert_actuators_to_motors +from motrix_env_core.config.scene.base import RobotCfg + +if TYPE_CHECKING: + import mujoco + + from motrix_deploy_mujoco.runtime import MujocoRuntime + +logger = logging.getLogger(__name__) + + +def wxyz_to_xyzw(quaternion: ArrayLike) -> NDArray[np.float32]: + """Convert MuJoCo's explicit ``wxyz`` order to the public ``xyzw`` order.""" + return np.asarray(quaternion, dtype=np.float32)[[1, 2, 3, 0]] + + +def xyzw_to_wxyz(quaternion: ArrayLike) -> NDArray[np.generic]: + """Convert the public ``xyzw`` order to MuJoCo's ``wxyz`` order, preserving dtype.""" + return np.asarray(quaternion)[[3, 0, 1, 2]] + + +class Robot(RobotInterface): + """Robot I/O and joint control in an externally owned MuJoCo simulation.""" + + def __init__(self, runtime: MujocoRuntime, spec: RobotSpec) -> None: + self.config = runtime.config + self._runtime = runtime + self._cfg = cast(RobotCfg, self._runtime.scene.objs.robot) + self._command: RobotCommand | None = None + self._spec = spec + self._capabilities: RobotCapabilities | None = None + self._joint_qpos_indices: NDArray[np.int64] = np.empty(0, dtype=np.int64) + self._joint_qvel_indices: NDArray[np.int64] = np.empty(0, dtype=np.int64) + self._actuator_indices: NDArray[np.int64] = np.empty(0, dtype=np.int64) + self._base_body_id = -1 + self._sensor_slices: dict[str, slice] = {} + self._last_communication_ns = 0 + self._health_reason = "backend is not open" + self._opened = False + self._closed = False + + @property + def spec(self) -> RobotSpec: + return self._spec + + @property + def capabilities(self) -> RobotCapabilities: + if self._capabilities is None: + raise RuntimeError("MuJoCo capabilities are available after open()") + return self._capabilities + + def bind(self) -> None: + spec = self._spec + self._bind_base() + self._bind_joints_and_actuators(spec) + self._validate_torque_actuator_contract(spec) + self._bind_sensors() + self._runtime.reset(self._joint_qpos_indices, spec.default_joint_position) + self._command = None + self._capabilities = RobotCapabilities( + control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), + state_fields=frozenset( + { + "joint_position", + "joint_velocity", + "base_orientation_xyzw", + "base_angular_velocity", + "base_linear_acceleration", + "base_position", + "base_linear_velocity", + } + ), + privileged_state_fields=frozenset({"base_position", "base_linear_velocity"}), + supports_rendering=True, + max_command_rate_hz=1.0 / self._runtime.sim.dt, + stop_semantics="zero_torque", + ) + self._opened = True + self._closed = False + self._last_communication_ns = time.monotonic_ns() + self._health_reason = "" + + def read_state(self, timeout_s: float) -> RobotState: + self._require_open() + if timeout_s <= 0: + raise TimeoutError(f"MuJoCo state timeout must be positive, got {timeout_s}") + self._runtime.check_viewer_running() + self._last_communication_ns = time.monotonic_ns() + return self._state() + + def write_command(self, command: RobotCommand) -> None: + self._require_open() + assert self._runtime.model is not None and self._runtime.data is not None + joint_count = self._spec.joint_count + fields: tuple[str, ...] + if isinstance(command, JointServoCommand): + fields = ("joint_position", "joint_velocity", "feedforward_torque", "kp", "kd") + elif isinstance(command, JointTorqueCommand): + fields = ("torque",) + else: + raise ValidationError("command", "JointServoCommand or JointTorqueCommand", type(command).__name__) + for field_name in fields: + value = getattr(command, field_name) + if value.shape != (joint_count,) or not np.isfinite(value).all(): + raise ValidationError(f"command.{field_name}", f"finite shape ({joint_count},)", value) + if isinstance(command, JointServoCommand): + if np.any(command.joint_position < self._spec.position_lower) or np.any( + command.joint_position > self._spec.position_upper + ): + raise ValidationError( + "command.joint_position", "inside RobotSpec position range", command.joint_position + ) + if np.any(command.kp < 0) or np.any(command.kd < 0): + raise ValidationError("command.gains", "non-negative kp/kd", (command.kp, command.kd)) + torque = command.feedforward_torque + torque_field = "feedforward_torque" + else: + torque = command.torque + torque_field = "torque" + if np.any(np.abs(torque) > self._spec.torque_limit): + raise ValidationError(f"command.{torque_field}", "inside RobotSpec torque limits", torque) + + # Snapshot the command: the caller may reuse or mutate its arrays afterwards. + self._command = type(command)(**{name: getattr(command, name).copy() for name in fields}) + self._last_communication_ns = time.monotonic_ns() + + def apply_control(self) -> None: + assert self._runtime.data is not None + command = self._command + if command is None: + self._runtime.data.ctrl[self._actuator_indices] = 0.0 + return + if isinstance(command, JointTorqueCommand): + self._runtime.data.ctrl[self._actuator_indices] = command.torque + return + position = self._runtime.data.qpos[self._joint_qpos_indices] + velocity = self._runtime.data.qvel[self._joint_qvel_indices] + torque = ( + command.kp * (command.joint_position - position) + + command.kd * (command.joint_velocity - velocity) + + command.feedforward_torque + ) + self._runtime.data.ctrl[self._actuator_indices] = np.clip( + torque, -self._spec.torque_limit, self._spec.torque_limit + ) + + def health(self) -> HealthStatus: + return HealthStatus( + healthy=self._opened and not self._closed and not self._health_reason, + reason=self._health_reason, + last_successful_communication_ns=self._last_communication_ns, + ) + + def stop(self) -> None: + self._command = None + if not self._opened or self._closed or self._runtime.data is None: + return + if self._joint_qpos_indices.size: + self._runtime.data.ctrl[self._actuator_indices] = 0.0 + + def open(self) -> None: + """Check that the owning runtime has prepared and bound robot I/O.""" + self._require_open() + + def close(self) -> None: + """Detach robot I/O without closing the externally owned simulation.""" + self.stop() + self._closed = True + self._health_reason = "robot interface is closed" + + def invalidate(self) -> None: + self._command = None + self._opened = False + self._closed = True + self._health_reason = "simulation is not open" + + def _bind_base(self) -> None: + assert self._runtime.model is not None + body_name = self._cfg.resolved_base_link_name + body_id = self._name_id(self._runtime.mj.mjtObj.mjOBJ_BODY, body_name, "base body") + joint_address = int(self._runtime.model.body_jntadr[body_id]) + if joint_address < 0 or self._runtime.model.jnt_type[joint_address] != self._runtime.mj.mjtJoint.mjJNT_FREE: + raise ValidationError("backend.robot.base_link_name", "a body with a free joint", body_name) + self._base_body_id = body_id + + def prepare(self, source_model: mujoco.MjModel, model_spec: mujoco.MjSpec) -> None: + robot = self._spec + self._bind_joints_and_actuators(robot) + self._validate_actuator_transmission() + motors = source_model.actuator_biastype[self._actuator_indices] == self._runtime.mj.mjtBias.mjBIAS_NONE + if np.all(motors): + self._validate_torque_actuator_contract(robot) + joint_ids = source_model.actuator_trnid[self._actuator_indices, 0] + if not np.all(source_model.jnt_limited[joint_ids]) or not np.allclose( + source_model.jnt_range[joint_ids], + np.column_stack((robot.position_lower, robot.position_upper)), + atol=1e-6, + rtol=0.0, + ): + raise ValidationError("backend.joints.position_range", "RobotSpec position limits", "mismatch") + return + if np.any(motors): + raise ValidationError("backend.actuators", "uniform position servos or torque motors", "mixed actuators") + self._validate_position_actuator_contract(robot) + + actuator_names = convert_actuators_to_motors( + model_spec, + source_model, + self._actuator_indices, + robot, + ) + logger.info( + "Converting %d actuators from position servos to torque motors with MjSpec: %s", + len(actuator_names), + ", ".join(actuator_names), + ) + + def _bind_joints_and_actuators(self, spec: RobotSpec) -> None: + assert self._runtime.model is not None + qpos_indices: list[int] = [] + qvel_indices: list[int] = [] + actuator_indices: list[int] = [] + for name in spec.joint_names: + joint_id = self._name_id(self._runtime.mj.mjtObj.mjOBJ_JOINT, self._cfg.resolve_name(name), "joint") + if self._runtime.model.jnt_type[joint_id] != self._runtime.mj.mjtJoint.mjJNT_HINGE: + raise ValidationError( + f"backend.joints.{name}", "a one-DoF hinge joint", self._runtime.model.jnt_type[joint_id] + ) + matches = np.flatnonzero(self._runtime.model.actuator_trnid[:, 0] == joint_id) + if matches.size != 1: + raise ValidationError(f"backend.actuators.{name}", "exactly one actuator", matches.tolist()) + qpos_indices.append(int(self._runtime.model.jnt_qposadr[joint_id])) + qvel_indices.append(int(self._runtime.model.jnt_dofadr[joint_id])) + actuator_indices.append(int(matches[0])) + self._joint_qpos_indices = np.asarray(qpos_indices, dtype=np.int64) + self._joint_qvel_indices = np.asarray(qvel_indices, dtype=np.int64) + self._actuator_indices = np.asarray(actuator_indices, dtype=np.int64) + + def _validate_actuator_transmission(self) -> None: + assert self._runtime.model is not None + model = self._runtime.model + indices = self._actuator_indices + if not np.all(model.actuator_trntype[indices] == self._runtime.mj.mjtTrn.mjTRN_JOINT): + raise ValidationError("backend.actuators.transmission", "direct joint transmission", "unsupported") + gear = np.zeros((indices.size, 6)) + gear[:, 0] = 1.0 + if not np.allclose(model.actuator_gear[indices], gear): + raise ValidationError("backend.actuators.gear", "unit joint transmission", model.actuator_gear[indices]) + for field, expected in ( + ("dyntype", self._runtime.mj.mjtDyn.mjDYN_NONE), + ("gaintype", self._runtime.mj.mjtGain.mjGAIN_FIXED), + ("ctrllimited", True), + ("forcelimited", True), + ): + actual = getattr(model, f"actuator_{field}")[indices] + if not np.all(actual == expected): + raise ValidationError(f"backend.actuators.{field}", str(expected), actual) + + def _validate_position_actuator_contract(self, spec: RobotSpec) -> None: + assert self._runtime.model is not None + indices = self._actuator_indices + ctrl_range = self._runtime.model.actuator_ctrlrange[indices] + force_range = self._runtime.model.actuator_forcerange[indices] + kp = self._runtime.model.actuator_gainprm[indices, 0] + stiffness = -self._runtime.model.actuator_biasprm[indices, 1] + kd = -self._runtime.model.actuator_biasprm[indices, 2] + checks = ( + ("ctrl_lower", ctrl_range[:, 0], spec.position_lower), + ("ctrl_upper", ctrl_range[:, 1], spec.position_upper), + ("force_lower", force_range[:, 0], -spec.torque_limit), + ("force_upper", force_range[:, 1], spec.torque_limit), + ) + for name, actual, expected in checks: + if not np.allclose(actual, expected, atol=1e-6, rtol=0.0): + raise ValidationError(f"backend.actuators.{name}", expected.tolist(), actual.tolist()) + if not np.allclose(kp, stiffness, atol=1e-7, rtol=0.0): + raise ValidationError("backend.actuators.kp", "gain equals stiffness", (kp.tolist(), stiffness.tolist())) + if not np.all(self._runtime.model.actuator_biastype[indices] == self._runtime.mj.mjtBias.mjBIAS_AFFINE): + raise ValidationError("backend.actuators.bias", "position servo affine bias", "unsupported") + if not np.allclose(self._runtime.model.actuator_biasprm[indices, 0], 0.0): + raise ValidationError("backend.actuators.bias", "no constant force bias", "unsupported") + if not np.isfinite(kp).all() or not np.isfinite(kd).all() or np.any(kp < 0) or np.any(kd < 0): + raise ValidationError("backend.actuators.gains", "finite non-negative kp/kd", (kp.tolist(), kd.tolist())) + + def _validate_torque_actuator_contract(self, spec: RobotSpec) -> None: + assert self._runtime.model is not None + indices = self._actuator_indices + checks = ( + ("ctrl_lower", self._runtime.model.actuator_ctrlrange[indices, 0], -spec.torque_limit), + ("ctrl_upper", self._runtime.model.actuator_ctrlrange[indices, 1], spec.torque_limit), + ("force_lower", self._runtime.model.actuator_forcerange[indices, 0], -spec.torque_limit), + ("force_upper", self._runtime.model.actuator_forcerange[indices, 1], spec.torque_limit), + ("gain", self._runtime.model.actuator_gainprm[indices, 0], np.ones(spec.joint_count)), + ("bias", self._runtime.model.actuator_biastype[indices], np.zeros(spec.joint_count)), + ) + for name, actual, expected in checks: + if not np.allclose(actual, expected, atol=1e-6, rtol=0.0): + raise ValidationError(f"backend.torque_actuators.{name}", str(expected.tolist()), actual.tolist()) + + def _bind_sensors(self) -> None: + assert self._runtime.model is not None + self._sensor_slices.clear() + for role, expected_type in ( + ("base_angular_velocity", self._runtime.mj.mjtSensor.mjSENS_GYRO), + ("base_linear_acceleration", self._runtime.mj.mjtSensor.mjSENS_ACCELEROMETER), + ("base_linear_velocity", self._runtime.mj.mjtSensor.mjSENS_FRAMELINVEL), + ): + path = f"runtime.sensor_bindings.{role}" + local_name = getattr(self.config.sensor_bindings, role) + if local_name is None: + raise ValidationError(path, "an explicit model-local sensor name", "missing") + sensor_name = self._cfg.resolve_name(local_name) + sensor_id = self._name_id(self._runtime.mj.mjtObj.mjOBJ_SENSOR, sensor_name, f"sensor_bindings.{role}") + actual_dimension = int(self._runtime.model.sensor_dim[sensor_id]) + if actual_dimension != 3: + raise ValidationError(path, "a 3D sensor", actual_dimension) + actual_type = self._runtime.model.sensor_type[sensor_id] + if actual_type != expected_type: + raise ValidationError(path, expected_type.name, int(actual_type)) + if role in ("base_angular_velocity", "base_linear_acceleration"): + site_id = int(self._runtime.model.sensor_objid[sensor_id]) + if ( + self._runtime.model.sensor_objtype[sensor_id] != self._runtime.mj.mjtObj.mjOBJ_SITE + or self._runtime.model.site_bodyid[site_id] != self._base_body_id + or not np.allclose(self._runtime.model.site_quat[site_id, 1:], 0.0, atol=1e-7, rtol=0.0) + ): + raise ValidationError(path, "a site on the base body aligned with its body frame", sensor_name) + else: + model = self._runtime.model + object_type = model.sensor_objtype[sensor_id] + object_id = int(model.sensor_objid[sensor_id]) + body_id = ( + int(model.site_bodyid[object_id]) + if object_type == self._runtime.mj.mjtObj.mjOBJ_SITE + else object_id + ) + if ( + model.sensor_refid[sensor_id] != -1 + or object_type not in (self._runtime.mj.mjtObj.mjOBJ_SITE, self._runtime.mj.mjtObj.mjOBJ_BODY) + or body_id != self._base_body_id + ): + raise ValidationError(path, "a world-frame linear velocity sensor on the base body", sensor_name) + address = int(self._runtime.model.sensor_adr[sensor_id]) + self._sensor_slices[role] = slice(address, address + actual_dimension) + + def _state(self) -> RobotState: + assert self._runtime.model is not None and self._runtime.data is not None + orientation_xyzw = wxyz_to_xyzw(self._runtime.data.xquat[self._base_body_id]) + return RobotState( + sample_time_ns=round(self._runtime.data.time * 1e9), + receive_time_ns=self._last_communication_ns, + joint_position=np.asarray(self._runtime.data.qpos[self._joint_qpos_indices], dtype=np.float32), + joint_velocity=np.asarray(self._runtime.data.qvel[self._joint_qvel_indices], dtype=np.float32), + base_orientation_xyzw=orientation_xyzw, + base_angular_velocity=self._sensor("base_angular_velocity"), + base_linear_acceleration=self._sensor("base_linear_acceleration"), + base_position=np.asarray(self._runtime.data.xpos[self._base_body_id], dtype=np.float32), + base_linear_velocity=self._sensor("base_linear_velocity"), + ) + + def _sensor(self, role: str) -> NDArray[np.float32]: + assert self._runtime.data is not None + return np.array(self._runtime.data.sensordata[self._sensor_slices[role]], dtype=np.float32, copy=True) + + def _name_id(self, object_type: mujoco.mjtObj, name: str, description: str) -> int: + assert self._runtime.model is not None + object_id = int(self._runtime.mj.mj_name2id(self._runtime.model, object_type, name)) + if object_id < 0: + raise ValidationError(f"backend.{description}", f"existing name {name!r}", "missing") + return object_id + + def _require_open(self) -> None: + if not self._opened or self._closed or self._runtime.model is None or self._runtime.data is None: + raise RuntimeError("MuJoCo backend is not open") diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py new file mode 100644 index 00000000..9cd8e238 --- /dev/null +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py @@ -0,0 +1,274 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Explicit external owner of MuJoCo resources, robot bindings and physics.""" + +from __future__ import annotations + +import logging +import time +from collections.abc import Callable +from copy import deepcopy +from dataclasses import replace +from types import ModuleType +from typing import TYPE_CHECKING, TypeVar, cast + +from motrix_deploy.contracts import RobotSpec +from motrix_deploy.robot.interface import RobotInterface +from motrix_deploy.runtime.base import SimulationRuntime +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_env_core.input import KeyboardDevice + +if TYPE_CHECKING: + import mujoco + + from motrix_deploy.runtime.control import ControlSession + +import numpy as np +from numpy.typing import NDArray + +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.result import RolloutResult +from motrix_deploy.runtime.scheduler import FixedStepScheduler, RealtimeScheduler +from motrix_deploy_mujoco.robot import Robot +from motrix_deploy_mujoco.viewer import MujocoGlfwViewer +from motrix_env_core.config.scene import SystemCameraCfg +from motrix_env_core.config.scene.base import RobotCfg +from motrix_env_mujoco.compiler import MuJoCoSceneCompiler + +CommandT = TypeVar("CommandT") + +logger = logging.getLogger(__name__) + + +class MujocoRuntime(SimulationRuntime): + """Own a scene and expose independently controlled robot I/O and physics.""" + + def __init__( + self, + config: SimulationRuntimeConfig, + control_period_s: float | None = None, + viewer_factory: Callable[[ModuleType, SystemCameraCfg], MujocoGlfwViewer] = MujocoGlfwViewer, + ) -> None: + super().__init__() + try: + import mujoco + except ImportError as error: + raise RuntimeError("MuJoCo backend support requires the 'motrix-deploy-mujoco' package") from error + self.scene = deepcopy(config.scene) + if not isinstance(self.scene.objs.robot, RobotCfg): + raise ValidationError("backend.scene.objs.robot", "a RobotCfg", self.scene.objs.robot) + self.mj = mujoco + self.sim = config.physics + self.physics_substeps: int | None = None + self.model: mujoco.MjModel | None = None + self.data: mujoco.MjData | None = None + self.viewer = viewer_factory(mujoco, self.scene.system_camera) if config.render else None + self.config = config + self.control_period_s: float | None = None + if control_period_s is not None: + self._configure_control_period(control_period_s) + self.realtime = config.realtime + self._closed = False + self._source_spec: mujoco.MjSpec | None = None + robot_spec = self._derive_robot_spec() + self._robot = Robot(self, robot_spec) + + @property + def robot(self) -> RobotInterface: + return self._robot + + def _configure_control_period(self, period_s: float) -> None: + ratio = period_s / self.sim.dt + if not np.isfinite(ratio) or ratio < 1 or not np.isclose(ratio, round(ratio), atol=1e-9): + raise ValidationError("control.period_s", "an integer multiple of physics.dt", period_s) + self.control_period_s = period_s + self.physics_substeps = round(ratio) + + def bind_control_session(self, control: ControlSession[CommandT]) -> None: + """Bind control ownership and derive physics cadence from the session.""" + # Resolve identity conflicts before changing the runtime's timing. + self._resolve_control(control) + self._configure_control_period(control.period_s) + super().bind_control_session(control) + + def _derive_robot_spec(self) -> RobotSpec: + """Inspect physical limits, never controller gains, before binding robot I/O.""" + robot = cast(RobotCfg, self.scene.objs.robot) + self._source_spec = MuJoCoSceneCompiler().create_spec(self.scene, self.sim) + model = self._source_spec.compile() + joint_names = tuple(robot.key_pose.joint_names) + actuators = [] + for name in joint_names: + joint_id = model.joint(robot.resolve_name(name)).id + matches = np.flatnonzero( + (model.actuator_trntype == self.mj.mjtTrn.mjTRN_JOINT) & (model.actuator_trnid[:, 0] == joint_id) + ) + if matches.size != 1: + raise ValidationError(f"backend.actuators.{name}", "exactly one joint actuator", matches.tolist()) + actuators.append(int(matches[0])) + indices = np.asarray(actuators, dtype=np.int64) + joint_ids = np.asarray([model.joint(robot.resolve_name(name)).id for name in joint_names]) + motors = model.actuator_biastype[indices] == self.mj.mjtBias.mjBIAS_NONE + position_range = np.where(motors[:, None], model.jnt_range[joint_ids], model.actuator_ctrlrange[indices]) + return RobotSpec( + base_link_name=robot.base_link_name, + joint_names=joint_names, + default_joint_position=np.asarray(robot.key_pose.poses[robot.init_key_pose], dtype=np.float32), + position_lower=position_range[:, 0].astype(np.float32), + position_upper=position_range[:, 1].astype(np.float32), + torque_limit=model.actuator_forcerange[indices, 1].astype(np.float32), + ) + + def get_keyboard_device(self) -> KeyboardDevice: + """Return keyboard input owned by this simulation's viewer.""" + if self.viewer is None: + raise RuntimeError("MuJoCo keyboard input requires viewer=true") + return self.viewer.keyboard_device + + def build_model(self) -> mujoco.MjModel: + """Validate and transform registered robot servos before final compilation.""" + model_spec = self._source_spec or MuJoCoSceneCompiler().create_spec(self.scene, self.sim) + self._source_spec = None + source_model = model_spec.compile() + self.model = source_model + self._robot.prepare(source_model, model_spec) + model = model_spec.compile() + logger.info("Built MuJoCo deployment model from SceneCfg %s", type(self.scene).__name__) + return model + + def open(self) -> None: + """Compile, prepare, bind, and reset the registered robot.""" + if self._opened: + raise RuntimeError("MuJoCo simulation is already open") + self._closed = False + try: + self.model = self.build_model() + self.data = self.mj.MjData(self.model) + self._robot.bind() + if self.viewer is not None: + self.viewer.open(self.model, self.data) + self.sync_viewer() + self._opened = True + except BaseException: + try: + self.close() + except Exception: + logger.exception("Cleanup failed after MuJoCo session open failure") + raise + + def reset( + self, + joint_qpos_indices: NDArray[np.int64], + joint_position: NDArray[np.float32], + ) -> None: + assert self.model is not None and self.data is not None + self.mj.mj_resetData(self.model, self.data) + qpos = self.data.qpos + # qpos0 already composes RobotCfg's attach transform with the imported + # model root pose. Keep it intact instead of applying placement twice. + qpos[joint_qpos_indices] = joint_position + self.data.qvel.fill(0.0) + self.data.ctrl.fill(0.0) + self.mj.mj_forward(self.model, self.data) + + def run( + self, + control: ControlSession[CommandT] | None = None, + *, + steps: int | None = None, + realtime: bool | None = None, + ) -> RolloutResult: + """Drive shared control at physics boundaries inside an already-open world. + + Control writes cached commands only. This owner re-evaluates robot PD on + every physics substep, and paces complete control intervals independently + of the shared control lifecycle. Stopping control leaves this world open. + """ + if not self._opened: + raise RuntimeError("MuJoCo simulation with a registered robot is not open") + control = self._resolve_control(control) + if steps is not None and (not isinstance(steps, int) or isinstance(steps, bool) or steps <= 0): + raise ValidationError("steps", "a positive integer", steps) + self._configure_control_period(control.period_s) + if realtime is None: + realtime = self.realtime + paced = self.viewer is not None if realtime is None else realtime + assert self.physics_substeps is not None and self.control_period_s is not None + scheduler = (RealtimeScheduler if paced else FixedStepScheduler)(self.control_period_s) + assert self.data is not None and self.model is not None + initial_time_s = float(self.data.time) + simulation_time_s = 0.0 + try: + if control.start(): + # Establish relative simulation time after control initialization. + initial_time_s = float(self.data.time) + scheduler.reset() + substep = 0 + while control.active: + if substep % self.physics_substeps == 0: + if steps is not None and control.completed_steps >= steps: + break + self.check_viewer_running() + if not control.tick(elapsed_time_s=float(self.data.time) - initial_time_s): + break + self._robot.apply_control() + self.mj.mj_step(self.model, self.data) + substep += 1 + if substep % self.physics_substeps == 0: + self.sync_viewer() + # Absolute deadlines avoid accumulated wall-clock drift, + # and include the final interval without an extra tick. + scheduler.wait(substep // self.physics_substeps) + except (Exception, KeyboardInterrupt) as error: + control.fail(error) + finally: + simulation_time_s = float(self.data.time) - initial_time_s + control.stop() + result = control.result(steps=steps, overrun_count=scheduler.overrun_count) + return replace( + result, + simulation_time_s=simulation_time_s, + real_time_factor=simulation_time_s / result.wall_time_s if result.wall_time_s > 0 else 0.0, + ) + + def advance_control_period(self) -> None: + """Advance physics, re-evaluating cached robot PD at every substep.""" + if not self._opened: + raise RuntimeError("MuJoCo simulation with a registered robot is not open") + if self.physics_substeps is None: + raise RuntimeError("Bind a control session before advancing a control period") + self.check_viewer_running() + for _ in range(self.physics_substeps): + self._robot.apply_control() + self.mj.mj_step(self.model, self.data) + self.sync_viewer() + + def check_viewer_running(self) -> None: + if self.viewer is not None and not self.viewer.is_running(): + raise KeyboardInterrupt("MuJoCo viewer was closed") + + def sync_viewer(self) -> None: + if self.viewer is not None: + self.check_viewer_running() + self.viewer.sync() + + def close(self) -> None: + if self._closed: + return + try: + if self.control is not None and self.control.active: + self.control.stop() + if self.viewer is not None: + self.viewer.close() + deadline = time.monotonic() + 2.0 + while self.viewer.is_running() and time.monotonic() < deadline: + time.sleep(0.001) + if self.viewer.is_running(): + raise RuntimeError("MuJoCo viewer did not stop within 2 seconds") + finally: + self._robot.invalidate() + self.data = None + self.model = None + self._opened = False + self._closed = True diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/transform.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/transform.py index 2a45fcd1..120a9685 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/transform.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/transform.py @@ -3,22 +3,29 @@ """Deployment-specific MuJoCo specification transforms.""" -from typing import Any +from __future__ import annotations + +from typing import TYPE_CHECKING import numpy as np +from numpy.typing import NDArray from motrix_deploy.contracts import RobotSpec from motrix_deploy.errors import ValidationError +if TYPE_CHECKING: + import mujoco + -def convert_position_actuators_to_motors( - mj: Any, - model_spec: Any, - source_model: Any, - actuator_indices: np.ndarray, +def convert_actuators_to_motors( + model_spec: mujoco.MjSpec, + source_model: mujoco.MjModel, + actuator_indices: NDArray[np.int64], robot: RobotSpec, ) -> list[str]: - """Convert canonical position actuators in an assembled MjSpec to torque motors.""" + """Convert selected, prevalidated position servos to torque motors.""" + import mujoco as mj + actuator_names: list[str] = [] for index in actuator_indices: name = mj.mj_id2name(source_model, mj.mjtObj.mjOBJ_ACTUATOR, index) @@ -38,4 +45,4 @@ def convert_position_actuators_to_motors( return actuator_names -__all__ = ["convert_position_actuators_to_motors"] +__all__ = ["convert_actuators_to_motors"] diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/viewer.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/viewer.py index cb151873..ba852f04 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/viewer.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/viewer.py @@ -3,7 +3,16 @@ """Minimal GLFW viewer and focused keyboard device for MuJoCo deployment.""" -from typing import Any +from __future__ import annotations + +from types import ModuleType +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + import ctypes + + import glfw + import mujoco from motrix_env_core.config.scene import SystemCameraCfg from motrix_env_core.input import KeyboardDevice @@ -12,7 +21,7 @@ class MujocoKeyboardDevice(KeyboardDevice): """Freeze keyboard events delivered to one focused GLFW viewer window.""" - def __init__(self, viewer: "MujocoGlfwViewer") -> None: + def __init__(self, viewer: MujocoGlfwViewer) -> None: self._viewer = viewer self._pressing: set[str] = set() self._pending_down: set[str] = set() @@ -45,7 +54,9 @@ def is_key_up(self, key: str) -> bool: def is_pressing(self, key: str) -> bool: return self._normalize_query(key) in self._frame_pressing - def _on_key(self, window: Any, key: int, scancode: int, action: int, modifiers: int) -> None: + def _on_key( + self, window: ctypes._Pointer[glfw._GLFWwindow], key: int, scancode: int, action: int, modifiers: int + ) -> None: del window, modifiers glfw = self._viewer._glfw assert glfw is not None @@ -62,13 +73,13 @@ def _on_key(self, window: Any, key: int, scancode: int, action: int, modifiers: self._pressing.remove(name) self._pending_up.add(name) - def _on_focus(self, window: Any, focused: int) -> None: + def _on_focus(self, window: ctypes._Pointer[glfw._GLFWwindow], focused: int) -> None: del window if not focused: self._pending_up.update(self._pressing) self._pressing.clear() - def _on_close(self, window: Any) -> None: + def _on_close(self, window: ctypes._Pointer[glfw._GLFWwindow]) -> None: del window self._interrupted = True @@ -114,27 +125,27 @@ class MujocoGlfwViewer: def __init__( self, - mujoco_module: Any, + mujoco_module: ModuleType, camera_config: SystemCameraCfg = SystemCameraCfg(), *, - glfw_module: Any | None = None, + glfw_module: ModuleType | None = None, ) -> None: self._mj = mujoco_module self._glfw = glfw_module self._camera_config = camera_config - self._model: Any = None - self._data: Any = None - self._window: Any = None - self._scene: Any = None - self._context: Any = None - self._camera: Any = None - self._option: Any = None - self._perturb: Any = None + self._model: mujoco.MjModel | None = None + self._data: mujoco.MjData | None = None + self._window: ctypes._Pointer[glfw._GLFWwindow] | None = None + self._scene: mujoco.MjvScene | None = None + self._context: mujoco.MjrContext | None = None + self._camera: mujoco.MjvCamera | None = None + self._option: mujoco.MjvOption | None = None + self._perturb: mujoco.MjvPerturb | None = None self._glfw_initialized = False self._cursor_position = (0.0, 0.0) self.keyboard_device = MujocoKeyboardDevice(self) - def open(self, model: Any, data: Any) -> None: + def open(self, model: mujoco.MjModel, data: mujoco.MjData) -> None: """Create the window and MuJoCo rendering resources.""" if self._window is not None: raise RuntimeError("MuJoCo GLFW viewer is already open") @@ -180,9 +191,13 @@ def open(self, model: Any, data: Any) -> None: raise def sync(self) -> None: - """Render current state without polling input or advancing physics.""" + """Pump window events and render without consuming keyboard input or stepping.""" window = self._require_window() - if self._glfw.window_should_close(window): + assert self._glfw is not None + # Window responsiveness must not depend on a keyboard command binding. + # Callbacks retain pending key edges until KeyboardDevice.poll freezes them. + self._glfw.poll_events() + if self.keyboard_device._interrupted or self._glfw.window_should_close(window): raise KeyboardInterrupt assert self._model is not None assert self._data is not None @@ -208,7 +223,10 @@ def sync(self) -> None: self._glfw.swap_buffers(window) def is_running(self) -> bool: - return self._window is not None and not self._glfw.window_should_close(self._window) + if self._window is None: + return False + assert self._glfw is not None + return not self._glfw.window_should_close(self._window) def close(self) -> None: """Release rendering resources and the GLFW window; safe to repeat.""" @@ -237,7 +255,8 @@ def close(self) -> None: self._perturb = None self.keyboard_device._clear() - def _on_cursor_position(self, window: Any, xpos: float, ypos: float) -> None: + def _on_cursor_position(self, window: ctypes._Pointer[glfw._GLFWwindow], xpos: float, ypos: float) -> None: + assert self._glfw is not None previous_x, previous_y = self._cursor_position self._cursor_position = (xpos, ypos) dx = xpos - previous_x @@ -262,16 +281,16 @@ def _on_cursor_position(self, window: Any, xpos: float, ypos: float) -> None: action = self._mj.mjtMouse.mjMOUSE_ZOOM self._move_camera(action, dx / height, dy / height) - def _on_scroll(self, window: Any, xoffset: float, yoffset: float) -> None: + def _on_scroll(self, window: ctypes._Pointer[glfw._GLFWwindow], xoffset: float, yoffset: float) -> None: del window, xoffset self._move_camera(self._mj.mjtMouse.mjMOUSE_ZOOM, 0.0, -0.05 * yoffset) - def _move_camera(self, action: Any, dx: float, dy: float) -> None: + def _move_camera(self, action: mujoco.mjtMouse, dx: float, dy: float) -> None: if self._model is None or self._scene is None or self._camera is None: return self._mj.mjv_moveCamera(self._model, action, dx, dy, self._scene, self._camera) - def _require_window(self) -> Any: + def _require_window(self) -> ctypes._Pointer[glfw._GLFWwindow]: if self._window is None: raise RuntimeError("MuJoCo GLFW viewer is not open") return self._window diff --git a/motrix_deploy_mujoco/tests/conftest.py b/motrix_deploy_mujoco/tests/conftest.py new file mode 100644 index 00000000..f4b04e11 --- /dev/null +++ b/motrix_deploy_mujoco/tests/conftest.py @@ -0,0 +1,30 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Shared explicit scene fixture for robot control integration tests.""" + +import numpy as np +import pytest +from scene_helpers import build_scene + +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_env_core.config.sim import SimCfg + + +@pytest.fixture +def control_gains() -> tuple[np.ndarray, np.ndarray]: + """Static test controller configuration, independent of model servo gains.""" + return np.ones(12, dtype=np.float32) * 30.0, np.ones(12, dtype=np.float32) * 0.8 + + +@pytest.fixture +def control_scene_config() -> SimulationRuntimeConfig: + return SimulationRuntimeConfig( + scene=build_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", + ), + ) diff --git a/motrix_deploy_mujoco/tests/control_helpers.py b/motrix_deploy_mujoco/tests/control_helpers.py new file mode 100644 index 00000000..e9c64a81 --- /dev/null +++ b/motrix_deploy_mujoco/tests/control_helpers.py @@ -0,0 +1,40 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Test-owned controller for backend lifecycle and cadence contracts.""" + +import numpy as np + +from motrix_deploy.contracts import FloatArray, JointServoCommand, RobotSpec, RobotState +from motrix_deploy.runtime.context import PolicyContext +from motrix_deploy.task import DeployTask +from motrix_env_core.input import PlanarVelocityCommand + + +class DummyServoTask(DeployTask[PlanarVelocityCommand]): + """Keep generic driver tests independent of examples and trained artifacts.""" + + def __init__(self, robot: RobotSpec, kp: FloatArray, kd: FloatArray) -> None: + self.robot = robot + self.kp = kp + self.kd = kd + self._zeros = np.zeros(robot.joint_count, dtype=np.float32) + + def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: + pass + + def build_observation(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: + return self._zeros.copy() + + def process_action(self, action: FloatArray) -> JointServoCommand: + return JointServoCommand( + joint_position=self.robot.default_joint_position + action, + joint_velocity=self._zeros.copy(), + feedforward_torque=self._zeros.copy(), + kp=self.kp, + kd=self.kd, + ) + + def validate_command(self, command: PlanarVelocityCommand) -> None: + if command.batch_size != 1 or np.any(command.values != 0.0): + raise ValueError("DummyServoTask accepts one zero velocity command") diff --git a/motrix_deploy_mujoco/tests/scene_helpers.py b/motrix_deploy_mujoco/tests/scene_helpers.py new file mode 100644 index 00000000..5197b913 --- /dev/null +++ b/motrix_deploy_mujoco/tests/scene_helpers.py @@ -0,0 +1,23 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Complete scenes assembled locally by adapter tests.""" + +from motrix_robots.unitree import UnitreeGo2Robot + +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg + + +@configclass +class AdapterSceneObjsCfg(SceneObjsCfg): + floor: FlatTerrainCfg | None = None + + +def build_scene() -> SceneCfg: + return SceneCfg( + objs=AdapterSceneObjsCfg( + robot=UnitreeGo2Robot(translation=(0.0, 0.0, -0.114)), + floor=FlatTerrainCfg(friction=(0.6, 0.005, 0.0001)), + ) + ) diff --git a/motrix_deploy_mujoco/tests/test_examples.py b/motrix_deploy_mujoco/tests/test_examples.py new file mode 100644 index 00000000..139a9b5f --- /dev/null +++ b/motrix_deploy_mujoco/tests/test_examples.py @@ -0,0 +1,211 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""End-to-end smoke tests for the standalone MuJoCo deployment example.""" + +from __future__ import annotations + +import json +import runpy +import subprocess +import sys +from pathlib import Path + +import numpy as np +import pytest +from control_helpers import DummyServoTask + +ROOT = Path(__file__).parents[2] +EXAMPLES = ROOT / "examples" +EXAMPLE_SCRIPT = EXAMPLES / "deploy_to_sim.py" + + +@pytest.fixture +def onnx_policy(tmp_path, control_scene_config, control_gains): + """Generate raw ONNX locally; never depend on a trained checkout policy.""" + import onnx + from onnx import TensorProto, helper, numpy_helper + + from motrix_deploy.task import create_task + from motrix_deploy_mujoco.runtime import MujocoRuntime + from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec + + runtime = MujocoRuntime(control_scene_config, control_period_s=0.02) + robot = runtime.robot.spec + kp, kd = control_gains + task_spec = Go2WalkTaskSpec( + action_scale=[0.1] * robot.joint_count, + command_lower=[-1.0] * 3, + command_upper=[1.0] * 3, + command_scale=[1.0] * 3, + feet_phase_offsets=[0.0, 0.5, 0.5, 0.0], + gait_frequency_hz=1.0, + standing_threshold=0.05, + termination_min_up_z=-1.0, + termination_min_base_height=None, + kp=kp.tolist(), + kd=kd.tolist(), + action_lower=[-1.0] * robot.joint_count, + action_upper=[1.0] * robot.joint_count, + ) + task = create_task(task_spec, robot) + from motrix_deploy.runtime.context import PolicyContext + from motrix_env_core.input import ConstantPlanarVelocityBinding + + with runtime: + runtime.robot.open() + context = PolicyContext( + step=0, + elapsed_time_s=0.0, + command=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)).read_command(), + ) + state = runtime.robot.read_state(0.1) + task.reset(state, context) + observation_size = task.build_observation(state, context).size + runtime.robot.close() + weight = numpy_helper.from_array(np.zeros((observation_size, robot.joint_count), dtype=np.float32), name="weight") + graph = helper.make_graph( + [helper.make_node("MatMul", ["obs", "weight"], ["actions"])], + "deterministic_example_fixture", + [helper.make_tensor_value_info("obs", TensorProto.FLOAT, [1, observation_size])], + [helper.make_tensor_value_info("actions", TensorProto.FLOAT, [1, robot.joint_count])], + [weight], + ) + model = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)]) + model.ir_version = 10 + onnx.checker.check_model(model) + path = tmp_path / "fixture.onnx" + onnx.save(model, path) + return path + + +def _result(stdout: str) -> dict[str, object]: + start = stdout.find("{") + assert start >= 0, stdout + return json.loads(stdout[start:]) + + +def test_example_builders_are_import_safe() -> None: + namespace = runpy.run_path(str(EXAMPLE_SCRIPT), run_name="motrix_deploy_mujoco_example_test") + assert callable(namespace["main"]) + assert callable(namespace["ExampleSceneObjsCfg"]) + + +@pytest.mark.parametrize("backend", ["mujoco"]) +def test_programmatic_example_uses_shared_control_execution(tmp_path, onnx_policy, backend) -> None: + command = [ + sys.executable, + "-c", + "import runpy, sys; from pathlib import Path; " + "namespace = runpy.run_path(sys.argv.pop(1)); " + "namespace['main'].__globals__['POLICY_PATH'] = Path(sys.argv.pop(1)); " + "raise SystemExit(namespace['main']())", + str(EXAMPLE_SCRIPT), + str(onnx_policy), + "--backend", + backend, + "--headless", + "--steps", + "3", + ] + completed = subprocess.run(command, check=False, capture_output=True, text=True, cwd=tmp_path) + assert completed.returncode == 0, completed.stderr + result = _result(completed.stdout) + assert result["success"] is True + assert result["completed_steps"] == 3 + assert result["exit_reason"] == "completed" + assert result["error"] is None + assert result["simulation_time_s"] == pytest.approx(0.06) + assert "latency" in result # Shared control execution report, not a separate preview loop. + repeated = subprocess.run(command, check=False, capture_output=True, text=True, cwd=tmp_path) + assert repeated.returncode == 0, repeated.stderr + assert _result(repeated.stdout)["trace_sha256"] == result["trace_sha256"] + + +def test_simulation_factory_supports_explicit_bound_control_and_world_ownership( + control_scene_config, control_gains +) -> None: + from motrix_deploy.policy import NoOpPolicyRuntime + from motrix_deploy.robot.interface import RobotInterface + from motrix_deploy.runtime.config import SimulationRuntimeConfig + from motrix_deploy.runtime.control import ControlSession + from motrix_deploy.runtime.factory import create_simulation_runtime + from motrix_env_core.input import ConstantPlanarVelocityBinding + + config = SimulationRuntimeConfig( + scene=control_scene_config.scene, + physics=control_scene_config.physics, + sensor_bindings=control_scene_config.sensor_bindings, + ) + runtime = create_simulation_runtime("mujoco", config) + robot = runtime.robot + assert isinstance(robot, RobotInterface) + assert not runtime.opened + assert runtime.model is None and runtime.data is None + control = ControlSession( + robot=robot, + task=DummyServoTask(robot.spec, *control_gains), + policy=NoOpPolicyRuntime(robot.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=0.012, + state_timeout_s=0.1, + ) + runtime.bind_control_session(control) + assert runtime.control_period_s == control.period_s + assert runtime.physics_substeps == 6 + assert runtime.control is control and control.robot is robot + assert not runtime.opened + assert runtime.model is None and runtime.data is None + + with runtime: + result = runtime.run(steps=2) + assert result.success and result.completed_steps == 2 + assert runtime.robot is robot and runtime.control is control + assert not control.active + # Control shutdown releases robot I/O, not the simulation world's resources. + assert runtime.opened and runtime.model is not None and runtime.data is not None + before = runtime.data.time + runtime.advance_control_period() + assert runtime.data.time > before + assert not runtime.opened + assert runtime.model is None and runtime.data is None + + +def test_robot_commands_do_not_advance_or_own_the_world(control_scene_config, control_gains) -> None: + from motrix_deploy.runtime.context import PolicyContext + from motrix_deploy_mujoco.runtime import MujocoRuntime + from motrix_env_core.input import ConstantPlanarVelocityBinding + + config = control_scene_config + simulation = MujocoRuntime(config, control_period_s=0.02) + spec = simulation.robot.spec + task = DummyServoTask(spec, *control_gains) + interface = simulation.robot + assert interface.spec is spec + with simulation: + interface.open() + before = interface.read_state(0.1) + context = PolicyContext( + step=0, + elapsed_time_s=0.0, + command=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)).read_command(), + ) + observation = task.build_observation(before, context) + np.testing.assert_array_equal(observation, np.zeros(spec.joint_count, dtype=np.float32)) + assert observation.dtype == np.float32 + interface.write_command(task.process_action(np.zeros(spec.joint_count, dtype=np.float32))) + after_write = interface.read_state(0.1) + assert after_write.sample_time_ns == before.sample_time_ns + np.testing.assert_array_equal(after_write.joint_position, before.joint_position) + simulation.advance_control_period() + advanced = interface.read_state(0.1) + assert advanced.sample_time_ns > before.sample_time_ns + interface.stop() + interface.close() + # Closing the robot port releases control, not the shared simulation. + assert simulation.model is not None and simulation.data is not None + time_before = simulation.data.time + simulation.advance_control_period() + assert simulation.data.time > time_before + np.testing.assert_array_equal(simulation.data.ctrl, np.zeros(simulation.model.nu)) + assert simulation.model is None and simulation.data is None diff --git a/motrix_deploy_mujoco/tests/test_joint_control.py b/motrix_deploy_mujoco/tests/test_joint_control.py new file mode 100644 index 00000000..a3c9d24c --- /dev/null +++ b/motrix_deploy_mujoco/tests/test_joint_control.py @@ -0,0 +1,143 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Servo and torque command boundaries and physics-owned application cadence.""" + +import numpy as np +import pytest + +from motrix_deploy.contracts import JointControlMode, JointServoCommand, JointTorqueCommand +from motrix_deploy.errors import ValidationError +from motrix_deploy_mujoco.runtime import MujocoRuntime +from motrix_env_core.config.scene.actuator import MotorActuatorCfg +from motrix_env_mujoco.compiler import MuJoCoSceneCompiler + + +def _servo(spec, **overrides): + zeros = np.zeros(spec.joint_count, dtype=np.float32) + fields = dict( + joint_position=spec.default_joint_position, joint_velocity=zeros, feedforward_torque=zeros, kp=zeros, kd=zeros + ) + fields.update(overrides) + return JointServoCommand(**fields) + + +def test_auto_spec_inspection_reuses_original_source_spec_on_first_open(control_scene_config, monkeypatch): + original = MuJoCoSceneCompiler.create_spec + specs = [] + + def record_spec(compiler, scene, sim): + spec = original(compiler, scene, sim) + specs.append(spec) + return spec + + monkeypatch.setattr(MuJoCoSceneCompiler, "create_spec", record_spec) + runtime = MujocoRuntime(control_scene_config, 0.02) + assert len(specs) == 1 and runtime._source_spec is specs[0] + assert runtime.model is runtime.data is None + with runtime: + assert len(specs) == 1 + with runtime: + assert len(specs) == 2 # Reopen starts from source, not already transformed motors. + + +def test_torque_is_snapshotted_and_applied_directly_every_physics_substep(control_scene_config, monkeypatch): + with MujocoRuntime(control_scene_config, 0.02) as runtime: + assert runtime.robot.capabilities.control_modes == (JointControlMode.SERVO, JointControlMode.TORQUE) + torque = np.linspace(-1, 1, runtime.robot.spec.joint_count, dtype=np.float32) + expected = torque.copy() + snapshots = [] + step = runtime.mj.mj_step + + def record_step(model, data): + snapshots.append(data.ctrl[runtime.robot._actuator_indices].copy()) + step(model, data) + + monkeypatch.setattr(runtime.mj, "mj_step", record_step) + runtime.robot.write_command(JointTorqueCommand(torque)) + torque[:] = 0 + assert runtime.data.time == 0 + runtime.advance_control_period() + assert len(snapshots) == runtime.physics_substeps + for applied in snapshots: + np.testing.assert_array_equal(applied, expected) + assert runtime.data.time == pytest.approx(runtime.control_period_s) + runtime.robot.stop() + np.testing.assert_array_equal(runtime.data.ctrl, 0) + + +@pytest.mark.parametrize("kind", ["torque", "feedforward", "position", "shape"]) +def test_command_limits_are_validated_before_application(control_scene_config, kind): + with MujocoRuntime(control_scene_config, 0.02) as runtime: + spec = runtime.robot.spec + if kind == "torque": + command = JointTorqueCommand(spec.torque_limit + np.float32(1)) + field = "torque" + elif kind == "feedforward": + command = _servo(spec, feedforward_torque=spec.torque_limit + np.float32(1)) + field = "feedforward_torque" + elif kind == "position": + command = _servo(spec, joint_position=spec.position_upper + np.float32(1)) + field = "joint_position" + else: + command = JointTorqueCommand(np.zeros(spec.joint_count - 1, dtype=np.float32)) + field = "torque" + with pytest.raises(ValidationError, match=f"command.{field}"): + runtime.robot.write_command(command) + np.testing.assert_array_equal(runtime.data.ctrl, 0) + + +def test_servo_total_pd_torque_is_clipped_and_mode_can_change(control_scene_config): + with MujocoRuntime(control_scene_config, 0.02) as runtime: + spec = runtime.robot.spec + runtime.robot.write_command( + _servo(spec, joint_position=spec.position_upper, kp=np.full(spec.joint_count, 1e6, dtype=np.float32)) + ) + runtime.robot.apply_control() + np.testing.assert_allclose(runtime.data.ctrl[runtime.robot._actuator_indices], spec.torque_limit) + torque = np.zeros(spec.joint_count, dtype=np.float32) + runtime.robot.write_command(JointTorqueCommand(torque)) + runtime.robot.apply_control() + np.testing.assert_array_equal(runtime.data.ctrl, 0) + + +def test_source_motor_model_derives_joint_limits_and_accepts_torque(control_scene_config): + robot = control_scene_config.scene.objs.robot + # Test-owned effort bounds differ from joint limits to exercise motor spec derivation. + robot.actuators = { + f"motor_{name}": MotorActuatorCfg(joint_name=name, ctrl_range=(-5.0, 5.0), force_range=(-5.0, 5.0)) + for name in robot.key_pose.joint_names + } + with MujocoRuntime(control_scene_config, 0.02) as runtime: + ids = [runtime.model.joint(name).id for name in runtime.robot.spec.joint_names] + np.testing.assert_allclose(runtime.robot.spec.position_lower, runtime.model.jnt_range[ids, 0]) + np.testing.assert_allclose(runtime.robot.spec.position_upper, runtime.model.jnt_range[ids, 1]) + np.testing.assert_array_equal(runtime.robot.spec.torque_limit, 5.0) + runtime.robot.write_command(JointTorqueCommand(np.ones(runtime.robot.spec.joint_count, dtype=np.float32))) + runtime.advance_control_period() + np.testing.assert_array_equal(runtime.data.ctrl, 1) + + +@pytest.mark.parametrize("field", ["gear", "ctrllimited", "forcelimited", "dyntype"]) +def test_noncanonical_source_actuation_is_rejected(control_scene_config, monkeypatch, field): + original = MuJoCoSceneCompiler.create_spec + + def invalid_spec(compiler, scene, sim): + spec = original(compiler, scene, sim) + actuator = next(iter(spec.actuators)) + if field == "gear": + actuator.gear = [2, 0, 0, 0, 0, 0] + elif field in {"ctrllimited", "forcelimited"}: + setattr(actuator, field, 0) + else: + import mujoco + + actuator.dyntype = mujoco.mjtDyn.mjDYN_FILTER + actuator.dynprm[0] = 0.01 + return spec + + monkeypatch.setattr(MuJoCoSceneCompiler, "create_spec", invalid_spec) + runtime = MujocoRuntime(control_scene_config, 0.02) + with pytest.raises(ValidationError, match=f"backend.actuators.{field}"): + runtime.open() + assert runtime.model is runtime.data is None diff --git a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py index 74c441f0..b247b1de 100644 --- a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py +++ b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py @@ -4,7 +4,9 @@ """MuJoCo Go2 adapter contract tests.""" import logging +import subprocess import sys +import textwrap from collections.abc import Callable from dataclasses import replace from pathlib import Path @@ -12,22 +14,26 @@ import numpy as np import pytest +from motrix_robots.unitree import UnitreeGo2Robot from omegaconf import OmegaConf +from scene_helpers import AdapterSceneObjsCfg, build_scene -from motrix_deploy.backend import KeyboardDeviceProvider -from motrix_deploy.contracts import RobotCommand, RobotSpec +from motrix_deploy.contracts import JointServoCommand, RobotSpec from motrix_deploy.errors import ValidationError +from motrix_deploy.robot.interface import KeyboardDeviceProvider +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig from motrix_deploy_mujoco import ( - MujocoBackendConfig, - MujocoRobotInterface, + MujocoRuntime, wxyz_to_xyzw, xyzw_to_wxyz, ) -from motrix_env_core.config.scene import SystemCameraCfg +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SystemCameraCfg +from motrix_env_core.config.sim import SimCfg +from motrix_env_mujoco.compiler import MuJoCoSceneCompiler ROOT = Path(__file__).parents[2] -CONFIG_PATH = ROOT / "configs/deploy/sim2sim/go2_walk_sim2sim.yaml" -FLAT_CONFIG_PATH = ROOT / "configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml" +CONFIG_PATH = ROOT / "motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/sim.yaml" +FLAT_CONFIG_PATH = CONFIG_PATH JOINT_NAMES = ( "FL_hip_joint", "FL_thigh_joint", @@ -45,8 +51,34 @@ CONTROL_PERIOD_S = 0.02 -def _config(config_path: Path = CONFIG_PATH) -> MujocoBackendConfig: - return MujocoBackendConfig.from_mapping(OmegaConf.load(config_path).backend) +def _mapping(config_path: Path = CONFIG_PATH) -> dict[str, Any]: + values = OmegaConf.to_container(OmegaConf.load(config_path).runtime) + values.pop("backend") + values.pop("deploy_env_id") + values.pop("robot_id") + values["scene"] = build_scene() + values["scene"].objs.robot.translation = tuple(values.pop("robot_translation")) + values["scene"].objs.robot.rotation = tuple(values.pop("robot_rotation")) + return values + + +def _config(config_path: Path | None = None) -> SimulationRuntimeConfig: + if config_path is not None: + return SimulationRuntimeConfig.from_mapping(_mapping(config_path)) + return SimulationRuntimeConfig( + scene=build_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", + ), + ) + + +def create_runtime(config, *, control_period_s, render=False, **kwargs): + runtime = MujocoRuntime(replace(config, render=render), control_period_s=control_period_s, **kwargs) + return runtime def _backend( @@ -54,9 +86,9 @@ def _backend( render: bool = False, control_period_s: float = CONTROL_PERIOD_S, viewer_factory: Callable[[Any, SystemCameraCfg], Any] | None = None, -) -> MujocoRobotInterface: +) -> MujocoRuntime: kwargs = {} if viewer_factory is None else {"viewer_factory": viewer_factory} - return MujocoRobotInterface( + return create_runtime( _config(), control_period_s=control_period_s, render=render, @@ -78,9 +110,9 @@ def _spec(joint_names: tuple[str, ...] = JOINT_NAMES) -> RobotSpec: ) -def _command(position: np.ndarray, *, kp: float = 35.0, kd: float = 0.5) -> RobotCommand: +def _command(position: np.ndarray, *, kp: float = 35.0, kd: float = 0.5) -> JointServoCommand: zeros = np.zeros(12, dtype=np.float32) - return RobotCommand( + return JointServoCommand( joint_position=position, joint_velocity=zeros, feedforward_torque=zeros, @@ -89,8 +121,8 @@ def _command(position: np.ndarray, *, kp: float = 35.0, kd: float = 0.5) -> Robo ) -def test_backend_control_period_must_match_scene_config() -> None: - with pytest.raises(ValidationError, match="backend.scene.ctrl_dt"): +def test_backend_control_period_requires_integral_physics_substeps() -> None: + with pytest.raises(ValidationError, match="integer multiple"): _backend(control_period_s=0.015) @@ -103,36 +135,113 @@ def test_quaternion_order_conversion_is_explicit_and_round_trips() -> None: np.testing.assert_array_equal(xyzw_to_wxyz(xyzw), wxyz) -def test_config_rejects_ambiguous_wxyz_orientation_key() -> None: - values = dict(OmegaConf.load(CONFIG_PATH).backend) - values["base_orientation_wxyz"] = values.pop("base_orientation_xyzw") +def test_from_mapping_accepts_ready_scene_without_reconstructing_it() -> None: + values = _mapping() + robot = values["scene"].objs.robot + robot.translation = (1.0, -0.5, 2.0) + robot.rotation = (0.0, 0.0, 1.0, 0.0) + config = SimulationRuntimeConfig.from_mapping(values) + assert config.scene is values["scene"] + assert config.scene.objs.robot is robot + assert robot.translation == (1.0, -0.5, 2.0) + assert robot.rotation == (0.0, 0.0, 1.0, 0.0) + assert config.physics.dt == values["physics"]["dt"] + + +def test_mapping_parses_all_canonical_physics_fields() -> None: + values = _mapping() + physics = {"dt": 0.004, "solver_iterations": 19, "solver_tolerance": 4e-7, "gravity": (0.2, -0.3, -5.0)} + values["physics"] = physics + config = SimulationRuntimeConfig.from_mapping(values) + assert isinstance(config.physics, SimCfg) + for name, value in physics.items(): + assert getattr(config.physics, name) == value + + +@pytest.mark.parametrize("field", ["dt", "solver_iterations"]) +def test_mapping_preserves_required_physics_fields(field) -> None: + values = _mapping() + del values["physics"][field] + with pytest.raises(ValidationError, match=field): + SimulationRuntimeConfig.from_mapping(values) + + +def test_mapping_rejects_unknown_physics_fields() -> None: + values = _mapping() + values["physics"]["unexpected_physics_setting"] = 1 + with pytest.raises(TypeError, match="unexpected_physics_setting"): + SimulationRuntimeConfig.from_mapping(values) + + +@pytest.mark.parametrize( + "field,value", [("dt", 0), ("solver_iterations", -1), ("solver_tolerance", 0), ("gravity", (1, 2))] +) +def test_backend_config_validates_physics_at_public_boundary(field, value) -> None: + with pytest.raises(ValueError, match=f"sim.{field}"): + SimulationRuntimeConfig(scene=build_scene(), physics=SimCfg(**{field: value})) + + +@pytest.mark.parametrize( + "scene", + [None, "flat", [], {}, {"_target_": "motrix_env_core.config.scene.SceneCfg"}], +) +def test_from_mapping_rejects_unassembled_scene_at_backend_boundary(scene: object) -> None: + values = _mapping() + values["scene"] = scene + with pytest.raises(ValidationError, match="runtime.scene.*complete SceneCfg"): + SimulationRuntimeConfig.from_mapping(values) - with pytest.raises(ValidationError, match="base_orientation_xyzw"): - MujocoBackendConfig.from_mapping(values) + +@pytest.mark.parametrize("sensor_name", [7, "", " "]) +def test_state_sensor_mapping_validates_model_local_names(sensor_name: object) -> None: + values = _mapping() + values["sensor_bindings"]["base_angular_velocity"] = sensor_name + + with pytest.raises((TypeError, ValueError), match="base_angular_velocity"): + SimulationRuntimeConfig.from_mapping(values) + + +def test_state_sensor_mapping_rejects_unknown_roles() -> None: + values = _mapping() + values["sensor_bindings"]["unexpected_role"] = "gyro" + with pytest.raises(TypeError, match="unexpected_role"): + SimulationRuntimeConfig.from_mapping(values) + + +@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"]) +def test_missing_state_sensor_fails_at_model_binding(role: str) -> None: + config = _config() + config = replace(config, sensor_bindings=replace(config.sensor_bindings, **{role: None})) + backend = create_runtime(config, control_period_s=CONTROL_PERIOD_S) + with pytest.raises(ValidationError, match=f"runtime.sensor_bindings.{role}"): + backend.open() + backend.close() def test_initial_state_command_step_contract_is_headless() -> None: spec = _spec() backend = _backend() viewer_modules_before = {name for name in sys.modules if name.startswith("mujoco.viewer")} - backend.open(spec) + backend.open() - initial = backend.read_state(timeout_s=0.1) - assert backend._data is not None - assert backend._data.ncon == 0 + initial = backend.robot.read_state(timeout_s=0.1) + assert backend.data is not None + assert backend.data.ncon == 0 target = spec.default_joint_position + np.float32(0.01) - backend.write_command(_command(target)) - stepped = backend.read_state(timeout_s=0.1) - backend.stop() + backend.robot.write_command(_command(target)) + backend.advance_control_period() + stepped = backend.robot.read_state(timeout_s=0.1) + backend.robot.stop() backend.close() - backend.stop() + backend.robot.stop() backend.close() np.testing.assert_allclose(initial.joint_position, spec.default_joint_position, atol=1e-6) np.testing.assert_array_equal(initial.base_orientation_xyzw, [0.0, 0.0, 0.0, 1.0]) assert stepped.sample_time_ns == 20_000_000 assert np.isfinite(stepped.joint_position).all() - assert backend.capabilities.max_command_rate_hz == 50.0 + # The robot's physical command-rate limit is independent of controller cadence. + assert backend.robot.capabilities.max_command_rate_hz == pytest.approx(1.0 / backend.sim.dt) assert {name for name in sys.modules if name.startswith("mujoco.viewer")} == viewer_modules_before @@ -172,13 +281,14 @@ def viewer_factory(mujoco: Any, camera_config: SystemCameraCfg) -> FakeViewer: spec = _spec() assert backend.get_keyboard_device() is viewer.keyboard_device - assert camera_configs == [backend._scene.system_camera] - backend.open(spec) - initial = backend.read_state(timeout_s=0.1) - backend.write_command(_command(spec.default_joint_position)) - stepped = backend.read_state(timeout_s=0.1) - - assert backend.capabilities.supports_rendering is True + assert camera_configs == [backend.scene.system_camera] + backend.open() + initial = backend.robot.read_state(timeout_s=0.1) + backend.robot.write_command(_command(spec.default_joint_position)) + backend.advance_control_period() + stepped = backend.robot.read_state(timeout_s=0.1) + + assert backend.robot.capabilities.supports_rendering is True assert viewer.open_count == 1 assert initial.sample_time_ns == 0 assert stepped.sample_time_ns == 20_000_000 @@ -186,11 +296,11 @@ def viewer_factory(mujoco: Any, camera_config: SystemCameraCfg) -> FakeViewer: viewer.running = False with pytest.raises(KeyboardInterrupt, match="viewer was closed"): - backend.read_state(timeout_s=0.1) + backend.robot.read_state(timeout_s=0.1) backend.close() assert viewer.close_count == 1 - backend.open(spec) + backend.open() assert backend.get_keyboard_device() is viewer.keyboard_device assert viewer.open_count == 2 backend.close() @@ -205,82 +315,261 @@ def test_headless_backend_does_not_provide_keyboard_input() -> None: backend.get_keyboard_device() -def test_swapped_joint_contract_fails_during_open() -> None: - swapped = list(JOINT_NAMES) - swapped[0], swapped[1] = swapped[1], swapped[0] +def test_swapped_artifact_joint_contract_fails_compatibility_comparison() -> None: backend = _backend() + actual = backend.robot.spec + swapped = list(actual.joint_names) + swapped[0], swapped[1] = swapped[1], swapped[0] + expected = replace(actual, joint_names=tuple(swapped)) - with pytest.raises(ValidationError, match="ctrl_lower"): - backend.open(_spec(tuple(swapped))) + with pytest.raises(ValidationError, match="joint_names"): + expected.validate_compatible(actual) -def test_missing_joint_contract_fails_during_open() -> None: - joint_names = list(JOINT_NAMES) +def test_artifact_base_link_must_match_selected_robot() -> None: + backend = _backend() + actual = backend.robot.spec + expected = replace(actual, base_link_name="wrong_base") + with pytest.raises(ValidationError, match="base_link_name"): + expected.validate_compatible(actual) + + +def test_missing_artifact_joint_fails_compatibility_comparison() -> None: + backend = _backend() + actual = backend.robot.spec + joint_names = list(actual.joint_names) joint_names[0] = "missing_joint" + expected = replace(actual, joint_names=tuple(joint_names)) + + with pytest.raises(ValidationError, match="joint_names"): + expected.validate_compatible(actual) + + +@pytest.mark.parametrize( + ("role", "sensor_name", "message"), + [ + ("base_angular_velocity", "missing_gyro", "missing_gyro"), + ("base_angular_velocity", "global_angvel", "mjSENS_GYRO"), + ("base_linear_acceleration", "gyro", "mjSENS_ACCELEROMETER"), + ("base_linear_velocity", "local_linvel", "mjSENS_FRAMELINVEL"), + ("base_linear_velocity", "FR_global_linvel", "on the base body"), + ], +) +def test_invalid_state_sensor_mapping_fails_during_open(role: str, sensor_name: str, message: str) -> None: + config = _config() + config = replace(config, sensor_bindings=replace(config.sensor_bindings, **{role: sensor_name})) + backend = create_runtime(config, control_period_s=CONTROL_PERIOD_S) + + with pytest.raises(ValidationError, match=message): + backend.open() + backend.close() + + +@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration"]) +def test_imu_sensor_must_use_base_body_frame(role: str, monkeypatch: pytest.MonkeyPatch) -> None: backend = _backend() + build_model = backend.build_model + + def rotated_imu_model() -> Any: + model = build_model() + sensor_id = backend.mj.mj_name2id( + model, + backend.mj.mjtObj.mjOBJ_SENSOR, + backend.robot._cfg.resolve_name(getattr(backend.config.sensor_bindings, role)), + ) + site_id = model.sensor_objid[sensor_id] + model.site_quat[site_id] = [0.7071067812, 0.0, 0.0, 0.7071067812] + return model + + monkeypatch.setattr(backend, "build_model", rotated_imu_model) + with pytest.raises(ValidationError, match="aligned with its body frame"): + backend.open() + backend.close() - with pytest.raises(ValidationError, match="missing_joint"): - backend.open(_spec(tuple(joint_names))) +def test_sensor_bindings_resolve_robot_names_and_are_cached(monkeypatch: pytest.MonkeyPatch) -> None: + config = _config() + config.scene.objs.robot.prefix = "robot_" + config.scene.objs.robot.suffix = "_instance" + backend = create_runtime(config, control_period_s=CONTROL_PERIOD_S) + assert backend.robot.spec.base_link_name == "base" + assert backend.robot.spec.joint_names == JOINT_NAMES + backend.open() + + for index, name in enumerate(backend.robot.spec.joint_names): + joint = backend.model.joint(backend.robot._cfg.resolve_name(name)) + assert backend.robot._joint_qpos_indices[index] == joint.qposadr[0] + assert backend.robot._joint_qvel_indices[index] == joint.dofadr[0] + actuator = backend.robot._actuator_indices[index] + assert backend.model.actuator_trnid[actuator, 0] == joint.id + assert backend.robot._base_body_id == backend.model.body(backend.robot._cfg.resolve_name("base")).id + + def unexpected_lookup(*args: Any) -> int: + pytest.fail("State reads must use bindings cached during open, not name lookups") + + monkeypatch.setattr(backend.mj, "mj_name2id", unexpected_lookup) + assert backend.data is not None + backend.data.qvel[:6] = [0.3, -0.2, 0.1, 0.4, -0.5, 0.6] + backend.mj.mj_forward(backend.model, backend.data) + state = backend.robot.read_state(0.1) + for role in ("base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"): + np.testing.assert_allclose(getattr(state, role), backend.data.sensordata[backend.robot._sensor_slices[role]]) + np.testing.assert_allclose(state.base_position, backend.model.qpos0[:3]) + np.testing.assert_allclose(state.base_angular_velocity, [0.4, -0.5, 0.6], atol=1e-6) + backend.close() -def test_actuator_force_range_mismatch_fails_during_open() -> None: - spec = replace(_spec(), torque_limit=np.full(12, 23.0, dtype=np.float32)) + +def test_actuator_force_range_mismatch_fails_during_open(monkeypatch: pytest.MonkeyPatch) -> None: backend = _backend() + prepare = backend.robot.prepare + + def invalid_actuator_model(source_model: Any, model_spec: Any) -> None: + source_model.actuator_forcerange[:, 0] = -23.0 + prepare(source_model, model_spec) + monkeypatch.setattr(backend.robot, "prepare", invalid_actuator_model) with pytest.raises(ValidationError, match="force_lower"): - backend.open(spec) + backend.open() + backend.close() def test_open_converts_actuators_and_uses_scene_cfg_ground(caplog: pytest.LogCaptureFixture) -> None: spec = _spec() backend = _backend() - with caplog.at_level(logging.INFO, logger="motrix_deploy_mujoco.interface"): - backend.open(spec) + with caplog.at_level(logging.INFO, logger="motrix_deploy_mujoco"): + backend.open() - model = backend._model + model = backend.model assert model is not None assert model.opt.timestep == pytest.approx(0.002) assert model.opt.iterations == 100 - floor_id = backend._mj.mj_name2id(model, backend._mj.mjtObj.mjOBJ_GEOM, "floor") + floor_id = backend.mj.mj_name2id(model, backend.mj.mjtObj.mjOBJ_GEOM, "floor") assert floor_id >= 0 np.testing.assert_allclose(model.geom_friction[floor_id], [0.6, 0.005, 0.0001]) np.testing.assert_array_equal( - model.actuator_biastype[backend._actuator_indices], - backend._mj.mjtBias.mjBIAS_NONE, + model.actuator_biastype[backend.robot._actuator_indices], + backend.mj.mjtBias.mjBIAS_NONE, ) - np.testing.assert_allclose(model.actuator_gainprm[backend._actuator_indices, 0], 1.0) + np.testing.assert_allclose(model.actuator_gainprm[backend.robot._actuator_indices, 0], 1.0) np.testing.assert_allclose( - model.actuator_ctrlrange[backend._actuator_indices], + model.actuator_ctrlrange[backend.robot._actuator_indices], np.column_stack((-spec.torque_limit, spec.torque_limit)), ) assert "Converting 12 actuators from position servos to torque motors" in caplog.text - assert "Built MuJoCo deployment model from SceneCfg go2-walk-rough" in caplog.text - backend.close() - - -def test_rough_scene_uses_configured_initial_base_position() -> None: - config = _config() - backend = MujocoRobotInterface(config, control_period_s=CONTROL_PERIOD_S) - backend.open(_spec()) - - initial = backend.read_state(timeout_s=0.1) - - assert initial.base_position == pytest.approx(config.base_position) + assert "Built MuJoCo deployment model from SceneCfg" in caplog.text backend.close() -def test_flat_scene_uses_nonpenetrating_initial_base_position() -> None: +def test_direct_scene_cfg_headless_uses_supplied_robot_floor_and_placement() -> None: + robot = UnitreeGo2Robot(translation=(1.0, -0.5, 0.155), rotation=(0.0, 0.0, 1.0, 0.0)) + scene = SceneCfg(objs=AdapterSceneObjsCfg(robot=robot, floor=FlatTerrainCfg(height=0.2))) + config = replace(_config(), scene=scene) + # Direct scenes have no training ctrl_dt: only integer physics substeps are required. + backend = create_runtime(config, control_period_s=0.01) + assert backend.robot._cfg is not robot + backend.open() + try: + state = backend.robot.read_state(0.1) + assert backend.robot.capabilities.privileged_state_fields == frozenset( + {"base_position", "base_linear_velocity"} + ) + np.testing.assert_allclose(state.base_position, backend.model.qpos0[:3]) + np.testing.assert_allclose(state.base_orientation_xyzw, robot.rotation) + floor_id = backend.mj.mj_name2id(backend.model, backend.mj.mjtObj.mjOBJ_GEOM, "floor") + assert backend.model.geom_pos[floor_id, 2] == pytest.approx(0.2) + backend.robot.write_command(_command(_spec().default_joint_position)) + backend.advance_control_period() + assert backend.robot.read_state(0.1).sample_time_ns == 10_000_000 + finally: + backend.close() + + +@pytest.mark.parametrize("robot_type", [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())) + source = MuJoCoSceneCompiler().create_spec(scene, SimCfg()).compile() + joints = tuple(robot.key_pose.joint_names) + actuators = np.asarray([np.flatnonzero(source.actuator_trnid[:, 0] == source.joint(name).id)[0] for name in joints]) + spec = RobotSpec( + base_link_name=robot.base_link_name, + joint_names=joints, + default_joint_position=np.asarray(robot.key_pose.poses[robot.init_key_pose], dtype=np.float32), + position_lower=source.actuator_ctrlrange[actuators, 0].astype(np.float32), + position_upper=source.actuator_ctrlrange[actuators, 1].astype(np.float32), + torque_limit=source.actuator_forcerange[actuators, 1].astype(np.float32), + ) + config = replace(_config(), scene=scene) + backend = create_runtime(config, control_period_s=CONTROL_PERIOD_S) + spec.validate_compatible(backend.robot.spec) + backend.open() + try: + state = backend.robot.read_state(0.1) + np.testing.assert_allclose(state.joint_position, spec.default_joint_position) + zeros = np.zeros(spec.joint_count, dtype=np.float32) + backend.robot.write_command(JointServoCommand(spec.default_joint_position, zeros, zeros, zeros, zeros)) + backend.advance_control_period() + assert backend.robot.read_state(0.1).sample_time_ns > state.sample_time_ns + assert backend.robot.health().healthy + finally: + backend.close() + + +def test_fallen_pose_does_not_change_resource_health() -> None: config = _config(FLAT_CONFIG_PATH) - backend = MujocoRobotInterface(config, control_period_s=CONTROL_PERIOD_S) - backend.open(_spec()) - - initial = backend.read_state(timeout_s=0.1) - - assert config.scene == "go2-walk-flat" - assert initial.base_position == pytest.approx(config.base_position) - assert backend._data is not None - assert backend._data.ncon == 0 - backend.close() + config.scene.objs.robot.translation = (0.0, 0.0, 0.545) + config.scene.objs.robot.rotation = (1.0, 0.0, 0.0, 0.0) + backend = create_runtime(config, control_period_s=CONTROL_PERIOD_S) + backend.open() + try: + backend.robot.write_command(_command(_spec().default_joint_position, kp=0.0, kd=0.0)) + backend.advance_control_period() + assert backend.robot.health().healthy + finally: + backend.close() + + +def test_direct_scene_recipe_runs_with_training_imports_blocked() -> None: + script = textwrap.dedent( + """ + import importlib.abc + import sys + class BlockTraining(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if fullname == 'motrix_envs' or fullname.startswith('motrix_envs.'): + raise AssertionError('Direct deployment must not import training environments') + sys.meta_path.insert(0, BlockTraining()) + from omegaconf import OmegaConf + from motrix_deploy_mujoco import MujocoRuntime + from motrix_deploy.runtime.config import SimulationRuntimeConfig + import numpy as np + from motrix_env_core.config import configclass + from motrix_env_core.config.scene import SceneCfg, SceneObjsCfg, FlatTerrainCfg + @configclass + class FlatWorldObjsCfg(SceneObjsCfg): + floor: FlatTerrainCfg = FlatTerrainCfg() + from motrix_robots.unitree import UnitreeGo2Robot + values = OmegaConf.to_container(OmegaConf.load(sys.argv[1]).runtime) + values.pop('backend') + values.pop('deploy_env_id') + values.pop('robot_id') + robot = UnitreeGo2Robot( + translation=tuple(values.pop('robot_translation')), + rotation=tuple(values.pop('robot_rotation')), + ) + values['scene'] = SceneCfg(objs=FlatWorldObjsCfg(robot=robot, floor=FlatTerrainCfg())) + config = SimulationRuntimeConfig.from_mapping(values) + backend = MujocoRuntime(config, control_period_s=0.02) + backend.open() + assert np.isfinite(backend.robot.read_state(0.1).joint_position).all() + backend.close() + assert not any(name == 'motrix_envs' or name.startswith('motrix_envs.') for name in sys.modules) + """ + ) + result = subprocess.run( + [sys.executable, "-c", script, str(FLAT_CONFIG_PATH)], capture_output=True, text=True, check=False + ) + assert result.returncode == 0, result.stdout + result.stderr def test_hybrid_pd_command_is_recomputed_as_torque_each_physics_substep( @@ -288,14 +577,14 @@ def test_hybrid_pd_command_is_recomputed_as_torque_each_physics_substep( ) -> None: spec = _spec() backend = _backend() - backend.open(spec) - initial = backend.read_state(0.1) + backend.open() + initial = backend.robot.read_state(0.1) desired_position = initial.joint_position + np.linspace(0.01, 0.02, 12, dtype=np.float32) desired_velocity = np.linspace(-0.3, 0.3, 12, dtype=np.float32) feedforward_torque = np.linspace(-1.0, 1.0, 12, dtype=np.float32) kp = np.linspace(10.0, 21.0, 12, dtype=np.float32) kd = np.linspace(0.1, 1.2, 12, dtype=np.float32) - command = RobotCommand( + command = JointServoCommand( joint_position=desired_position, joint_velocity=desired_velocity, feedforward_torque=feedforward_torque, @@ -303,23 +592,24 @@ def test_hybrid_pd_command_is_recomputed_as_torque_each_physics_substep( kd=kd, ) snapshots: list[tuple[np.ndarray, np.ndarray, np.ndarray]] = [] - mj_step = backend._mj.mj_step + mj_step = backend.mj.mj_step def record_step(model: object, data: object) -> None: snapshots.append( ( - data.qpos[backend._joint_qpos_indices].copy(), - data.qvel[backend._joint_qvel_indices].copy(), - data.ctrl[backend._actuator_indices].copy(), + data.qpos[backend.robot._joint_qpos_indices].copy(), + data.qvel[backend.robot._joint_qvel_indices].copy(), + data.ctrl[backend.robot._actuator_indices].copy(), ) ) mj_step(model, data) - monkeypatch.setattr(backend._mj, "mj_step", record_step) + monkeypatch.setattr(backend.mj, "mj_step", record_step) - backend.write_command(command) + backend.robot.write_command(command) + backend.advance_control_period() - assert backend._data is not None + assert backend.data is not None assert len(snapshots) == 10 for position, velocity, applied_torque in snapshots: expected_torque = np.clip( @@ -329,7 +619,7 @@ def record_step(model: object, data: object) -> None: ) np.testing.assert_allclose(applied_torque, expected_torque, atol=1e-6) assert not np.array_equal(snapshots[0][2], snapshots[1][2]) - assert backend.read_state(0.1).sample_time_ns == 20_000_000 - backend.stop() - np.testing.assert_array_equal(backend._data.ctrl[backend._actuator_indices], 0.0) + assert backend.robot.read_state(0.1).sample_time_ns == 20_000_000 + backend.robot.stop() + np.testing.assert_array_equal(backend.data.ctrl[backend.robot._actuator_indices], 0.0) backend.close() diff --git a/motrix_deploy_mujoco/tests/test_scene_session.py b/motrix_deploy_mujoco/tests/test_scene_session.py new file mode 100644 index 00000000..a4df8a3a --- /dev/null +++ b/motrix_deploy_mujoco/tests/test_scene_session.py @@ -0,0 +1,371 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Complete scene ownership, absolute placement and runtime lifecycle contracts.""" + +from dataclasses import replace + +import numpy as np +import pytest +from motrix_robots.unitree import UnitreeGo2Robot +from scene_helpers import AdapterSceneObjsCfg, build_scene +from test_mujoco_deploy_backend import _command, _spec, create_runtime + +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg, SystemCameraCfg +from motrix_env_core.config.sim import SimCfg +from motrix_env_mujoco.compiler import MuJoCoSceneCompiler + + +@configclass +class CustomSceneObjsCfg(SceneObjsCfg): + platform: FlatTerrainCfg = FlatTerrainCfg(height=0.2) + + +def _config(scene=None): + return SimulationRuntimeConfig( + scene=build_scene() if scene is None else scene, + physics=SimCfg(dt=0.002, solver_iterations=20), + sensor_bindings=SensorBindings( + base_angular_velocity="gyro", + base_linear_acceleration="accelerometer", + base_linear_velocity="global_linvel", + ), + ) + + +def test_complete_scene_copies_all_fields_without_reconstructing_derived_contacts() -> None: + @configclass + class DerivedScene(SceneCfg): + contact_name: str | None = None + + def __post_init__(self): + self.contact_name = self.objs.robot.resolved_base_link_name + + robot = UnitreeGo2Robot(translation=(4, 5, 6), rotation=(0, 0, 1, 0), prefix="deployment_") + source = DerivedScene(objs=CustomSceneObjsCfg(robot=robot), system_camera=SystemCameraCfg(distance=4.0)) + backend = create_runtime(_config(source), control_period_s=0.02) + assert backend.robot.spec.base_link_name == robot.base_link_name + assert backend.robot.spec.joint_names == tuple(robot.key_pose.joint_names) + scene = backend.scene + assert type(scene.objs) is CustomSceneObjsCfg + assert scene.objs.platform is not source.objs.platform + assert scene.assets is not source.assets and scene.sensors is not source.sensors + assert scene.system_camera is not source.system_camera + assert scene.system_camera.distance == source.system_camera.distance + assert scene.contact_name == source.contact_name == "deployment_base" + assert robot.translation == (4, 5, 6) and robot.rotation == (0, 0, 1, 0) + assert scene.objs.robot.translation == robot.translation and scene.objs.robot.rotation == robot.rotation + scene.objs.platform.height = 9 + assert source.objs.platform.height == 0.2 + + +def test_complete_scene_reuses_one_copy_without_inserting_another_robot() -> None: + robot = UnitreeGo2Robot(translation=(4, 5, 6), rotation=(0, 0, 1, 0)) + scene = SceneCfg(objs=AdapterSceneObjsCfg(robot=robot, floor=FlatTerrainCfg())) + backend = create_runtime(_config(scene), control_period_s=0.02) + assert backend.scene is not scene + assert len(list(backend.scene.iter_objs())) == 2 + backend.open() + try: + assert backend.model.njnt == 13 + np.testing.assert_allclose(backend.robot.read_state(0.1).base_orientation_xyzw, robot.rotation, atol=1e-6) + finally: + backend.close() + assert robot.translation == (4, 5, 6) + + +def test_file_scene_and_custom_objects_preserved_placement_has_no_double_transform(tmp_path) -> None: + path = tmp_path / "world.xml" + path.write_text( + '' + ) + robot = UnitreeGo2Robot(translation=(5, 6, 7), rotation=(0.6, 0, 0, 0.8)) + scene = SceneCfg(file=path, objs=CustomSceneObjsCfg(robot=robot), system_camera=SystemCameraCfg(distance=3)) + config = _config(scene) + backend = create_runtime(config, control_period_s=0.02) + backend.open() + try: + state = backend.robot.read_state(0.1) + np.testing.assert_allclose(state.base_position, backend.model.qpos0[:3]) + np.testing.assert_allclose(state.base_orientation_xyzw, robot.rotation, atol=1e-6) + np.testing.assert_allclose(backend.model.geom("obstacle").pos, [5, 6, 7]) + assert backend.model.geom("platform").pos[2] == pytest.approx(0.2) + assert state.sample_time_ns == 0 + assert backend.robot.read_state(0.1).receive_time_ns >= state.receive_time_ns + backend.robot.write_command(_command(_spec().default_joint_position)) + backend.advance_control_period() + assert backend.robot.read_state(0.1).sample_time_ns == 20_000_000 + finally: + backend.close() + assert scene.file == path and scene.objs.robot is robot + assert robot.translation == (5, 6, 7) and robot.rotation == (0.6, 0, 0, 0.8) + + +@pytest.mark.parametrize("rotation", [None, (0.6, 0.0, 0.0, 0.8)]) +def test_robot_attach_transform_and_init_key_pose_are_reset_once(rotation) -> None: + import mujoco + + from motrix_deploy.runtime.config import SimulationRuntimeConfig + from motrix_deploy.runtime.factory import create_simulation_runtime + + robot = UnitreeGo2Robot() + scene = SceneCfg(objs=AdapterSceneObjsCfg(robot=robot)) + intrinsic = MuJoCoSceneCompiler().create_spec(scene, SimCfg()).compile().qpos0[:7].copy() + translation = (1.2, -0.7, 2.1) + robot.translation = translation + robot.rotation = rotation + robot.key_pose.poses["custom"] = tuple(np.asarray(robot.key_pose.poses["default"]) + 0.01) + robot.init_key_pose = "custom" + attach_quat = np.asarray((0, 0, 0, 1) if rotation is None else rotation)[[3, 0, 1, 2]] + rotated_position = np.empty(3) + expected_quat = np.empty(4) + mujoco.mju_rotVecQuat(rotated_position, intrinsic[:3], attach_quat) + mujoco.mju_mulQuat(expected_quat, attach_quat, intrinsic[3:]) + expected_position = np.asarray(translation) + rotated_position + runtime = create_simulation_runtime( + "mujoco", SimulationRuntimeConfig(scene=scene, sensor_bindings=_config().sensor_bindings) + ) + with runtime: + for _ in range(2): + state = runtime.robot.read_state(0.1) + np.testing.assert_allclose(state.base_position, expected_position, atol=1e-6) + np.testing.assert_allclose(state.base_orientation_xyzw, expected_quat[[1, 2, 3, 0]], atol=1e-6) + np.testing.assert_allclose(state.joint_position, robot.key_pose.poses["custom"], atol=1e-6) + runtime.data.qpos[:] = 0.0 + runtime.data.qvel[:] = 1.0 + runtime.reset(runtime.robot._joint_qpos_indices, runtime.robot.spec.default_joint_position) + np.testing.assert_array_equal(runtime.data.qvel, 0.0) + assert scene.objs.robot.translation == translation and scene.objs.robot.rotation == rotation + + +def test_environment_scene_is_resolved_by_application_before_backend() -> None: + import motrix_envs # noqa: F401 registers configurations for this application test + from motrix_env_core import registry + + source = registry.make_env_config("go2-walk-rough", mode="play").scene + backend = create_runtime(_config(source), control_period_s=0.02) + backend.open() + try: + assert backend.model.njnt == 13 + assert backend.scene is not source + np.testing.assert_allclose(backend.robot.read_state(0.1).base_position, backend.model.qpos0[:3]) + finally: + backend.close() + + +@pytest.mark.parametrize("failure", ["prepare", "bind", "viewer"]) +def test_failed_open_releases_session_and_viewer_idempotently(failure, monkeypatch) -> None: + class Viewer: + running = False + closes = 0 + + def open(self, model, data): + self.running = True + if failure == "viewer": + raise ValueError("viewer failed") + + def close(self): + self.closes += 1 + self.running = False + + def is_running(self): + return self.running + + def sync(self): + pass + + viewer = Viewer() + config = _config() + if failure == "bind": + config = replace(config, sensor_bindings=SensorBindings()) + backend = create_runtime(config, control_period_s=0.02, render=True, viewer_factory=lambda *args: viewer) + if failure == "prepare": + prepare = backend.robot.prepare + + def invalid_actuator_model(source_model, model_spec): + source_model.actuator_forcerange[:, 0] = -23.0 + prepare(source_model, model_spec) + + monkeypatch.setattr(backend.robot, "prepare", invalid_actuator_model) + with pytest.raises((ValidationError, ValueError)): + backend.open() + assert backend.model is None and backend.data is None + assert not backend.robot.health().healthy + assert not viewer.running and viewer.closes == 1 + backend.close() + backend.close() + assert viewer.closes == 1 + with pytest.raises(RuntimeError, match="not open"): + backend.robot.read_state(0.1) + + +def test_robot_io_lifecycle_preserves_world_and_rebinds_on_runtime_reopen(control_scene_config) -> None: + from motrix_deploy.contracts import JointTorqueCommand + from motrix_deploy.robot.interface import RobotInterface + from motrix_deploy_mujoco.runtime import MujocoRuntime + + runtime = MujocoRuntime(control_scene_config, control_period_s=0.02) + robot = runtime.robot + assert isinstance(robot, RobotInterface) + spec = robot.spec + with pytest.raises(RuntimeError, match="not open"): + robot.open() + + with runtime: + robot.open() + model, data = runtime.model, runtime.data + robot.write_command(JointTorqueCommand(np.ones(spec.joint_count, dtype=np.float32))) + runtime.advance_control_period() + time_before_close = data.time + robot.close() + robot.close() + robot.stop() + assert runtime.opened + assert runtime.model is model and runtime.data is data + assert data.time == time_before_close + np.testing.assert_array_equal(data.ctrl, 0.0) + assert not robot.health().healthy + with pytest.raises(RuntimeError, match="not open"): + robot.read_state(0.1) + with pytest.raises(RuntimeError, match="not open"): + robot.open() + + assert runtime.model is None and runtime.data is None + with runtime: + assert runtime.robot is robot and robot.spec is spec + robot.open() + assert robot.health().healthy + state = robot.read_state(0.1) + assert state.sample_time_ns == 0 + np.testing.assert_array_equal(state.joint_position, spec.default_joint_position) + assert not robot.health().healthy + with pytest.raises(RuntimeError, match="not open"): + robot.read_state(0.1) + + +def test_runtime_requires_primary_scene_robot() -> None: + with pytest.raises(ValidationError, match="backend.scene.objs.robot"): + create_runtime(_config(SceneCfg()), control_period_s=0.02) + + +def test_physics_requires_integral_substeps() -> None: + with pytest.raises(ValidationError, match="integer multiple"): + create_runtime(_config(), control_period_s=0.003) + + +@pytest.mark.parametrize("gravity,tolerance", [((0.3, -0.2, -4.1), 2e-7), ((-0.1, 0.4, -6.3), 3e-6)]) +def test_public_factory_forwards_shared_physics_once_to_compiler(monkeypatch, gravity, tolerance) -> None: + from motrix_deploy.runtime.config import SimulationRuntimeConfig + from motrix_deploy.runtime.factory import create_simulation_runtime + + physics = SimCfg(dt=0.003, solver_iterations=17, solver_tolerance=tolerance, gravity=gravity) + config = _config() + calls = [] + original = MuJoCoSceneCompiler.create_spec + + def create_spec(compiler, scene, sim): + calls.append(sim) + return original(compiler, scene, sim) + + monkeypatch.setattr(MuJoCoSceneCompiler, "create_spec", create_spec) + runtime = create_simulation_runtime( + "mujoco", SimulationRuntimeConfig(scene=config.scene, physics=physics, sensor_bindings=config.sensor_bindings) + ) + assert runtime.config.physics is physics + assert runtime.config.sensor_bindings is config.sensor_bindings + with runtime: + assert calls == [physics] + assert calls[0] is physics + assert runtime.model.opt.timestep == physics.dt + assert runtime.model.opt.iterations == physics.solver_iterations + assert runtime.model.opt.tolerance == physics.solver_tolerance + np.testing.assert_allclose(runtime.model.opt.gravity, physics.gravity) + + +@pytest.mark.parametrize("render", [False, True]) +@pytest.mark.parametrize("realtime", [None, False, True]) +def test_direct_runtime_and_plugin_use_config_render_and_realtime(control_scene_config, render, realtime) -> None: + from motrix_deploy_mujoco.plugin import create_runtime as create_plugin_runtime + from motrix_deploy_mujoco.runtime import MujocoRuntime + + config = replace(control_scene_config, render=render, realtime=realtime) + direct = MujocoRuntime(config) + plugin = create_plugin_runtime(config) + for runtime in (direct, plugin): + assert runtime.config is config + assert (runtime.viewer is not None) == render + assert runtime.realtime is realtime + assert runtime.model is None and runtime.data is None + + +def test_artifact_recipe_normalizes_to_simulation_config(control_scene_config) -> None: + from motrix_deploy_mujoco.plugin import create_runtime as create_plugin_runtime + + config = control_scene_config + runtime_config = SimulationRuntimeConfig.from_mapping( + { + "name": "mujoco", + "scene": config.scene, + "physics": {"dt": config.physics.dt, "solver_iterations": config.physics.solver_iterations}, + "sensor_bindings": { + "base_angular_velocity": "gyro", + "base_linear_acceleration": "accelerometer", + "base_linear_velocity": "global_linvel", + }, + }, + render=True, + realtime=False, + ) + runtime = create_plugin_runtime(runtime_config) + assert isinstance(runtime.config, SimulationRuntimeConfig) + assert runtime.config.scene is config.scene + assert runtime.config.render is True and runtime.config.realtime is False + assert runtime.viewer is not None and runtime.realtime is False + assert runtime.robot.spec.joint_names == tuple(config.scene.objs.robot.key_pose.joint_names) + + +def test_public_simulation_factory_returns_model_derived_robot_spec() -> None: + from motrix_deploy.runtime.base import SimulationRuntime + from motrix_deploy.runtime.config import SimulationRuntimeConfig + from motrix_deploy.runtime.factory import create_simulation_runtime as create_selected_runtime + + config = _config() + runtime = create_selected_runtime( + "mujoco", SimulationRuntimeConfig(scene=config.scene, sensor_bindings=config.sensor_bindings) + ) + assert isinstance(runtime, SimulationRuntime) + assert runtime.config.scene is config.scene + spec = runtime.robot.spec + assert runtime.model is None and runtime.data is None + assert spec.joint_names == tuple(config.scene.objs.robot.key_pose.joint_names) + port = runtime.robot + assert runtime.robot is port and port.spec is spec + with runtime: + port.open() + np.testing.assert_array_equal(port.read_state(0.1).joint_position, spec.default_joint_position) + assert runtime.model is None and runtime.data is None + + +@pytest.mark.parametrize("field", ["default_joint_position", "position_lower", "position_upper", "torque_limit"]) +def test_artifact_robot_vectors_are_compared_against_derived_spec(field) -> None: + runtime = create_runtime(_config(), control_period_s=0.02) + actual = runtime.robot.spec + expected = replace(actual, **{field: getattr(actual, field) + np.float32(0.01)}) + with pytest.raises(ValidationError, match=field): + expected.validate_compatible(actual) + assert runtime.model is None and runtime.data is None + + +@pytest.mark.parametrize("field", ["default_joint_position", "position_lower", "position_upper", "torque_limit"]) +def test_artifact_robot_vector_comparison_uses_absolute_tolerance(field) -> None: + runtime = create_runtime(_config(), control_period_s=0.02) + actual = runtime.robot.spec + expected = replace(actual, **{field: getattr(actual, field) + np.float32(5e-7)}) + expected.validate_compatible(actual) + # Values such as torque limits must not get a scale-dependent relative tolerance. + expected = replace(actual, **{field: getattr(actual, field) + np.float32(5e-6)}) + with pytest.raises(ValidationError, match=field): + expected.validate_compatible(actual) diff --git a/motrix_deploy_mujoco/tests/test_simulation_driver.py b/motrix_deploy_mujoco/tests/test_simulation_driver.py new file mode 100644 index 00000000..545c785b --- /dev/null +++ b/motrix_deploy_mujoco/tests/test_simulation_driver.py @@ -0,0 +1,304 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Physics-owned control cadence, pacing, and shared lifecycle integration.""" + +import numpy as np +import pytest +from control_helpers import DummyServoTask + +from motrix_deploy.errors import ValidationError +from motrix_deploy.policy import NoOpPolicyRuntime +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy_mujoco.runtime import MujocoRuntime +from motrix_env_core.input import ConstantPlanarVelocityBinding + + +@pytest.fixture +def driver(control_scene_config, control_gains): + config = control_scene_config + simulation = MujocoRuntime(config, control_period_s=0.02) + task = DummyServoTask(simulation.robot.spec, *control_gains) + interface = simulation.robot + control = ControlSession( + robot=interface, + task=task, + policy=NoOpPolicyRuntime(interface.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=simulation.control_period_s, + state_timeout_s=0.1, + ) + with simulation: + yield simulation, control, interface, task + + +def test_physics_substeps_and_control_sample_times(driver, monkeypatch): + simulation, control, interface, task = driver + events = [] + samples = [] + contexts = [] + apply = simulation.robot.apply_control + step = simulation.mj.mj_step + observe = task.build_observation + + def record_apply(): + events.append("pd") + apply() + + def record_step(model, data): + events.append("physics") + step(model, data) + + def record_observation(state, context): + events.append("tick") + samples.append(state.sample_time_ns) + contexts.append(context.elapsed_time_s) + return observe(state, context) + + monkeypatch.setattr(simulation.robot, "apply_control", record_apply) + monkeypatch.setattr(simulation.mj, "mj_step", record_step) + monkeypatch.setattr(task, "build_observation", record_observation) + # The driver must not delegate a full physics period to a control callback. + monkeypatch.setattr(simulation, "advance_control_period", lambda: pytest.fail("legacy advance called")) + result = simulation.run(control, steps=3) + assert result.success and result.completed_steps == 3 + assert samples == [0, 20_000_000, 40_000_000] + assert contexts == pytest.approx([0.0, 0.02, 0.04]) + assert events == (["tick"] + ["pd", "physics"] * simulation.physics_substeps) * 3 + assert result.simulation_time_s == pytest.approx(0.06) + assert simulation.data.time == pytest.approx(0.06) + assert simulation.model is not None and not interface.health().healthy + np.testing.assert_array_equal(simulation.data.ctrl, np.zeros(simulation.model.nu)) + + +def test_torque_control_session_preserves_control_and_physics_cadence(driver, monkeypatch): + from motrix_deploy.contracts import JointTorqueCommand + + simulation, _, interface, task = driver + samples = [] + torques = [] + observe = task.build_observation + step = simulation.mj.mj_step + desired = np.linspace(-1, 1, interface.spec.joint_count, dtype=np.float32) + + def record_observation(state, context): + samples.append(state.sample_time_ns) + return observe(state, context) + + def record_step(model, data): + torques.append(data.ctrl[simulation.robot._actuator_indices].copy()) + step(model, data) + + monkeypatch.setattr(task, "build_observation", record_observation) + monkeypatch.setattr(task, "process_action", lambda action: JointTorqueCommand(desired)) + monkeypatch.setattr(simulation.mj, "mj_step", record_step) + control = ControlSession( + robot=interface, + task=task, + policy=NoOpPolicyRuntime(interface.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=simulation.control_period_s, + state_timeout_s=0.1, + ) + result = simulation.run(control, steps=3) + assert result.success and result.completed_steps == 3 + assert samples == [0, 20_000_000, 40_000_000] + assert len(torques) == 3 * simulation.physics_substeps + for torque in torques: + np.testing.assert_array_equal(torque, desired) + assert result.simulation_time_s == pytest.approx(0.06) + np.testing.assert_array_equal(simulation.data.ctrl, 0) + + +def test_shared_tick_never_advances_physics(driver): + simulation, control, _, _ = driver + try: + assert control.start() + before = simulation.data.time + assert control.tick(elapsed_time_s=0.0) + assert simulation.data.time == before + finally: + control.stop() + + +def test_headless_driver_is_deterministic_and_never_paces(monkeypatch, control_scene_config, control_gains): + from motrix_deploy_mujoco import runtime + + monkeypatch.setattr(runtime, "RealtimeScheduler", lambda *args: pytest.fail("headless pacing")) + config = control_scene_config + traces = [] + for _ in range(2): + simulation = MujocoRuntime(config, control_period_s=0.02) + task = DummyServoTask(simulation.robot.spec, *control_gains) + interface = simulation.robot + with simulation: + control = ControlSession( + robot=interface, + task=task, + policy=NoOpPolicyRuntime(interface.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=simulation.control_period_s, + state_timeout_s=0.1, + ) + result = simulation.run(control, steps=4) + assert result.success and result.overrun_count == 0 + traces.append(result.trace_sha256) + assert traces[0] == traces[1] + + +def test_realtime_paces_every_complete_interval_without_extra_tick(driver, monkeypatch): + from motrix_deploy_mujoco import runtime + + simulation, control, _, _ = driver + waits = [] + + class Scheduler: + overrun_count = 2 + + def __init__(self, period_s): + assert period_s == simulation.control_period_s + + def reset(self): + waits.append("reset") + + def wait(self, step): + waits.append(step) + + monkeypatch.setattr(runtime, "RealtimeScheduler", Scheduler) + result = simulation.run(control, steps=3, realtime=True) + assert result.success and result.completed_steps == 3 + assert waits == ["reset", 1, 2, 3] + assert result.overrun_count == 2 + + +def test_physics_failure_cleans_robot_but_keeps_world_and_reports_actual_time(driver, monkeypatch): + simulation, control, interface, _ = driver + step = simulation.mj.mj_step + calls = 0 + + def fail_mid_interval(model, data): + nonlocal calls + calls += 1 + if calls == 3: + raise RuntimeError("physics failed") + step(model, data) + + monkeypatch.setattr(simulation.mj, "mj_step", fail_mid_interval) + result = simulation.run(control, steps=3) + assert not result.success and result.exit_reason == "backend_error" + assert result.error == "physics failed" + assert result.completed_steps == 1 + assert result.simulation_time_s == pytest.approx(2 * simulation.config.physics.dt) + assert result.real_time_factor == pytest.approx(result.simulation_time_s / result.wall_time_s) + assert simulation.model is not None and simulation.data is not None + assert not interface.health().healthy and not control.active + np.testing.assert_array_equal(simulation.data.ctrl, np.zeros(simulation.model.nu)) + control.stop() # Shared cleanup remains idempotent after driver failure. + + +def test_start_exception_still_cleans_shared_control(driver, monkeypatch): + simulation, control, interface, _ = driver + + def failed_start(): + raise RuntimeError("start failed") + + monkeypatch.setattr(control, "start", failed_start) + result = simulation.run(control, steps=3) + assert result.exit_reason == "backend_error" and result.error == "start failed" + assert result.completed_steps == 0 and result.simulation_time_s == 0.0 + assert simulation.model is not None and not interface.health().healthy + + +def test_elapsed_control_time_is_relative_to_this_run(driver, monkeypatch): + simulation, control, _, task = driver + simulation.advance_control_period() + before = simulation.data.time + contexts = [] + observe = task.build_observation + + def record_context(state, context): + contexts.append(context.elapsed_time_s) + return observe(state, context) + + monkeypatch.setattr(task, "build_observation", record_context) + result = simulation.run(control, steps=2) + assert result.success + assert contexts == pytest.approx([0.0, simulation.control_period_s]) + assert result.simulation_time_s == pytest.approx(2 * simulation.control_period_s) + assert simulation.data.time - before == pytest.approx(result.simulation_time_s) + + +def test_start_termination_does_not_advance_physics(driver, monkeypatch): + simulation, control, interface, task = driver + monkeypatch.setattr(task, "check_termination", lambda state: "done") + result = simulation.run(control, steps=3) + assert not result.success and result.exit_reason == "task_terminated" + assert result.completed_steps == 0 and result.simulation_time_s == 0.0 + assert simulation.data.time == 0.0 and not interface.health().healthy + + +def test_tick_termination_has_no_extra_physics_interval(driver, monkeypatch): + simulation, control, _, task = driver + monkeypatch.setattr(task, "check_termination", lambda state: "done" if state.sample_time_ns > 0 else None) + result = simulation.run(control, steps=3) + assert result.exit_reason == "task_terminated" and result.completed_steps == 1 + assert result.simulation_time_s == pytest.approx(simulation.control_period_s) + + +def test_viewer_interrupt_uses_shared_cleanup(driver, monkeypatch): + simulation, control, interface, _ = driver + + def interrupt(): + raise KeyboardInterrupt("viewer closed") + + monkeypatch.setattr(simulation, "check_viewer_running", interrupt) + result = simulation.run(control, steps=3) + assert result.exit_reason == "interrupted" and result.completed_steps == 0 + assert result.simulation_time_s == 0.0 and not interface.health().healthy + assert simulation.model is not None + + +@pytest.mark.parametrize("steps", [0, -1, True, 1.5]) +def test_run_rejects_invalid_tick_count(driver, steps): + simulation, control, _, _ = driver + with pytest.raises(ValidationError, match="steps"): + simulation.run(control, steps=steps) + + +def test_run_uses_supplied_control_session_period(driver): + simulation, _, interface, task = driver + control = ControlSession( + robot=interface, + task=task, + policy=NoOpPolicyRuntime(interface.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=0.01, + state_timeout_s=0.1, + ) + result = simulation.run(control, steps=2) + assert result.success + assert simulation.control_period_s == control.period_s + assert result.simulation_time_s == pytest.approx(2 * control.period_s) + + +def test_binding_rejects_nonintegral_period_without_binding_control(driver): + from types import SimpleNamespace + + simulation, _, _, _ = driver + period_s = simulation.control_period_s + substeps = simulation.physics_substeps + invalid = SimpleNamespace(period_s=simulation.sim.dt * 1.5, robot=simulation.robot) + with pytest.raises(ValidationError, match="control.period_s"): + simulation.bind_control_session(invalid) + assert simulation.control is None + assert simulation.control_period_s == period_s and simulation.physics_substeps == substeps + + +def test_run_requires_integral_control_period(driver): + simulation, _, _, _ = driver + # Physics cannot resample a control period that does not span whole substeps. + from types import SimpleNamespace + + mismatched = SimpleNamespace(period_s=simulation.sim.dt * 1.5, robot=simulation.robot) + with pytest.raises(ValidationError, match="control.period_s"): + simulation.run(mismatched, steps=1) diff --git a/motrix_deploy_mujoco/tests/test_viewer.py b/motrix_deploy_mujoco/tests/test_viewer.py index 764ca942..eca539a0 100644 --- a/motrix_deploy_mujoco/tests/test_viewer.py +++ b/motrix_deploy_mujoco/tests/test_viewer.py @@ -49,6 +49,7 @@ def __init__(self) -> None: self.poll_count = 0 self.destroy_count = 0 self.terminate_count = 0 + self.events: list[Any] = [] def init(self) -> bool: return True @@ -92,6 +93,9 @@ def set_scroll_callback(self, window: _Window, callback: Any) -> None: def poll_events(self) -> None: self.poll_count += 1 + events, self.events = self.events, [] + for event in events: + event() def window_should_close(self, window: _Window) -> bool: return window.should_close @@ -252,6 +256,108 @@ def test_glfw_escape_and_window_close_interrupt_input() -> None: viewer.close() +def test_render_event_pump_retains_keyboard_edges_for_command_input() -> None: + viewer, glfw, _ = _viewer() + device = viewer.keyboard_device + try: + glfw.events.append(lambda: glfw.callbacks["key"](glfw.window, glfw.KEY_A, 0, glfw.PRESS, 0)) + viewer.sync() + viewer.sync() + # Rendering delivers callbacks but never freezes or consumes input frames. + assert not device.is_key_down("a") + device.poll() + assert device.is_key_down("a") and device.is_pressing("a") + viewer.sync() + assert device.is_key_down("a") + device.poll() + assert not device.is_key_down("a") and device.is_pressing("a") + glfw.events.append(lambda: glfw.callbacks["key"](glfw.window, glfw.KEY_A, 0, glfw.RELEASE, 0)) + viewer.sync() + device.poll() + assert device.is_key_up("a") and not device.is_pressing("a") + finally: + viewer.close() + + +@pytest.mark.parametrize("event", ["escape", "close"]) +def test_render_event_pump_interrupts_without_keyboard_binding(event) -> None: + viewer, glfw, mujoco = _viewer() + try: + if event == "escape": + glfw.events.append(lambda: glfw.callbacks["key"](glfw.window, glfw.KEY_ESCAPE, 0, glfw.PRESS, 0)) + else: + glfw.events.append(lambda: glfw.callbacks["close"](glfw.window)) + with pytest.raises(KeyboardInterrupt): + viewer.sync() + assert glfw.poll_count == 1 + assert mujoco.render_count == 0 + finally: + viewer.close() + + +def test_gui_runtime_pumps_startup_camera_and_close_with_constant_input( + control_scene_config, control_gains, monkeypatch +) -> None: + from control_helpers import DummyServoTask + + from motrix_deploy.policy import NoOpPolicyRuntime + from motrix_deploy.runtime.control import ControlSession + from motrix_deploy_mujoco import runtime + from motrix_env_core.input import ConstantPlanarVelocityBinding + + glfw = _FakeGlfw() + mujoco = _FakeMujoco() + viewer = MujocoGlfwViewer(mujoco, glfw_module=glfw) + from dataclasses import replace + + simulation = runtime.MujocoRuntime(replace(control_scene_config, render=True), viewer_factory=lambda *_: viewer) + robot = simulation.robot + control = ControlSession( + robot=robot, + task=DummyServoTask(robot.spec, *control_gains), + policy=NoOpPolicyRuntime(robot.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=0.02, + state_timeout_s=0.1, + ) + waits = [] + + class Scheduler: + overrun_count = 0 + + def __init__(self, period_s): + assert period_s == simulation.control_period_s + + def reset(self): + pass + + def wait(self, step): + waits.append(step) + + monkeypatch.setattr(runtime, "RealtimeScheduler", Scheduler) + poll = glfw.poll_events + + def deliver_events(): + if glfw.poll_count == 1: + glfw.events.append(lambda: glfw.callbacks["scroll"](glfw.window, 0.0, 2.0)) + elif glfw.poll_count == 3: + glfw.events.append(lambda: glfw.callbacks["close"](glfw.window)) + poll() + + monkeypatch.setattr(glfw, "poll_events", deliver_events) + simulation.bind_control_session(control) + with simulation: + assert glfw.poll_count == 1 and glfw.swap_count == 1 + result = simulation.run(steps=10) + assert result.exit_reason == "interrupted" and result.completed_steps == 3 + assert result.simulation_time_s == pytest.approx(0.06) + assert waits == [1, 2] + assert mujoco.camera_moves == [(mujoco.mjtMouse.mjMOUSE_ZOOM, 0.0, -0.1)] + assert not control.active + assert glfw.destroy_count == 1 and glfw.terminate_count == 1 + assert mujoco.context.free_count == 1 and not viewer.is_running() + + def test_glfw_viewer_renders_and_moves_camera_without_stepping_physics() -> None: camera_config = SystemCameraCfg(lookat=(1.0, 2.0, 3.0), distance=4.0, elevation=-25.0, azimuth=120.0) viewer, glfw, mujoco = _viewer(camera_config) @@ -265,6 +371,7 @@ def test_glfw_viewer_renders_and_moves_camera_without_stepping_physics() -> None assert mujoco.update_count == 1 assert mujoco.render_count == 1 assert glfw.swap_count == 1 + assert glfw.poll_count == 1 glfw.mouse_buttons[glfw.MOUSE_BUTTON_LEFT] = glfw.PRESS glfw.callbacks["cursor"](glfw.window, 100.0, 200.0) diff --git a/motrix_deploy_tasks/README.md b/motrix_deploy_tasks/README.md index bc8e7283..f1655dec 100644 --- a/motrix_deploy_tasks/README.md +++ b/motrix_deploy_tasks/README.md @@ -1,23 +1,28 @@ # Motrix Deploy Tasks -`motrix_deploy_tasks` contains concrete, versioned task implementations. It publishes the `motrix-deploy` executable as a -thin bootstrap that imports these tasks before delegating to `motrix_deploy.cli`. -In the MotrixLab workspace, the bootstrap selects `configs/deploy/sim2sim/go2_walk_sim2sim.yaml` for `sim2sim` and -`configs/deploy/sim2real/go2_walk_flat_sim2real.yaml` for `sim2real`. Outside the workspace, -pass an explicit Hydra `--config-path` and `--config-name`. - -The default sim2sim recipe targets `go2-walk-rough`; the default sim2real recipe targets `go2-walk-flat`. From the workspace root, select the flat-terrain recipe while retaining the -workspace config path with: +`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: ```bash -motrix-deploy sim2sim \ - --config-name go2_walk_flat_sim2sim \ - 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 ``` -The package currently provides the `go2_walk/v1` task used by the Go2 flat and rough walking environments. -Importing `motrix_deploy_tasks` registers its concrete implementations with the registry in `motrix_deploy.task`. The -artifact stores this versioned task name; the runtime resolves it to -`motrix_deploy_tasks.go2_walk.Go2WalkDeployTaskV1` for both sim2sim and sim2real. Deployment recipes select the backend and -input binding, while the artifact remains the authority for task selection. -The environment-to-artifact profile compiler is owned by `motrix_envs.deploy`, alongside the source environment config. +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`. + +For a scene/task/policy assembled directly in Python, see +[`examples/deploy_to_sim.py`](../examples/deploy_to_sim.py). diff --git a/motrix_deploy_tasks/pyproject.toml b/motrix_deploy_tasks/pyproject.toml index 9497b08f..d3cd6fc7 100644 --- a/motrix_deploy_tasks/pyproject.toml +++ b/motrix_deploy_tasks/pyproject.toml @@ -13,12 +13,18 @@ license = "Apache-2.0" dependencies = [ "motrix-deploy", "motrix-env-core", + "motrix-robots", + "hydra-core>=1.3,<1.4", "numpy>=1.26", ] -[project.optional-dependencies] -mujoco = ["motrix-deploy-mujoco"] -unitree = ["motrix-deploy-unitree"] +[project.entry-points."motrix_deploy.tasks"] +"go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTaskV1" -[project.scripts] -motrix-deploy = "motrix_deploy_tasks:main" +[project.entry-points."motrix_deploy.envs"] +flat = "motrix_deploy_tasks.envs.flat:build_scene" +rough = "motrix_deploy_tasks.envs.rough:build_scene" + +[tool.uv.build-backend] +module-name = ["motrix_deploy_tasks", "hydra_plugins"] +namespace = true diff --git a/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py b/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py new file mode 100644 index 00000000..6285285b --- /dev/null +++ b/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py @@ -0,0 +1,17 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Expose installed deployment recipes without importing task implementations.""" + +from hydra.core.config_search_path import ConfigSearchPath +from hydra.plugins.search_path_plugin import SearchPathPlugin + + +class MotrixDeployTasksSearchPathPlugin(SearchPathPlugin): + def manipulate_search_path(self, search_path: ConfigSearchPath) -> None: + # Deployment task options share Hydra's task group name with training; + # expose them only for the deployment application's configuration root. + if any( + entry.provider == "main" and entry.path == "pkg://motrix_deploy.config" for entry in search_path.get_path() + ): + search_path.append(provider="motrix-deploy-tasks", path="pkg://motrix_deploy_tasks.config") diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py index be70cc27..50ae5b38 100644 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py @@ -1,40 +1,6 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Import concrete tasks and register them with :mod:`motrix_deploy.task`.""" - -import sys -from collections.abc import Sequence -from pathlib import Path - -from motrix_deploy.cli import main as deploy_main -from motrix_deploy_tasks.go2_walk import Go2WalkDeployTaskV1 +"""Concrete deployment tasks discovered through installed package entry points.""" __version__ = "0.3.0" - -_DEFAULT_CONFIG_ROOT = Path("configs/deploy") -_DEFAULT_CONFIG_NAMES = { - "sim2sim": "go2_walk_sim2sim", - "sim2real": "go2_walk_flat_sim2real", -} - - -def main(argv: Sequence[str] | None = None) -> int | None: - """Register concrete tasks and run the core CLI with the workspace recipe by default.""" - args = list(sys.argv[1:] if argv is None else argv) - subcommand = args[0] if args[:1] in (["sim2sim"], ["sim2real"]) else "sim2sim" - default_config_name = _DEFAULT_CONFIG_NAMES[subcommand] - uses_runtime_config = bool(args) and args[:1] != ["inspect"] and args != ["--version"] - has_config_path = any(arg == "--config-path" or arg.startswith("--config-path=") for arg in args) - has_config_name = any(arg == "--config-name" or arg.startswith("--config-name=") for arg in args) - default_config_dir = Path.cwd() / _DEFAULT_CONFIG_ROOT / subcommand - default_config = default_config_dir / f"{default_config_name}.yaml" - if uses_runtime_config and default_config.is_file(): - if not has_config_path: - args.extend(("--config-path", str(default_config_dir))) - if not has_config_name: - args.extend(("--config-name", default_config_name)) - return deploy_main(args) - - -__all__ = ["Go2WalkDeployTaskV1", "main"] diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/__init__.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/__init__.py new file mode 100644 index 00000000..e3818141 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/__init__.py @@ -0,0 +1,4 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Packaged Hydra task groups and deployment recipes.""" diff --git a/configs/deploy/sim2real/go2_walk_flat_sim2real.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/hardware.yaml similarity index 86% rename from configs/deploy/sim2real/go2_walk_flat_sim2real.yaml rename to motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/hardware.yaml index fa69fc2f..0d9d1faa 100644 --- a/configs/deploy/sim2real/go2_walk_flat_sim2real.yaml +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/hardware.yaml @@ -1,9 +1,12 @@ +# @package _global_ defaults: - - base + - /runtime: hardware - _self_ -backend: - name: unitree_go2 +runtime: + backend: unitree_go2 + kp: 50 + kd: 1 network_interface: enp5s0 domain_id: 0 lowcmd_topic: rt/lowcmd @@ -34,3 +37,4 @@ command: invert_linear_y: true invert_yaw: true deadman_button: L1 +duration_s: 300.0 diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/sim.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/sim.yaml new file mode 100644 index 00000000..ccaa36a1 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-flat/sim.yaml @@ -0,0 +1,23 @@ +# @package _global_ +defaults: + - /runtime: sim + - _self_ + +runtime: + backend: mujoco + sensor_bindings: + base_angular_velocity: gyro + base_linear_acceleration: accelerometer + base_linear_velocity: global_linvel + deploy_env_id: flat + robot_id: go2 + # Attach offset from the model's root height (0.445), keeping the default stance clear of the floor. + robot_translation: [0.0, 0.0, -0.114] + robot_rotation: [0.0, 0.0, 0.0, 1.0] + physics: + dt: 0.002 + solver_iterations: 100 + +command: + source: constant + velocity: [0.0, 0.0, 0.0] diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-rough/sim.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-rough/sim.yaml new file mode 100644 index 00000000..9c266cc9 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/go2-walk-rough/sim.yaml @@ -0,0 +1,23 @@ +# @package _global_ +defaults: + - /runtime: sim + - _self_ + +runtime: + backend: mujoco + sensor_bindings: + base_angular_velocity: gyro + base_linear_acceleration: accelerometer + base_linear_velocity: global_linvel + deploy_env_id: rough + robot_id: go2 + # Attach offset from the model's root height (0.445), clearing the terrain at reset. + robot_translation: [0.0, 0.0, -0.025] + robot_rotation: [0.0, 0.0, 0.0, 1.0] + physics: + dt: 0.002 + solver_iterations: 100 + +command: + source: constant + velocity: [0.0, 0.0, 0.0] diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/__init__.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/__init__.py new file mode 100644 index 00000000..fbb283fd --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/__init__.py @@ -0,0 +1,4 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot-independent deployment worlds discovered through entry points.""" diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py new file mode 100644 index 00000000..20ccb359 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py @@ -0,0 +1,17 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""A robot-free flat-ground deployment world.""" + +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg + + +@configclass +class FlatWorldObjsCfg(SceneObjsCfg): + floor: FlatTerrainCfg = FlatTerrainCfg(friction=(0.6, 0.005, 0.0001)) + + +def build_scene() -> SceneCfg: + """Build fresh flat ground with an empty robot slot.""" + return SceneCfg(objs=FlatWorldObjsCfg()) diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py new file mode 100644 index 00000000..69bbede3 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py @@ -0,0 +1,35 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Deterministic procedural rough ground, independent of robots and RL tasks.""" + +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import ( + HFieldTerrainCfg, + NoiseTerrainGeneratorCfg, + ProceduralHFieldAssetCfg, + SceneAssetsCfg, + SceneCfg, + SceneObjsCfg, +) + + +@configclass +class RoughWorldAssetsCfg(SceneAssetsCfg): + # Match the built-in quadruped rough-walk terrain without importing its + # robot-scoped scene or training task configuration. + terrain: ProceduralHFieldAssetCfg = ProceduralHFieldAssetCfg( + generator=NoiseTerrainGeneratorCfg(seed=0, height_scale=0.1, flip_y=True), + size=(64.0, 64.0), + shape=(320, 320), + ) + + +@configclass +class RoughWorldObjsCfg(SceneObjsCfg): + floor: HFieldTerrainCfg = HFieldTerrainCfg(hfield="terrain", friction=(0.6, 0.005, 0.0001)) + + +def build_scene() -> SceneCfg: + """Build fresh seeded rough ground with an empty robot slot.""" + return SceneCfg(assets=RoughWorldAssetsCfg(), objs=RoughWorldObjsCfg()) diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/go2_walk.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/go2_walk.py deleted file mode 100644 index ffaa0c9d..00000000 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/go2_walk.py +++ /dev/null @@ -1,188 +0,0 @@ -# Copyright Motphys Technology Co., Ltd. 2025, 2026 -# SPDX-License-Identifier: Apache-2.0 - -"""Direct observation and action implementation for ``go2_walk/v1``.""" - -import numpy as np - -from motrix_deploy.artifact import TaskSpec -from motrix_deploy.contracts import FloatArray, RobotCommand, RobotSpec, RobotState, float32_array -from motrix_deploy.errors import ValidationError -from motrix_deploy.runtime import PolicyContext -from motrix_deploy.task import DeployTask, register_task -from motrix_env_core.input import PlanarVelocityCommand - -TASK_NAME = "go2_walk/v1" -OBSERVATION_SIZE = 49 -ACTION_SIZE = 12 - - -@register_task(TASK_NAME) -class Go2WalkDeployTaskV1(DeployTask[PlanarVelocityCommand]): - """Go2 walking policy I/O semantics frozen as task version 1.""" - - def __init__(self, spec: TaskSpec, robot: RobotSpec) -> None: - if spec.name != TASK_NAME: - raise ValidationError("task.name", TASK_NAME, spec.name) - if spec.observation_size != OBSERVATION_SIZE or spec.action_size != ACTION_SIZE: - raise ValidationError( - "task.policy_shape", - f"observation={OBSERVATION_SIZE}, action={ACTION_SIZE}", - f"observation={spec.observation_size}, action={spec.action_size}", - ) - if robot.joint_count != ACTION_SIZE: - raise ValidationError("robot.joint_count", str(ACTION_SIZE), robot.joint_count) - config = spec.config - shape = (ACTION_SIZE,) - - def cfg(key: str, shp: tuple[int, ...] = shape) -> FloatArray: - return _config_array(config[key], f"task.config.{key}", shp) - - self.spec = spec - self.robot = robot - self._action_scale = cfg("action_scale") - self._kp = cfg("kp") - self._kd = cfg("kd") - self._raw_lower, self._raw_upper = _config_range(config["raw_clip"], "task.config.raw_clip", shape) - self._command_lower = cfg("command_lower", (3,)) - self._command_upper = cfg("command_upper", (3,)) - self._command_scale = cfg("command_scale", (3,)) - self._feet_phase_offsets = cfg("feet_phase_offsets", (4,)) - gait_frequency = config["gait_frequency_hz"] - if isinstance(gait_frequency, bool) or not isinstance(gait_frequency, (int, float)): - raise ValidationError("task.config.gait_frequency_hz", "a positive finite number", gait_frequency) - self._gait_frequency_hz = gait_frequency - if not np.isfinite(self._gait_frequency_hz) or self._gait_frequency_hz <= 0: - raise ValidationError("task.config.gait_frequency_hz", "a positive finite number", gait_frequency) - standing_threshold = config["standing_threshold"] - if isinstance(standing_threshold, bool) or not isinstance(standing_threshold, (int, float)): - raise ValidationError("task.config.standing_threshold", "a non-negative finite number", standing_threshold) - self._standing_threshold = float(standing_threshold) - if not np.isfinite(self._standing_threshold) or self._standing_threshold < 0.0: - raise ValidationError("task.config.standing_threshold", "a non-negative finite number", standing_threshold) - if ( - np.any(self._action_scale < 0) - or np.any(self._kp < 0) - or np.any(self._kd < 0) - or np.any(self._raw_lower > self._raw_upper) - or np.any(self._command_lower > self._command_upper) - or np.any(self._command_scale < 0.0) - or np.any(self._command_scale > 1.0) - ): - raise ValidationError( - "task.config", - "ordered ranges, non-negative control values, and command_scale inside [0, 1]", - "invalid value", - ) - self._previous_action: FloatArray = np.zeros(ACTION_SIZE, dtype=np.float32) - - @property - def observation_size(self) -> int: - return OBSERVATION_SIZE - - @property - def action_size(self) -> int: - return ACTION_SIZE - - @property - def command_lower(self) -> FloatArray: - return self._command_lower * self._command_scale - - @property - def command_upper(self) -> FloatArray: - return self._command_upper * self._command_scale - - def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: - self._validate_state(state) - if context.step != 0: - raise ValidationError("context.step", "0 during task reset", context.step) - self.validate_command(context.command) - self._previous_action = np.zeros(ACTION_SIZE, dtype=np.float32) - - def build_observation(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: - self._validate_state(state) - velocity = self._velocity_command(context) - phase = np.zeros(4, dtype=np.float32) - if context.step > 0 and np.linalg.norm(velocity) >= self._standing_threshold: - phase = np.mod( - context.elapsed_time_s * self._gait_frequency_hz + self._feet_phase_offsets, - 1.0, - ).astype(np.float32) - observation = np.concatenate( - ( - state.base_angular_velocity, - _projected_gravity(state.base_orientation_xyzw), - state.joint_position - self.robot.default_joint_position, - state.joint_velocity, - self._previous_action, - velocity, - phase, - ) - ).astype(np.float32) - return float32_array(observation, path="observation", shape=(OBSERVATION_SIZE,)) - - def process_action(self, action: FloatArray) -> RobotCommand: - raw = float32_array(action, path="action.raw", shape=(ACTION_SIZE,)) - executed = np.clip(raw, self._raw_lower, self._raw_upper).astype(np.float32) - target = self.robot.default_joint_position + self._action_scale * executed - target = np.clip(target, self.robot.position_lower, self.robot.position_upper).astype(np.float32) - self._previous_action = executed - zeros = np.zeros(ACTION_SIZE, dtype=np.float32) - return RobotCommand( - joint_position=target, - joint_velocity=zeros, - feedforward_torque=zeros, - kp=self._kp, - kd=self._kd, - ) - - def validate_command(self, command: PlanarVelocityCommand) -> None: - if not isinstance(command, PlanarVelocityCommand): - raise ValidationError("command.type", "PlanarVelocityCommand", type(command).__name__) - if command.batch_size != 1: - raise ValidationError("command.batch_size", "1", command.batch_size) - velocity = command.values[0, :] - if np.any(velocity < self._command_lower) or np.any(velocity > self._command_upper): - raise ValidationError( - "command.velocity", - f"inside [{self._command_lower.tolist()}, {self._command_upper.tolist()}]", - velocity.tolist(), - ) - - def _velocity_command(self, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: - self.validate_command(context.command) - return context.command.values[0, :] - - def _validate_state(self, state: RobotState) -> None: - expected = (ACTION_SIZE,) - if state.joint_position.shape != expected or state.joint_velocity.shape != expected: - raise ValidationError("state.joints.shape", str(expected), state.joint_position.shape) - - -def _config_array(value: object, path: str, shape: tuple[int, ...]) -> FloatArray: - array = np.asarray(value, dtype=np.float32) - if array.ndim == 0: # broadcast scalar config across the required shape - array = np.broadcast_to(array, shape) - return float32_array(array, path=path, shape=shape) - - -def _config_range(value: object, path: str, shape: tuple[int, ...]) -> tuple[FloatArray, FloatArray]: - if not isinstance(value, list) or len(value) != 2: - raise ValidationError(path, "[lower, upper] scalars or vectors", value) - return _config_array(value[0], f"{path}[0]", shape), _config_array(value[1], f"{path}[1]", shape) - - -def _projected_gravity(orientation_xyzw: FloatArray) -> FloatArray: - x, y, z, w = orientation_xyzw - rotation = np.array( - [ - [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], - [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], - [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], - ], - dtype=np.float32, - ) - return rotation @ np.array([0.0, 0.0, -1.0], dtype=np.float32) - - -__all__ = ["Go2WalkDeployTaskV1"] diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/go2_walk.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/go2_walk.py new file mode 100644 index 00000000..9e453b78 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/go2_walk.py @@ -0,0 +1,180 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Typed artifact specification and runtime for ``go2_walk/v1``.""" + +from typing import Annotated, ClassVar + +import numpy as np +from pydantic import Field, model_validator + +from motrix_deploy.artifact.schema import TaskSpec +from motrix_deploy.contracts import FloatArray, JointServoCommand, RobotSpec, RobotState, float32_array +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime import PolicyContext +from motrix_deploy.task import DeployTask +from motrix_env_core.input import PlanarVelocityCommand + +FLOAT32_MAX = float(np.finfo(np.float32).max) +FiniteFloat = Annotated[float, Field(allow_inf_nan=False)] +Float32Float = Annotated[FiniteFloat, Field(ge=-FLOAT32_MAX, le=FLOAT32_MAX)] +NonNegativeFloat = Annotated[FiniteFloat, Field(ge=0)] +PositiveFloat = Annotated[FiniteFloat, Field(gt=0)] +JointVector = Annotated[list[Float32Float], Field(min_length=1)] +NonNegativeJointVector = Annotated[list[Annotated[Float32Float, Field(ge=0)]], Field(min_length=1)] +CommandVector = Annotated[list[Float32Float], Field(min_length=3, max_length=3)] +CommandScale = Annotated[list[Annotated[Float32Float, Field(ge=0, le=1)]], Field(min_length=3, max_length=3)] +FeetPhaseOffsets = Annotated[list[Float32Float], Field(min_length=4, max_length=4)] + + +class Go2WalkTaskSpec(TaskSpec): + """Self-contained control, command, gait and termination parameters.""" + + task_name: ClassVar[str] = "go2_walk/v1" + + action_scale: NonNegativeJointVector + kp: NonNegativeJointVector + kd: NonNegativeJointVector + # Policy action bounds before scaling/default offset, not robot position limits. + action_lower: JointVector + action_upper: JointVector + command_lower: CommandVector + command_upper: CommandVector + command_scale: CommandScale + feet_phase_offsets: FeetPhaseOffsets + gait_frequency_hz: PositiveFloat + standing_threshold: NonNegativeFloat + termination_min_up_z: Annotated[FiniteFloat, Field(ge=-1, le=1)] + termination_min_base_height: FiniteFloat | None + + @model_validator(mode="after") + def validate_joint_vectors_and_ranges(self) -> "Go2WalkTaskSpec": + joint_count = len(self.action_scale) + for key in ("kp", "kd", "action_lower", "action_upper"): + if len(getattr(self, key)) != joint_count: + raise ValueError(f"{key} must have the same length as action_scale ({joint_count})") + if any(lower > upper for lower, upper in zip(self.action_lower, self.action_upper)): + raise ValueError("action_lower must be less than or equal to action_upper") + if any(lower > upper for lower, upper in zip(self.command_lower, self.command_upper)): + raise ValueError("command_lower must be less than or equal to command_upper") + return self + + +class Go2WalkDeployTaskV1(DeployTask[PlanarVelocityCommand]): + """Go2 walking policy I/O and artifact-owned fall termination. + + Orientation termination is supported by both simulated and real IMU state. + Optional world-frame base-height termination requires base_position when + enabled; no height is invented when the backend cannot provide it. + """ + + spec_type = Go2WalkTaskSpec + + def __init__(self, spec: Go2WalkTaskSpec, robot: RobotSpec) -> None: + if len(spec.action_scale) != robot.joint_count: + raise ValidationError( + "task.config.action_scale", f"{robot.joint_count} joint values", len(spec.action_scale) + ) + self.spec = spec + self.robot = robot + self._action_scale = np.asarray(spec.action_scale, dtype=np.float32) + self._kp = np.asarray(spec.kp, dtype=np.float32) + self._kd = np.asarray(spec.kd, dtype=np.float32) + self._action_lower = np.asarray(spec.action_lower, dtype=np.float32) + self._action_upper = np.asarray(spec.action_upper, dtype=np.float32) + self._command_lower = np.asarray(spec.command_lower, dtype=np.float32) + self._command_upper = np.asarray(spec.command_upper, dtype=np.float32) + self._command_scale = np.asarray(spec.command_scale, dtype=np.float32) + self._feet_phase_offsets = np.asarray(spec.feet_phase_offsets, dtype=np.float32) + self._gait_frequency_hz = spec.gait_frequency_hz + self._standing_threshold = spec.standing_threshold + self._termination_min_up_z = spec.termination_min_up_z + self._termination_min_base_height = spec.termination_min_base_height + self._previous_action: FloatArray = np.zeros(self.robot.joint_count, dtype=np.float32) + + @property + def command_lower(self) -> FloatArray: + return self._command_lower * self._command_scale + + @property + def command_upper(self) -> FloatArray: + return self._command_upper * self._command_scale + + def check_termination(self, state: RobotState) -> str | None: + x, y, _, _ = state.base_orientation_xyzw + up_z = 1.0 - 2.0 * (x * x + y * y) + if up_z <= self._termination_min_up_z: + return "fall_orientation" + if self._termination_min_base_height is not None: + if state.base_position is None: + raise ValidationError("state.base_position", "available for enabled base-height termination", "missing") + if state.base_position[2] <= self._termination_min_base_height: + return "fall_height" + return None + + def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: + """Reset action history; callers provide validated state and command.""" + self._previous_action.fill(0) + + def build_observation(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: + """Consume state and command validated by ControlSession or the caller.""" + velocity = context.command.values[0, :] + phase = np.zeros(4, dtype=np.float32) + if context.step > 0 and np.linalg.norm(velocity) >= self._standing_threshold: + phase = np.mod( + context.elapsed_time_s * self._gait_frequency_hz + self._feet_phase_offsets, + 1.0, + ).astype(np.float32) + observation = np.concatenate( + ( + state.base_angular_velocity, + _projected_gravity(state.base_orientation_xyzw), + state.joint_position - self.robot.default_joint_position, + state.joint_velocity, + self._previous_action, + velocity, + phase, + ) + ).astype(np.float32) + return observation + + def process_action(self, action: FloatArray) -> JointServoCommand: + raw = float32_array(action, path="action.raw", shape=(self.robot.joint_count,)) + executed = np.clip(raw, self._action_lower, self._action_upper).astype(np.float32) + target = self.robot.default_joint_position + self._action_scale * executed + target = np.clip(target, self.robot.position_lower, self.robot.position_upper).astype(np.float32) + self._previous_action = executed + zeros = np.zeros(self.robot.joint_count, dtype=np.float32) + return JointServoCommand( + joint_position=target, + joint_velocity=zeros, + feedforward_torque=zeros, + kp=self._kp, + kd=self._kd, + ) + + def validate_command(self, command: PlanarVelocityCommand) -> None: + if not isinstance(command, PlanarVelocityCommand): + raise ValidationError("command.type", "PlanarVelocityCommand", type(command).__name__) + if command.batch_size != 1: + raise ValidationError("command.batch_size", "1", command.batch_size) + velocity = command.values[0, :] + if np.any(velocity < self._command_lower) or np.any(velocity > self._command_upper): + raise ValidationError( + "command.velocity", + f"inside [{self._command_lower.tolist()}, {self._command_upper.tolist()}]", + velocity.tolist(), + ) + + +def _projected_gravity(orientation_xyzw: FloatArray) -> FloatArray: + x, y, z, w = orientation_xyzw + rotation = np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ], + dtype=np.float32, + ) + return rotation @ np.array([0.0, 0.0, -1.0], dtype=np.float32) diff --git a/motrix_deploy_tasks/tests/test_cli.py b/motrix_deploy_tasks/tests/test_cli.py index 4ec7c3d7..f82cf230 100644 --- a/motrix_deploy_tasks/tests/test_cli.py +++ b/motrix_deploy_tasks/tests/test_cli.py @@ -1,18 +1,21 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Task bootstrap and deployment CLI configuration tests.""" +"""Installed task discovery and deployment CLI configuration tests.""" import json import subprocess import sys +from dataclasses import asdict from pathlib import Path -from typing import Any import numpy as np import onnx import pytest -from hydra import compose, initialize_config_dir +from hydra import compose, initialize_config_dir, initialize_config_module +from hydra.core.global_hydra import GlobalHydra +from hydra.core.object_type import ObjectType +from motrix_robots.unitree import UnitreeGo2Robot from onnx import TensorProto, helper, numpy_helper from motrix_deploy.artifact import ( @@ -20,16 +23,13 @@ DeploymentManifest, PolicySpec, SourceSpec, - TaskSpec, sha256_bytes, write_artifact, ) from motrix_deploy.contracts import RobotSpec, TensorSpec - -ROOT = Path(__file__).parents[2] -ROUGH_CONFIG_PATH = ROOT / "configs/deploy/sim2sim/go2_walk_sim2sim.yaml" -FLAT_CONFIG_PATH = ROOT / "configs/deploy/sim2sim/go2_walk_flat_sim2sim.yaml" -REAL_CONFIG_PATH = ROOT / "configs/deploy/sim2real/go2_walk_flat_sim2real.yaml" +from motrix_deploy.env import assemble_deploy_scene, create_deploy_env +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec +from motrix_env_core.config.scene import FlatTerrainCfg, HFieldTerrainCfg, SceneCfg def _rollout_result(stdout: str) -> dict[str, object]: @@ -38,12 +38,26 @@ def _rollout_result(stdout: str) -> dict[str, object]: return json.loads(stdout[json_start:]) +@pytest.mark.parametrize("args", [[], ["--help"]]) +def test_cli_general_help(args: list[str]) -> None: + result = subprocess.run( + [sys.executable, "-m", "motrix_deploy.cli", *args], + check=False, + capture_output=True, + text=True, + ) + assert result.returncode == 0, result.stderr + assert "artifact: ???" in result.stdout + assert "runtime: ???" in result.stdout + + def test_cli_help() -> None: result = subprocess.run( [ sys.executable, "-c", - "from motrix_deploy_tasks import main; raise SystemExit(main())", + "from motrix_deploy.cli import main; raise SystemExit(main())", + "task=go2-walk-rough/sim", "--help", ], check=False, @@ -53,16 +67,16 @@ def test_cli_help() -> None: assert result.returncode == 0 assert "artifact: ???" in result.stdout - assert "scene: go2-walk-rough" in result.stdout + assert "deploy_env_id: rough" in result.stdout -def test_sim2real_cli_help_selects_hardware_recipe() -> None: +def test_hardware_cli_help_selects_hardware_recipe() -> None: result = subprocess.run( [ sys.executable, "-c", - "from motrix_deploy_tasks import main; raise SystemExit(main())", - "sim2real", + "from motrix_deploy.cli import main; raise SystemExit(main())", + "task=go2-walk-flat/hardware", "--help", ], check=False, @@ -71,17 +85,52 @@ def test_sim2real_cli_help_selects_hardware_recipe() -> None: ) assert result.returncode == 0 - assert "name: unitree_go2" in result.stdout - assert "confirm: false" in result.stdout + assert "backend: unitree_go2" in result.stdout + assert "kind: hardware" in result.stdout -def test_import_registers_go2_task_with_motrix_deploy() -> None: - script = ( - "from motrix_deploy.task import registered_tasks; " - "assert 'go2_walk/v1' not in registered_tasks(); " - "import motrix_deploy_tasks; " - "assert 'go2_walk/v1' in registered_tasks()" +@pytest.mark.parametrize( + ("task", "runtime_kind", "backend"), + [ + ("go2-walk-flat/sim", "simulation", "mujoco"), + ("go2-walk-rough/sim", "simulation", "mujoco"), + ("go2-walk-flat/hardware", "hardware", "unitree_go2"), + ], +) +def test_installed_recipes_compose_outside_workspace(tmp_path, task, runtime_kind, backend) -> None: + result = subprocess.run( + [sys.executable, "-m", "motrix_deploy.cli", f"task={task}", "--cfg", "job"], + cwd=tmp_path, + check=False, + capture_output=True, + text=True, ) + assert result.returncode == 0, result.stderr + assert f"backend: {backend}" in result.stdout + assert "artifact: ???" in result.stdout + assert f"kind: {runtime_kind}" in result.stdout + if runtime_kind == "simulation": + assert "sensor_bindings:" in result.stdout + assert "viewer: true" in result.stdout + assert "realtime: null" in result.stdout + + +def test_deployment_plugin_preserves_other_app_task_discovery(tmp_path: Path) -> None: + config_dir = tmp_path / "config" + task_dir = config_dir / "task" + task_dir.mkdir(parents=True) + (task_dir / "custom.yaml").write_text("value: custom\n") + (config_dir / "app.yaml").write_text("defaults:\n - task: custom\n") + + with initialize_config_dir(version_base=None, config_dir=str(config_dir)): + loader = GlobalHydra.instance().config_loader() + assert loader.get_group_options("task", results_filter=ObjectType.CONFIG) == ["custom"] + cfg = compose(config_name="app") + assert cfg.task.value == "custom" + + +def test_installed_go2_task_is_available_without_preimport() -> None: + script = "from motrix_deploy.task import available_tasks; assert 'go2_walk/v1' in available_tasks()" result = subprocess.run([sys.executable, "-c", script], check=False, capture_output=True, text=True) @@ -89,99 +138,129 @@ def test_import_registers_go2_task_with_motrix_deploy() -> None: @pytest.mark.parametrize( - ("config_path", "scene", "base_height"), + ("task", "deploy_env_id", "terrain_type"), [ - (ROUGH_CONFIG_PATH, "go2-walk-rough", 0.42), - (FLAT_CONFIG_PATH, "go2-walk-flat", 0.331), + ("go2-walk-rough/sim", "rough", HFieldTerrainCfg), + ("go2-walk-flat/sim", "flat", FlatTerrainCfg), ], ) -def test_sim2sim_hydra_configs_contain_all_runtime_parameters( - config_path: Path, - scene: str, - base_height: float, -) -> None: - with initialize_config_dir(version_base=None, config_dir=str(config_path.parent)): - cfg = compose( - config_name=config_path.stem, - overrides=["artifact=artifact.deploy"], - ) - - assert cfg.artifact == "artifact.deploy" - assert cfg.backend.name == "mujoco" - assert cfg.backend.scene == scene - assert cfg.backend.sim_dt == 0.002 - assert cfg.backend.solver_iterations == 100 - assert cfg.backend.base_position[2] == pytest.approx(base_height) - assert cfg.viewer is True - - -def test_workspace_bootstrap_keeps_default_path_with_explicit_flat_config_name( - monkeypatch: pytest.MonkeyPatch, +def test_mujoco_hydra_recipes_assemble_robot_and_selected_world( + task: str, + deploy_env_id: str, + terrain_type: type, ) -> None: - import motrix_deploy_tasks + with initialize_config_module(version_base=None, config_module="motrix_deploy.config"): + cfg = compose(config_name="deploy", overrides=[f"task={task}", "artifact=artifact.deploy"]) - captured: list[str] = [] + assert cfg.runtime.kind == "simulation" - def capture(args: list[str]) -> int: - captured.extend(args) - return 0 - - monkeypatch.chdir(ROOT) - monkeypatch.setattr(motrix_deploy_tasks, "deploy_main", capture) + scene = assemble_deploy_scene(cfg.runtime.deploy_env_id, cfg.runtime.robot_id) + assert isinstance(scene, SceneCfg) + assert isinstance(scene.objs.robot, UnitreeGo2Robot) + assert isinstance(scene.objs.floor, terrain_type) + assert cfg.runtime.deploy_env_id == deploy_env_id + assert cfg.runtime.robot_id == "go2" + assert cfg.artifact == "artifact.deploy" + assert cfg.runtime.backend == "mujoco" + assert cfg.runtime.physics.dt > 0 + assert cfg.runtime.physics.solver_iterations > 0 + assert len(cfg.runtime.robot_translation) == 3 + assert np.linalg.norm(cfg.runtime.robot_rotation) == pytest.approx(1.0) + assert cfg.runtime.viewer is True + assert cfg.runtime.realtime is None + assert cfg.duration_s is None + + +@pytest.mark.parametrize("deploy_env_id", ["flat", "rough"]) +def test_installed_world_factories_are_robot_free_and_fresh(deploy_env_id: str) -> None: + first = create_deploy_env(deploy_env_id) + second = create_deploy_env(deploy_env_id) + assert isinstance(first, SceneCfg) + assert first.objs.robot is None + assert second.objs.robot is None + assert first.objs.floor is not second.objs.floor + first.objs.robot = UnitreeGo2Robot() + assert second.objs.robot is None + if isinstance(first.objs.floor, HFieldTerrainCfg): + terrain = first.assets[first.objs.floor.hfield] + other = second.assets[second.objs.floor.hfield] + heights = terrain.generator.generate(terrain.size, terrain.shape) + np.testing.assert_array_equal(heights, other.generator.generate(other.size, other.shape)) + assert np.ptp(heights) > 0 + + +def test_installed_go2_task_can_be_created_without_preimport(tmp_path: Path) -> None: + specs_path = tmp_path / "specs.json" + robot = { + key: value.tolist() if isinstance(value, np.ndarray) else value for key, value in asdict(_robot_spec()).items() + } + specs_path.write_text(json.dumps({"task": _task_spec().to_dict(), "robot": robot})) + script = ( + "import json, sys; import numpy as np; " + "from motrix_deploy.artifact import TaskSpec; " + "from motrix_deploy.contracts import RobotSpec; " + "from motrix_deploy.task import create_task; " + "specs = json.load(open(sys.argv[1])); " + "specs['robot']['joint_names'] = tuple(specs['robot']['joint_names']); " + "specs['robot'].update({key: np.asarray(value, dtype=np.float32) " + "for key, value in specs['robot'].items() if isinstance(value, list)}); " + "spec = TaskSpec.from_dict(specs['task']); " + "assert type(spec).__name__ == 'Go2WalkTaskSpec'; " + "assert type(spec).__module__ == 'motrix_deploy_tasks.tasks.go2_walk'; " + "assert not any(name in sys.modules for name in ('motrix_envs', 'motrixsim', 'mujoco', 'torch')); " + "task = create_task(spec, RobotSpec(**specs['robot'])); " + "assert type(task).__module__ == 'motrix_deploy_tasks.tasks.go2_walk'; " + "assert type(task).__name__ == 'Go2WalkDeployTaskV1'" + ) - result: Any = motrix_deploy_tasks.main( - ["sim2sim", "--config-name", "go2_walk_flat_sim2sim", "artifact=artifact.deploy"] + result = subprocess.run( + [sys.executable, "-c", script, str(specs_path)], check=False, capture_output=True, text=True ) - assert result == 0 - assert captured.count("--config-name") == 1 - assert captured[captured.index("--config-name") + 1] == "go2_walk_flat_sim2sim" - assert captured[captured.index("--config-path") + 1] == str(ROOT / "configs/deploy/sim2sim") + assert result.returncode == 0, result.stderr -def test_sim2real_hydra_config_contains_hardware_guards() -> None: - with initialize_config_dir(version_base=None, config_dir=str(REAL_CONFIG_PATH.parent)): +def test_hardware_hydra_config_selects_runtime_and_command_defaults() -> None: + with initialize_config_module(version_base=None, config_module="motrix_deploy.config"): cfg = compose( - config_name=REAL_CONFIG_PATH.stem, + config_name="deploy", overrides=[ + "task=go2-walk-flat/hardware", "artifact=artifact.deploy", - "backend.network_interface=enp3s0", + "runtime.network_interface=enp3s0", ], ) - assert cfg.backend.name == "unitree_go2" - assert cfg.rollout.steps is None - assert cfg.rollout.duration_s == 300.0 - assert cfg.hardware.confirm is False - assert cfg.realtime is True - assert cfg.viewer is False + assert cfg.runtime.kind == "hardware" + assert cfg.runtime.backend == "unitree_go2" + assert cfg.duration_s is not None and cfg.duration_s > 0 assert cfg.command.source == "gamepad" - assert cfg.backend.kp == 50 - assert cfg.backend.kd == 1 - assert cfg.backend.lie_down_button == "B" - assert cfg.backend.lie_down_duration_s == pytest.approx(2.0) + assert cfg.runtime.lie_down_button == "B" assert cfg.command.gamepad.deadman_button == "L1" assert cfg.command.gamepad.invert_linear_x is False assert cfg.command.gamepad.invert_linear_y is True assert cfg.command.gamepad.invert_yaw is True -def test_sim2real_hydra_config_accepts_explicit_gain_overrides() -> None: - with initialize_config_dir(version_base=None, config_dir=str(REAL_CONFIG_PATH.parent)): +def test_hardware_hydra_config_accepts_explicit_gain_overrides() -> None: + with initialize_config_module(version_base=None, config_module="motrix_deploy.config"): cfg = compose( - config_name=REAL_CONFIG_PATH.stem, + config_name="deploy", overrides=[ + "task=go2-walk-flat/hardware", "artifact=artifact.deploy", - "backend.kp=25.0", - "backend.kd=[0.4,0.4,0.4,0.5,0.5,0.5,0.6,0.6,0.6,0.7,0.7,0.7]", + "runtime.kp=25.0", + "runtime.kd=[0.4,0.4,0.4,0.5,0.5,0.5,0.6,0.6,0.6,0.7,0.7,0.7]", ], ) - assert cfg.backend.kp == pytest.approx(25.0) - assert list(cfg.backend.kd) == pytest.approx([0.4, 0.4, 0.4, 0.5, 0.5, 0.5, 0.6, 0.6, 0.6, 0.7, 0.7, 0.7]) + assert cfg.runtime.kp == pytest.approx(25.0) + assert list(cfg.runtime.kd) == pytest.approx([0.4, 0.4, 0.4, 0.5, 0.5, 0.5, 0.6, 0.6, 0.6, 0.7, 0.7, 0.7]) -def test_headless_sim2sim_cli_runs_deterministic_onnx_fixture(tmp_path: Path) -> None: +@pytest.mark.parametrize("backend", ["mujoco"]) +@pytest.mark.parametrize("task", ["go2-walk-flat", "go2-walk-rough"]) +def test_headless_simulation_cli_runs_deterministic_onnx_fixture(tmp_path: Path, task: str, backend: str) -> None: policy_path = tmp_path / "fixture.onnx" weight = numpy_helper.from_array(np.zeros((49, 12), dtype=np.float32), name="weight") graph = helper.make_graph( @@ -214,41 +293,40 @@ def test_headless_sim2sim_cli_runs_deterministic_onnx_fixture(tmp_path: Path) -> command = [ sys.executable, "-c", - "from motrix_deploy_tasks import main; raise SystemExit(main())", - "sim2sim", + "from motrix_deploy.cli import main; raise SystemExit(main())", + f"task={task}/sim", + f"runtime.backend={backend}", f"artifact={artifact_path}", "command.velocity=[0.5,0.0,0.0]", - "rollout.steps=5", - "seed=1", - "viewer=false", + "duration_s=0.1", + "runtime.viewer=false", ] first = subprocess.run(command, check=False, capture_output=True, text=True) - second = subprocess.run(command, check=False, capture_output=True, text=True) - duration_command = command.copy() - steps_index = duration_command.index("rollout.steps=5") - duration_command[steps_index : steps_index + 1] = ["rollout.steps=null", "rollout.duration_s=0.1"] - duration = subprocess.run(duration_command, check=False, capture_output=True, text=True) - invalid_bound = subprocess.run( - [*command, "rollout.duration_s=0.1"], - check=False, - capture_output=True, - text=True, - ) - first_result = _rollout_result(first.stdout) - second_result = _rollout_result(second.stdout) - duration_result = _rollout_result(duration.stdout) - assert first.returncode == 0, first.stderr - assert second.returncode == 0, second.stderr - assert duration.returncode == 0, duration.stderr - assert invalid_bound.returncode == 2 - assert "Exactly one of rollout.steps and rollout.duration_s" in invalid_bound.stderr + first_result = _rollout_result(first.stdout) assert first_result["success"] is True assert first_result["completed_steps"] == 5 - assert first_result["trace_sha256"] == second_result["trace_sha256"] - assert duration_result["completed_steps"] == 5 - assert "Converting 12 actuators from position servos to torque motors" in first.stdout + + # Both worlds smoke-test each backend; repeat only one world per backend. + if task == "go2-walk-flat": + second = subprocess.run(command, check=False, capture_output=True, text=True) + assert second.returncode == 0, second.stderr + assert first_result["trace_sha256"] == _rollout_result(second.stdout)["trace_sha256"] + + # Duration validation is shared, so cover it once rather than per world/backend. + if task == "go2-walk-flat" and backend == "mujoco": + for invalid_duration in ("0", "-0.1", "nan", "inf"): + invalid_command = command.copy() + invalid_command[invalid_command.index("duration_s=0.1")] = f"duration_s={invalid_duration}" + invalid_bound = subprocess.run( + invalid_command, + check=False, + capture_output=True, + text=True, + ) + assert invalid_bound.returncode == 2, invalid_bound.stderr + assert "duration_s" in invalid_bound.stderr def _robot_spec() -> RobotSpec: @@ -278,21 +356,19 @@ def _robot_spec() -> RobotSpec: ) -def _task_spec() -> TaskSpec: - return TaskSpec( - name="go2_walk/v1", - observation_size=49, - action_size=12, - config={ - "action_scale": 0.25, - "command_lower": [0.5, 0.0, 0.0], - "command_upper": [0.5, 0.0, 0.0], - "command_scale": [1.0, 1.0, 1.0], - "feet_phase_offsets": [0.0, 0.5, 0.5, 0.0], - "gait_frequency_hz": 2.0, - "standing_threshold": 0.05, - "kp": [35.0] * 12, - "kd": [0.5] * 12, - "raw_clip": [[-1.0] * 12, [1.0] * 12], - }, +def _task_spec() -> Go2WalkTaskSpec: + return Go2WalkTaskSpec( + action_scale=[0.25] * 12, + command_lower=[0.5, 0.0, 0.0], + command_upper=[0.5, 0.0, 0.0], + command_scale=[1.0, 1.0, 1.0], + feet_phase_offsets=[0.0, 0.5, 0.5, 0.0], + gait_frequency_hz=2.0, + standing_threshold=0.05, + termination_min_up_z=0.25, + termination_min_base_height=None, + kp=[35.0] * 12, + kd=[0.5] * 12, + action_lower=[-1.0] * 12, + action_upper=[1.0] * 12, ) diff --git a/motrix_deploy_tasks/tests/test_go2_walk.py b/motrix_deploy_tasks/tests/test_go2_walk.py index 2f8012cd..809a4b3a 100644 --- a/motrix_deploy_tasks/tests/test_go2_walk.py +++ b/motrix_deploy_tasks/tests/test_go2_walk.py @@ -3,17 +3,24 @@ """Direct Go2 walking deployment-task tests.""" +from dataclasses import replace + import numpy as np import pytest +from pydantic import ValidationError as PydanticValidationError -from motrix_deploy.artifact import TaskSpec +from motrix_deploy.artifact.schema import TaskSpec from motrix_deploy.contracts import RobotSpec, RobotState from motrix_deploy.errors import ValidationError from motrix_deploy.runtime import PolicyContext -from motrix_deploy_tasks.go2_walk import Go2WalkDeployTaskV1 +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkDeployTaskV1, Go2WalkTaskSpec from motrix_env_core.input import PlanarVelocityCommand +def _replace_spec(spec: Go2WalkTaskSpec, **overrides: object) -> Go2WalkTaskSpec: + return type(spec).model_validate({**spec.model_dump(), **overrides}) + + def _context(step: int, elapsed_time_s: float, velocity: list[float] | np.ndarray) -> PolicyContext: return PolicyContext( step=step, @@ -45,31 +52,262 @@ def _state(robot: RobotSpec) -> RobotState: ) -def _task_spec() -> TaskSpec: - return TaskSpec( - name="go2_walk/v1", - observation_size=49, - action_size=12, - config={ - "action_scale": 0.25, - "command_lower": [-1.0, -1.0, -1.0], - "command_upper": [1.0, 1.0, 1.0], - "command_scale": [0.5, 0.5, 0.5], - "feet_phase_offsets": [0.0, 0.5, 0.5, 0.0], - "gait_frequency_hz": 2.0, - "standing_threshold": 0.05, - "kp": [35.0] * 12, - "kd": [0.5] * 12, - "raw_clip": [[-1.0] * 12, [1.0] * 12], - }, +def _task_spec() -> Go2WalkTaskSpec: + return Go2WalkTaskSpec( + action_scale=[0.25] * 12, + command_lower=[-1.0, -1.0, -1.0], + command_upper=[1.0, 1.0, 1.0], + command_scale=[0.5, 0.5, 0.5], + feet_phase_offsets=[0.0, 0.5, 0.5, 0.0], + gait_frequency_hz=2.0, + standing_threshold=0.05, + termination_min_up_z=0.25, + termination_min_base_height=None, + kp=[35.0] * 12, + kd=[0.5] * 12, + action_lower=[-1.0] * 12, + action_upper=[1.0] * 12, ) +@pytest.mark.parametrize("up_z", [-1.0, 0.0, 0.75, 1.0]) +def test_fall_orientation_uses_artifact_threshold(up_z: float) -> None: + robot = _robot() + state = _state(robot) + angle = np.arccos(up_z) + state.base_orientation_xyzw = np.array([np.sin(angle / 2), 0.0, 0.0, np.cos(angle / 2)], dtype=np.float32) + spec = _task_spec() + task = Go2WalkDeployTaskV1(spec, robot) + reason = task.check_termination(state) + assert (reason == "fall_orientation") == (up_z <= spec.termination_min_up_z) + + +def test_fall_orientation_includes_threshold_boundary() -> None: + spec = _task_spec() + robot = _robot() + task = Go2WalkDeployTaskV1(_replace_spec(spec, termination_min_up_z=1.0), robot) + assert task.check_termination(_state(robot)) == "fall_orientation" + + +def test_enabled_fall_height_requires_position_and_uses_artifact_threshold() -> None: + robot = _robot() + state = _state(robot) + spec = _task_spec() + threshold = 0.25 # Exactly representable synthetic cutoff, not a tuned task default. + task = Go2WalkDeployTaskV1(_replace_spec(spec, termination_min_base_height=threshold), robot) + assert Go2WalkDeployTaskV1(spec, robot).check_termination(state) is None + with pytest.raises(ValidationError, match="state.base_position.*enabled base-height termination"): + task.check_termination(state) + state.base_position = np.array([0.0, 0.0, threshold - 0.1], dtype=np.float32) + assert task.check_termination(state) == "fall_height" + assert Go2WalkDeployTaskV1(spec, robot).check_termination(state) is None + state.base_position[2] = threshold + assert task.check_termination(state) == "fall_height" + state.base_position[2] = threshold + 0.1 + assert task.check_termination(state) is None + + +@pytest.mark.parametrize("key", ["termination_min_up_z", "termination_min_base_height", "standing_threshold"]) +def test_missing_required_artifact_field_is_rejected_during_decoding(key: str) -> None: + wire = _task_spec().to_dict() + del wire["config"][key] + with pytest.raises(ValidationError, match=key): + TaskSpec.from_dict(wire) + + +def test_typed_task_spec_roundtrips_through_common_codec() -> None: + spec = _task_spec() + decoded = TaskSpec.from_dict(spec.to_dict()) + assert type(decoded) is Go2WalkTaskSpec + assert decoded == spec + assert decoded.name == "go2_walk/v1" + + +@pytest.mark.parametrize( + ("key", "value"), + [("termination_min_up_z", 2.0), ("termination_min_up_z", True), ("termination_min_base_height", float("nan"))], +) +def test_termination_config_is_validated(key: str, value: object) -> None: + with pytest.raises(PydanticValidationError, match=key): + _replace_spec(_task_spec(), **{key: value}) + + +@pytest.mark.parametrize( + ("key", "value"), + [ + ("action_scale", [-0.1] * 12), + ("kp", [-1.0] * 12), + ("kd", [-1.0] * 12), + ("action_lower", [2.0] * 12), + ("action_upper", [-2.0] * 12), + ("command_lower", [2.0] * 3), + ("command_scale", [1.1] * 3), + ("command_scale", [-0.1] * 3), + ("gait_frequency_hz", 0.0), + ("standing_threshold", -0.1), + ("termination_min_up_z", -1.1), + ("action_lower", [-1.0] * 11), + ("action_upper", [1.0] * 11), + ("action_lower", [float("nan")] * 12), + ("action_upper", [float("inf")] * 12), + ("action_scale", [0.25] * 11), + ("action_scale", []), + ("kp", [float("inf")] * 12), + ("command_scale", [float("nan")] * 3), + ("feet_phase_offsets", [0.0] * 3), + ("feet_phase_offsets", [0.0] * 5), + ("feet_phase_offsets", [float("inf")] * 4), + ("command_lower", [-1.0] * 2), + ("command_upper", [1.0] * 4), + ("command_scale", [0.5] * 4), + ("kp", []), + ("kd", [0.5] * 11), + ("gait_frequency_hz", float("inf")), + ("standing_threshold", float("nan")), + ], +) +def test_semantic_validation_runs_for_construction_and_wire_decoding(key: str, value: object) -> None: + spec = _task_spec() + with pytest.raises(PydanticValidationError): + _replace_spec(spec, **{key: value}) + wire = spec.to_dict() + wire["config"][key] = value + with pytest.raises(ValidationError, match="task.config"): + TaskSpec.from_dict(wire) + + +@pytest.mark.parametrize( + ("key", "value"), + [ + ("action_scale", 0.25), + ("action_lower", -1.0), + ("action_upper", 1.0), + ("action_lower", [[-1.0] * 12]), + ("action_upper", [True] * 12), + ("termination_min_up_z", True), + ], +) +def test_invalid_wire_types_are_rejected_by_common_codec(key: str, value: object) -> None: + wire = _task_spec().to_dict() + wire["config"][key] = value + with pytest.raises(ValidationError, match="task.config"): + TaskSpec.from_dict(wire) + + +@pytest.mark.parametrize("key", ["gait_frequency_hz", "standing_threshold", "termination_min_base_height"]) +def test_programmatic_numeric_fields_reject_booleans(key: str) -> None: + with pytest.raises(PydanticValidationError, match=key): + _replace_spec(_task_spec(), **{key: True}) + + +@pytest.mark.parametrize( + ("key", "value"), + [ + ("action_scale", (0.25,) * 12), + ("kp", ["35.0"] * 12), + ("kd", [True] * 12), + ("command_lower", [[-1.0] * 3]), + ("gait_frequency_hz", "2.0"), + ("unknown_field", 1.0), + ], +) +def test_programmatic_construction_is_strict(key: str, value: object) -> None: + with pytest.raises(PydanticValidationError, match=key): + _replace_spec(_task_spec(), **{key: value}) + + +def test_task_spec_fields_are_frozen_but_lists_remain_public() -> None: + spec = _task_spec() + assert isinstance(spec.action_scale, list) + with pytest.raises(PydanticValidationError, match="frozen_instance"): + spec.standing_threshold = 0.1 + + +@pytest.mark.parametrize("key", ["action_scale", "kp", "kd", "action_lower", "action_upper"]) +def test_mutated_joint_vector_length_is_rejected_during_serialization(key: str) -> None: + spec = _task_spec() + getattr(spec, key).pop() + with pytest.raises(ValidationError, match="task.config"): + spec.to_dict() + + +@pytest.mark.parametrize( + ("key", "value"), + [("kp", -1.0), ("action_upper", -2.0), ("command_lower", 2.0), ("feet_phase_offsets", float("nan"))], +) +def test_mutated_list_values_are_rejected_during_serialization(key: str, value: float) -> None: + spec = _task_spec() + getattr(spec, key)[0] = value + with pytest.raises(ValidationError, match="task.config"): + spec.to_dict() + + +@pytest.mark.parametrize( + "key", + [ + "action_scale", + "kp", + "kd", + "action_lower", + "action_upper", + "command_lower", + "command_upper", + "feet_phase_offsets", + ], +) +@pytest.mark.parametrize("value", [-1e100, 1e100]) +def test_vectors_reject_finite_values_outside_float32_range(key: str, value: float) -> None: + # Runtime vectors are float32, so finite Python floats must not overflow on conversion. + spec = _task_spec() + vector = list(getattr(spec, key)) + vector[0] = value + with pytest.raises(PydanticValidationError, match=key): + _replace_spec(spec, **{key: vector}) + wire = spec.to_dict() + wire["config"][key] = vector + with pytest.raises(ValidationError, match=f"task.config.{key}.0"): + TaskSpec.from_dict(wire) + getattr(spec, key)[0] = value + with pytest.raises(ValidationError, match=f"task.config.{key}.0"): + spec.to_dict() + + +def test_runtime_checks_robot_joint_count() -> None: + robot = _robot() + short_robot = replace( + robot, + joint_names=robot.joint_names[:-1], + default_joint_position=robot.default_joint_position[:-1], + position_lower=robot.position_lower[:-1], + position_upper=robot.position_upper[:-1], + torque_limit=robot.torque_limit[:-1], + ) + with pytest.raises(ValidationError, match="task.config.action_scale"): + Go2WalkDeployTaskV1(_task_spec(), short_robot) + + joint_count = short_robot.joint_count + spec = _replace_spec( + _task_spec(), + action_scale=[0.25] * joint_count, + kp=[35.0] * joint_count, + kd=[0.5] * joint_count, + action_lower=[-1.0] * joint_count, + action_upper=[1.0] * joint_count, + ) + task = Go2WalkDeployTaskV1(spec, short_robot) + command = task.process_action(np.zeros(joint_count, dtype=np.float32)) + assert command.joint_position.shape == (joint_count,) + assert command.joint_velocity.shape == (joint_count,) + with pytest.raises(ValidationError, match="action.raw.shape"): + task.process_action(np.zeros(joint_count + 1, dtype=np.float32)) + + def test_go2_observation_has_exact_49_element_contract() -> None: robot = _robot() state = _state(robot) task = Go2WalkDeployTaskV1(_task_spec(), robot) context = _context(0, 0.0, [0.5, 0.0, -0.2]) + task.validate_command(context.command) task.reset(state, context) observation = task.build_observation(state, context) @@ -88,13 +326,13 @@ def test_previous_action_and_trot_phase_advance_after_first_tick() -> None: state = _state(robot) task = Go2WalkDeployTaskV1(_task_spec(), robot) reset_context = _context(0, 0.0, [0.5, 0.0, 0.0]) + task.validate_command(reset_context.command) task.reset(state, reset_context) task.process_action(np.ones(12, dtype=np.float32)) - observation = task.build_observation( - state, - _context(1, 0.02, [0.5, 0.0, 0.0]), - ) + context = _context(1, 0.02, [0.5, 0.0, 0.0]) + task.validate_command(context.command) + observation = task.build_observation(state, context) np.testing.assert_array_equal(observation[30:42], 1.0) np.testing.assert_allclose(observation[45:49], [0.04, 0.54, 0.54, 0.04]) @@ -104,46 +342,41 @@ def test_standing_command_freezes_deployment_gait_phase() -> None: robot = _robot() task = Go2WalkDeployTaskV1(_task_spec(), robot) - observation = task.build_observation( - _state(robot), - _context(10, 0.2, np.zeros(3, dtype=np.float32)), - ) + context = _context(10, 0.2, np.zeros(3, dtype=np.float32)) + task.validate_command(context.command) + observation = task.build_observation(_state(robot), context) np.testing.assert_array_equal(observation[45:49], np.zeros(4, dtype=np.float32)) -def test_standing_threshold_is_required() -> None: - spec = _task_spec() - config = dict(spec.config) - del config["standing_threshold"] - spec = TaskSpec( - name=spec.name, - observation_size=spec.observation_size, - action_size=spec.action_size, - config=config, - ) - - with pytest.raises(KeyError, match="standing_threshold"): - Go2WalkDeployTaskV1(spec, _robot()) - - def test_action_processing_applies_clip_scale_and_position_limits() -> None: - robot = _robot() + # Synthetic asymmetric action bounds and nonzero offsets distinguish policy + # clipping from the subsequent robot-position clipping. + robot = replace( + _robot(), + default_joint_position=np.full(12, 0.5, dtype=np.float32), + position_lower=np.full(12, 0.25, dtype=np.float32), + position_upper=np.full(12, 0.75, dtype=np.float32), + ) state = _state(robot) - task = Go2WalkDeployTaskV1(_task_spec(), robot) - task.reset( - state, - _context(0, 0.0, np.zeros(3, dtype=np.float32)), + spec = _replace_spec( + _task_spec(), + action_lower=[-2.0] * 12, + action_upper=[0.5] * 12, ) + task = Go2WalkDeployTaskV1(spec, robot) + reset_context = _context(0, 0.0, np.zeros(3, dtype=np.float32)) + task.validate_command(reset_context.command) + task.reset(state, reset_context) - command = task.process_action(np.full(12, 2.0, dtype=np.float32)) - next_observation = task.build_observation( - state, - _context(1, 0.02, np.zeros(3, dtype=np.float32)), - ) + raw_action = np.tile(np.array([-3.0, 2.0], dtype=np.float32), 6) + command = task.process_action(raw_action) + next_context = _context(1, 0.02, np.zeros(3, dtype=np.float32)) + task.validate_command(next_context.command) + next_observation = task.build_observation(state, next_context) - np.testing.assert_allclose(command.joint_position, 0.25) - np.testing.assert_array_equal(next_observation[30:42], 1.0) + np.testing.assert_allclose(command.joint_position, np.tile([0.25, 0.625], 6)) + np.testing.assert_array_equal(next_observation[30:42], np.tile([-2.0, 0.5], 6)) np.testing.assert_array_equal(command.kp, 35.0) np.testing.assert_array_equal(command.kd, 0.5) diff --git a/motrix_deploy_tasks/tests/test_training_parity.py b/motrix_deploy_tasks/tests/test_training_parity.py index cfe0ab5b..00c421c1 100644 --- a/motrix_deploy_tasks/tests/test_training_parity.py +++ b/motrix_deploy_tasks/tests/test_training_parity.py @@ -10,6 +10,7 @@ from motrix_deploy.contracts import RobotState from motrix_deploy.runtime import PolicyContext from motrix_deploy.task import create_task +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec from motrix_env_core import registry from motrix_env_core.input import PlanarVelocityCommand from motrix_envs.deploy import build_deployment_profile @@ -30,11 +31,20 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: command=PlanarVelocityCommand(env._commands), ) task = create_task(profile.task, profile.robot) - command_scale = np.asarray(profile.task.config["command_scale"], dtype=np.float32) + assert isinstance(profile.task, Go2WalkTaskSpec) + command_scale = np.asarray(profile.task.command_scale, dtype=np.float32) assert command_scale.shape == (3,) np.testing.assert_allclose(task.command_lower, env.cfg.commands.velocity.lower * command_scale) 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. + 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) + robot_state = _robot_state_from_env(env, env_state, probe) np.testing.assert_allclose( task.build_observation(robot_state, context), @@ -55,6 +65,7 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: elapsed_time_s=profile.control.period_s, command=PlanarVelocityCommand(env._commands), ) + task.validate_command(next_context.command) np.testing.assert_allclose( task.build_observation(next_state, next_context), stepped.obs.policy[0], diff --git a/motrix_deploy_unitree/README.md b/motrix_deploy_unitree/README.md index 58e9329a..06152691 100644 --- a/motrix_deploy_unitree/README.md +++ b/motrix_deploy_unitree/README.md @@ -15,9 +15,8 @@ The Motphys fork includes the CRC shared libraries in built distributions. The S 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. -Deployment recipes inherit common physical-runtime guards and default policy gains from -`configs/deploy/sim2real/base.yaml`. Concrete recipes such as -`configs/deploy/sim2real/go2_walk_flat_sim2real.yaml` own their robot-specific backend fields and command input configuration. +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`. @@ -41,7 +40,6 @@ from motrix_deploy_unitree import UnitreeGo2DirectInterface robot = UnitreeGo2DirectInterface.from_artifact( "artifacts/go2-walk-rough.deploy", network_interface="enp5s0", - hardware_confirmed=True, ) robot.open() @@ -76,7 +74,7 @@ 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 or `--hardware-confirm`, and it never creates a `LowCmd` publisher. Each printed sample contains: +an artifact, and it never creates a `LowCmd` publisher. Each printed sample contains: | Output | Contents and units | | ---------------- | ---------------------------------------------------------------------------------- | @@ -100,8 +98,7 @@ motrix-deploy-unitree joint-control \ enp5s0 \ FL_thigh_joint \ 0.9 \ - --artifact artifacts/go2-walk-rough.deploy \ - --hardware-confirm + --artifact artifacts/go2-walk-rough.deploy ``` The three positional arguments are the network interface, canonical joint name, and absolute target position in radians. @@ -117,20 +114,22 @@ helper changes one target, each Unitree `LowCmd` necessarily contains commands f ## Policy gain overrides -`UnitreeGo2BackendConfig` defaults `backend.kp` and `backend.kd` to `null`, while -`configs/deploy/sim2real/base.yaml` sets them to `50` and `1` for physical deployment recipes. Each accepts either one +`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 `backend.damping_kd`. +default-pose transition and policy commands; damping stop still uses `kp=0` and `runtime.damping_kd`. From the repository root: ```bash -motrix-deploy sim2real \ - artifact=artifacts/go2-walk-rough.deploy \ - backend.network_interface=enp5s0 \ - hardware.confirm=true +motrix-deploy task=go2-walk-flat/hardware \ + artifact=artifacts/go2-walk-flat.deploy \ + runtime.network_interface=enp5s0 ``` -`hardware.confirm=true`, `viewer=false`, and `realtime=true` are enforced before DDS initialization. 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 +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. diff --git a/motrix_deploy_unitree/pyproject.toml b/motrix_deploy_unitree/pyproject.toml index 32a97745..988fb45b 100644 --- a/motrix_deploy_unitree/pyproject.toml +++ b/motrix_deploy_unitree/pyproject.toml @@ -20,7 +20,7 @@ dependencies = [ motrix-deploy-unitree = "motrix_deploy_unitree.cli:main" [project.entry-points."motrix_deploy.backends"] -unitree_go2 = "motrix_deploy_unitree.plugin:create_backend" +unitree_go2 = "motrix_deploy_unitree.plugin:create_runtime" [tool.uv.sources] unitree-sdk2py = { git = "https://github.com/motphys-developers/unitree_sdk2_python", rev = "ff87e731174f7258e0d09980897c55116634939a" } diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py index b6a3bcae..537f0784 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py @@ -81,12 +81,6 @@ def _parser() -> argparse.ArgumentParser: 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) - joint.add_argument( - "--hardware-confirm", - action="store_true", - required=True, - help="Confirm the robot is suspended, in low-level/debug mode, and an operator is ready", - ) return parser @@ -112,7 +106,6 @@ def main(argv: Sequence[str] | None = None) -> int: move_duration=args.move_duration, hold_duration=args.hold_duration, return_duration=args.return_duration, - hardware_confirmed=args.hardware_confirm, ) return 0 diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py index 69a6607b..3e5e43b1 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py @@ -74,15 +74,15 @@ class UnitreeGo2BackendConfig: def __post_init__(self) -> None: if not isinstance(self.network_interface, str) or not self.network_interface: - raise ValidationError("backend.network_interface", "a non-empty interface name", self.network_interface) + raise ValidationError("runtime.network_interface", "a non-empty interface name", self.network_interface) for name in ("wait_for_remote_buttons", "validate_crc"): value = getattr(self, name) if not isinstance(value, bool): - raise ValidationError(f"backend.{name}", "a boolean", value) + raise ValidationError(f"runtime.{name}", "a boolean", value) for name in ("lowcmd_topic", "lowstate_topic"): value = getattr(self, name) if not isinstance(value, str) or not value: - raise ValidationError(f"backend.{name}", "a non-empty DDS topic", value) + raise ValidationError(f"runtime.{name}", "a non-empty DDS topic", value) for name in ( "connect_timeout_s", "default_transition_duration_s", @@ -92,31 +92,31 @@ def __post_init__(self) -> None: ): value = getattr(self, name) if not isinstance(value, (int, float)) or isinstance(value, bool) or not np.isfinite(value): - raise ValidationError(f"backend.{name}", "a finite number", value) + raise ValidationError(f"runtime.{name}", "a finite number", value) if self.connect_timeout_s <= 0: - raise ValidationError("backend.connect_timeout_s", "a positive duration", self.connect_timeout_s) + raise ValidationError("runtime.connect_timeout_s", "a positive duration", self.connect_timeout_s) if self.default_transition_duration_s <= 0: raise ValidationError( - "backend.default_transition_duration_s", + "runtime.default_transition_duration_s", "a positive duration", self.default_transition_duration_s, ) if self.damping_duration_s < 0 or self.damping_kd < 0: - raise ValidationError("backend.damping", "non-negative duration and kd", "invalid value") + raise ValidationError("runtime.damping", "non-negative duration and kd", "invalid value") if self.lie_down_duration_s <= 0: - raise ValidationError("backend.lie_down_duration_s", "a positive duration", self.lie_down_duration_s) + raise ValidationError("runtime.lie_down_duration_s", "a positive duration", self.lie_down_duration_s) 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: - raise ValidationError("backend.domain_id", "a non-negative integer", self.domain_id) + raise ValidationError("runtime.domain_id", "a non-negative integer", self.domain_id) if ( not isinstance(self.subscriber_queue_depth, int) or isinstance(self.subscriber_queue_depth, bool) or self.subscriber_queue_depth <= 0 ): raise ValidationError( - "backend.subscriber_queue_depth", + "runtime.subscriber_queue_depth", "a positive integer", self.subscriber_queue_depth, ) @@ -125,17 +125,17 @@ def __post_init__(self) -> None: or isinstance(self.motor_mode, bool) or not 0 <= self.motor_mode <= 0xFF ): - raise ValidationError("backend.motor_mode", "an integer in [0, 255]", self.motor_mode) + raise ValidationError("runtime.motor_mode", "an integer in [0, 255]", self.motor_mode) for name in ("start_button", "enable_button", "emergency_stop_button", "lie_down_button"): value = getattr(self, name) if value not in BUTTON_NAMES: - raise ValidationError(f"backend.{name}", f"one of {sorted(BUTTON_NAMES)}", value) + raise ValidationError(f"runtime.{name}", f"one of {sorted(BUTTON_NAMES)}", value) buttons = (self.start_button, self.enable_button, self.emergency_stop_button, self.lie_down_button) if len(set(buttons)) != len(buttons): - raise ValidationError("backend.remote_buttons", "four distinct buttons", "duplicate button") + raise ValidationError("runtime.remote_buttons", "four distinct buttons", "duplicate button") if not isinstance(self.joint_name_to_motor_index, Mapping): raise ValidationError( - "backend.joint_name_to_motor_index", + "runtime.joint_name_to_motor_index", "a joint-name to motor-index mapping", type(self.joint_name_to_motor_index).__name__, ) @@ -153,9 +153,9 @@ def from_mapping(cls, value: Mapping[str, Any]) -> "UnitreeGo2BackendConfig": } actual_fields = set(values) if missing := required_fields - actual_fields: - raise ValidationError("backend", f"required fields {sorted(required_fields)}", f"missing={sorted(missing)}") + raise ValidationError("runtime", f"required fields {sorted(required_fields)}", f"missing={sorted(missing)}") if unknown := actual_fields - expected_fields: - raise ValidationError("backend", f"known fields {sorted(expected_fields)}", f"unknown={sorted(unknown)}") + raise ValidationError("runtime", f"known fields {sorted(expected_fields)}", f"unknown={sorted(unknown)}") mapping = values.get("joint_name_to_motor_index") if isinstance(mapping, Mapping): values["joint_name_to_motor_index"] = dict(mapping) @@ -166,18 +166,18 @@ def _validate_gain_override(name: str, value: object | None) -> None: if value is None: return if isinstance(value, bool): - raise ValidationError(f"backend.{name}", "a non-negative scalar or 12 values", value) + raise ValidationError(f"runtime.{name}", "a non-negative scalar or 12 values", value) try: array = np.asarray(value, dtype=np.float32) except (TypeError, ValueError) as error: raise ValidationError( - f"backend.{name}", + f"runtime.{name}", "a non-negative scalar or 12 values", value, ) from error if array.shape not in ((), (GO2_MOTOR_COUNT,)) or not np.all(np.isfinite(array)) or np.any(array < 0): raise ValidationError( - f"backend.{name}", + f"runtime.{name}", "a non-negative scalar or 12 finite non-negative values", value, ) @@ -188,13 +188,13 @@ def lie_down_position(self) -> np.ndarray: position = np.asarray(self.lie_down_joint_position, dtype=np.float32) except (TypeError, ValueError) as error: raise ValidationError( - "backend.lie_down_joint_position", + "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( - "backend.lie_down_joint_position", + "runtime.lie_down_joint_position", "12 finite canonical joint positions", self.lie_down_joint_position, ) @@ -209,7 +209,7 @@ def gain_override(self, name: str, joint_count: int) -> np.ndarray | None: if array.shape == (): return np.full(joint_count, array.item(), dtype=np.float32) if array.shape != (joint_count,): - raise ValidationError(f"backend.{name}", f"{joint_count} canonical joint gains", value) + raise ValidationError(f"runtime.{name}", f"{joint_count} canonical joint gains", value) return np.array(array, dtype=np.float32, copy=True) def motor_indices(self, joint_names: tuple[str, ...]) -> np.ndarray: @@ -218,16 +218,16 @@ def motor_indices(self, joint_names: tuple[str, ...]) -> np.ndarray: actual = set(self.joint_name_to_motor_index) if actual != expected: raise ValidationError( - "backend.joint_name_to_motor_index", + "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) if any(not isinstance(index, int) or isinstance(index, bool) for index in values): - raise ValidationError("backend.motor_indices", "integer motor indices", 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): raise ValidationError( - "backend.motor_indices", + "runtime.motor_indices", f"unique values in [0, {GO2_MOTOR_COUNT - 1}]", values, ) diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py index 037a9659..5a4ee0d3 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/direct.py @@ -5,6 +5,7 @@ import time from collections.abc import Callable, Mapping +from dataclasses import replace from pathlib import Path from types import TracebackType from typing import Any @@ -12,7 +13,7 @@ import numpy as np from motrix_deploy.artifact import read_artifact -from motrix_deploy.contracts import HealthStatus, RobotCommand, RobotSpec, RobotState +from motrix_deploy.contracts import HealthStatus, JointServoCommand, RobotSpec, RobotState from motrix_deploy.errors import ValidationError from motrix_deploy.profile import DeploymentProfile from motrix_deploy_unitree.config import ( @@ -53,8 +54,11 @@ def __init__( self.backend = backend self._opened = False self._closed = False - self._task_gain("kp") - self._task_gain("kd") + 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, + ) @classmethod def from_profile( @@ -62,7 +66,6 @@ def from_profile( profile: DeploymentProfile, *, network_interface: str, - hardware_confirmed: bool, joint_name_to_motor_index: Mapping[str, int] = GO2_JOINT_NAME_TO_MOTOR_INDEX, backend_options: Mapping[str, Any] | None = None, sdk: UnitreeSdkBindings | None = None, @@ -72,11 +75,10 @@ def from_profile( """Build the direct API from an explicit robot, gain, and timing profile.""" return cls._from_contract( robot=profile.robot, - task_config=profile.task.config, + task_config=profile.task.model_dump(mode="python"), control_period_s=profile.control.period_s, state_timeout_s=profile.control.state_timeout_s, network_interface=network_interface, - hardware_confirmed=hardware_confirmed, joint_name_to_motor_index=joint_name_to_motor_index, backend_options=backend_options, sdk=sdk, @@ -90,7 +92,6 @@ def from_artifact( artifact_path: str | Path, *, network_interface: str, - hardware_confirmed: bool, joint_name_to_motor_index: Mapping[str, int] = GO2_JOINT_NAME_TO_MOTOR_INDEX, backend_options: Mapping[str, Any] | None = None, sdk: UnitreeSdkBindings | None = None, @@ -102,11 +103,10 @@ def from_artifact( manifest = artifact.manifest return cls._from_contract( robot=manifest.robot, - task_config=manifest.task.config, + task_config=manifest.task.model_dump(mode="python"), control_period_s=manifest.control.period_s, state_timeout_s=manifest.control.state_timeout_s, network_interface=network_interface, - hardware_confirmed=hardware_confirmed, joint_name_to_motor_index=joint_name_to_motor_index, backend_options=backend_options, sdk=sdk, @@ -123,7 +123,6 @@ def _from_contract( control_period_s: float, state_timeout_s: float, network_interface: str, - hardware_confirmed: bool, joint_name_to_motor_index: Mapping[str, int], backend_options: Mapping[str, Any] | None, sdk: UnitreeSdkBindings | None, @@ -141,7 +140,7 @@ def _from_contract( config, control_period_s=control_period_s, state_timeout_s=state_timeout_s, - hardware_confirmed=hardware_confirmed, + spec=robot, sdk=sdk, clock_ns=clock_ns, sleep=sleep, @@ -161,7 +160,7 @@ def open(self) -> None: if self._opened: raise RuntimeError("direct Unitree interface is already open") try: - self.backend.open(self.robot) + self.backend.open() except Exception: try: self.backend.close() @@ -175,10 +174,10 @@ def read_data(self, timeout_s: float | None = None) -> RobotState: self._require_open() return self.backend.read_state(self.state_timeout_s if timeout_s is None else timeout_s) - def default_pose_command(self) -> RobotCommand: + def default_pose_command(self) -> JointServoCommand: """Build the zero-action default-pose command stored by the control contract.""" zeros = np.zeros(self.robot.joint_count, dtype=np.float32) - return RobotCommand( + return JointServoCommand( joint_position=np.array(self.robot.default_joint_position, copy=True), joint_velocity=zeros, feedforward_torque=zeros, @@ -186,10 +185,10 @@ def default_pose_command(self) -> RobotCommand: kd=self._task_gain("kd"), ) - def enable_command_output(self, initial_command: RobotCommand | None = None) -> None: + def enable_command_output(self) -> None: """Run the production Start -> default pose -> A hardware enable sequence.""" self._require_open() - self.backend.enable(self.default_pose_command() if initial_command is None else initial_command) + self.backend.enable() def make_joint_command( self, @@ -199,10 +198,10 @@ def make_joint_command( feedforward_torque: object | None = None, kp: object | None = None, kd: object | None = None, - ) -> RobotCommand: + ) -> JointServoCommand: """Construct one canonical float32 command using configured gains by default.""" zeros = np.zeros(self.robot.joint_count, dtype=np.float32) - return RobotCommand( + return JointServoCommand( joint_position=self._command_array(joint_position, "joint_position"), joint_velocity=(zeros if joint_velocity is None else self._command_array(joint_velocity, "joint_velocity")), feedforward_torque=( @@ -212,7 +211,7 @@ def make_joint_command( kd=self._task_gain("kd") if kd is None else self._command_array(kd, "kd"), ) - def send_command(self, command: RobotCommand) -> None: + def send_command(self, command: JointServoCommand) -> None: """Send one canonical command through the production LowCmd writer.""" self._require_open() self.backend.write_command(command) @@ -225,7 +224,7 @@ def send_joint_command( feedforward_torque: object | None = None, kp: object | None = None, kd: object | None = None, - ) -> RobotCommand: + ) -> JointServoCommand: """Construct and send one command, returning the exact command sent.""" command = self.make_joint_command( joint_position, 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 056b7541..d25423ff 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,13 +16,10 @@ def _build_robot( artifact: str | Path, network_interface: str, - *, - hardware_confirmed: bool, ) -> UnitreeGo2DirectInterface: return UnitreeGo2DirectInterface.from_artifact( artifact, network_interface=network_interface, - hardware_confirmed=hardware_confirmed, ) @@ -75,7 +72,6 @@ def run_joint_control( move_duration: float, hold_duration: float, return_duration: float, - hardware_confirmed: bool, ) -> None: _validate_duration("--move-duration", move_duration, allow_zero=False) _validate_duration("--hold-duration", hold_duration, allow_zero=True) @@ -83,7 +79,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, hardware_confirmed=hardware_confirmed) + robot = _build_robot(artifact, network_interface) 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/interface.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py index a0abab7b..e24a2323 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py @@ -12,9 +12,17 @@ import numpy as np -from motrix_deploy.backend import RobotInterface -from motrix_deploy.contracts import HealthStatus, RobotCapabilities, RobotCommand, RobotSpec, RobotState +from motrix_deploy.contracts import ( + HealthStatus, + JointControlMode, + JointServoCommand, + RobotCapabilities, + RobotCommand, + RobotSpec, + RobotState, +) from motrix_deploy.errors import EmergencyStopError, LieDownRequestedError, ValidationError +from motrix_deploy.robot.interface import RobotInterface from motrix_deploy_unitree.config import GO2_MOTOR_COUNT, UnitreeGo2BackendConfig from motrix_deploy_unitree.remote import UnitreeRemoteGamePadDevice, UnitreeRemoteState, decode_wireless_remote from motrix_env_core.input import GamePadDevice @@ -76,8 +84,8 @@ def __init__( config: UnitreeGo2BackendConfig, control_period_s: float, state_timeout_s: float, - hardware_confirmed: bool, *, + spec: RobotSpec, sdk: UnitreeSdkBindings | None = None, clock_ns: Callable[[], int] = time.monotonic_ns, sleep: Callable[[float], None] = time.sleep, @@ -89,11 +97,8 @@ 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) - if not isinstance(hardware_confirmed, bool): - raise ValidationError("hardware.confirm", "a boolean", hardware_confirmed) self._control_period_s = float(control_period_s) self._state_timeout_s = float(state_timeout_s) - self._hardware_confirmed = hardware_confirmed self._sdk = sdk self._clock_ns = clock_ns self._sleep = sleep @@ -104,7 +109,7 @@ def __init__( self._low_cmd: Any = None self._command_crc: Any = None self._state_crc: Any = None - self._spec: RobotSpec | None = None + self._spec = spec self._motor_indices = np.empty(0, dtype=np.int64) self._kp_override: np.ndarray | None = None self._kd_override: np.ndarray | None = None @@ -127,6 +132,10 @@ def __init__( self._stopped = False self._closed = False + @property + def spec(self) -> RobotSpec: + return self._spec + @property def capabilities(self) -> RobotCapabilities: if self._capabilities is None: @@ -175,15 +184,10 @@ def _disable_high_level_motion_services(self, sdk: UnitreeSdkBindings) -> None: if service_status != 0: raise RuntimeError(f"RobotState ServiceSwitch(sport_mode, false) failed with code {service_status}") - def open(self, spec: RobotSpec) -> None: + def open(self) -> None: + spec = self._spec if self._opened and not self._closed: raise RuntimeError("Unitree backend is already open") - if not self._hardware_confirmed: - raise ValidationError( - "hardware.confirm", - "true after the operator completes the physical safety checklist", - False, - ) self._motor_indices = self.config.motor_indices(spec.joint_names) self._spec = spec self._kp_override = self.config.gain_override("kp", spec.joint_count) @@ -216,7 +220,7 @@ def open(self, spec: RobotSpec) -> None: len(self._low_cmd.motor_cmd), ) self._capabilities = RobotCapabilities( - control_modes=("joint_pd",), + control_modes=(JointControlMode.SERVO,), state_fields=frozenset( { "joint_position", @@ -238,18 +242,25 @@ def open(self, spec: RobotSpec) -> None: self._health_reason = "" self._last_communication_ns = self._latest_receive_time_ns - def enable(self, initial_command: RobotCommand) -> None: + def enable(self) -> None: self._require_open() if self._enabled: return - self._validate_command(initial_command) - assert self._spec is not None - if not np.allclose(initial_command.joint_position, self._spec.default_joint_position, atol=1e-6, rtol=0.0): + if self._kp_override is None or self._kd_override is None: raise ValidationError( - "backend.initial_command.joint_position", - "RobotSpec.default_joint_position", - initial_command.joint_position.tolist(), + "backend.enable", + "explicit backend kp and kd safety gains for startup", + "missing safety gains", ) + zeros = np.zeros(self._spec.joint_count, dtype=np.float32) + initial_command = JointServoCommand( + joint_position=self._spec.default_joint_position, + joint_velocity=zeros, + feedforward_torque=zeros, + kp=self._kp_override, + 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) @@ -464,6 +475,8 @@ def _validate_message_fields(self, message: Any) -> None: raise ValidationError(f"state.imu.{name}", 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"): diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py index a0a89325..798f7df1 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py @@ -1,40 +1,30 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Entry-point factory for the Unitree Go2 deployment backend.""" +"""Entry-point factory for the Unitree Go2 deployment runtime.""" from collections.abc import Mapping from typing import Any -from motrix_deploy.backend import BackendCreateContext, RobotInterface -from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.factory import RuntimeCreateContext +from motrix_deploy.runtime.hardware import HardwareRuntime from motrix_deploy_unitree.config import UnitreeGo2BackendConfig from motrix_deploy_unitree.interface import UnitreeGo2RobotInterface -def create_backend(config: Mapping[str, Any], context: BackendCreateContext) -> RobotInterface: - """Construct a guarded Unitree Go2 hardware backend.""" - if context.viewer: - raise ValidationError("viewer", "false for physical hardware", context.viewer) - if not isinstance(context.realtime, bool): - raise ValidationError("realtime", "a boolean", context.realtime) - if not context.realtime: - raise ValidationError("realtime", "true for physical hardware", context.realtime) - if not isinstance(context.hardware_confirmed, bool): - raise ValidationError("hardware.confirm", "a boolean", context.hardware_confirmed) - if not context.hardware_confirmed: - raise ValidationError( - "hardware.confirm", - "true after the operator completes the physical safety checklist", - context.hardware_confirmed, - ) - - return UnitreeGo2RobotInterface( +def create_runtime( + config: Mapping[str, Any], + context: RuntimeCreateContext, +) -> HardwareRuntime: + """Construct a realtime runtime around the existing SDK2 hardware facade.""" + + robot = UnitreeGo2RobotInterface( UnitreeGo2BackendConfig.from_mapping(config), control_period_s=context.control.period_s, state_timeout_s=context.control.state_timeout_s, - hardware_confirmed=context.hardware_confirmed, + spec=context.robot, ) + return HardwareRuntime(robot=robot, realtime=True) -__all__ = ["create_backend"] +__all__ = ["create_runtime"] diff --git a/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py b/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py index 7b28ecf8..beb4fd24 100644 --- a/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py +++ b/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py @@ -11,10 +11,12 @@ import pytest from motrix_deploy.artifact import ControlSpec, TaskSpec -from motrix_deploy.backend import BackendCreateContext, GamePadDeviceProvider -from motrix_deploy.contracts import RobotCommand, RobotSpec +from motrix_deploy.contracts import 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, @@ -23,8 +25,8 @@ UnitreeSdkBindings, decode_wireless_remote, ) -from motrix_deploy_unitree.cli import _parser -from motrix_deploy_unitree.plugin import create_backend +from motrix_deploy_unitree.cli import _parser, main +from motrix_deploy_unitree.plugin import create_runtime JOINT_NAMES = ( "FL_hip_joint", @@ -253,6 +255,8 @@ def _config(**overrides) -> UnitreeGo2BackendConfig: "default_transition_duration_s": 0.02, "damping_duration_s": 0.04, "wait_for_remote_buttons": False, + "kp": 35.0, + "kd": 0.5, } values.update(overrides) return UnitreeGo2BackendConfig(**values) @@ -261,22 +265,20 @@ def _config(**overrides) -> UnitreeGo2BackendConfig: def _backend( config: UnitreeGo2BackendConfig, bus: _Bus, - *, - hardware_confirmed: bool = True, ) -> UnitreeGo2RobotInterface: return UnitreeGo2RobotInterface( config, control_period_s=0.02, state_timeout_s=0.1, - hardware_confirmed=hardware_confirmed, + spec=_spec(), sdk=bus.sdk(), sleep=lambda _: None, ) -def _command(position: np.ndarray) -> RobotCommand: +def _command(position: np.ndarray) -> JointServoCommand: zeros = np.zeros(12, dtype=np.float32) - return RobotCommand( + return JointServoCommand( joint_position=np.asarray(position, dtype=np.float32), joint_velocity=zeros, feedforward_torque=zeros, @@ -322,7 +324,7 @@ def test_open_releases_mcf_mode_and_disables_sport_service_before_lowcmd() -> No bus = _Bus() backend = _backend(_config(), bus) - backend.open(_spec()) + backend.open() call_names = [name for name, _ in bus.service_calls] assert call_names.index("stand_down") < call_names.index("release_mode") @@ -337,34 +339,24 @@ def test_sport_service_shutdown_failure_aborts_before_lowcmd_channels() -> None: backend = _backend(_config(), bus) with pytest.raises(RuntimeError, match="ServiceSwitch.*code 42"): - backend.open(_spec()) + backend.open() assert bus.callback is None assert bus.commands == [] -def test_hardware_confirmation_fails_before_dds_initialization() -> None: - bus = _Bus() - backend = _backend(_config(), bus, hardware_confirmed=False) - - with pytest.raises(ValidationError, match="hardware.confirm"): - backend.open(_spec()) - - assert bus.factory_calls == [] - - def test_lowstate_and_lowcmd_are_mapped_by_canonical_joint_name() -> None: bus = _Bus() spec = _spec() backend = _backend(_config(), bus) - backend.open(spec) + backend.open() state = backend.read_state(0.1) expected_position = np.asarray([MOTOR_MAPPING[name] for name in JOINT_NAMES], dtype=np.float32) np.testing.assert_array_equal(state.joint_position, expected_position) np.testing.assert_array_equal(state.base_orientation_xyzw, [0.0, 0.0, 0.0, 1.0]) - backend.enable(_command(spec.default_joint_position)) + backend.enable() target = spec.default_joint_position + np.linspace(-0.01, 0.01, 12, dtype=np.float32) command = _command(target) backend.write_command(command) @@ -390,14 +382,38 @@ 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: + bus = _Bus() + backend = _backend(_config(kp=None, kd=None), bus) + backend.open() + + 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: + bus = _Bus() + backend = _backend(_config(), bus) + backend.open() + backend.enable() + command_count = len(bus.commands) + + with pytest.raises(ValidationError, match="joint_servo"): + backend.write_command(JointTorqueCommand(torque=np.zeros(12, dtype=np.float32))) + assert len(bus.commands) == command_count + backend.close() + + def test_runtime_gain_overrides_apply_to_transition_and_policy_commands() -> None: bus = _Bus() spec = _spec() kd = np.linspace(0.2, 1.3, 12, dtype=np.float32) backend = _backend(_config(kp=25.0, kd=kd.tolist()), bus) - backend.open(spec) + backend.open() - backend.enable(_command(spec.default_joint_position)) + backend.enable() transition = copy.deepcopy(bus.commands[-1]) backend.write_command(_command(spec.default_joint_position)) policy = copy.deepcopy(bus.commands[-1]) @@ -454,15 +470,20 @@ def test_enable_waits_for_start_transitions_then_waits_for_a() -> None: 2: _button("start"), 3: _button("start") | _button("A"), } - spec = _spec() backend = _backend( - _config(wait_for_remote_buttons=True, damping_duration_s=0.0), + UnitreeGo2BackendConfig( + network_interface="enp3s0", + default_transition_duration_s=0.02, + damping_duration_s=0.0, + kp=35.0, + kd=0.5, + ), bus, ) - backend.open(spec) + backend.open() backend.read_state(0.1) - backend.enable(_command(spec.default_joint_position)) + backend.enable() assert len(bus.commands) == 3 assert all(motor.kp == 0.0 and motor.kd == 0.0 for motor in bus.commands[0].motor_cmd) @@ -473,14 +494,13 @@ def test_enable_waits_for_start_transitions_then_waits_for_a() -> None: def test_select_remote_button_raises_emergency_stop_and_stop_publishes_damping() -> None: bus = _Bus() - spec = _spec() backend = _backend( _config(damping_duration_s=0.02), bus, ) - backend.open(spec) + backend.open() backend.read_state(0.1) - backend.enable(_command(spec.default_joint_position)) + backend.enable() commands_before_stop = len(bus.commands) bus.state.wireless_remote = _remote_payload(_button("select")) @@ -498,12 +518,11 @@ def test_select_remote_button_raises_emergency_stop_and_stop_publishes_damping() def test_b_button_lies_down_then_enters_damping() -> None: bus = _Bus() - spec = _spec() config = _config(lie_down_duration_s=0.04, damping_duration_s=0.02) backend = _backend(config, bus) - backend.open(spec) + backend.open() backend.read_state(0.1) - backend.enable(_command(spec.default_joint_position)) + backend.enable() commands_before_stop = len(bus.commands) bus.state.wireless_remote = _remote_payload(_button("B")) @@ -530,11 +549,10 @@ def test_b_button_lies_down_then_enters_damping() -> None: def test_select_has_priority_over_simultaneous_lie_down_request() -> None: bus = _Bus() - spec = _spec() backend = _backend(_config(damping_duration_s=0.02), bus) - backend.open(spec) + backend.open() backend.read_state(0.1) - backend.enable(_command(spec.default_joint_position)) + backend.enable() commands_before_stop = len(bus.commands) bus.state.wireless_remote = _remote_payload(_button("select") | _button("B")) @@ -551,7 +569,7 @@ def test_select_has_priority_over_simultaneous_lie_down_request() -> None: def test_lie_down_config_rejects_invalid_pose() -> None: - with pytest.raises(ValidationError, match="backend.lie_down_joint_position"): + with pytest.raises(ValidationError, match="runtime.lie_down_joint_position"): _config(lie_down_joint_position=[0.0] * 11) @@ -571,50 +589,58 @@ def test_motor_mapping_must_exactly_cover_artifact_joint_names() -> None: config.motor_indices(JOINT_NAMES) -def test_hardware_confirmation_must_be_a_real_boolean() -> None: - with pytest.raises(ValidationError, match="hardware.confirm"): - UnitreeGo2RobotInterface( - _config(), - control_period_s=0.02, - state_timeout_s=0.1, - hardware_confirmed="true", - ) - - -def _context( - *, - viewer: bool = False, - realtime: bool = True, - hardware_confirmed: bool = True, -) -> BackendCreateContext: - return BackendCreateContext( +def _context() -> RuntimeCreateContext: + return RuntimeCreateContext( + robot=_spec(), control=ControlSpec(period_s=0.02, state_timeout_s=0.1), - viewer=viewer, - realtime=realtime, - hardware_confirmed=hardware_confirmed, ) -def test_plugin_factory_enforces_hardware_guards_and_strict_config() -> None: +def test_plugin_factory_creates_realtime_hardware_runtime_and_strict_config() -> None: config = { "name": "unitree_go2", "network_interface": "enp3s0", } - with pytest.raises(ValidationError, match="viewer"): - create_backend(config, _context(viewer=True)) - with pytest.raises(ValidationError, match="realtime"): - create_backend(config, _context(realtime=False)) - with pytest.raises(ValidationError, match="hardware.confirm"): - create_backend(config, _context(hardware_confirmed=False)) - - backend = create_backend(config, _context()) - assert isinstance(backend, UnitreeGo2RobotInterface) + 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 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"), [ @@ -625,20 +651,23 @@ def test_plugin_factory_enforces_hardware_guards_and_strict_config() -> None: ], ) def test_backend_config_rejects_invalid_gain_overrides(name: str, value: object) -> None: - with pytest.raises(ValidationError, match=f"backend.{name}"): + with pytest.raises(ValidationError, match=f"runtime.{name}"): _config(**{name: value}) +class _JointControlTaskSpec(TaskSpec): + task_name = "unitree_joint_control/v1" + kp: list[float] + kd: list[float] + + def _profile() -> DeploymentProfile: return DeploymentProfile( robot=_spec(), - task=TaskSpec( - name="go2_walk/v1", - observation_size=1, - action_size=12, - config={"kp": [35.0] * 12, "kd": [0.5] * 12}, - ), + task=_JointControlTaskSpec(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, ) @@ -647,7 +676,6 @@ def test_direct_interface_builds_from_profile_without_artifact() -> None: direct = UnitreeGo2DirectInterface.from_profile( _profile(), network_interface="enp5s0", - hardware_confirmed=True, backend_options={ "default_transition_duration_s": 0.02, "damping_duration_s": 0.0, @@ -671,9 +699,25 @@ 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)) + + 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_joint_control_requires_artifact() -> None: with pytest.raises(SystemExit): - _parser().parse_args(["joint-control", "enp5s0", "FL_thigh_joint", "0.9", "--hardware-confirm"]) + _parser().parse_args(["joint-control", "enp5s0", "FL_thigh_joint", "0.9"]) def test_read_lowstate_uses_unified_cli_defaults() -> None: @@ -697,6 +741,5 @@ def test_joint_control_rejects_environment_option() -> None: "example.deploy", "--env", "go2-walk-rough", - "--hardware-confirm", ] ) diff --git a/motrix_env_core/src/motrix_env_core/config/sim.py b/motrix_env_core/src/motrix_env_core/config/sim.py index 96ac442f..c1632cd5 100644 --- a/motrix_env_core/src/motrix_env_core/config/sim.py +++ b/motrix_env_core/src/motrix_env_core/config/sim.py @@ -1,6 +1,8 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 +from math import isfinite + from motrix_env_core.config.decorate import configclass @@ -14,11 +16,15 @@ class SimCfg: gravity: tuple[float, float, float] | None = None def validate(self) -> None: - if self.dt <= 0.0: - raise ValueError(f"sim.dt must be positive, got {self.dt}") - if self.solver_iterations is not None and self.solver_iterations <= 0: - raise ValueError(f"sim.solver_iterations must be positive, got {self.solver_iterations}") - if self.solver_tolerance is not None and self.solver_tolerance <= 0.0: - raise ValueError(f"sim.solver_tolerance must be positive, got {self.solver_tolerance}") + if isinstance(self.dt, bool) or not isfinite(self.dt) or self.dt <= 0.0: + raise ValueError(f"sim.dt must be positive and finite, got {self.dt}") + if self.solver_iterations is not None and ( + isinstance(self.solver_iterations, bool) + or not isinstance(self.solver_iterations, int) + or self.solver_iterations <= 0 + ): + raise ValueError(f"sim.solver_iterations must be a positive integer, got {self.solver_iterations}") + if self.solver_tolerance is not None and (not isfinite(self.solver_tolerance) or self.solver_tolerance <= 0.0): + raise ValueError(f"sim.solver_tolerance must be positive and finite, got {self.solver_tolerance}") if self.gravity is not None and len(self.gravity) != 3: raise ValueError(f"sim.gravity must contain 3 values, got {self.gravity!r}") diff --git a/motrix_env_core/tests/test_sim_cfg.py b/motrix_env_core/tests/test_sim_cfg.py index a38eda5e..cce35ab9 100644 --- a/motrix_env_core/tests/test_sim_cfg.py +++ b/motrix_env_core/tests/test_sim_cfg.py @@ -3,8 +3,9 @@ import pytest -from motrix_env_core.base import EnvCfg, SimCfg +from motrix_env_core.base import EnvCfg from motrix_env_core.config.scene import SceneCfg, SceneCompiler +from motrix_env_core.config.sim import SimCfg def test_scene_compiler_is_an_abstract_backend_boundary(): @@ -31,3 +32,32 @@ def test_env_cfg_uses_sim_config_dt_for_substeps_and_validation(): def test_env_cfg_requires_scene_config(): with pytest.raises(ValueError, match="EnvCfg.scene must be configured"): EnvCfg().validate() + + +@pytest.mark.parametrize("dt", [0.0, -0.1, float("nan"), float("inf"), -float("inf"), True, False]) +def test_sim_cfg_rejects_invalid_timestep(dt): + with pytest.raises(ValueError, match=r"sim\.dt must be positive and finite"): + SimCfg(dt=dt).validate() + + +@pytest.mark.parametrize("iterations", [0, -1, 1.5, 1.0, True, False, "100"]) +def test_sim_cfg_requires_positive_integer_solver_iterations(iterations): + with pytest.raises(ValueError, match=r"sim\.solver_iterations must be a positive integer"): + SimCfg(solver_iterations=iterations).validate() + + +@pytest.mark.parametrize("tolerance", [0.0, -0.1, float("nan"), float("inf"), -float("inf")]) +def test_sim_cfg_requires_positive_finite_solver_tolerance(tolerance): + with pytest.raises(ValueError, match=r"sim\.solver_tolerance must be positive and finite"): + SimCfg(solver_tolerance=tolerance).validate() + + +def test_sim_cfg_supports_backend_defaults_and_explicit_physics(): + # None preserves model/backend solver and gravity defaults. + SimCfg(solver_iterations=None, solver_tolerance=None, gravity=None).validate() + SimCfg(dt=1, solver_iterations=10, solver_tolerance=1e-6, gravity=(0.0, 0.0, -9.81)).validate() + + +def test_sim_cfg_retains_gravity_length_validation(): + with pytest.raises(ValueError, match=r"sim\.gravity must contain 3 values"): + SimCfg(gravity=(0.0, -9.81)).validate() diff --git a/motrix_envs/pyproject.toml b/motrix_envs/pyproject.toml index 2a75a2e4..55ad3b86 100644 --- a/motrix_envs/pyproject.toml +++ b/motrix_envs/pyproject.toml @@ -20,4 +20,4 @@ dependencies = [ ] [project.optional-dependencies] -deploy = ["motrix-deploy"] +deploy = ["motrix-deploy", "motrix-deploy-tasks"] diff --git a/motrix_envs/src/motrix_envs/deploy/go2_walk.py b/motrix_envs/src/motrix_envs/deploy/go2_walk.py index f2733dd7..8af0d3db 100644 --- a/motrix_envs/src/motrix_envs/deploy/go2_walk.py +++ b/motrix_envs/src/motrix_envs/deploy/go2_walk.py @@ -6,13 +6,13 @@ import numpy as np from motrix_robots.unitree import UnitreeGo2Robot -from motrix_deploy.artifact import ControlSpec, TaskSpec +from motrix_deploy.artifact import ControlSpec from motrix_deploy.profile import DeploymentProfile, register_profile_compiler +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec from motrix_env_core import registry from motrix_envs.deploy.robot import build_robot_model, build_robot_spec, read_position_servo_gains from motrix_envs.locomotion.quadruped.cfg import QuadrupedWalkEnvCfg -GO2_TASK_NAME = "go2_walk/v1" GO2_COMMAND_SCALES = { "go2-walk-flat": np.ones(3, dtype=np.float32), "go2-walk-rough": np.full(3, 0.5, dtype=np.float32), @@ -33,11 +33,13 @@ def build_go2_walk_profile(env_name: str) -> DeploymentProfile: if env_cfg.control_config.simulate_action_latency: raise ValueError("Go2 deployment v1 requires control_config.simulate_action_latency=false") + model = build_robot_model(robot_cfg) robot = build_robot_spec( robot_cfg, key_pose_name=env_cfg.key_pose_name, + model=model, ) - kp, kd = read_position_servo_gains(build_robot_model(robot_cfg), robot.joint_names) + kp, kd = read_position_servo_gains(model, robot.joint_names) action_shape = env.action_space.shape observation_shape = env.policy_observation_space.shape if len(action_shape) != 1 or len(observation_shape) != 1: @@ -47,31 +49,32 @@ def build_go2_walk_profile(env_name: str) -> DeploymentProfile: if action_size != robot.joint_count: raise ValueError("Go2 deployment requires every model actuator to target one canonical robot joint") action_scale = env_cfg.control_config.action_scale - raw_lower = env.action_space.low - raw_upper = env.action_space.high + action_lower = env.action_space.low + action_upper = env.action_space.high command_cfg = env_cfg.commands.velocity feet_offsets = _feet_phase_offsets(env_cfg) - task = TaskSpec( - name=GO2_TASK_NAME, - observation_size=observation_size, - action_size=action_size, - config={ - "action_scale": action_scale, - "command_lower": command_cfg.lower.tolist(), - "command_upper": command_cfg.upper.tolist(), - "command_scale": GO2_COMMAND_SCALES[env_name].tolist(), - "feet_phase_offsets": feet_offsets.tolist(), - "gait_frequency_hz": env_cfg.gait_frequency, - "standing_threshold": env_cfg.commands.velocity.standing_threshold, - "kp": kp.tolist(), - "kd": kd.tolist(), - "raw_clip": [raw_lower.tolist(), raw_upper.tolist()], - }, + task = Go2WalkTaskSpec( + action_scale=np.broadcast_to(action_scale, (action_size,)).tolist(), + command_lower=command_cfg.lower.tolist(), + command_upper=command_cfg.upper.tolist(), + command_scale=GO2_COMMAND_SCALES[env_name].tolist(), + 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, + termination_min_base_height=None, + kp=kp.tolist(), + kd=kd.tolist(), + action_lower=action_lower.tolist(), + action_upper=action_upper.tolist(), ) return DeploymentProfile( robot=robot, task=task, control=ControlSpec(period_s=env_cfg.ctrl_dt, state_timeout_s=0.1), + observation_size=observation_size, + action_size=action_size, ) diff --git a/motrix_envs/tests/test_deploy_profile.py b/motrix_envs/tests/test_deploy_profile.py index 0470ef9d..973b7783 100644 --- a/motrix_envs/tests/test_deploy_profile.py +++ b/motrix_envs/tests/test_deploy_profile.py @@ -9,6 +9,7 @@ import pytest from motrix_robots.unitree import UnitreeGo2Robot +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec from motrix_env_core import registry from motrix_envs.deploy import build_deployment_profile, registered_profile_compilers from motrix_envs.deploy.robot import ( @@ -36,14 +37,16 @@ def test_go2_walk_environments_compile_shared_deployment_profiles( env_cfg = registry.make_env_config(env_name) assert {"go2-walk-flat", "go2-walk-rough"} <= set(registered_profile_compilers()) + assert isinstance(profile.task, Go2WalkTaskSpec) assert profile.task.name == "go2_walk/v1" - assert profile.task.observation_size == 49 - assert profile.task.action_size == 12 + env = registry.make(env_name, num_envs=1, mode="train") + assert profile.observation_size == env.policy_observation_space.shape[0] + assert profile.action_size == env.action_space.shape[0] assert profile.control.period_s == pytest.approx(env_cfg.ctrl_dt) - assert profile.task.config["command_lower"] == env_cfg.commands.velocity.lower.tolist() - assert profile.task.config["command_upper"] == env_cfg.commands.velocity.upper.tolist() + assert profile.task.command_lower == env_cfg.commands.velocity.lower.tolist() + assert profile.task.command_upper == env_cfg.commands.velocity.upper.tolist() # Command scale is an artifact/runtime input contract and intentionally differs by deployment environment. - assert profile.task.config["command_scale"] == command_scale + assert profile.task.command_scale == command_scale def test_robot_spec_is_built_without_an_environment_config() -> None: diff --git a/motrix_rl/src/motrix_rl/deploy/service.py b/motrix_rl/src/motrix_rl/deploy/service.py index ace236cb..c1609026 100644 --- a/motrix_rl/src/motrix_rl/deploy/service.py +++ b/motrix_rl/src/motrix_rl/deploy/service.py @@ -50,12 +50,12 @@ def export_deploy_run( policy_bytes = exported.model_bytes input_spec = _deployment_tensor_spec( exported.report.input_spec, - expected_size=profile.task.observation_size, + expected_size=profile.observation_size, kind="input", ) output_spec = _deployment_tensor_spec( exported.report.output_spec, - expected_size=profile.task.action_size, + expected_size=profile.action_size, kind="output", ) diff --git a/motrix_rl/tests/test_deploy_export.py b/motrix_rl/tests/test_deploy_export.py index e5ef112b..ac11c0b6 100644 --- a/motrix_rl/tests/test_deploy_export.py +++ b/motrix_rl/tests/test_deploy_export.py @@ -3,17 +3,58 @@ """Task-agnostic deployment export service tests.""" +from importlib import metadata +from types import SimpleNamespace +from typing import ClassVar + import numpy as np import pytest from motrix_deploy.artifact import ControlSpec, TaskSpec from motrix_deploy.contracts import RobotSpec from motrix_deploy.profile import DeploymentProfile +from motrix_deploy.task import DeployTask from motrix_rl import checkpoints, runs from motrix_rl.deploy.api import OnnxExportReport, OnnxModelArtifact, OnnxParityMetrics, PolicyTensorSpec from motrix_rl.deploy.service import export_deploy_run +class TestTaskSpec(TaskSpec): + __test__ = False + task_name: ClassVar[str] = "test/v1" + + +class TestDeployTask(DeployTask): + __test__ = False + spec_type = TestTaskSpec + + def reset(self, state, context) -> None: + pass + + def build_observation(self, state, context): + raise NotImplementedError + + def process_action(self, action): + raise NotImplementedError + + def validate_command(self, command) -> None: + pass + + +@pytest.fixture(autouse=True) +def test_task_spec_plugin(monkeypatch: pytest.MonkeyPatch) -> None: + installed_entry_points = metadata.entry_points + entry = SimpleNamespace(name=TestTaskSpec.task_name, load=lambda: TestDeployTask) + + def entry_points(**kwargs): + entries = installed_entry_points(**kwargs) + if kwargs.get("group") == "motrix_deploy.tasks": + return (*entries, entry) + return entries + + monkeypatch.setattr(metadata, "entry_points", entry_points) + + def _profile() -> DeploymentProfile: return DeploymentProfile( robot=RobotSpec( @@ -24,19 +65,23 @@ def _profile() -> DeploymentProfile: position_upper=np.full(2, 1.0, dtype=np.float32), torque_limit=np.full(2, 3.0, dtype=np.float32), ), - task=TaskSpec( - name="test/v1", - observation_size=4, - action_size=2, - config={}, - ), + task=TestTaskSpec(), control=ControlSpec(period_s=0.02, state_timeout_s=0.1), + observation_size=4, + action_size=2, ) +@pytest.mark.parametrize( + ("input_size", "output_size", "invalid_kind"), + [(4, 2, None), (5, 2, "input"), (4, 3, "output")], +) def test_deployment_export_injects_profile_builder_selected_by_run_metadata( tmp_path, monkeypatch: pytest.MonkeyPatch, + input_size: int, + output_size: int, + invalid_kind: str | None, ) -> None: run = runs.create_run_context( env_name="cartpole", @@ -59,14 +104,25 @@ def test_deployment_export_injects_profile_builder_selected_by_run_metadata( model = OnnxModelArtifact( model_bytes=b"validated ONNX payload", report=OnnxExportReport( - input_spec=PolicyTensorSpec(name="obs", shape=(None, 4), dtype="float32"), - output_spec=PolicyTensorSpec(name="actions", shape=(None, 2), dtype="float32"), + input_spec=PolicyTensorSpec(name="obs", shape=(None, input_size), dtype="float32"), + output_spec=PolicyTensorSpec(name="actions", shape=(None, output_size), dtype="float32"), parity=OnnxParityMetrics(samples=8, max_abs_error=1e-6, max_rel_error=2e-6), ), ) monkeypatch.setattr("motrix_rl.deploy.service.export_onnx_model", lambda *args, **kwargs: model) selected: list[str] = [] + if invalid_kind is not None: + with pytest.raises(ValueError, match=f"deployment policy {invalid_kind} shape"): + export_deploy_run( + run.run_dir, + tmp_path / "test.deploy", + profile_builder=lambda env_name: selected.append(env_name) or _profile(), + validation_samples=8, + ) + assert not (tmp_path / "test.deploy").exists() + return + result = export_deploy_run( run.run_dir, tmp_path / "test.deploy", @@ -75,7 +131,11 @@ def test_deployment_export_injects_profile_builder_selected_by_run_metadata( ) assert selected == ["cartpole"] - assert result.artifact.manifest.task.name == "test/v1" + assert isinstance(result.artifact.manifest.task, TestTaskSpec) + manifest = result.artifact.manifest.to_dict() + assert manifest["task"] == {"name": "test/v1", "config": {}} + assert result.artifact.manifest.policy.input.shape == (1, _profile().observation_size) + assert result.artifact.manifest.policy.output.shape == (1, _profile().action_size) assert result.artifact.manifest.source.framework == "skrl.ppo/torch" assert result.artifact.policy_path.read_bytes() == model.model_bytes assert result.validation_samples == 8 diff --git a/uv.lock b/uv.lock index 6246c943..1fc1a259 100644 --- a/uv.lock +++ b/uv.lock @@ -73,6 +73,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/2b/cf/bacc741d9926d662e76fd694f5bc63dd4c2471e32cedfb1c3cca7e47aa3c/amdsmi-7.0.2-py3-none-any.whl", hash = "sha256:db5aa757f8ed82dfd799c4d39e2828542678dc4e485b0ab7fabe5f398fec5652", size = 64366, upload-time = "2025-10-11T05:17:44.047Z" }, ] +[[package]] +name = "annotated-types" +version = "0.8.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/5f/56/a8120250d128bed162cd73c76d45f6ef9991f3e068f62a8ee060afa3104a/annotated_types-0.8.0.tar.gz", hash = "sha256:13b2beaad985e05e2d6407ee4c4f35590b11f8d693a258a561055cac8f64cab7", size = 15893, upload-time = "2026-07-23T20:16:13.995Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/99/91/8acff4f5e50511b911bbccb72b8628a49c68ce14148cd9f6431094859a90/annotated_types-0.8.0-py3-none-any.whl", hash = "sha256:f072f4d804ea359e4eaf198b1af7a8b0943881a87f31bb764f8bf219bb9419e0", size = 13427, upload-time = "2026-07-23T20:16:12.938Z" }, +] + [[package]] name = "antlr4-python3-runtime" version = "4.9.3" @@ -912,6 +921,7 @@ dependencies = [ { name = "motrix-env-core" }, { name = "numpy" }, { name = "onnxruntime" }, + { name = "pydantic" }, ] [package.metadata] @@ -920,6 +930,7 @@ requires-dist = [ { name = "motrix-env-core", editable = "motrix_env_core" }, { name = "numpy", specifier = ">=1.26" }, { name = "onnxruntime", specifier = "==1.23.2" }, + { name = "pydantic", specifier = "===2.12.5" }, ] [[package]] @@ -927,17 +938,17 @@ name = "motrix-deploy-mujoco" version = "0.4.0b0" source = { editable = "motrix_deploy_mujoco" } dependencies = [ + { name = "hydra-core" }, { name = "motrix-deploy" }, { name = "motrix-env-mujoco" }, - { name = "motrix-envs" }, { name = "numpy" }, ] [package.metadata] requires-dist = [ + { name = "hydra-core", specifier = ">=1.3" }, { name = "motrix-deploy", editable = "motrix_deploy" }, { name = "motrix-env-mujoco", editable = "motrix_env_mujoco" }, - { name = "motrix-envs", editable = "motrix_envs" }, { name = "numpy", specifier = ">=1.26" }, ] @@ -946,28 +957,21 @@ name = "motrix-deploy-tasks" version = "0.4.0b0" source = { editable = "motrix_deploy_tasks" } dependencies = [ + { name = "hydra-core" }, { name = "motrix-deploy" }, { name = "motrix-env-core" }, + { name = "motrix-robots" }, { name = "numpy" }, ] -[package.optional-dependencies] -mujoco = [ - { name = "motrix-deploy-mujoco" }, -] -unitree = [ - { name = "motrix-deploy-unitree" }, -] - [package.metadata] requires-dist = [ + { name = "hydra-core", specifier = ">=1.3,<1.4" }, { name = "motrix-deploy", editable = "motrix_deploy" }, - { name = "motrix-deploy-mujoco", marker = "extra == 'mujoco'", editable = "motrix_deploy_mujoco" }, - { name = "motrix-deploy-unitree", marker = "extra == 'unitree'", editable = "motrix_deploy_unitree" }, { name = "motrix-env-core", editable = "motrix_env_core" }, + { name = "motrix-robots", editable = "motrix_robots" }, { name = "numpy", specifier = ">=1.26" }, ] -provides-extras = ["mujoco", "unitree"] [[package]] name = "motrix-deploy-unitree" @@ -1083,12 +1087,14 @@ dependencies = [ [package.optional-dependencies] deploy = [ { name = "motrix-deploy" }, + { name = "motrix-deploy-tasks" }, ] [package.metadata] requires-dist = [ { name = "gymnasium", specifier = "==1.1.1" }, { name = "motrix-deploy", marker = "extra == 'deploy'", editable = "motrix_deploy" }, + { name = "motrix-deploy-tasks", marker = "extra == 'deploy'", editable = "motrix_deploy_tasks" }, { name = "motrix-env-core", extras = ["numba", "video"], editable = "motrix_env_core" }, { name = "motrix-env-motrixsim", editable = "motrix_env_motrixsim" }, { name = "motrix-robots", editable = "motrix_robots" }, @@ -2111,6 +2117,53 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/08/b4/46310463b4f6ceef310f8348786f3cff181cea671578e3d9743ba61a459e/protobuf-6.33.1-py3-none-any.whl", hash = "sha256:d595a9fd694fdeb061a62fbe10eb039cc1e444df81ec9bb70c7fc59ebcb1eafa", size = 170477, upload-time = "2025-11-13T16:44:17.633Z" }, ] +[[package]] +name = "pydantic" +version = "2.12.5" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "annotated-types" }, + { name = "pydantic-core" }, + { name = "typing-extensions" }, + { name = "typing-inspection" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/69/44/36f1a6e523abc58ae5f928898e4aca2e0ea509b5aa6f6f392a5d882be928/pydantic-2.12.5.tar.gz", hash = "sha256:4d351024c75c0f085a9febbb665ce8c0c6ec5d30e903bdb6394b7ede26aebb49", size = 821591, upload-time = "2025-11-26T15:11:46.471Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/5a/87/b70ad306ebb6f9b585f114d0ac2137d792b48be34d732d60e597c2f8465a/pydantic-2.12.5-py3-none-any.whl", hash = "sha256:e561593fccf61e8a20fc46dfc2dfe075b8be7d0188df33f221ad1f0139180f9d", size = 463580, upload-time = "2025-11-26T15:11:44.605Z" }, +] + +[[package]] +name = "pydantic-core" +version = "2.41.5" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "typing-extensions" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/71/70/23b021c950c2addd24ec408e9ab05d59b035b39d97cdc1130e1bce647bb6/pydantic_core-2.41.5.tar.gz", hash = "sha256:08daa51ea16ad373ffd5e7606252cc32f07bc72b28284b6bc9c6df804816476e", size = 460952, upload-time = "2025-11-04T13:43:49.098Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c6/90/32c9941e728d564b411d574d8ee0cf09b12ec978cb22b294995bae5549a5/pydantic_core-2.41.5-cp310-cp310-macosx_10_12_x86_64.whl", hash = "sha256:77b63866ca88d804225eaa4af3e664c5faf3568cea95360d21f4725ab6e07146", size = 2107298, upload-time = "2025-11-04T13:39:04.116Z" }, + { url = "https://files.pythonhosted.org/packages/fb/a8/61c96a77fe28993d9a6fb0f4127e05430a267b235a124545d79fea46dd65/pydantic_core-2.41.5-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:dfa8a0c812ac681395907e71e1274819dec685fec28273a28905df579ef137e2", size = 1901475, upload-time = "2025-11-04T13:39:06.055Z" }, + { url = "https://files.pythonhosted.org/packages/5d/b6/338abf60225acc18cdc08b4faef592d0310923d19a87fba1faf05af5346e/pydantic_core-2.41.5-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5921a4d3ca3aee735d9fd163808f5e8dd6c6972101e4adbda9a4667908849b97", size = 1918815, upload-time = "2025-11-04T13:39:10.41Z" }, + { url = "https://files.pythonhosted.org/packages/d1/1c/2ed0433e682983d8e8cba9c8d8ef274d4791ec6a6f24c58935b90e780e0a/pydantic_core-2.41.5-cp310-cp310-manylinux_2_17_armv7l.manylinux2014_armv7l.whl", hash = "sha256:e25c479382d26a2a41b7ebea1043564a937db462816ea07afa8a44c0866d52f9", size = 2065567, upload-time = "2025-11-04T13:39:12.244Z" }, + { url = "https://files.pythonhosted.org/packages/b3/24/cf84974ee7d6eae06b9e63289b7b8f6549d416b5c199ca2d7ce13bbcf619/pydantic_core-2.41.5-cp310-cp310-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:f547144f2966e1e16ae626d8ce72b4cfa0caedc7fa28052001c94fb2fcaa1c52", size = 2230442, upload-time = "2025-11-04T13:39:13.962Z" }, + { url = "https://files.pythonhosted.org/packages/fd/21/4e287865504b3edc0136c89c9c09431be326168b1eb7841911cbc877a995/pydantic_core-2.41.5-cp310-cp310-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:6f52298fbd394f9ed112d56f3d11aabd0d5bd27beb3084cc3d8ad069483b8941", size = 2350956, upload-time = "2025-11-04T13:39:15.889Z" }, + { url = "https://files.pythonhosted.org/packages/a8/76/7727ef2ffa4b62fcab916686a68a0426b9b790139720e1934e8ba797e238/pydantic_core-2.41.5-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:100baa204bb412b74fe285fb0f3a385256dad1d1879f0a5cb1499ed2e83d132a", size = 2068253, upload-time = "2025-11-04T13:39:17.403Z" }, + { url = "https://files.pythonhosted.org/packages/d5/8c/a4abfc79604bcb4c748e18975c44f94f756f08fb04218d5cb87eb0d3a63e/pydantic_core-2.41.5-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:05a2c8852530ad2812cb7914dc61a1125dc4e06252ee98e5638a12da6cc6fb6c", size = 2177050, upload-time = "2025-11-04T13:39:19.351Z" }, + { url = "https://files.pythonhosted.org/packages/67/b1/de2e9a9a79b480f9cb0b6e8b6ba4c50b18d4e89852426364c66aa82bb7b3/pydantic_core-2.41.5-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:29452c56df2ed968d18d7e21f4ab0ac55e71dc59524872f6fc57dcf4a3249ed2", size = 2147178, upload-time = "2025-11-04T13:39:21Z" }, + { url = "https://files.pythonhosted.org/packages/16/c1/dfb33f837a47b20417500efaa0378adc6635b3c79e8369ff7a03c494b4ac/pydantic_core-2.41.5-cp310-cp310-musllinux_1_1_armv7l.whl", hash = "sha256:d5160812ea7a8a2ffbe233d8da666880cad0cbaf5d4de74ae15c313213d62556", size = 2341833, upload-time = "2025-11-04T13:39:22.606Z" }, + { url = "https://files.pythonhosted.org/packages/47/36/00f398642a0f4b815a9a558c4f1dca1b4020a7d49562807d7bc9ff279a6c/pydantic_core-2.41.5-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:df3959765b553b9440adfd3c795617c352154e497a4eaf3752555cfb5da8fc49", size = 2321156, upload-time = "2025-11-04T13:39:25.843Z" }, + { url = "https://files.pythonhosted.org/packages/7e/70/cad3acd89fde2010807354d978725ae111ddf6d0ea46d1ea1775b5c1bd0c/pydantic_core-2.41.5-cp310-cp310-win32.whl", hash = "sha256:1f8d33a7f4d5a7889e60dc39856d76d09333d8a6ed0f5f1190635cbec70ec4ba", size = 1989378, upload-time = "2025-11-04T13:39:27.92Z" }, + { url = "https://files.pythonhosted.org/packages/76/92/d338652464c6c367e5608e4488201702cd1cbb0f33f7b6a85a60fe5f3720/pydantic_core-2.41.5-cp310-cp310-win_amd64.whl", hash = "sha256:62de39db01b8d593e45871af2af9e497295db8d73b085f6bfd0b18c83c70a8f9", size = 2013622, upload-time = "2025-11-04T13:39:29.848Z" }, + { url = "https://files.pythonhosted.org/packages/e6/b0/1a2aa41e3b5a4ba11420aba2d091b2d17959c8d1519ece3627c371951e73/pydantic_core-2.41.5-pp310-pypy310_pp73-macosx_10_12_x86_64.whl", hash = "sha256:b5819cd790dbf0c5eb9f82c73c16b39a65dd6dd4d1439dcdea7816ec9adddab8", size = 2103351, upload-time = "2025-11-04T13:43:02.058Z" }, + { url = "https://files.pythonhosted.org/packages/a4/ee/31b1f0020baaf6d091c87900ae05c6aeae101fa4e188e1613c80e4f1ea31/pydantic_core-2.41.5-pp310-pypy310_pp73-macosx_11_0_arm64.whl", hash = "sha256:5a4e67afbc95fa5c34cf27d9089bca7fcab4e51e57278d710320a70b956d1b9a", size = 1925363, upload-time = "2025-11-04T13:43:05.159Z" }, + { url = "https://files.pythonhosted.org/packages/e1/89/ab8e86208467e467a80deaca4e434adac37b10a9d134cd2f99b28a01e483/pydantic_core-2.41.5-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ece5c59f0ce7d001e017643d8d24da587ea1f74f6993467d85ae8a5ef9d4f42b", size = 2135615, upload-time = "2025-11-04T13:43:08.116Z" }, + { url = "https://files.pythonhosted.org/packages/99/0a/99a53d06dd0348b2008f2f30884b34719c323f16c3be4e6cc1203b74a91d/pydantic_core-2.41.5-pp310-pypy310_pp73-manylinux_2_5_i686.manylinux1_i686.whl", hash = "sha256:16f80f7abe3351f8ea6858914ddc8c77e02578544a0ebc15b4c2e1a0e813b0b2", size = 2175369, upload-time = "2025-11-04T13:43:12.49Z" }, + { url = "https://files.pythonhosted.org/packages/6d/94/30ca3b73c6d485b9bb0bc66e611cff4a7138ff9736b7e66bcf0852151636/pydantic_core-2.41.5-pp310-pypy310_pp73-musllinux_1_1_aarch64.whl", hash = "sha256:33cb885e759a705b426baada1fe68cbb0a2e68e34c5d0d0289a364cf01709093", size = 2144218, upload-time = "2025-11-04T13:43:15.431Z" }, + { url = "https://files.pythonhosted.org/packages/87/57/31b4f8e12680b739a91f472b5671294236b82586889ef764b5fbc6669238/pydantic_core-2.41.5-pp310-pypy310_pp73-musllinux_1_1_armv7l.whl", hash = "sha256:c8d8b4eb992936023be7dee581270af5c6e0697a8559895f527f5b7105ecd36a", size = 2329951, upload-time = "2025-11-04T13:43:18.062Z" }, + { url = "https://files.pythonhosted.org/packages/7d/73/3c2c8edef77b8f7310e6fb012dbc4b8551386ed575b9eb6fb2506e28a7eb/pydantic_core-2.41.5-pp310-pypy310_pp73-musllinux_1_1_x86_64.whl", hash = "sha256:242a206cd0318f95cd21bdacff3fcc3aab23e79bba5cac3db5a841c9ef9c6963", size = 2318428, upload-time = "2025-11-04T13:43:20.679Z" }, + { url = "https://files.pythonhosted.org/packages/2f/02/8559b1f26ee0d502c74f9cca5c0d2fd97e967e083e006bbbb4e97f3a043a/pydantic_core-2.41.5-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:d3a978c4f57a597908b7e697229d996d77a6d3c94901e9edee593adada95ce1a", size = 2147009, upload-time = "2025-11-04T13:43:23.286Z" }, +] + [[package]] name = "pydata-sphinx-theme" version = "0.16.1" @@ -2953,6 +3006,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/18/67/36e9267722cc04a6b9f15c7f3441c2363321a3ea07da7ae0c0707beb2a9c/typing_extensions-4.15.0-py3-none-any.whl", hash = "sha256:f0fa19c6845758ab08074a0cfa8b7aecb71c999ca73d62883bc25cc018c4e548", size = 44614, upload-time = "2025-08-25T13:49:24.86Z" }, ] +[[package]] +name = "typing-inspection" +version = "0.4.4" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "typing-extensions" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/a3/26/b09b8010994eccc3c09092e6b34058f36a460eea2d4c3e8b910c695975a0/typing_inspection-0.4.4.tar.gz", hash = "sha256:547274fa6b0a561ccf549cc9524b999a578e737d015d8709d021f9d0d13bea47", size = 76928, upload-time = "2026-08-12T12:37:25.997Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/67/81/4add07e5172b7ac40d8ed5ff580409a7801a4fe26d529bdd915401dabfbe/typing_inspection-0.4.4-py3-none-any.whl", hash = "sha256:65b8397ba37ccbce054456aaccddfc91e6e3083c92824df348d96ca832f3f147", size = 14750, upload-time = "2026-08-12T12:37:24.648Z" }, +] + [[package]] name = "unitree-sdk2py" version = "1.0.1" diff --git a/wiki/design/motrix-deploy.md b/wiki/design/motrix-deploy.md index 7a5069d7..bf368498 100644 --- a/wiki/design/motrix-deploy.md +++ b/wiki/design/motrix-deploy.md @@ -403,7 +403,7 @@ event-based device 执行一次 `poll()`。`ControlLoop.run()` 的单步顺序 和确定性 CI。 两种 scheduler 向 `PolicyContext` 提供相同的 step 和 elapsed control time；wall-clock latency 单独计量。 -realtime/fixed-step scheduler 由 runtime recipe 显式选择，不从 device 推断。 +仿真的 realtime/fixed-step scheduler 由 `runtime.realtime` 选择；`null` 跟随 `runtime.viewer`。硬件始终在内部实时调度，不从 device 推断。 Fixed-step 模式记录“若实时运行是否 overrun”，但默认不因此终止 rollout。第一版 policy rate 与 command rate 相同；未来需要多速率时通过 scheduler 扩展，不能由 backend 私自重复或插值 policy action。 @@ -853,9 +853,10 @@ timestep。初始化时调用 `mj_resetData`、写入确定的初始 root pose ### 11.4 运行终止与指标 -sim2sim 的 Hydra runtime config 只配置 artifact、backend 和 viewer。CLI 不提供 rollout steps、duration、seed、input -type 或 command scale override；Go2 command scale 由 artifact 的 `TaskSpec` 持有。启动后持续运行，直到 viewer -关闭、用户按 Esc/Ctrl-C、机器人跌倒或发生错误。运行时还须： +sim2sim 的 Hydra 配置将 artifact、duration_s 和 command 放在顶层，仿真设置直接放在 runtime 下。 +`runtime.backend=mujoco` 选择 MuJoCo；`runtime.viewer` 和 `runtime.realtime` 只用于仿真。`duration_s` 按 artifact 的 +控制周期换算为 `ceil(duration_s / control_period_s)` 个控制 tick，不是墙钟超时。未设置 duration_s 时持续运行， +直到 viewer 关闭、用户按 Esc/Ctrl-C、机器人跌倒或发生错误。运行时还须： - 使用 task 从 artifact 读取的 command range 乘以 scale 后创建 keyboard binding； - reset 后从 step 0 重新初始化 phase、previous action 和 scheduler； @@ -864,9 +865,10 @@ type 或 command scale override；Go2 command scale 由 artifact 的 `TaskSpec` - 可配置任务指标：base height、fall、速度跟踪误差等，但这些指标不进入通用控制循环。 deployment backend 使用最小 GLFW viewer：每次 `sync()` 只更新 MuJoCo scene 并 render，不拥有仿真步进；同一 -window 的 callbacks 提供 keyboard event frame 与 mouse camera control，失焦后立即释放 held keys。viewer 模式使用 -realtime scheduler。headless backend 仍可配合程序传入的 binding 和 fixed-step scheduler 用于测试，但内置交互式 -CLI keyboard path 要求 `viewer=true`。`ControlLoop.run(steps=...)` 保留为内部测试和确定性验证能力，不进入用户配置。 +window 的 callbacks 提供 keyboard event frame 与 mouse camera control，失焦后立即释放 held keys。viewer 模式默认使用 +realtime scheduler，可通过 `runtime.realtime=false` 关闭实时节奏控制。headless CLI 使用配方中的 constant command，可通过 `command.velocity` 覆盖； +`runtime.realtime=null` 时跟随 `runtime.viewer`，也可显式配置实时节奏。`ControlLoop.run(steps=...)` 保留为 +程序化确定步数运行能力，不进入 CLI 配置。 ## 12. CLI 设计 @@ -876,30 +878,33 @@ package 暴露统一入口： # 只检查 artifact，不创建设备 motrix-deploy inspect artifact=path/to/go2_walk.deploy -# sim2sim 持续运行，直到用户退出、机器人跌倒或发生错误 -motrix-deploy sim2sim \ - artifact=path/to/go2_walk.deploy +# sim2sim 有界无窗口检查 +motrix-deploy task=go2-walk-flat/sim \ + artifact=path/to/go2_walk.deploy \ + runtime.viewer=false duration_s=2.0 # 实时可视化 -motrix-deploy sim2sim \ +motrix-deploy task=go2-walk-flat/sim \ artifact=path/to/go2_walk.deploy \ - viewer=true + runtime.viewer=true -# 后续阶段的真机入口 -motrix-deploy sim2real \ +# 真机入口 +motrix-deploy task=go2-walk-flat/hardware \ artifact=path/to/go2_walk.deploy \ - backend.name=unitree \ - hardware.confirm=true + runtime.network_interface=enp5s0 ``` `inspect` 只执行 artifact、RobotSpec 和 policy 静态校验，不查询 robot registry，也不打开 backend。 -当前 sim2sim 的类型化 Hydra 配置只包含 artifact、backend 和 viewer，CLI override 只修改所需字段；application -从 backend 的可选 keyboard device capability 创建 binding。后续 sim2real 必须使用独立模式、显式确认参数和 adapter 自身的 enable 流程， -默认配置不能把真机 enable 设为 true。 +Hydra 配置的公共执行字段是 artifact、duration_s 和 command；task 配方选择互斥的 runtime/sim 或 runtime/hardware 配置组。 +所有目标专用设置直接位于 runtime 下：`runtime.backend` 是标量字符串选择器，取值为 `mujoco` 或 `unitree_go2`， +不是嵌套配置映射或 Hydra 配置组；场景、physics、sensor_bindings、network_interface、kp、kd 等字段也直接位于 runtime 下。viewer 和 realtime 仅用于仿真，硬件由 runtime 在内部强制实时调度。 +application 从 runtime 的可选 keyboard device capability 创建 binding。真机不使用静态确认布尔值， +必须保留 adapter 的启动等待、enable、急停和安全停止流程；默认配置等待操作员按下启动和使能按钮。 +duration_s 换算为控制 tick 上限，不是墙钟超时；程序化 runtime.run(steps=...) 保留确定步数运行。 -具体 task/backend recipe 属于应用层，放在根目录 `configs/deploy/`，例如 -`configs/deploy/sim2sim/go2_walk_sim2sim.yaml`。`motrix_deploy` 包内只保留不含具体机器人、模型路径或 command 默认值的 -通用 mandatory-field 模板；workspace bootstrap 可以选择默认 recipe，外部部署则显式传入 Hydra config path/name。 +具体 task/runtime recipe 属于应用层，由 `motrix_deploy_tasks` 插件安装，提供 flat/rough 场景、机器人和 command 默认值。 +`motrix_deploy` 包内保留通用配置；任务 defaults 选择 runtime/sim 或 runtime/hardware 配置组。 +安装任务和目标 runtime 插件后，部署可在仓库外运行，不需要 workspace 配置路径。 ## 13. 与现有 MotrixLab 的集成 @@ -907,7 +912,7 @@ motrix-deploy sim2real \ policy exporter 与 deployment profile compiler 复用现有： -- `metadata.json`：训练环境、framework、backend、算法、seed 和版本来源； +- `metadata.json`：训练环境、framework、backend、算法和版本来源； - `task_config.yaml`：已解析的训练配置快照； - `checkpoints/manifest.json`：按语义定位 `best_policy`，不按文件名搜索 checkpoint。 @@ -969,14 +974,15 @@ uv run scripts/export_deploy.py env=go2-walk-rough ### 14.2 Contract test -提供 `FakeRobotInterface` 和可复用 adapter contract suite，验证： +`FakeRobotInterface` 是测试目录内共享的确定性 robot I/O 替身，不作为部署包的运行时 adapter 发布。控制面测试使用它验证： - lifecycle 调用顺序； -- name-based joint mapping； -- reset 与 seed； -- state timeout、backend unhealthy、write failure； -- stop/close 在正常结束、异常和用户中断下都执行； -- 第二个 fake backend 无需修改控制循环即可通过。 +- 不同状态反馈响应下的控制循环与确定性闭环； +- state timeout、backend unhealthy、write failure 和启动校验失败； +- stop/close 在正常结束、异常和用户中断下都执行。 + +name-based joint mapping、模型关节缺失以及 reset 等具体 adapter 行为由对应后端的测试验证， +不使用 fake 的名字集合检查替代真实映射测试。 command input 的 device contract suite 独立维护在 [Command Input 第一版测试边界](./deploy-command-input.md#9-第一版测试边界) 中。 @@ -994,7 +1000,7 @@ command input 的 device contract suite 独立维护在 CI smoke test 使用短 rollout，不以训练 reward 达标作为框架正确性的唯一条件。策略质量回归可以在 #139 基于 同一 CLI 和 `RolloutResult` 增加独立阈值。 -Go2 vertical slice 的验收基线为：固定 seed 的 1000-tick headless rollout trace SHA-256 +Go2 vertical slice 的验收基线为：确定性初始化的 1000-tick headless rollout trace SHA-256 `8db7e74506dbe36147746696cb15a57b0a4bd2ddf95096f3dce597e94dececda`；GLFW viewer 的 2 秒真实 GUI smoke 完成 100 ticks、无 overrun，并正常退出。viewer 与 headless 的同长度 trace SHA-256 均为 `b7b94aaa2cfba6e4700e92b650d408da13dd73091ce870c55e326e2012ce21a0`，两种模式共用同一 backend stepping 路径。