diff --git a/AGENTS.md b/AGENTS.md index 9c0b21bc..69436fb3 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -22,6 +22,7 @@ MotrixLab 是构建在 MotrixSim 仿真后端之上的强化学习框架,提 - `motrix_rl_rslrl`:RSLRL PPO provider 插件(Torch) - `motrix_deploy`:框架无关的 artifact、运行时契约与部署 CLI - `motrix_deploy_mujoco`:SceneCfg 支撑的 MuJoCo 后端插件 +- `motrix_deploy_motrixsim`:SceneCfg 支撑的原生 MotrixSim 部署后端插件 - `motrix_deploy_unitree`:Unitree SDK2 DDS 硬件后端插件 - `motrix_deploy_tasks`:具体部署任务实现 diff --git a/README.md b/README.md index a17a1b63..a3711e49 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ _Microduck locomotion policies trained with MotrixLab, rendered in MotrixRender ## What is MotrixLab? -**MotrixLab** is an open-source reinforcement learning framework for robot training, built on the high-performance [MotrixSim](https://github.com/Motphys/motrixsim-docs) physics engine. Define an environment once, train it with thousands of parallel environment instances using SKRL, RSL-RL, or the built-in FastSAC, and deploy the resulting policy to MuJoCo or Unitree hardware — all through a single command-line interface. +**MotrixLab** is an open-source reinforcement learning framework for robot training, built on the high-performance [MotrixSim](https://github.com/Motphys/motrixsim-docs) physics engine. Define an environment once, train it with thousands of parallel environment instances using SKRL, RSL-RL, or the built-in FastSAC, and deploy the resulting policy to MuJoCo, native MotrixSim, or Unitree hardware — all through a single command-line interface.
@@ -46,7 +46,7 @@ _Microduck locomotion policies trained with MotrixLab, rendered in MotrixRender - **Unified Interface**: Provides a concise and unified reinforcement learning training and evaluation interface - **Multi-framework Support**: Supports SKRL (JAX/PyTorch), RSLRL (PyTorch), and the built-in FastSAC implementation - **Rich Environments**: Includes various robot simulation environments such as basic control, locomotion, and manipulation tasks -- **Sim-to-Real Deployment**: The same policy code deploys via the deploy CLI — Sim2Sim to MuJoCo, Sim2Real to real hardware +- **Sim-to-Real Deployment**: The same policy code deploys via the deploy CLI — Sim2Sim to MuJoCo or native MotrixSim, Sim2Real to real hardware - **High-precision, High-performance Simulation**: Built on [MotrixSim](https://motrixsim.readthedocs.io/), a high-precision, high-performance physics engine - **Visual Training**: Supports real-time rendering and training process visualization @@ -180,6 +180,7 @@ MotrixLab is a [uv](https://docs.astral.sh/uv/) workspace with the following pac | --- | --- | --- | | **motrix_deploy** | `motrix-deploy` | Artifacts, control sessions, runtime contracts, and deployment CLI | | **motrix_deploy_mujoco** | `motrix-deploy-mujoco` | MuJoCo deployment backend plugin | +| **motrix_deploy_motrixsim** | `motrix-deploy-motrixsim` | Native MotrixSim deployment backend plugin | | **motrix_deploy_unitree** | `motrix-deploy-unitree` | Unitree SDK2 DDS hardware backend plugin | | **motrix_deploy_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 | diff --git a/README.zh-CN.md b/README.zh-CN.md index a0e5c718..22af734f 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -32,7 +32,7 @@ _使用 MotrixLab 训练的 microduck 行走策略,由 MotrixRender 实时渲 ## MotrixLab 是什么? -**MotrixLab** 是一个开源的机器人强化学习训练框架,构建于高性能的 [MotrixSim](https://github.com/Motphys/motrixsim-docs) 物理引擎之上。环境只需定义一次,即可通过 SKRL、RSLRL 或内置的 FastSAC 使用数千个并行环境实例进行训练,训练得到的策略可部署到 MuJoCo 或 Unitree 硬件 —— 全程只需一个命令行接口。 +**MotrixLab** 是一个开源的机器人强化学习训练框架,构建于高性能的 [MotrixSim](https://github.com/Motphys/motrixsim-docs) 物理引擎之上。环境只需定义一次,即可通过 SKRL、RSLRL 或内置的 FastSAC 使用数千个并行环境实例进行训练,训练得到的策略可部署到 MuJoCo、原生 MotrixSim 或 Unitree 硬件 —— 全程只需一个命令行接口。
@@ -46,7 +46,7 @@ _使用 MotrixLab 训练的 microduck 行走策略,由 MotrixRender 实时渲 - **统一接口**: 提供简洁统一的强化学习训练和评估接口 - **多框架支持**: 支持 SKRL(JAX/PyTorch)、RSLRL(PyTorch)和内置 FastSAC 实现 - **丰富环境**: 包含基础控制、运动、操作等多种机器人仿真环境 -- **Sim-to-Real 部署**: 同一套策略代码,通过部署 CLI Sim2Sim 到 MuJoCo 仿真、Sim2Real 到真实硬件 +- **Sim-to-Real 部署**: 同一套策略代码,通过部署 CLI Sim2Sim 到 MuJoCo 或原生 MotrixSim 仿真、Sim2Real 到真实硬件 - **高精度高性能仿真**: 基于 [MotrixSim](https://motrixsim.readthedocs.io/) 高精度、高性能物理仿真引擎 - **可视化训练**: 支持实时渲染和训练过程可视化 @@ -180,6 +180,7 @@ MotrixLab 是一个 [uv](https://docs.astral.sh/uv/) workspace,包含以下 pa | --- | --- | --- | | **motrix_deploy** | `motrix-deploy` | artifact、控制会话、运行时契约与部署 CLI | | **motrix_deploy_mujoco** | `motrix-deploy-mujoco` | MuJoCo 部署 backend plugin | +| **motrix_deploy_motrixsim** | `motrix-deploy-motrixsim` | 原生 MotrixSim 部署 backend plugin | | **motrix_deploy_unitree** | `motrix-deploy-unitree` | Unitree SDK2 DDS 硬件 backend plugin | | **motrix_deploy_tasks** | `motrix-deploy-tasks` | 行走任务、部署场景与安装的 Hydra 配方 | | **motrix_env_core** | `motrix-env-core` | backend 无关的环境框架、配置、registry 与渲染 | 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 9e1c7aa1..f61676c7 100644 --- a/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md +++ b/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md @@ -38,7 +38,7 @@ runtime plugin for the target. The recipes work outside the repository without a 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 +or `motrixsim` for simulation, or `unitree_go2` for hardware). `runtime.backend` is a scalar string selector, not a nested configuration mapping or Hydra group. ## 3. Check in MuJoCo @@ -72,6 +72,21 @@ It creates a MuJoCo runtime with `create_simulation_runtime`, attaches a `Contro 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. +### Select native MotrixSim + +With `motrix-deploy-motrixsim` installed, switch the backend to `motrixsim` to reuse the same artifact and scene recipe: + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.backend=motrixsim runtime.viewer=true +``` + +The Python example also supports native MotrixSim: + +```bash +python examples/deploy_to_sim.py --backend motrixsim +``` + ## 4. Run on the real robot Suspend the robot, put it in low-level/debug mode, connect Ethernet, and keep an independent emergency stop and operator 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 96258a1b..23374b39 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 @@ -34,7 +34,7 @@ artifact 包含 ONNX 策略、机器人与控制设置,以及任务规格。 部署场景和 Hydra 配方,以及目标对应的运行时插件。配方安装在插件中,在仓库外运行也不需要 workspace 配置路径。 任务 defaults 选择 Hydra 运行时组(`sim` 或 `hardware`),无需另行覆盖运行时组。 顶层公共设置为 `artifact`、`duration_s` 和 `command`。所有目标专用设置直接位于 `runtime` 下; -`runtime.kind` 选择仿真或硬件执行路径;`runtime.backend` 选择插件(仿真为 `mujoco`,硬件为 `unitree_go2`)。 +`runtime.kind` 选择仿真或硬件执行路径;`runtime.backend` 选择插件(仿真为 `mujoco` 或 `motrixsim`,硬件为 `unitree_go2`)。 `runtime.backend` 是标量字符串选择器,不是嵌套配置映射或 Hydra 配置组。 ## 3. 在 MuJoCo 中检查 @@ -67,6 +67,21 @@ python examples/deploy_to_sim.py --headless --steps 100 `ControlSession`,并在 `with runtime:` 中运行。控制会话负责输入、观察、推理和机器人指令;运行时负责物理推进、 调度和 viewer。直接使用 ONNX 文件时需要显式指定任务预处理和动作设置;artifact 则为 CLI 携带这些设置。 +### 选择原生 MotrixSim + +安装 `motrix-deploy-motrixsim` 后,将 backend 切换为 `motrixsim`,即可复用同一 artifact 和场景配方: + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.backend=motrixsim runtime.viewer=true +``` + +Python 示例同样支持原生 MotrixSim: + +```bash +python examples/deploy_to_sim.py --backend motrixsim +``` + ## 4. 运行真机 将机器人悬空,切换到低层/调试模式,连接网线,并准备好独立急停和操作人员。启动过程可能在遥控器 Start 门控前 diff --git a/examples/deploy_to_sim.py b/examples/deploy_to_sim.py index d35ec993..4ad990ca 100644 --- a/examples/deploy_to_sim.py +++ b/examples/deploy_to_sim.py @@ -35,7 +35,9 @@ class ExampleSceneObjsCfg(SceneObjsCfg): def main() -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--backend", choices=("mujoco",), default="mujoco", help="Simulation runtime plugin") + parser.add_argument( + "--backend", choices=("mujoco", "motrixsim"), 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() @@ -65,7 +67,7 @@ def main() -> int: haze=(0.1, 0.1, 0.1, 1.0), tone_mapping="none", ), - system_camera=SystemCameraCfg(distance=3.0, elevation=-20.0, azimuth=90.0), + system_camera=SystemCameraCfg(lookat=(0.0, 0.0, 0.3), distance=3.0, elevation=-20.0, azimuth=90.0), ) config = SimulationRuntimeConfig( scene=scene, diff --git a/motrix_deploy/src/motrix_deploy/contracts.py b/motrix_deploy/src/motrix_deploy/contracts.py index 6f4519f0..708a836f 100644 --- a/motrix_deploy/src/motrix_deploy/contracts.py +++ b/motrix_deploy/src/motrix_deploy/contracts.py @@ -257,8 +257,6 @@ class RobotCapabilities: Attributes: 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. @@ -273,7 +271,6 @@ class RobotCapabilities: 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") @@ -288,15 +285,6 @@ def __post_init__(self) -> None: 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: diff --git a/motrix_deploy/tests/test_contracts.py b/motrix_deploy/tests/test_contracts.py index 6bc99a87..e73a6eba 100644 --- a/motrix_deploy/tests/test_contracts.py +++ b/motrix_deploy/tests/test_contracts.py @@ -131,31 +131,6 @@ 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") diff --git a/motrix_deploy/tests/test_runtime.py b/motrix_deploy/tests/test_runtime.py index 1ce8d22d..289ce139 100644 --- a/motrix_deploy/tests/test_runtime.py +++ b/motrix_deploy/tests/test_runtime.py @@ -696,26 +696,6 @@ def fail(state: RobotState) -> str | None: 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: diff --git a/motrix_deploy_motrixsim/README.md b/motrix_deploy_motrixsim/README.md new file mode 100644 index 00000000..1812ccd9 --- /dev/null +++ b/motrix_deploy_motrixsim/README.md @@ -0,0 +1,127 @@ +# motrix-deploy-motrixsim + +`motrix-deploy-motrixsim` provides the native MotrixSim deployment runtime, registered +as `motrixsim` in the `motrix_deploy.backends` entry-point group. It uses +`MotrixSimSceneCompiler`, the native MSD world/model/data APIs, query/write programs, +and the SDK `RenderApp`; it does not run physics through MuJoCo or a training environment. + +## Run the shared example + +From an installed workspace environment: + +```bash +python examples/deploy_to_sim.py --backend motrixsim --headless --steps 50 +python examples/deploy_to_sim.py --backend motrixsim +``` + +See [the shared simulation example](../examples/deploy_to_sim.py) for the complete +raw ONNX walking controller with explicit task preprocessing and control settings. +The example uses the bundled Go2 policy. For another matching policy, adapt the +policy path in the Python example; this programmatic path does not read a `.deploy` manifest. MuJoCo remains that script's default backend. +Artifact deployment reuses the installed simulation recipes rather than introducing +backend-specific task copies: + +```bash +motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ + runtime.backend=motrixsim runtime.viewer=false runtime.realtime=false duration_s=1 +``` + +The same override applies to `task=go2-walk-rough/sim`. Install this backend package +explicitly for standalone use, alongside `motrix-deploy-tasks` and the robot models. +MotrixSim is supplied by `motrix-env-motrixsim`, using the workspace's pinned SDK version. + +## Explicit runtime and controller assembly + +`create_simulation_runtime("motrixsim", config)` accepts a complete `SimulationRuntimeConfig`. +The factory returns a prepared host with `runtime.robot` and its read-only `spec` +available before opening. Construct the task, policy, command binding, and control +session in the application: + +```python +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.factory import create_simulation_runtime + +runtime = create_simulation_runtime("motrixsim", config) +robot = runtime.robot +control = ControlSession( + robot=robot, + task=task, + policy=policy, + command_binding=command_binding, + period_s=0.02, +) +runtime.bind_control_session(control) +with runtime: + result = runtime.run(steps=50) +``` + +Here `config` contains the complete scene, physics, state sources, and rendering +settings; `task`, `policy`, and `command_binding` are application-owned components. +A robot-dependent task can be built from `robot.spec` after creation. Runtime factories +do not construct controllers through callbacks. Direct callers can also pass a +session to `runtime.run(control, steps=...)`. + +`ControlSession.period_s` is the single control-period setting. Binding or resolving +the session validates that it is an integer multiple of `config.physics.dt`. +The runtime owns the compiled MSD world, model, data, optional viewer, and physics +advancement. Robot ports delegate I/O only: `open()` takes no spec, writes cache targets, +and neither reads nor writes advance physics or close the world. + +## Model, placement, and state contract + +The primary `scene.objs.robot` must contain a floating-base `RobotCfg` with canonical +key-pose joints and one unit-gear actuator per joint. Position actuators are converted +to torque motors; native torque motors are supported too. Physical joint and actuator +limits determine the scene-derived `RobotSpec`; controller gains are not model defaults. +The runtime does not accept an external `RobotSpec`. Artifact deployment uses the same +runtime; the CLI validates the artifact's robot contract against `runtime.robot.spec`. + +Robot `translation` and xyzw `rotation` remain attachment transforms composed once +with the asset's intrinsic base pose. Reset allocates fresh native data, restores the +compiled initial placement, applies the selected joint key pose, and clears velocity +and cached controls. MotrixSim uses xyzw quaternion ordering natively. + +State sources are explicit model-local names, resolved through the scene robot's +`resolve_name()` so instance prefixes/suffixes are preserved: + +```yaml +sensor_bindings: + base_angular_velocity: gyro + base_linear_acceleration: accelerometer + base_linear_velocity: global_linvel +``` + +For Go2 these names select local `FrameAngVel`, local `FrameLinAcc`, and world +`FrameLinVel` sensors. IMU sites must belong to the base body and align with its body +frame; the velocity source must target the base link or an aligned base-body site. +Missing or incompatible sources fail rather than fabricating state. Base position and +linear velocity come from the simulation; tasks check optional fields +at their usage sites. Public state reads own snapshots of native query buffers. + +## Control timing and viewer + +Each control tick uses shared `ControlSession.start/tick/stop` logic. Every physics +substep recomputes servo torque from current joint state: + +```text +tau = kp * (q_target - q) + kd * (dq_target - dq) + tau_ff +``` + +The result is clipped to `RobotSpec.torque_limit`. Direct torque commands bypass PD +but retain limit checks. Stop clears cached commands and writes zero torque; port +closure does not close the runtime-owned world. + +State sample timestamps use simulation time, while receive timestamps and realtime +pacing use the monotonic clock. Headless runs do not pace unless requested. Rendering +uses SDK `RenderApp`, synchronizes native data, and exposes a viewer-owned keyboard +device for the deployment CLI. Closing the viewer interrupts control; leaving the +runtime context closes the viewer and releases model/data. Native interactive rendering +requires a working graphical environment; headless execution can validate control +without opening a window. Keyboard bindings can be assembled before opening the +runtime: `runtime.get_keyboard_device()` returns the viewer's stable keyboard device, +which binds to SDK input when the viewer opens and detaches when it closes. + +With MotrixSim SDK `0.10.1`, closing and recreating an interactive viewer in the same +process is unsupported: the native window event loop raises `RecreationAttempt`. +Use a new process for another interactive viewer. Headless runtime close/reopen and +repeated resets are supported and covered by native tests. diff --git a/motrix_deploy_motrixsim/pyproject.toml b/motrix_deploy_motrixsim/pyproject.toml new file mode 100644 index 00000000..1e006bc1 --- /dev/null +++ b/motrix_deploy_motrixsim/pyproject.toml @@ -0,0 +1,20 @@ +[build-system] +requires = ["uv_build>=0.11.23,<0.12"] +build-backend = "uv_build" + +[project] +name = "motrix-deploy-motrixsim" +version = "0.4.0b0" +description = "Native SceneCfg-backed MotrixSim deployment runtime plugin." +authors = [{ name = "Motphys", email = "developers@motphys.com" }] +requires-python = "==3.10.*" +readme = "README.md" +license = "Apache-2.0" +dependencies = [ + "motrix-deploy", + "motrix-env-motrixsim", + "numpy>=1.26", +] + +[project.entry-points."motrix_deploy.backends"] +motrixsim = "motrix_deploy_motrixsim.plugin:create_runtime" diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/__init__.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/__init__.py new file mode 100644 index 00000000..b8fb7856 --- /dev/null +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/__init__.py @@ -0,0 +1,4 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Native MotrixSim deployment plugin, discovered via motrix_deploy.backends.""" diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/plugin.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/plugin.py new file mode 100644 index 00000000..cfeab179 --- /dev/null +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/plugin.py @@ -0,0 +1,12 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Installed entry-point factory for native MotrixSim simulation owners.""" + +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_deploy_motrixsim.runtime import MotrixSimRuntime + + +def create_runtime(config: SimulationRuntimeConfig) -> MotrixSimRuntime: + """Create a simulation host from the shared, assembled scene recipe.""" + return MotrixSimRuntime(config) diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py new file mode 100644 index 00000000..0a6c72cb --- /dev/null +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py @@ -0,0 +1,275 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Scene-derived robot I/O without simulation lifecycle ownership.""" + +from __future__ import annotations + +import time +from typing import TYPE_CHECKING + +import motrixsim as mtx +import numpy as np + +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_env_core.config.scene.base import RobotCfg + +if TYPE_CHECKING: + from motrix_deploy_motrixsim.runtime import MotrixSimRuntime + + +class Robot(RobotInterface): + """Bind canonical I/O to a compiled scene without owning or stepping it. + + The runtime converts actuators before construction, and calls ``bind`` on + every open (including reopen with newly allocated model/data). It invokes + ``apply_control`` before *every* physics step, not just each policy tick. + """ + + def __init__(self, runtime: MotrixSimRuntime, spec: RobotSpec, actuator_names: tuple[str, ...]) -> None: + self._runtime = runtime + self._spec = spec + self._robot: RobotCfg = runtime.scene.objs.robot + self._command: RobotCommand | None = None + self._opened = False + self._closed = False + self._last_communication_ns = 0 + self._health_reason = "simulation is not open" + self._read: mtx.query.QueryProgram | None = None + self._joint_read: mtx.query.QueryProgram | None = None + self._write: mtx.write.WriteProgram | None = None + self._joint_names = tuple(self._robot.resolve_name(name) for name in spec.joint_names) + self._sensor_names: dict[str, str] = {} + self._actuator_names = actuator_names + self._joint_dof_pos_indices = np.empty(0, dtype=np.intp) + 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", + } + ), + supports_rendering=True, + max_command_rate_hz=1.0 / runtime.config.physics.dt, + stop_semantics="zero_torque", + ) + + @property + def spec(self) -> RobotSpec: + return self._spec + + @property + def capabilities(self) -> RobotCapabilities: + return self._capabilities + + @property + def joint_dof_pos_indices(self) -> np.ndarray: + """Canonical joint position addresses in the currently bound model.""" + return self._joint_dof_pos_indices + + def _bind_model(self) -> None: + """Bind final-model addresses; the runtime validated the source model.""" + model = self._runtime.model + self._joint_dof_pos_indices = np.asarray( + [model.get_joint(name).dof_pos_index for name in self._joint_names], dtype=np.intp + ) + self._validate_sensors() + + def _validate_sensors(self) -> None: + sensors = {sensor.name: sensor for sensor in self._runtime.world.sensors.frame} + self._sensor_names.clear() + for role, sensor_type, reference in ( + ("base_angular_velocity", mtx.msd.FrameSensorType.FrameAngVel, "local"), + ("base_linear_acceleration", mtx.msd.FrameSensorType.FrameLinAcc, "local"), + ("base_linear_velocity", mtx.msd.FrameSensorType.FrameLinVel, "world"), + ): + path = f"backend.sensor_bindings.{role}" + local_name = getattr(self._runtime.config.sensor_bindings, role) + if local_name is None: + raise ValidationError(path, "an explicit model-local sensor name", local_name) + name = self._robot.resolve_name(local_name) + sensor = sensors.get(name) + if sensor is None or sensor.sensor_type != sensor_type or sensor.ref_frame.variant != reference: + raise ValidationError(path, f"{sensor_type} in the {reference} frame", name) + target = sensor.object_type + if reference == "local" or target.variant == "site": + site = self._runtime.model.get_site(target.value) if target.variant == "site" else None + if ( + site is None + or site.parent_link is None + or site.parent_link.name != self._robot.resolved_base_link_name + or not np.allclose(np.asarray(site.local_quat).reshape(4)[:3], 0, atol=1e-7, rtol=0) + ): + raise ValidationError(path, "a base-body site aligned with the body frame", name) + elif target.variant != "link" or target.value != self._robot.resolved_base_link_name: + raise ValidationError(path, "the base link or an aligned base-body site", name) + self._sensor_names[role] = name + + def bind(self) -> None: + """Rebuild model-bound programs, then reset through the owning runtime.""" + self._bind_model() + self._runtime.reset() + self.refresh_reads() + self._command = None + assert self._write is not None + self._write["torque"][:] = 0 + self._write.execute(self._runtime.data) + self._opened = True + self._closed = False + self._health_reason = "" + self._last_communication_ns = time.monotonic_ns() + + def refresh_reads(self) -> None: + """Reallocate query/write programs after runtime-owned data replacement.""" + model, data = self._runtime.model, self._runtime.data + joint_queries = { + "joint_position": mtx.query.JointPosition(self._joint_names), + "joint_velocity": mtx.query.JointVelocity(self._joint_names), + } + self._joint_read = model.compile_query(joint_queries).allocate(data) + queries = { + **joint_queries, + "base_position": mtx.query.LinkPosition((self._robot.resolved_base_link_name,)), + "base_orientation_xyzw": mtx.query.LinkRotation((self._robot.resolved_base_link_name,)), + } + queries.update({role: mtx.query.SensorValues((name,)) for role, name in self._sensor_names.items()}) + self._read = model.compile_query(queries).allocate(data) + self._write = model.compile_write({"torque": mtx.write.ActuatorCtrls(self._actuator_names)}).allocate(data) + + def open(self) -> None: + """Check activation without opening the runtime-owned simulation.""" + self._require_open() + + def read_state(self, timeout_s: float) -> RobotState: + self._require_open() + if not np.isfinite(timeout_s) or timeout_s <= 0: + raise TimeoutError(f"MotrixSim state timeout must be positive and finite, got {timeout_s}") + assert self._read is not None + self._read.execute(self._runtime.data) + self._last_communication_ns = time.monotonic_ns() + # Native programs reuse their buffers; public samples must own snapshots. + # RobotState validates the canonical shapes, dtype and finite values. + fields = { + name: np.array(self._read[name].reshape(-1), dtype=np.float32, copy=True) + for name in ( + "joint_position", + "joint_velocity", + "base_position", + "base_orientation_xyzw", + "base_angular_velocity", + "base_linear_acceleration", + "base_linear_velocity", + ) + } + return RobotState( + sample_time_ns=round(self._runtime.simulation_time_s * 1e9), + receive_time_ns=self._last_communication_ns, + **fields, + ) + + def write_command(self, command: RobotCommand) -> None: + self._require_open() + shape = (self._spec.joint_count,) + if isinstance(command, JointServoCommand): + snapshot = JointServoCommand( + joint_position=command.joint_position.copy(), + joint_velocity=command.joint_velocity.copy(), + feedforward_torque=command.feedforward_torque.copy(), + kp=command.kp.copy(), + kd=command.kd.copy(), + ) + if snapshot.joint_position.shape != shape: + raise ValidationError("command.joint_position.shape", shape, snapshot.joint_position.shape) + if np.any(snapshot.joint_position < self._spec.position_lower) or np.any( + snapshot.joint_position > self._spec.position_upper + ): + raise ValidationError( + "command.joint_position", "inside RobotSpec position range", snapshot.joint_position + ) + if np.any(np.abs(snapshot.feedforward_torque) > self._spec.torque_limit): + raise ValidationError( + "command.feedforward_torque", "inside RobotSpec torque limits", snapshot.feedforward_torque + ) + self._command = snapshot + elif isinstance(command, JointTorqueCommand): + snapshot = JointTorqueCommand(torque=command.torque.copy()) + if snapshot.torque.shape != shape: + raise ValidationError("command.torque.shape", shape, snapshot.torque.shape) + if np.any(np.abs(snapshot.torque) > self._spec.torque_limit): + raise ValidationError("command.torque", "inside RobotSpec torque limits", snapshot.torque) + self._command = snapshot + else: + raise ValidationError("command", "JointServoCommand or JointTorqueCommand", type(command).__name__) + self._last_communication_ns = time.monotonic_ns() + + def apply_control(self) -> None: + """Recompute PD + feedforward from current physics state and clip torque.""" + if not self._opened or self._closed: + return + assert self._joint_read is not None and self._write is not None + command = self._command + if command is None: + torque = 0.0 + elif isinstance(command, JointTorqueCommand): + torque = command.torque + else: + self._joint_read.execute(self._runtime.data) + position = self._joint_read["joint_position"].reshape(-1) + velocity = self._joint_read["joint_velocity"].reshape(-1) + torque = ( + command.kp * (command.joint_position - position) + + command.kd * (command.joint_velocity - velocity) + + command.feedforward_torque + ) + self._write["torque"][:] = np.clip(torque, -self._spec.torque_limit, self._spec.torque_limit) + self._write.execute(self._runtime.data) + + def health(self) -> HealthStatus: + healthy = self._opened and not self._closed and self._runtime.opened and not self._health_reason + return HealthStatus( + healthy=healthy, + reason=self._health_reason if healthy or self._health_reason else "simulation is not open", + last_successful_communication_ns=self._last_communication_ns, + ) + + def stop(self) -> None: + self._command = None + if self._opened and not self._closed and self._write is not None: + self._write["torque"][:] = 0 + self._write.execute(self._runtime.data) + + def close(self) -> None: + """Detach I/O only; the simulation, renderer and clock remain runtime-owned.""" + self.stop() + self._closed = True + self._health_reason = "robot port is closed" + + def invalidate(self) -> None: + self._command = None + self._read = None + self._joint_read = None + self._write = None + self._opened = False + self._closed = True + self._health_reason = "simulation is not open" + + def _require_open(self) -> None: + if not self._opened or self._closed or not self._runtime.opened: + raise RuntimeError("MotrixSim robot port is not open") diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py new file mode 100644 index 00000000..b9864ac3 --- /dev/null +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py @@ -0,0 +1,264 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Native world owner: scene compilation, lifecycle and physics advancement.""" + +from copy import deepcopy +from dataclasses import replace +from typing import TYPE_CHECKING + +import motrixsim as mtx +import numpy as np + +from motrix_deploy.contracts import RobotSpec +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.base import SimulationRuntime +from motrix_deploy.runtime.config import SimulationRuntimeConfig +from motrix_deploy.runtime.scheduler import FixedStepScheduler, RealtimeScheduler +from motrix_deploy_motrixsim.robot import Robot +from motrix_deploy_motrixsim.viewer import MotrixSimViewer +from motrix_env_core.config.scene.base import RobotCfg +from motrix_env_motrixsim.compiler import MotrixSimSceneCompiler + +if TYPE_CHECKING: + from motrix_deploy.runtime.control import ControlSession + from motrix_deploy.runtime.result import RolloutResult + + +class MotrixSimRuntime(SimulationRuntime): + """Own the native world; expose robot I/O before session assembly.""" + + def __init__(self, config: SimulationRuntimeConfig, *, viewer_factory=None) -> None: + super().__init__() + self.config = config + self.scene = deepcopy(config.scene) + robot = self.scene.objs.robot + if not isinstance(robot, RobotCfg): + raise ValidationError("backend.scene.objs.robot", "a RobotCfg", robot) + compiler = MotrixSimSceneCompiler() + self.world = compiler.build_world(self.scene) + compiler.configure_world(self.world, config.physics) + source_model = self.world.build() + derived, actuator_names = self._derive_spec(source_model, robot) + self._prepare_motors(source_model, derived, actuator_names) + self.model = self.world.build() + self.data: mtx.SceneData | None = None + self.simulation_time_s = 0.0 + self.physics_substeps: int | None = None + self.realtime = config.realtime + factory = viewer_factory or MotrixSimViewer + self.viewer = factory(self.scene.system_camera) if config.render else None + self._robot = Robot(self, derived, actuator_names) + + @property + def robot(self): + return self._robot + + @staticmethod + def _derive_spec(model: mtx.SceneModel, robot: RobotCfg) -> tuple[RobotSpec, tuple[str, ...]]: + names = tuple(robot.key_pose.joint_names) + lower, upper, torque = [], [], [] + actuator_names = [] + base = model.get_body(robot.resolved_base_link_name) + if base is None or base.floatingbase is None: + raise ValidationError( + "backend.robot.base_link_name", "a floating-base robot", robot.resolved_base_link_name + ) + for name in names: + resolved_name = robot.resolve_name(name) + joint = model.get_joint(resolved_name) + if joint is None or joint.num_dof_pos != 1 or joint.num_dof_vel != 1: + raise ValidationError(f"backend.joints.{name}", "an existing one-DoF joint", joint) + matches = [ + actuator + for actuator in model.actuators + if actuator.target_type == "joint" and actuator.target_name == resolved_name + ] + if len(matches) != 1: + raise ValidationError(f"backend.actuators.{name}", "exactly one joint actuator", len(matches)) + actuator = matches[0] + if not isinstance(actuator, (mtx.PositionActuator, mtx.MotorActuator)): + raise ValidationError(f"backend.actuators.{name}", "a position servo or torque motor", actuator.typ) + limits = actuator.ctrl_range if isinstance(actuator, mtx.PositionActuator) else joint.range + limits = np.asarray(limits, dtype=np.float32).reshape(-1) + forces = np.asarray(actuator.force_range, dtype=np.float32).reshape(-1) + if limits.shape != (2,) or not np.isfinite(limits).all() or limits[0] >= limits[1]: + raise ValidationError(f"backend.actuators.{name}.position_range", "finite increasing limits", limits) + if ( + forces.shape != (2,) + or not np.isfinite(forces).all() + or forces[1] <= 0 + or not np.isclose(forces[0], -forces[1]) + ): + raise ValidationError(f"backend.actuators.{name}.force_range", "finite symmetric torque limits", forces) + physical_range = np.asarray(joint.range).reshape(2) + if limits[0] < physical_range[0] - 1e-6 or limits[1] > physical_range[1] + 1e-6: + raise ValidationError( + f"backend.joints.{name}.range", "control limits inside physical joint range", limits + ) + actuator_names.append(actuator.name) + lower.append(limits[0]) + upper.append(limits[1]) + torque.append(forces[1]) + spec = RobotSpec( + base_link_name=robot.base_link_name, + joint_names=names, + default_joint_position=np.asarray(robot.key_pose.poses[robot.init_key_pose], dtype=np.float32), + position_lower=np.asarray(lower, dtype=np.float32), + position_upper=np.asarray(upper, dtype=np.float32), + torque_limit=np.asarray(torque, dtype=np.float32), + ) + return spec, tuple(actuator_names) + + def _prepare_motors(self, source_model: mtx.SceneModel, spec: RobotSpec, actuator_names: tuple[str, ...]) -> None: + selected = dict(zip(actuator_names, spec.torque_limit)) + for actuator in self.world.actuators: + if actuator.name not in selected: + continue + if actuator.target.variant != "joint" or not np.allclose(actuator.gear, [1, 0, 0, 0, 0, 0]): + raise ValidationError(f"backend.actuators.{actuator.name}", "unit-gear joint transmission", actuator) + if actuator.actuator_type.variant not in ("position", "motor"): + raise ValidationError(f"backend.actuators.{actuator.name}", "position or motor", actuator.actuator_type) + limit = selected[actuator.name] + native = source_model.get_actuator(actuator.name) + if isinstance(native, mtx.MotorActuator) and ( + native.ctrl_range is None or not np.allclose(native.ctrl_range, [-limit, limit], atol=1e-6, rtol=0) + ): + raise ValidationError( + f"backend.actuators.{actuator.name}.ctrl_range", [-limit, limit], native.ctrl_range + ) + if isinstance(native, mtx.PositionActuator) and ( + not np.isfinite(native.kp) + or native.kp < 0 + or native.kd is None + or not np.isfinite(native.kd) + or native.kd < 0 + ): + raise ValidationError( + f"backend.actuators.{actuator.name}.gains", "finite non-negative kp/kd", (native.kp, native.kd) + ) + actuator.actuator_type = mtx.msd.ActuatorType.motor() + actuator.ctrlrange = mtx.msd.Range(-limit, limit) + actuator.forcerange = mtx.msd.Range(-limit, limit) + # Imported keyframe servo targets must not become motor torque defaults. + for keyframe in self.world.keyframes: + keyframe.ctrl = [0.0] * len(self.world.actuators) + + def bind_control_session(self, control: "ControlSession") -> None: + ratio = control.period_s / self.config.physics.dt + if not np.isfinite(ratio) or not np.isclose(ratio, round(ratio), atol=1e-9, rtol=0) or round(ratio) < 1: + raise ValidationError("control.period_s", "an integer multiple of physics.dt", control.period_s) + super().bind_control_session(control) + self.physics_substeps = round(ratio) + + def open(self) -> None: + if self._opened: + raise RuntimeError("MotrixSim simulation is already open") + if self.model is None: + self.model = self.world.build() + try: + self._robot.bind() + if self.viewer is not None: + self.viewer.open(self.model) + self.sync_viewer() + self._opened = True + except BaseException: + self.close() + raise + + def reset(self) -> None: + """Restore compiled placement without stale solver state or sensor reports.""" + if self.model is None: + raise RuntimeError("MotrixSim model is not available") + position = np.asarray(self.model.compute_init_dof_pos(), dtype=np.float32).copy() + position[self._robot.joint_dof_pos_indices] = self.robot.spec.default_joint_position + self.data = mtx.SceneData(self.model, batch=[1]) + self.data.reset( + self.model, + dof_pos=position, + dof_vel=np.zeros(self.model.num_dof_vel, dtype=np.float32), + forward_kinematic=True, + ) + self.simulation_time_s = 0.0 + if self._opened: + self._robot.refresh_reads() + self._robot.stop() + + def _step(self) -> None: + self._robot.apply_control() + self.model.step(self.data) + self.simulation_time_s += self.config.physics.dt + + def advance_control_period(self) -> None: + if not self._opened: + raise RuntimeError("MotrixSim simulation 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._step() + self.sync_viewer() + + def run( + self, control: "ControlSession | None" = None, *, steps: int | None = None, realtime: bool | None = None + ) -> "RolloutResult": + if not self._opened: + raise RuntimeError("MotrixSim simulation is not open") + control = self._resolve_control(control) + self.bind_control_session(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) + paced = self.realtime if realtime is None else realtime + if paced is None: + paced = self.viewer is not None + scheduler = (RealtimeScheduler if paced else FixedStepScheduler)(control.period_s) + initial_time_s = self.simulation_time_s + try: + if control.start(): + initial_time_s = self.simulation_time_s + scheduler.reset() + while control.active: + if steps is not None and control.completed_steps >= steps: + break + self.check_viewer_running() + if not control.tick(elapsed_time_s=self.simulation_time_s - initial_time_s): + break + self.advance_control_period() + scheduler.wait(control.completed_steps) + except (Exception, KeyboardInterrupt) as error: + control.fail(error) + finally: + control.stop() + result = control.result(steps=steps, overrun_count=scheduler.overrun_count) + elapsed = self.simulation_time_s - initial_time_s + return replace( + result, + simulation_time_s=elapsed, + real_time_factor=elapsed / result.wall_time_s if result.wall_time_s > 0 else 0.0, + ) + + def check_viewer_running(self) -> None: + if self.viewer is not None and not self.viewer.is_running(): + raise KeyboardInterrupt("MotrixSim viewer was closed") + + def sync_viewer(self) -> None: + if self.viewer is not None: + self.check_viewer_running() + self.viewer.sync(self.data) + + def get_keyboard_device(self): + if self.viewer is None: + raise RuntimeError("MotrixSim keyboard input requires render=true") + return self.viewer.keyboard_device + + def close(self) -> None: + try: + if self.control is not None and self.control.active: + self.control.stop() + if self.viewer is not None: + self.viewer.close() + finally: + self._robot.invalidate() + self.data = None + self.model = None + self._opened = False diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/viewer.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/viewer.py new file mode 100644 index 00000000..dfb1cf98 --- /dev/null +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/viewer.py @@ -0,0 +1,95 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Interactive lifecycle using the installed MotrixSim RenderApp API.""" + +from motrixsim.render import RenderApp + +from motrix_env_core.input import KeyboardDevice + + +class MotrixSimKeyboard(KeyboardDevice): + def __init__(self, input=None): + self._input = input + self._previous = {} + self._held = {} + self._down = {} + self._up = {} + + def bind(self, input) -> None: + self._input = input + self._previous.clear() + self._held.clear() + self._down.clear() + self._up.clear() + + def poll(self) -> None: + self._require_input() + self._previous = self._held + self._held = {} + self._down = {} + self._up = {} + for key in self._previous: + self._sample(key) + + def _require_input(self): + if self._input is None: + raise RuntimeError("Open the MotrixSim viewer before polling keyboard input") + return self._input + + def _sample(self, key): + input = self._require_input() + if key not in self._held: + held = input.is_key_pressed(key) + self._held[key] = held + self._down[key] = input.is_key_just_pressed(key) + self._up[key] = self._previous.get(key, False) and not held + + def is_key_down(self, key: str) -> bool: + self._sample(key) + return self._down[key] + + def is_key_up(self, key: str) -> bool: + self._sample(key) + return self._up[key] + + def is_pressing(self, key: str) -> bool: + self._sample(key) + return self._held[key] + + +class MotrixSimViewer: + def __init__(self, camera) -> None: + self._camera = camera + self._render = None + self.keyboard_device = MotrixSimKeyboard() + + def open(self, model) -> None: + self._render = RenderApp(headless=False) + try: + self._render.launch(model, batch=1) + camera = self._camera + self._render.system_camera.set_view( + list(camera.lookat) if camera.lookat is not None else [0.0, 0.0, 0.0], + camera.distance, + camera.elevation, + camera.azimuth, + ) + self.keyboard_device.bind(self._render.input) + except BaseException: + self.close() + raise + + def is_running(self) -> bool: + return self._render is not None and not self._render.is_closed + + def sync(self, data) -> None: + self._render.sync(data=data) + + def close(self) -> None: + try: + if self._render is not None: + self._render.close() + finally: + self._render = None + self.keyboard_device.bind(None) diff --git a/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py b/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py new file mode 100644 index 00000000..0929051b --- /dev/null +++ b/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py @@ -0,0 +1,414 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Real installed SDK coverage of native deployment contracts.""" + +from dataclasses import asdict, replace +from types import SimpleNamespace + +import motrixsim as mtx +import numpy as np +import pytest +from motrix_deploy_motrixsim.runtime import MotrixSimRuntime +from motrix_deploy_motrixsim.viewer import MotrixSimKeyboard, MotrixSimViewer +from motrix_robots.unitree import UnitreeGo2Robot + +from motrix_deploy.contracts import JointControlMode, JointServoCommand, JointTorqueCommand +from motrix_deploy.errors import ValidationError +from motrix_deploy.policy import NoOpPolicyRuntime +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.task import DeployTask +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg +from motrix_env_core.config.scene.actuator import MotorActuatorCfg +from motrix_env_core.config.sim import SimCfg +from motrix_env_core.input import ConstantPlanarVelocityBinding + + +@configclass +class NativeSceneObjs(SceneObjsCfg): + floor: FlatTerrainCfg | None = None + + +def config(**kwargs): + robot = UnitreeGo2Robot(translation=(0.2, -0.1, 0.4), rotation=(0.0, 0.0, np.sin(0.2), np.cos(0.2))) + return SimulationRuntimeConfig( + scene=SceneCfg(objs=NativeSceneObjs(robot=robot, floor=FlatTerrainCfg())), + physics=SimCfg(dt=0.002, solver_iterations=30), + sensor_bindings=SensorBindings( + base_angular_velocity="gyro", + base_linear_acceleration="accelerometer", + base_linear_velocity="global_linvel", + ), + **kwargs, + ) + + +class HoldTask(DeployTask): + def __init__(self, spec): + self.spec = spec + + def reset(self, state, context): + pass + + def validate_command(self, command): + pass + + def build_observation(self, state, context): + return np.zeros(self.spec.joint_count, dtype=np.float32) + + def process_action(self, action): + z = np.zeros(self.spec.joint_count, dtype=np.float32) + return JointServoCommand(self.spec.default_joint_position, z, z, z + 30, z + 1) + + +def session(runtime, period=0.006): + return ControlSession( + robot=runtime.robot, + task=HoldTask(runtime.robot.spec), + policy=NoOpPolicyRuntime(runtime.robot.spec.joint_count), + command_binding=ConstantPlanarVelocityBinding((0, 0, 0)), + period_s=period, + state_timeout_s=0.1, + ) + + +@pytest.mark.parametrize("gravity,tolerance", [((0.3, -0.2, -4.1), 2e-7), ((-0.1, 0.4, -6.3), 3e-6)]) +def test_native_factory_forwards_shared_physics_once_to_compiler(monkeypatch, gravity, tolerance): + from motrix_env_motrixsim.compiler import MotrixSimSceneCompiler + + cfg = config() + physics = SimCfg(dt=0.003, solver_iterations=17, solver_tolerance=tolerance, gravity=gravity) + cfg = replace(cfg, physics=physics) + calls = [] + original = MotrixSimSceneCompiler.configure_world + + def configure_world(compiler, world, sim): + calls.append(sim) + return original(compiler, world, sim) + + monkeypatch.setattr(MotrixSimSceneCompiler, "configure_world", configure_world) + runtime = create_simulation_runtime("motrixsim", cfg) + try: + assert runtime.config.physics is physics + assert calls == [physics] + assert calls[0] is physics + options = runtime.world.simulate_option + assert options.timestep == pytest.approx(physics.dt) + assert options.constraint_solver_iterations == physics.solver_iterations + assert options.constraint_solver_tolerance == pytest.approx(physics.solver_tolerance) + np.testing.assert_allclose(options.gravity, physics.gravity) + finally: + runtime.close() + + +def test_native_factory_session_cadence_and_owner(monkeypatch): + runtime = create_simulation_runtime("motrixsim", config()) + spec = runtime.robot.spec + assert spec.joint_names == tuple(runtime.scene.objs.robot.key_pose.joint_names) + assert runtime.robot.capabilities.control_modes == (JointControlMode.SERVO, JointControlMode.TORQUE) + assert runtime.robot.capabilities.max_command_rate_hz == 1 / runtime.config.physics.dt + with pytest.raises(RuntimeError): + runtime.robot.read_state(0.1) + control = session(runtime) + runtime.bind_control_session(control) + calls = [] + original = runtime.robot.apply_control + + def count_apply(): + calls.append(runtime.simulation_time_s) + original() + + monkeypatch.setattr(runtime.robot, "apply_control", count_apply) + with runtime: + result = runtime.run(steps=5) + assert result.success + assert result.completed_steps == 5 + assert result.simulation_time_s == pytest.approx(5 * control.period_s) + assert len(calls) == 5 * 3 + assert runtime.opened + runtime.robot.close() + assert runtime.opened and runtime.model is not None + runtime.advance_control_period() + assert runtime.model is None and runtime.data is None + + +def test_native_servo_feedforward_snapshots_and_clipping(): + runtime = MotrixSimRuntime(config()) + with runtime: + state = runtime.robot.read_state(0.1) + old_position = state.joint_position.copy() + count = runtime.robot.spec.joint_count + zeros = np.zeros(count, dtype=np.float32) + target = state.joint_position.copy() + 0.01 + feedforward = zeros + 0.5 + command = JointServoCommand(target, zeros, feedforward, zeros + 10, zeros + 1) + runtime.robot.write_command(command) + command.joint_position[:] += 0.1 + runtime.robot.apply_control() + expected = np.full(count, 0.6) + actual = np.asarray(runtime.model.get_actuator_ctrls(runtime.data)).reshape(-1) + np.testing.assert_allclose(actual, expected, atol=1e-5) + runtime._step() + runtime.robot.apply_control() + current = runtime.robot.read_state(0.1) + expected = 10 * (target - 0.1 - current.joint_position) - current.joint_velocity + 0.5 + expected = np.clip(expected, -runtime.robot.spec.torque_limit, runtime.robot.spec.torque_limit) + np.testing.assert_allclose( + np.asarray(runtime.model.get_actuator_ctrls(runtime.data)).reshape(-1), expected, atol=1e-5 + ) + np.testing.assert_array_equal(state.joint_position, old_position) + runtime.robot.write_command( + JointServoCommand(runtime.robot.spec.position_upper, zeros, zeros, zeros + 1e6, zeros) + ) + runtime.robot.apply_control() + np.testing.assert_allclose( + np.asarray(runtime.model.get_actuator_ctrls(runtime.data)).reshape(-1), runtime.robot.spec.torque_limit + ) + runtime.robot.stop() + np.testing.assert_array_equal(runtime.model.get_actuator_ctrls(runtime.data), 0) + + +def test_native_servo_substeps_only_query_joint_state(monkeypatch): + runtime = MotrixSimRuntime(config()) + with runtime: + state = runtime.robot.read_state(0.1) + zeros = np.zeros(runtime.robot.spec.joint_count, dtype=np.float32) + runtime.robot.write_command(JointServoCommand(state.joint_position + 0.01, zeros, zeros, zeros + 10, zeros + 1)) + + def unexpected_full_state_query(data): + pytest.fail("Servo substeps must not query base state or sensors") + + monkeypatch.setattr(runtime.robot, "_read", SimpleNamespace(execute=unexpected_full_state_query)) + runtime._step() + assert runtime.simulation_time_s == pytest.approx(runtime.config.physics.dt) + + +def test_native_repeated_reset_preserves_compiled_placement_and_sensor_state(): + runtime = MotrixSimRuntime(config()) + with runtime: + initial = runtime.robot.read_state(0.1) + assert initial.base_position[0] == pytest.approx(0.2) + assert initial.base_position[1] == pytest.approx(-0.1) + assert initial.base_position[2] > 0.4 # placement composes with imported root, never replaces it + for _ in range(2): + runtime.robot.write_command(JointTorqueCommand(np.ones(runtime.robot.spec.joint_count, dtype=np.float32))) + for _ in range(5): + runtime._step() + assert runtime.robot.read_state(0.1).sample_time_ns == 10_000_000 + runtime.reset() + reset = runtime.robot.read_state(0.1) + assert reset.sample_time_ns == 0 + for field in ( + "base_position", + "base_orientation_xyzw", + "joint_position", + "joint_velocity", + "base_angular_velocity", + "base_linear_acceleration", + "base_linear_velocity", + ): + np.testing.assert_allclose(getattr(reset, field), getattr(initial, field), atol=1e-6) + runtime.robot.write_command(JointTorqueCommand(np.zeros(runtime.robot.spec.joint_count, dtype=np.float32))) + with runtime: + np.testing.assert_allclose(runtime.robot.read_state(0.1).base_position, initial.base_position) + + +def test_native_sensor_bindings_are_actual_live_native_values(): + runtime = MotrixSimRuntime(config()) + with runtime: + q = runtime.data.dof_pos.copy() + v = runtime.data.dof_vel.copy() + base = runtime.model.floating_bases[0] + v[0, base.dof_vel_indices] = [0.7, 0.4, 0.2, 0.1, 0.2, 0.3] + runtime.data.reset(runtime.model, dof_pos=q, dof_vel=v, forward_kinematic=True) + runtime._step() + state = runtime.robot.read_state(0.1) + for role in ("base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"): + name = getattr(runtime.config.sensor_bindings, role) + expected = runtime.model.get_sensor_value(name, runtime.data).reshape(3) + np.testing.assert_allclose(getattr(state, role), expected, atol=1e-6) + assert np.linalg.norm(state.base_linear_velocity) > 0.1 + assert np.linalg.norm(state.base_angular_velocity) > 0.1 + assert np.isfinite(state.base_linear_acceleration).all() + + +def test_native_instance_names_preserve_artifact_local_robot_contract(): + cfg = config() + cfg.scene.objs.robot.prefix = "deployed_" + cfg.scene.objs.robot.suffix = "_robot" + with MotrixSimRuntime(cfg) as runtime: + assert runtime.robot.spec.joint_names == tuple(cfg.scene.objs.robot.key_pose.joint_names) + assert runtime.robot.spec.base_link_name == cfg.scene.objs.robot.base_link_name + np.testing.assert_allclose( + runtime.robot.read_state(0.1).joint_position, runtime.robot.spec.default_joint_position, atol=1e-6 + ) + runtime.bind_control_session(session(runtime)) + assert runtime.run(steps=2).success + + +def test_native_motor_source_supported(): + cfg = config() + robot = cfg.scene.objs.robot + names = robot.key_pose.joint_names + source = MotrixSimRuntime(cfg) + limits = source.robot.spec.torque_limit + robot.model.actuators = { + name: MotorActuatorCfg( + joint_name=name, ctrl_range=(-float(limit), float(limit)), force_range=(-float(limit), float(limit)) + ) + for name, limit in zip(names, limits) + } + runtime = MotrixSimRuntime(cfg) + assert all(isinstance(actuator, mtx.MotorActuator) for actuator in runtime.model.actuators) + with runtime: + runtime.robot.write_command(JointTorqueCommand(np.ones(len(names), dtype=np.float32))) + runtime._step() + assert runtime.robot.read_state(0.1).sample_time_ns == 2_000_000 + + +@pytest.mark.parametrize("period", [0.001, 0.003]) +def test_native_session_period_requires_whole_physics_steps(period): + runtime = MotrixSimRuntime(config()) + with pytest.raises(ValidationError, match="integer multiple"): + runtime.bind_control_session(session(runtime, period)) + runtime.close() + + +@pytest.mark.parametrize( + "role,name", + [ + ("base_angular_velocity", "missing"), + ("base_angular_velocity", "global_angvel"), + ("base_linear_acceleration", "gyro"), + ("base_linear_velocity", "local_linvel"), + ], +) +def test_native_explicit_sensor_semantics(role, name): + cfg = config() + cfg = replace(cfg, sensor_bindings=replace(cfg.sensor_bindings, **{role: name})) + with pytest.raises(ValidationError, match="sensor_bindings"): + with MotrixSimRuntime(cfg): + pass + + +@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"]) +def test_native_missing_sensor_fails_at_model_binding(role): + cfg = config() + cfg = replace(cfg, sensor_bindings=replace(cfg.sensor_bindings, **{role: None})) + with pytest.raises(ValidationError, match=f"backend.sensor_bindings.{role}"): + with MotrixSimRuntime(cfg): + pass + + +@pytest.mark.parametrize( + "bindings,message", + [ + ({"base_angular_velocity": " "}, "base_angular_velocity"), + ({"base_angular_velocity": 7}, "base_angular_velocity"), + ({"unexpected_role": "gyro"}, "unexpected_role"), + ], +) +def test_native_artifact_mapping_validates_sensor_fields(bindings, message): + cfg = config() + with pytest.raises((TypeError, ValueError), match=message): + SimulationRuntimeConfig.from_mapping( + {"scene": cfg.scene, "physics": {"dt": 0.002, "solver_iterations": 30}, "sensor_bindings": bindings} + ) + + +def test_native_artifact_mapping_same_contract(): + cfg = config() + runtime = create_simulation_runtime( + "motrixsim", + SimulationRuntimeConfig.from_mapping( + { + "scene": cfg.scene, + "physics": { + "dt": 0.002, + "solver_iterations": 30, + "solver_tolerance": 4e-7, + "gravity": (0.2, -0.3, -5.0), + }, + "sensor_bindings": asdict(cfg.sensor_bindings), + } + ), + ) + assert isinstance(runtime.config.physics, SimCfg) + assert runtime.world.simulate_option.constraint_solver_tolerance == pytest.approx(4e-7) + np.testing.assert_allclose(runtime.world.simulate_option.gravity, (0.2, -0.3, -5.0)) + runtime.bind_control_session(session(runtime)) + with runtime: + assert runtime.run(steps=2).success + + +@pytest.mark.parametrize("physics", [{}, {"dt": 0.004}, {"solver_iterations": None}]) +def test_native_artifact_mapping_requires_complete_physics_recipe(physics): + cfg = config() + with pytest.raises(ValidationError, match="dt and solver_iterations"): + SimulationRuntimeConfig.from_mapping({"scene": cfg.scene, "physics": physics}) + + +def test_native_artifact_mapping_rejects_unknown_physics_field(): + cfg = config() + with pytest.raises(TypeError, match="unexpected_physics_setting"): + SimulationRuntimeConfig.from_mapping( + {"scene": cfg.scene, "physics": {"dt": 0.002, "solver_iterations": 30, "unexpected_physics_setting": 1}} + ) + + +def test_sdk_viewer_lifecycle_and_keyboard(monkeypatch): + calls = [] + + class Render: + is_closed = False + input = SimpleNamespace(is_key_pressed=lambda key: False, is_key_just_pressed=lambda key: False) + system_camera = SimpleNamespace(set_view=lambda *args: calls.append("camera")) + + def __init__(self, **kwargs): + assert kwargs == {"headless": False} + + def launch(self, model, batch): + assert batch == 1 + calls.append("launch") + + def sync(self, data): + calls.append("sync") + + def close(self): + self.is_closed = True + calls.append("close") + + monkeypatch.setattr("motrix_deploy_motrixsim.viewer.RenderApp", Render) + runtime = MotrixSimRuntime(config(render=True), viewer_factory=MotrixSimViewer) + # CLI and application assembly obtain input before the world/viewer opens. + device = runtime.get_keyboard_device() + assert device is runtime.get_keyboard_device() + assert calls == [] and not runtime.viewer.is_running() + with pytest.raises(RuntimeError, match="Open the MotrixSim viewer"): + device.poll() + control = session(runtime) + runtime.bind_control_session(control) + with runtime: + assert runtime.viewer.is_running() + assert device is runtime.get_keyboard_device() + device.poll() + assert not device.is_pressing("w") + assert calls == ["launch", "camera", "sync", "close"] + assert device is runtime.get_keyboard_device() + assert not runtime.viewer.is_running() + with pytest.raises(RuntimeError, match="Open the MotrixSim viewer"): + device.is_pressing("w") + runtime.close() + assert calls == ["launch", "camera", "sync", "close"] + held = {"w": True} + keyboard = MotrixSimKeyboard( + SimpleNamespace(is_key_pressed=lambda key: held[key], is_key_just_pressed=lambda key: held[key]) + ) + keyboard.poll() + assert keyboard.is_key_down("w") and keyboard.is_pressing("w") + held["w"] = False + keyboard.poll() + assert keyboard.is_key_up("w") and not keyboard.is_pressing("w") diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py index 5ea4ef33..99b283b0 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py @@ -96,7 +96,6 @@ def bind(self) -> None: "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", diff --git a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py index b247b1de..5c9fdb17 100644 --- a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py +++ b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py @@ -470,9 +470,6 @@ def test_direct_scene_cfg_headless_uses_supplied_robot_floor_and_placement() -> 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") diff --git a/motrix_deploy_tasks/tests/test_cli.py b/motrix_deploy_tasks/tests/test_cli.py index f82cf230..d959bc8a 100644 --- a/motrix_deploy_tasks/tests/test_cli.py +++ b/motrix_deploy_tasks/tests/test_cli.py @@ -258,7 +258,7 @@ def test_hardware_hydra_config_accepts_explicit_gain_overrides() -> None: 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]) -@pytest.mark.parametrize("backend", ["mujoco"]) +@pytest.mark.parametrize("backend", ["mujoco", "motrixsim"]) @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" diff --git a/pyproject.toml b/pyproject.toml index c6099c89..588d4c24 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,6 +7,7 @@ requires-python = "==3.10.*" dependencies = [ "motrix-deploy", "motrix-deploy-mujoco", + "motrix-deploy-motrixsim", "motrix-deploy-tasks", "motrix-env-motrixsim", "motrix-env-mujoco", @@ -77,6 +78,7 @@ members = [ "motrix_robots", "motrix_deploy", "motrix_deploy_mujoco", + "motrix_deploy_motrixsim", "motrix_deploy_unitree", "motrix_deploy_tasks", "motrix_env_core", @@ -121,6 +123,7 @@ motrix-robots = { workspace = true } motrix-env-core = { workspace = true } motrix-deploy = { workspace = true } motrix-deploy-mujoco = { workspace = true } +motrix-deploy-motrixsim = { workspace = true } motrix-deploy-unitree = { workspace = true } motrix-deploy-tasks = { workspace = true } motrix-env-motrixsim = { workspace = true } diff --git a/uv.lock b/uv.lock index 1fc1a259..be301684 100644 --- a/uv.lock +++ b/uv.lock @@ -19,6 +19,7 @@ conflicts = [[ [manifest] members = [ "motrix-deploy", + "motrix-deploy-motrixsim", "motrix-deploy-mujoco", "motrix-deploy-tasks", "motrix-deploy-unitree", @@ -933,6 +934,23 @@ requires-dist = [ { name = "pydantic", specifier = "===2.12.5" }, ] +[[package]] +name = "motrix-deploy-motrixsim" +version = "0.4.0b0" +source = { editable = "motrix_deploy_motrixsim" } +dependencies = [ + { name = "motrix-deploy" }, + { name = "motrix-env-motrixsim" }, + { name = "numpy" }, +] + +[package.metadata] +requires-dist = [ + { name = "motrix-deploy", editable = "motrix_deploy" }, + { name = "motrix-env-motrixsim", editable = "motrix_env_motrixsim" }, + { name = "numpy", specifier = ">=1.26" }, +] + [[package]] name = "motrix-deploy-mujoco" version = "0.4.0b0" @@ -1109,6 +1127,7 @@ version = "0.4.0b0" source = { virtual = "." } dependencies = [ { name = "motrix-deploy" }, + { name = "motrix-deploy-motrixsim" }, { name = "motrix-deploy-mujoco" }, { name = "motrix-deploy-tasks" }, { name = "motrix-env-motrixsim" }, @@ -1178,6 +1197,7 @@ requires-dist = [ { name = "amdsmi", marker = "platform_machine == 'x86_64' and sys_platform == 'linux' and extra == 'rocm'" }, { name = "autodocsumm", marker = "extra == 'docs'" }, { name = "motrix-deploy", editable = "motrix_deploy" }, + { name = "motrix-deploy-motrixsim", editable = "motrix_deploy_motrixsim" }, { name = "motrix-deploy-mujoco", editable = "motrix_deploy_mujoco" }, { name = "motrix-deploy-tasks", editable = "motrix_deploy_tasks" }, { name = "motrix-deploy-unitree", marker = "extra == 'unitree'", editable = "motrix_deploy_unitree" },